#include "MTracking.h"

#include "MLogManip.h"

#include "dkc.h"

#include "SlaStars.h"

#include "MCosy.h"
#include "MStarguider.h"

#include "MDriveCom.h"

#include "MMoonPointing.h"

ClassImp(MTracking);

using namespace std;

//#define EXPERT
#undef EXPERT

MTracking::MTracking(MCosy *cosy)
    : MSlewing(cosy), MThread("MTracking"), fSlalib(fCosy->fObservatory),
    fTrackAcc(0, 0), fWobbleOffset(-1), fWobbleAngle(0), fOut(0)
{
    fMoon = new MMoonPointing("MoonShadowOffsets.root");
}

MTracking::~MTracking()
{
    delete fMoon;
}

// --------------------------------------------------------------------------
//
// Sets the tracking velocity
//
// The velocities are given in a ZdAz object in re/min. Return kTRUE
// in case of success, kFALSE in case of failure.
//
Bool_t MTracking::SetVelocity(const ZdAz &v)
{
    const Double_t vrzd = fCosy->fMac2->GetVelRes();
    const Double_t vraz = fCosy->fMac1->GetVelRes();

    //
    // Send the new velocities for both axes.
    //
    fCosy->fMac2->SendSDO(0x3007, (LWORD_t)(v.Zd()*vrzd));  // SetRpmVelocity [re/min]
    fCosy->fMac1->SendSDO(0x3007, (LWORD_t)(v.Az()*vraz));  // SetRpmVelocity [re/min]

    //
    // Wait for the objects to be acknoledged.
    //
    fCosy->fMac2->WaitForSdo(0x3007, 0, 100);
    fCosy->fMac1->WaitForSdo(0x3007, 0, 100);

    //
    // If the waiting for the objects wasn't interrupted return kTRUE
    //
    if (!Break())
        return kTRUE;

    fCosy->PrintError();

    //
    // print a message if the interruption was due to a Can-node Error
    //
    if (fCosy->HasError())
        gLog << err << "ERROR - while setting tracking velocity (SDO #3006)" << endl;

    return kFALSE;
}

// --------------------------------------------------------------------------
//
// Initializes Tracking mode
//
// Initializes the accelerations of both axes with 90% of the maximum
// acceleration. Set the status for moving and tracking and starts thr
// revolution mode.
//
bool MTracking::InitTracking()
{
    // FIXME? Handling of Zombie OK?
    if (fCosy->fMac1->IsZombieNode() || fCosy->fMac2->IsZombieNode())
        return false;

    //
    // Start revolution mode
    //
    if (!SetAcc(fCosy->fMac2, fTrackAcc.Zd()))
        return false;

    if (!SetAcc(fCosy->fMac1, fTrackAcc.Az()))
        return false;

    fCosy->fMac2->SetRpmMode(TRUE);
    if (fCosy->fMac2->IsZombieNode())
        return false;

    fCosy->fMac1->SetRpmMode(TRUE);
    if (fCosy->fMac1->IsZombieNode())
        return false;

    fCosy->SetStatus(MDriveCom::kMoving | MDriveCom::kTracking);

    return true;
}

/*
void MTracking::StopTracking()
{
    //
    // Set status to Stopping
    //
    fCosy->SetStatus(MDriveCom::kStopping);

    //
    // set deceleration to 50%
    //
    cout << "Stopping tracking (dec=20%)..." << endl;
    fCosy->fMac1->SetDeceleration(0.2*fMac1->GetVelRes());
    fCosy->fMac2->SetDeceleration(0.2*fMac2->GetVelRes());

    fCosy->fMac2->SendSDO(0x3006, 1, (LWORD_t)0);  // SetRpmVelocity [re/min]
    fCosy->fMac1->SendSDO(0x3006, 1, (LWORD_t)0);  // SetRpmVelocity [re/min]
    fCosy->fMac2->WaitForSdo(0x3006, 1);
    fCosy->fMac1->WaitForSdo(0x3006, 1);

    cout << "Waiting for end of movement..." << endl;
    fCosy->WaitForEndMovement();

    //
    // Wait for the objects to be OKed.
    //
    fCosy->fMac1->SetRpmMode(FALSE);
    fCosy->fMac2->SetRpmMode(FALSE);

    //
    // Wait for the movement to really be finished.
    //
    //cout << "Waiting for end of movement..." << endl;
    //WaitForEndMovement();

    //
    // Check whether everything works fine.
    //
    fCosy->CheckForError();
    cout << "Movement stopped." << endl;
}
*/

