#include "MTracking.h"

#include "macs.h"
#include "shaftencoder.h"

#include "MCosy.h"
#include "SlaStars.h"

#include "MDriveCom.h"

ClassImp(MTracking);

//#define EXPERT
#undef EXPERT

// --------------------------------------------------------------------------
//
// 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 (!SetAccDec(fCosy->fMac2, fTrackAcc, fTrackDec))
        return false;

    if (!SetAccDec(fCosy->fMac1, fTrackAcc, fTrackDec))
        return false;

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

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

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

    return true;
}

// --------------------------------------------------------------------------
//
// 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(ZdAz *vt, const ZdAz &vcalc) const
{
    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)
    {
        lout << "Warning: Azimuth speed limit (" << maxtrack*100 << "%) exceeded (" << fabs(vt->Az()) << " > " << maxtrack*vraz << ")... limited." << endl;
        vt->Az(maxtrack*vraz*sgn(vcalc.Az()));
        rc=kTRUE;
    }
    if (fabs(vt->Zd()) > maxtrack*vrzd)
    {
        lout << "Warning: Altitude speed limit (" << maxtrack*100 << "%) exceeded (" << fabs(vt->Zd()) <<" > " << maxtrack*vrzd << ")... limited." << endl;
        vt->Zd(maxtrack*vrzd*sgn(vcalc.Zd()));
        rc=kTRUE;
    }
    return rc;
}

// --------------------------------------------------------------------------
//
// 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)
{
    //
    // Send the new velocities for both axes.
    //
    fCosy->fMac2->SendSDO(0x3006, 1, (LWORD_t)v.Zd());  // SetRpmVelocity [re/min]
    fCosy->fMac1->SendSDO(0x3006, 1, (LWORD_t)v.Az());  // SetRpmVelocity [re/min]

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

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

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

    return kFALSE;
}

