#include "MTracking.h"

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

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

#include "MDriveCom.h"

ClassImp(MTracking);

//#define EXPERT
#undef EXPERT

// --------------------------------------------------------------------------
//
// request the current positions from the rotary encoders.
// use GetRePos to get the psotions. If the request fails the function
// returns kFALSE, otherwise kTRUE
//
bool MTracking::RequestRePos()
{
    //
    // Send request
    //
    fCosy->fMac2->RequestSDO(0x6004);
    fCosy->fMac1->RequestSDO(0x6004);

    //
    // Wait until the objects are received.
    //
    fCosy->fMac2->WaitForSdo(0x6004);
    fCosy->fMac1->WaitForSdo(0x6004);

    //
    // If waiting was not interrupted everything is ok. return.
    //
    if (!Break())
        return true;

    //
    // If the waiting was interrupted due to a network error,
    // print some logging message.
    //
    if (fCosy->HasError())
        lout << "Error while requesting re pos from Macs (SDO #6004)" << endl;

    return false;
}

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

// --------------------------------------------------------------------
//
// Return pointing position of the telescope based on the
// Shaftencoders with interpolation with motor encoders.
//
// GetPointingPos [re]
//
ZdAz MTracking::GetPointingPosRE(Bool_t pdo) const
{
    // Conversion factor from se to re
    const XY re = fCosy->kGearTot/fCosy->kResSE; //[re/se]

    // Get current shaftencoder position of the telescope
    Double_t seposzd1 = ((fCosy->fZd1->GetPos()+8192)%16384)*re.X();
    Double_t seposzd2 = ((fCosy->fZd2->GetPos()+8192)%16384)*re.X();
    Double_t seposaz  =   fCosy->fAz->GetPos() *re.Y();

    // distance between (To+dt) and To [re]
    // position time difference < 5usec
    // fRePos does the synchronization between the
    // Shaft- and the rotary encoders
    const ZdAz repos = pdo ? fCosy->GetRePosPdo() : fCosy->GetRePos();

    // Calculate the part of one SE which the motors moved
    // since the last SE has changed its value
    const Double_t offzd1 = repos.Zd() - fCosy->fZd1->GetOffset();
    const Double_t offzd2 = repos.Zd() - fCosy->fZd2->GetOffset();
    const Double_t offaz  = repos.Az() - fCosy->fAz->GetOffset();

    // Correct for the direction in which the motor is moving
    // (in which the shaftencoders should change its values)
    if (offaz<0)
        seposaz += re.Y();
    if (offzd1<0)
        seposzd1 += re.X();
    if (offzd2<0)
        seposzd2 += re.X();

    // and interpolate the shaftencoder steps using the motor
    // encoder positon (Be carefull the minus-sign is important)
    seposzd1 += offzd1;
    seposzd2 -= offzd2;
    seposaz  += offaz;

    return ZdAz((seposzd1-seposzd2)/2, seposaz);
}

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;

    // 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
        lout << "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))
    {
        lout << "Error: Cannot start tracking, positioning failed." << endl;
        return;
    }

    //
    // calculate offset from present se position
    //
    //const ZdAz sepos = fCosy->GetSePos()*fCosy->kGearTot/fCosy->kResSE; //[re]
    if (!RequestRePos())
        return;

    // Estimate Offset before starting to track
    ZdAz repos = fCosy->GetRePos();
    fCosy->fZd1->SetOffset(repos.Zd());
    fCosy->fZd2->SetOffset(repos.Zd());
    fCosy->fAz->SetOffset(repos.Az());

    fCosy->SetTrackingPosRE(GetPointingPosRE());

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

    // StartThread
    Start();

    //
    // 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(Rad2Deg(dst.Ra())*24/360, Rad2Deg(dst.Dec()));
    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;

    //
    // 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 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 vcalc = sla.GetApproxVel(fCosy->fRaDec);
        //vcalc *= fCosy->kGearRatio2*4./60.; // [re/min]

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

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

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

            // Request absolute position of rotary encoder from Macs
            // Such that the RE position used in GetPointingPos is
            // as up-to-date as possible.