// --------------------------------------------------------------------------
//
// Limits the speed. 
//
// This function should work as a limiter. If a tracking error is too large
// to be corrected fast enough we would get enormous velocities. These
// velocities are limited to the maximum velocity.
//
Bool_t MTracking::LimitSpeed(const ZdAz &vt) const
{
    // vt [deg/min]

    // We can set a maximum speed here
    // And we can limit the change of the speed (which is done
    //  by acceleration in the drive anyway)

    return kTRUE;
/*
    // vt[re/min]

    // Calculate approximate velocity of both axis
    ZdAz vcalc = fSlalib.GetApproxVel(fCosy->fRaDec);  // [rad/rad]

    //vcalc *= 1./(24*60);          // [U_tel/min]
    //vcalc *= fCosy->kGearTot; // [U_mot/min]
    //vcalc *= fCosy->kResRE;   // [re/min]

    vcalc *= fCosy->kGearTot*fCosy->kResRE/(24*60); // [re/min]

    // Set return code
    Bool_t rc = kFALSE;

    //
    // How to limit the speed. If the wind comes and blowes
    // we cannot forbid changing of the sign. But on the other hand
    // we don't want fast changes!
    //
    ULong_t vrzd = fCosy->fMac1->GetVelRes();
    ULong_t vraz = fCosy->fMac2->GetVelRes();

#define sgn(x) (x<0?-1:1)

    //
    // When speed changes sign, the maximum allowed speed
    // is 25% of the |v|
    //
    //const Float_t limit    = 0.25;

    //
    // The maximum allowed speed while tracking is 10%
    //
    const Float_t maxtrack = 0.1;

    if (fabs(vt->Az()) > maxtrack*vraz)
    {
        vt->Az(maxtrack*vraz*sgn(vcalc.Az()));
        gLog << warn << "Warning: Azimuth speed limit (" << maxtrack*100 << "%) exceeded (" << fabs(vt->Az()) << " > " << maxtrack*vraz << ")... limited." << endl;
        gLog << "Vcalc: " << vcalc.Zd() << " " << vcalc.Az() << "re/min" <<endl;
        rc=kTRUE;
    }
    if (fabs(vt->Zd()) > maxtrack*vrzd)
    {
        vt->Zd(maxtrack*vrzd*sgn(vcalc.Zd()));
        gLog << warn << "Warning: Altitude speed limit (" << maxtrack*100 << "%) exceeded (" << fabs(vt->Zd()) <<" > " << maxtrack*vrzd << ")... limited." << endl;
        gLog << "Vcalc: " << vcalc.Zd() << " " << vcalc.Az() << "re/min" <<endl;
        rc=kTRUE;
    }
    return rc;
    */
}

Bool_t MTracking::UpdateSlalib(SlaPlanets &sla)
{
    if (fTrackType<0)
        sla.Set(fTrackPos/TMath::DegToRad());
    else
        sla.SetPlanet((ePlanets_t)(fTrackType&0xff));

    if (fTrackType==(kEMoon|0x100))
    {
        if (fMoon->IsZombie())
        {
            gLog << err << "ERROR - Could not initialize MMoonPointing." << endl;
            return kFALSE;
        }

        //moon.SetOffsetShadow(fWobbleAngle);
        //moon.SetOffsetWobble(fWobbleOffset);

        const ZdAz za = sla.GetZdAzRad();

        ZdAz srcpos, pointpos;
        if (!fMoon->CalcPosition(za, srcpos, pointpos))
        {
            gLog << err << "ERROR - Calculation of moon shadow pointing position failed." << endl;
            gLog << "        WoAngle    =" << fWobbleAngle << endl;
            gLog << "        WoOffset   =" << fWobbleOffset << endl;
            gLog << "        TrackType  =" << fTrackType << endl;
            gLog << "        TT&0xff    =" << (fTrackType&0xff) << endl;
            gLog << "        mjd        =" << sla.GetMjd() << endl;
            gLog << "        za.Zd      =" << za.Zd() << endl;
            gLog << "        za.Az      =" << za.Az() << endl;
            gLog << "        srcpos.Zd  =" << srcpos.Zd() << endl;
            gLog << "        srcpos.Az  =" << srcpos.Az() << endl;
            gLog << "        pointpos.Zd=" << pointpos.Zd() << endl;
            gLog << "        pointpos.Az=" << pointpos.Az() << endl;
            return kFALSE;
        }

        sla.Set(pointpos/TMath::DegToRad());

//        return kTRUE;

        //RaDec rd = sla.GetRaDec();
        //ZdAz  zd = sla.GetZdAz();

        // Ra/Dec, Zd/Az from pointpos
        // NEW: Source pos from srcpos
    }
        /*
    else
        if (fWobbleOffset>0)
        {
            MPositionOffsetCalc calc(fWobbleOffset, fWobbleAngle);
            const ZdAz offset = calc.GetOffset(sla.GetZdAzRad());
            //if (srcpos.Zd()==0)
            //    return kFALSE;
            sla.ApplyOffsetAltAz(offset);
        }
        */
    return kTRUE;
}

