#include "MTracking.h"

#include "MLogManip.h"

#include "dkc.h"
//#include "shaftencoder.h"

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

#include "MDriveCom.h"

ClassImp(MTracking);

using namespace std;

//#define EXPERT
#undef EXPERT

// --------------------------------------------------------------------------
//
// 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(0x3006, 1, (LWORD_t)(v.Zd()*vrzd));  // SetRpmVelocity [re/min]
    fCosy->fMac1->SendSDO(0x3006, 1, (LWORD_t)(v.Az()*vraz));  // SetRpmVelocity [re/min]

    //
    // Wait for the objects to be acknoledged.
    //
    fCosy->fMac2->WaitForSdo(0x3006, 1, 100);
    fCosy->fMac1->WaitForSdo(0x3006, 1, 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 SlaStars &sla) 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 = sla.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;
    */
}

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

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

    gLog << all << sla.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)
    if (sla.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());
    }

    // 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;
    }

    // Initialize Tracker (slalib or starguider)
    fCosy->fRaDec = dst;

    RunThread();

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

    // Get current nominal local position
    sla.Now();
    ZdAz pos = sla.CalcZdAz(fCosy->fRaDec);

    // Some output
    XY xy(TMath::RadToDeg()*dst.Ra()*24/360, TMath::RadToDeg()*dst.Dec());
    gLog << all << sla.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
    const float dt = 5;//3;  // 2 second
    while (!Break()/* && !fCosy->HasError() && !fCosy->HasZombie()*/)
    {
        /*
        sla.Now(1);
        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, 0, (LWORD_t)(dest.Zd()*fCosy->fMac2->GetPosRes()+.5), false);
        fCosy->fMac1->SendSDO(0x6004, 0, (LWORD_t)(dest.Az()*fCosy->fMac1->GetPosRes()+.5), false);
        usleep(100000); // 1s
        continue;
        */

        //
        // Request Target position for Now+dt
        //
        sla.Now(dt);

        //
        // Request nominal position for this time in the future (To+dt)
        //
        const ZdAz pointing = sla.CalcZdAz(fCosy->fRaDec); // [rad]
        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, sla))
        {
            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;

        //
        // 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)
        //
        usleep(1000000); // 1s
    }

    sla.Now();

    CancelThread();

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

    gLog << all << sla.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;

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

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

    //
    // 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)
        {
            ZdAz dummy = sla.CalcZdAz(fCosy->fRaDec, mjdzd);
            dummy = fCosy->AlignTrackingPos(dummy);
            fCosy->fZdAzSoll.Zd(dummy.Zd());
            soll.Zd(fCosy->fBending(dummy).Zd()); // [rad]

            fCosy->fTrackingError.Zd(soll.Zd()-istse.Zd());

            TThread::CancelPoint();
        }
        if (phcaz)
        {
            ZdAz dummy = sla.CalcZdAz(fCosy->fRaDec, mjdaz);
            dummy = fCosy->AlignTrackingPos(dummy);
            fCosy->fZdAzSoll.Az(dummy.Az());
            soll.Az(fCosy->fBending(dummy).Az()); // [rad]

            fCosy->fTrackingError.Az(soll.Az()-istse.Az());

            TThread::CancelPoint();
        }
    }

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