void MTracking::TrackPosition(const RaDec &dst) // ra, dec [rad]
{
    SlaStars sla(fCosy->fObservatory);

    //
    // Position to actual position
    //
    sla.Now();
    ZdAz dest = sla.CalcZdAz(dst);

    lout << sla.GetTime() << ": Track Position " << dst.Ra()*kRad2Deg/15 << "h, " << dst.Dec()*kRad2Deg <<"deg" << endl;

    // az between -180 and 180
    if (dst.Dec()>sla.GetPhi() && dest.Az()<0)
    {
        // align az between (roughly) 60 and 320
        lout << "Adding 360deg to Azimuth " << dest.Az()*kRad2Deg << endl;
        dest.Az(dest.Az() + TMath::Pi()*2);
    }
/*
    // FIXME: Determin tracking start point by star culmination
    if (dest.Az()<-TMath::Pi()/2)
    {
        lout << "Adding 360deg to Azimuth " << dest.Az()*kRad2Deg << endl;
        dest.Az(dest.Az() + TMath::Pi()*2);
    }

    if (dest.Az()>3*TMath::Pi()/2)
    {
        lout << "Substracting 360deg to Azimuth " << dest.Az()*kRad2Deg << endl;
        dest.Az(dest.Az() -TMath::Pi()*2);
    }
 */
    if (!SetPosition(dest, kTRUE))
    //if (!SetPosition(dest, kFALSE))
    {
        lout << "Error: Cannot start tracking, positioning failed." << endl;
        return;
    }

    //
    // calculate offset from present se position
    //
    const ZdAz sepos = fCosy->GetSePos()*fCosy->kGearRatio;

    if (!fCosy->RequestRePos())
        return;

    //
    // Estimate Offset before starting to track
    //
    fCosy->fOffset = sepos-fCosy->GetRePos();

    /*
     cout << "Sepos:  " << sepos.Zd() << "re, " << sepos.Az() << "re" << endl;
     cout << "Repos:  " << repos.Zd() << "re, " << repos.Az() << "re" << endl;
     cout << "Offset: " << fOffset.Zd() << "re, " << fOffset.Az() << "re" << endl;
     */

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

    XY xy(Rad2Deg(dst.Ra())*24/360, Rad2Deg(dst.Dec()));

    sla.Now();
//    lout << sla.GetTime() << " - Start tracking:";
//    lout << " Ra: " << xy.X() << "h  " << "Dec: " << xy.Y() << "\xb0" << endl;

/*#ifdef EXPERT
    ofstream fout("coordinates.txt");
    fout << xy;
    fout.close();
#endif
*/    //
    // Initialize Tracker (slalib or starguider)
    //
    fCosy->fRaDec = dst;

    // StartThread
    Start();

    ZdAz pos = sla.CalcZdAz(fCosy->fRaDec);

    lout << sla.GetTime() << " - Start Tracking: Ra=" <<xy.X() << "h Dec=";
    lout << xy.Y() << "\xb0 @ Zd=" << pos.Zd()*kRad2Deg <<"deg Az=" << pos.Az()*kRad2Deg <<"deg" << endl;

//---    ofstream fout("log/cosy.pos");
//---    fout << "Tracking:";
//---    fout << " Ra: " << Rad2Deg(dst.Ra())  << "\x9c  ";
//---    fout << "Dec: " << Rad2Deg(dst.Dec()) << "\x9c" << endl << endl;
//---    fout << "     Mjd/10ms    V/re/min/4" << endl;

    //
    // We want to reach the theoretical position exactly in about 0.5s
    //
    // *OLD*const float dt = 1;  // 1 second
    const float dt = 5;//3;  // 2 second
    while (!Break())
    {
        //
        // Request Target position for this moment
        //
        sla.Now(dt);

        //
        // Request theoretical Position for a time in the future (To+dt) from CPU
        //
        const ZdAz pointing = sla.CalcZdAz(fCosy->fRaDec); // soll pointing [rad]

        //lout << sla.GetTime() << pointing.Zd()*kRad2Deg << " " << pointing.Az()*kRad2Deg << endl;
        /*
        ZdAz dest;
        if (!AlignTrackingPos(pointing, dest))
            break;
            */
        ZdAz dest = fCosy->AlignTrackingPos(pointing);

        // lout << "DEST: " << dest.Zd()*kRad2Deg << " " <<dest.Az()*kRad2Deg << endl;

        ZdAz vcalc = sla.GetApproxVel(fCosy->fRaDec) * fCosy->kGearRatio2*4./60.;
        //lout << "Vcalc: " << dest.Zd() << " " << dest.Az() << endl;
        vcalc *= fCosy->kGearRatio2*4./60.; // [re/min]

        float dtime = -1;
        //if (kFALSE /*fUseStarguider*/)
        //    dtime = Starguider(sla.GetMjd(), dest);

        if (dtime<0)
        {
            dest = fCosy->fBending(dest);       // [rad]

            //lout << "DEST-BEND: " << dest.Zd()*kRad2Deg << " " <<dest.Az()*kRad2Deg << endl;
              
            if (!fCosy->CheckRange(dest))
                break;

            dest *= 16384/TMath::Pi()/2; // [se]
            dest *= fCosy->kGearRatio;          // [re]

            *fCosy->fOutRep << "> ReqRePos1 " << endl;

            //
            // Request absolute position of rotary encoder from Macs
            //
            if (!fCosy->RequestRePos())
                break;

            *fCosy->fOutRep << "> ReqRePos2 " << endl;

            //
            // distance between (To+dt) and To [re]
            // position time difference < 5usec
            // fOffset does the synchronization between the
            // Shaft- and the rotary encoders
            dest -= fCosy->GetRePos() + fCosy->fOffset;

            dtime = dt;

            ZdAz repos = fCosy->GetRePos();
    //        lout << "Repos: " << repos.Zd()/kGearRatio.X() << " " << repos.Az()*kGearRatio.Y() << endl;
   //         repos /= kGearRatio;
            repos /= 16384/TMath::Pi()/2;
            repos *= kRad2Deg;
        }

        //
        // 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.0/(dtime/*-(fMac2->GetTime()-sla)*/);

        //
        // calculate real velocity of future [re/min]
        // believing the Macs manual '/4' shouldn't be necessary, but it is.
        //
        ZdAz vt = v/4;
        if (LimitSpeed(&vt, vcalc))
        {
            lout << "Vcalc: " << vcalc.Zd() << " " << vcalc.Az() << "re/min" <<endl;
            lout << "vt: " << vt.Zd() << " " << vt.Az() << "re/min" << endl;
            lout << "Dest: " << dest.Zd() << " " << dest.Az() << endl;
        }              
        vt.Round();

        //
        // check if the drive is fast enough to follow the star
        //
        if (vt.Zd()>.9*fCosy->fMac1->GetVelRes() || vt.Az()>.9*fCosy->fMac2->GetVelRes())
        {
            lout << "Error: Tracking speed faster than 90% of possible maximum velocity." << endl;
            break;
        }

        //
        // Set theoretical velocity (as early after calculation as possible)
        // Maybe we should attenuate the changes
        //
        *fCosy->fOutRep << "> SetVelocity1 " << endl;
        if (!SetVelocity(vt))
            break;
        *fCosy->fOutRep << "> SetVelocity2 " << endl;

        //
        // Now do 'unnecessary' things
        //
        fCosy->fVelocity = vt/fCosy->kGearRatio2*4;

//---        const double mjd = fMac2->GetMjd();
//---        fout << setprecision(15) << setw(17) << mjd*60.*60.*24. << " ";
//---        fout << setw(4) << vt.Zd() << " ";
//---        fout << setw(4) << vt.Az() << endl;
        //
        // FIXME? Calculate an accuracy for the tracking system?
        // How good do we reach the calculated position in 'real'
        // re valus?
        //


        //
        // 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)
        //
        /*
        MTimeout t(1000);
        while (!t.HasTimedOut())
            usleep(1);
         */
        usleep(1000000); // 1s
        cout << "." << flush;
        //usleep(50000); // 0.05s
    }

    sla.Now();

    // StopThread
    Stop();

    fCosy->StopMovement();

    lout << sla.GetTime() << " - Tracking stopped." << endl;
}