Bool_t MTracking::UpdateSlalib(Double_t dt)
{
    fSlalib.Now(dt);
    if (!UpdateSlalib(fSlalib))
        return kFALSE;

    return kTRUE;
}

Bool_t MTracking::UpdateSlalib(SlaPlanets &sla, Double_t mjd)
{
    sla.SetMjd(mjd);
    return UpdateSlalib(sla);
}

bool MTracking::Move()
{
    const RaDec &dst = fSlalib.GetRaDecRad();

    ZdAz dest = fSlalib.GetZdAzRad();

    if (fTrackType>=0)
       gLog << all << fSlalib.GetTime() << ": Tracking Planet with Id " << fTrackType << " (" << fWobbleOffset << ", " << fWobbleAngle << ")" << endl;
    gLog << all << fSlalib.GetTime() << ": Track Position " << dst.Ra()*kRad2Deg/15 << "h, " << dst.Dec()*kRad2Deg <<"deg" << endl;

    // If the star is culminating behind the zenith (South) we want to
    // align the azimuth angle between -180 and 180deg. If the star is
    // culminating before the zenith (north) we want the star to be
    // aligned between -180 and 180deg (which is the default of CalcZdAz)

#ifdef FACT
    if (fSlalib.GetPhi()>dst.Dec() && dest.Az()>0)
    {
        // align az from -180/180 to 0/360
        gLog << inf2 << "Star culminating behind zenith: Adding 360deg to Azimuth " << dest.Az()*kRad2Deg << endl;
        dest.Az(dest.Az() - TMath::TwoPi());
    }
#else
    if (fSlalib.GetPhi()>dst.Dec() && dest.Az()<0)
    {
        // align az from -180/180 to 0/360
        gLog << inf2 << "Star culminating behind zenith: Adding 360deg to Azimuth " << dest.Az()*kRad2Deg << endl;
        dest.Az(dest.Az() + TMath::TwoPi());
    }
#endif

    // Position the telescope to the current local position of the
    // star. Do not reposition but start the tracking after the
    // first positioning step
    if (!SetPosition(dest, kTRUE))
    {
        gLog << err << "ERROR - Cannot start tracking, positioning failed." << endl;
        return false;
    }

    return true;
}

void MTracking::TrackPosition(const RaDec &dst)
{
    fTrackPos  = dst;
    fTrackType = -1;

    // Just for convenience
    fMoon->SetOffsetShadow(0);
    fMoon->SetOffsetWobble(0);

    // Start tracking
    Track();
}

void MTracking::TrackPlanet(ePlanets_t planet)
{
    fTrackPos  = RaDec();
    fTrackType = planet;

    // Just for convenience
    fMoon->SetOffsetShadow(0);
    fMoon->SetOffsetWobble(0);

    // Start tracking
    Track();
}

void MTracking::TrackMoon(Double_t wobble, Double_t offset)
{
    fTrackPos  = RaDec();
    fTrackType = kEMoon|0x100;

    fWobbleOffset = TMath::DegToRad()*wobble;
    fWobbleAngle  = TMath::DegToRad()*offset;

    fMoon->SetOffsetShadow(fWobbleAngle);
    fMoon->SetOffsetWobble(fWobbleOffset);

    // Start tracking
    Track();
}