// DO I NEED THIS OR IS THE PDOPOS ENOUGH?
            if (!RequestRePos())
                break;

            // *NEW* offset handling
            // Get current position of the telescope and
            // forward this position to MCosy
            ZdAz sepos = GetPointingPosRE(); //[re]
            fCosy->SetTrackingPosRE(sepos);

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

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

            dtime = dt;
        }

        //
        // Velocity to go [re/min] to reach the right position at time t+dt
        // correct for the duration of RaDec2AltAz
        //
        /* --- OLD --- */
        ZdAz v = dest*60.0/dtime; //[re/min]
        /* --- NEW --- seems to work worse! */
        //const Double_t dtaz = sla.GetTime() - fCosy->fMac1->GetPosTime();
        //const Double_t dtzd = sla.GetTime() - fCosy->fMac2->GetPosTime();
        //
        //ZdAz v = dest*60.0;
        //v.Zd(v.Zd()/dtzd);
        //v.Az(v.Az()/dtaz);
        /* --- END --- */

        //*fCosy->fOutRep << "> Dt:  " << dtaz << "  " << dtzd << endl;

        if (LimitSpeed(&v, sla))
        {
            lout << "vt: " << v.Zd() << " " << v.Az() << "re/min" << endl;
            lout << "Dest: " << dest.Zd() << " " << dest.Az() << endl;
        }              

        //
        // calculate real velocity of future [re/min]
        // believing the Macs manual '/4' shouldn't be necessary, but it is.
        //
        ZdAz vt = v/4; //[re'/min]
        //lout << " " << vt.Zd() << " " << vt.Az() << " ";
        vt.Round();
        //lout << " " << vt.Zd() << " " << vt.Az() << endl;

        //
        // 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:  " << vt.Zd() << "  " << vt.Az() << endl;
        if (!SetVelocity(vt))
            break;
        //*fCosy->fOutRep << "> SetVelocity2 " << endl;

        //
        // Now do 'unnecessary' things (timing)
        //
        fCosy->fVelocity = vt*4/fCosy->kGear; // [U_mot/min]
        // *OLD* fVelocity = vt/kGearRatio2*4;

        if (fOut)
        {
            fOut->Lock("MTracking::TrackPosition");
            *fOut << "RE-REPORT " << MTime(-1) << " " << repos.Zd() << " " << repos.Az() <<" " << vt.Zd() << " " << vt.Az() << endl;
            fOut->UnLock("MTracking::TrackPosition");
        }

        //
        // 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
// *****FIXME****        cout << "." << flush;
    }

    sla.Now();

    // StopThread
    Stop();

    fCosy->StopMovement();

    lout << sla.GetTime() << " - Tracking stopped @ Zd=";
    lout << fCosy->fZdAzSoll.Zd()*TMath::RadToDeg() <<"deg Az=";
    lout << fCosy->fZdAzSoll.Az()*TMath::RadToDeg() <<"deg" << 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;

    //const XY re2se = fCosy->kGearTot/fCosy->kResSE; //[re/se]

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

    //ZdAz time;

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

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

    //ZdAz wasse = fCosy->GetSePos();
    //ZdAz oldse = fCosy->GetSePos();

    while (!HasStopFlag())
    {
        // 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->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());

        // Get time from last shaftencoder position change (position: ist)
        // FIXME: Is this correct?
        //        time.Az(fCosy->fMac1->GetMjd());
        //        time.Zd(fCosy->fMac2->GetMjd());

        //Double_t mjd1 = fCosy->fZd1->GetMjd();
        //Double_t mjd2 = fCosy->fZd2->GetMjd();
        //Double_t mjd0 = fCosy->fAz->GetMjd();

        Double_t mjdaz = fCosy->fMac1->GetPdoMjd();//mjd0;
        Double_t mjdzd = fCosy->fMac2->GetPdoMjd();//TMath::Max(mjd1, mjd2);

        // get current position of shaftencoders (interpolated
        // using motor encoders)
        const ZdAz istse = GetPointingPosRE(kTRUE)/fCosy->kGearTot*TMath::TwoPi();
        //const ZdAz istse = fCosy->GetSePosPdo();

        // calculate offset for both axis (only one is needed)
        // *NEW* offset handling
        //.const ZdAz offset = istre; //(istse*re2se - istre)*weight + fRePos*(weight-1);
        // if Shaftencoder changed position, calculate nominal position
        if (phca1 || phca2)
        {
            ZdAz dummy = sla.CalcZdAz(fCosy->fRaDec, mjdzd);
            dummy = fCosy->AlignTrackingPos(dummy);
            fCosy->fZdAzSoll.Zd(dummy.Zd());
            soll.Zd(fCosy->fBending(dummy).Zd()); // [rad]
        }
        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->fZdAzSoll = soll;

        // Calculate the aligned tracking posotion from 'soll'-position
        if (phca1 || phca2)
            fCosy->fTrackingError.Zd(soll.Zd()-istse.Zd());
        if (phcaz)
            fCosy->fTrackingError.Az(soll.Az()-istse.Az());
    }

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

    return 0;
}