void *MTracking::Thread()
{
    if (fCosy->fZd1->IsZombieNode() && fCosy->fZd2->IsZombieNode())
        return (void*)1;

    if (fCosy->fAz->IsZombieNode())
        return (void*)2;

    if (!fCosy->fMac1 || !fCosy->fMac2)
        return (void*)3;

    lout << "- Tracking Thread started..." << endl;

    SlaStars sla(fCosy->fObservatory);
    sla.Now();

    ZdAz old;
    ZdAz ist = fCosy->GetSePos();              // [se]

    ZdAz time;

    ZdAz sollzd = sla.CalcZdAz(fCosy->fRaDec); // [rad]
    ZdAz sollaz = sollzd;               // [rad]

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

    while (!HasStopFlag())
    {
        //
        // Make changes (eg wind) smoother - attenuation of control function
        //
        const float weight = 1.; //0.3;

        //
        // This is the time constant which defines how fast
        // you correct for external influences (like wind)
        //
        *fCosy->fOutRep << "> ResetPosHasChanged" << endl;
        fCosy->fZd1->ResetPosHasChanged();
        fCosy->fZd2->ResetPosHasChanged();
        fCosy->fAz->ResetPosHasChanged();
        *fCosy->fOutRep << "> Check for PosHasChanged" << endl;
        do
        {
            phca1 = fCosy->fZd1->PosHasChanged();
            phca2 = fCosy->fZd2->PosHasChanged();
            phcaz = fCosy->fAz->PosHasChanged();
            usleep(1);
        } while (!phca1 && !phca2 && !phcaz && !HasStopFlag());

        //---usleep(100000); // 0.1s

        *fCosy->fOutRep << "> Do Calculation" << endl;

        //
        // get position, where we are
        //
        old = ist;
        ist = fCosy->GetSePos(); // [se]

        //
        // if the position didn't change continue
        //
        /*---
         if ((int)ist.Zd() == (int)old.Zd() &&
         (int)ist.Az() == (int)old.Az())
         continue;
         */
        ZdAz istre = fCosy->GetRePosPdo();

        //
        // Get time from last shaftencoder position change (position: ist)
        // FIXME: I cannot take the avarage
        //
        // FIXME
        //time.Zd(fZd1->GetMjd());
        /* OLD* */
        if (fCosy->fZd1->GetMjd()>fCosy->fZd2->GetMjd())
            time.Zd(fCosy->fZd1->GetMjd());
        else
            time.Zd(fCosy->fZd2->GetMjd());

        //time.Zd((fZd1->GetMjd()+fZd2->GetMjd())/2.0);
        time.Az(fCosy->fAz->GetMjd());

        //
        // if Shaftencoder changed position
        // calculate were we should be
        //
        if (phca1 || phca2 /*(int)ist.Zd() != (int)old.Zd()*/)
        {
            sollzd = sla.CalcZdAz(fCosy->fRaDec, time.Zd()); // [rad]
            /*
            ZdAz dummy = fBending(sla.CalcZdAz(fRaDec));
            sollzd = CorrectTarget(ist, dummy); // [se]
            */
            fCosy->fOffset.Zd(fCosy->fOffset.Zd()*(1.-weight)+(ist.Zd()*fCosy->kGearRatio.X()-istre.Zd())*weight);
        }

        if (phcaz /*(int)ist.Az() != (int)old.Az()*/)
        {
            sollaz = sla.CalcZdAz(fCosy->fRaDec, time.Az()); // [rad]
            /*
            ZdAz dummy = fBending(sla.CalcZdAz(fRaDec));
            sollaz = CorrectTarget(ist, dummy); // [se]
            */
            fCosy->fOffset.Az(fCosy->fOffset.Az()*(1.-weight)+(ist.Az()*fCosy->kGearRatio.Y()-istre.Az())*weight);
        }

        ZdAz soll(sollzd.Zd(), sollaz.Az()); // [rad]

        fCosy->fZdAzSoll = fCosy->AlignTrackingPos(soll);

        ist *= TMath::Pi()*2/16384;
        soll = fCosy->fBending(fCosy->fZdAzSoll);
        fCosy->fTrackingError.Set(ist.Zd()-soll.Zd(), ist.Az()-soll.Az());

        //---            fout << setprecision(15) << setw(17) << time.Zd()*60.*60.*24. << " ";
        //---            fout << setprecision(5)  << setw(7)  << fTrackingError.Zd() << "  ";
        //---            fout << setprecision(15) << setw(17) << time.Az()*60.*60.*24. << " ";
        //---            fout << setprecision(5)  << setw(7)  << fTrackingError.Az() << endl;
    }

    lout << "- Tracking Thread done." << endl;

    return 0;
}