void MTracking::Track() // ra, dec [rad]
{
    // Position to corrent nominal position
    if (!UpdateSlalib())
        return;

    if (!Move())
        return;

    //fCosy->fRaDec = fSlalib.GetRaDec();

    // Initialize Tracker (slalib or starguider)
    RunThread();

    //
    // Init accelerations and Rpm Mode
    //
    if (!InitTracking())
    {
        fCosy->StopMovement();
        return;
    }

    // Get current nominal local position
    if (!UpdateSlalib())
    {
        fCosy->StopMovement();
        return;
    }

    ZdAz pos = fSlalib.GetZdAzRad();

    // Some output
    XY xy(TMath::RadToDeg()*fCosy->fRaDec.Ra()/15, TMath::RadToDeg()*fCosy->fRaDec.Dec());
    gLog << all << fSlalib.GetTime() << " - Start Tracking: Ra=" << xy.X() << "h Dec=";
    gLog << xy.Y() << "\xb0 @ Zd=" << pos.Zd()*kRad2Deg <<"deg Az=" << pos.Az()*kRad2Deg <<"deg" << endl;

    //
    // We want to reach the theoretical position exactly in about 0.5s
    //
    // *OLD*const float dt = 1;  // 1 second
#ifdef FACT
    const float dt = 7.6;//3;  // 2 second
#else
    const float dt = 5;//3;  // 2 second
#endif
    while (!Break()/* && !fCosy->HasError() && !fCosy->HasZombie()*/)
    {
        /*
         // Code for position control mode
         // Set: S-0-0001
         // Set: S-0-0002
         // Set: P-0-0099
         // Set: P-0-0187 (Spline)
         // Replace S-0-0258 by S-0-0047 in communication

        sla.Now();
        const ZdAz pointing = sla.CalcZdAz(fCosy->fRaDec); // [rad]
        ZdAz dest = fCosy->AlignTrackingPos(pointing);     // fix ambiguity
        dest = fCosy->fBending(dest);            // [rad]
        if (!fCosy->CheckRange(dest))
            break;
        dest *= 1./TMath::TwoPi(); //[rev]
        fCosy->fMac2->SendSDO(0x6004, (LWORD_t)(dest.Zd()*fCosy->fMac2->GetPosRes()+.5), false);
        fCosy->fMac1->SendSDO(0x6004, (LWORD_t)(dest.Az()*fCosy->fMac1->GetPosRes()+.5), false);
        usleep(10000); // 10ms
        continue;
        */

        //
        // Request Target position for Now+dt
        //
        if (!UpdateSlalib(dt))
            break;

        //
        // Request nominal position for this time in the future (To+dt)
        //
        const ZdAz pointing = fSlalib.GetZdAzRad();
        ZdAz dest = fCosy->AlignTrackingPos(pointing);     // fix ambiguity

        //ZdAz repos;
        dest = fCosy->fBending(dest);            // [rad]
        if (!fCosy->CheckRange(dest))
            break;

        // Destination position at t+dt in re-units
        dest *= TMath::RadToDeg();  // [re]

        const ZdAz sepos = fCosy->GetSePos()*360;  // [deg]

        // Now calculate the distance to move from now
        // to a time in t+dt.
        dest -= sepos;

        //
        // Velocity to go [re/min] to reach the right position at time t+dt
        // correct for the duration of RaDec2AltAz
        //
        const ZdAz v  = dest * 60/dt;  // [deg/min]
        const ZdAz vt = v/360;         // [rpm]

        //Double_t kpZd = TMath::Abs(fCosy->fTrackingError.Zd()*TMath::RadToDeg()*60*4);
        //Double_t kpAz = TMath::Abs(fCosy->fTrackingError.Az()*TMath::RadToDeg()*60*12);
        //v.Zd(v.Zd()*(1+TMath::Min(0.3, kpZd)));
        //v.Az(v.Az()*(1+TMath::Min(0.3, kpAz)));

        if (LimitSpeed(v))
        {
            gLog << dbg << "vt: " << v.Zd() << " " << v.Az() << "re/min" << endl;
            gLog << "Dest: " << dest.Zd() << " " << dest.Az() << endl;
        }

        //
        // check if the drive is fast enough to follow the star
        //
        if (TMath::Abs(vt.Zd())>0.5*fCosy->fMac2->GetVelMaxRev() ||
            TMath::Abs(vt.Az())>0.5*fCosy->fMac1->GetVelMaxRev())
        {
            gLog << err << "ERROR - Tracking speed faster than 50% of possible maximum velocity." << endl;
            gLog << "Zd: " << vt.Zd() << "   Az: " << vt.Az() << endl;
            break;
        }

        //
        // Set theoretical velocity (as early after calculation as possible)
        // Maybe we should attenuate the changes
        //
        if (!SetVelocity(vt))
            break;

        // Now the tracking procedure is finished and we have some time
        // left to set the nominal pointing position for the starguider
        // if available. This should be thread safe. Doing this in
        // UpdateSlalib would also update it from the Thread
        // which leads to problems.
        if (fCosy->fStarguider)
            fCosy->fStarguider->SetPointingPosition(fSlalib.GetRaDec());

        //
        // Update speed as often as possible.
        // make sure, that dt is around 10 times larger than the
        // update time
        //
        // The loop should not be executed faster than the ramp of
        // a change in the velocity can be followed.
        // (This is important on fast machines >500MHz)
        //
#ifdef FACT
        usleep(760000/4); // 1.4s
#else
        usleep(1000000);
#endif

        //
        // If we would do it in the beginnign of the loop it can happen
        // that we check before we got the feedback through the pdo
        //
        if (!fCosy->fMac1->IsRpmActive() || !fCosy->fMac2->IsRpmActive())
        {
            gLog << warn << fSlalib.GetTime() << " - RPM mode not active anymore." << endl;
            break;
        }
    }

    if (fCosy->Break())
        gLog << all << fSlalib.GetTime() << " - Break signal received." << endl;
    if (fCosy->HasError())
        gLog << all << fSlalib.GetTime() << " - HasError received." << endl;
    if (fCosy->HasZombie())
        gLog << all << fSlalib.GetTime() << " - HasZombie received." << endl;

    fSlalib.Now();

    CancelThread();
    fCosy->fMJD = 0;

    // If CancelPoints are used we have to make this a Cleanup!
    fCosy->StopMovement();

    gLog << all << fSlalib.GetTime() << " - Tracking stopped @ Zd=";
    gLog << fCosy->fZdAzSoll.Zd()*TMath::RadToDeg() <<"deg Az=";
    gLog << fCosy->fZdAzSoll.Az()*TMath::RadToDeg() <<"deg" << endl;
}

Int_t MTracking::Thread()
{
    if (!fCosy->fMac1 || !fCosy->fMac2)
        return 3;

    gLog << inf2 << "- Tracking Thread started (" << MTime(-1) << ")" << endl;

    //SlaPlanets sla(fSlalib.GetObservatoryKey());
    SlaPlanets sla(MObservatory::kMagic1);
    sla.Now();

    if (!UpdateSlalib(sla))
        gLog << err << "UpdateSlalib in Thread failed." << endl;

    //
    // only update fTrackingError while tracking
    //
    bool phca1=false;
    bool phcaz=false;

    while (1)
    {
        // Make changes (eg wind) smoother - attenuation of control function
        // This is the time constant which defines how fast
        // you correct for external influences (like wind)
        //const float weight = 1.; //0.3;

        // Check for changes of the shaftencoder values
        fCosy->fMac1->ResetHasChangedPos2();
        fCosy->fMac2->ResetHasChangedPos2();
        do
        {
            phcaz = fCosy->fMac1->HasChangedPos2();
            phca1 = fCosy->fMac2->HasChangedPos2();

            usleep(1);

            TThread::CancelPoint();

        } while (!phca1 && !phcaz);

        // get current position and corresponding time of shaftencoders
        const ZdAz istse = fCosy->GetSePos()*TMath::TwoPi();  // [deg]

        const Double_t mjdaz = fCosy->fMac1->GetMjdPos2();
        const Double_t mjdzd = fCosy->fMac2->GetMjdPos2();


        // calculate offset for both axis (only one is needed)
        // if Shaftencoder changed position, calculate nominal position
        if (phca1)
        {
            UpdateSlalib(sla, mjdzd);

            ZdAz dummy = sla.GetZdAzRad();
            dummy = fCosy->AlignTrackingPos(dummy);
            fCosy->fRaDec = sla.GetRaDecRad();
            fCosy->fHourAngle = sla.GetHourAngle();
            fCosy->fMJD = mjdaz;
            fCosy->fZdAzSoll.Zd(dummy.Zd());
            fCosy->fTrackingError.Zd(fCosy->fBending(dummy).Zd()-istse.Zd());

            TThread::CancelPoint();
        }
        if (phcaz)
        {
            UpdateSlalib(sla, mjdaz);

            ZdAz dummy = sla.GetZdAzRad();
            dummy = fCosy->AlignTrackingPos(dummy);
            fCosy->fRaDec = sla.GetRaDecRad();
            fCosy->fHourAngle = sla.GetHourAngle();
            fCosy->fMJD = mjdaz;
            fCosy->fZdAzSoll.Az(dummy.Az());
            fCosy->fTrackingError.Az(fCosy->fBending(dummy).Az()-istse.Az());

            TThread::CancelPoint();
        }
    }

    gLog << inf2 << "- Tracking Thread done. (" << MTime(-1) << ")" << endl;
    return 0;
}