#include "MPointing.h"

#include "MCosy.h"
#include "macs.h"
#include "MDriveCom.h"

ClassImp(MPointing);

//#define EXPERT
#undef EXPERT

// --------------------------------------------------------------------------
//
//  set the velocity and accelerations for position maneuvers.
//
//  The acceleratin is set as given (in percent of maximum).
//  The velocity is given in percent, depending on the ratio (<1 or >1)
//  one of the axis becomes a slower velocity. This is used for maneuvers
//  in which both axis are moved synchromously and should reach their
//  target position at the same time.
//
void MPointing::SetPosVelocity(const Float_t ratio, Float_t vel)
{
    //
    // Set velocities
    //
    const int vr = fCosy->fMac1->GetVelRes();
    vel *= vr;

    if (ratio<1)
    {
        fCosy->fMac1->SetVelocity(vel);
        fCosy->fMac2->SetVelocity(vel*ratio);
    }
    else
    {
        fCosy->fMac1->SetVelocity(vel/ratio);
        fCosy->fMac2->SetVelocity(vel);
    }
}

// --------------------------------------------------------------------------
//
// Does a relative positioning.
//
// The steps to move are given in a ZdAz object relative to the current
// position. The coordinates are given in Roteryencoder steps.
// Axis 1 is moved only if axe1==kTRUE, Axis 2 is moved only
// if Axis 2==kTRUE. The function waits for the movement to be finished.
//
void MPointing::DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2)
{
    if (fCosy->HasZombie())
        return;

    fCosy->SetStatus(MDriveCom::kMoving);

    if (axe1) fCosy->fMac2->StartRelPos(rd.Zd());
    if (axe2) fCosy->fMac1->StartRelPos(rd.Az());
#ifdef EXPERT
    cout << "Waiting for positioning..." << flush;
#endif
    if (axe1) fCosy->fMac2->WaitForSdo(0x6004, 1);
    if (axe2) fCosy->fMac1->WaitForSdo(0x6004, 1);

    fCosy->WaitForEndMovement();
#ifdef EXPERT
    cout << "done." << endl;
#endif
}

bool MPointing::SetAccDec(Macs *mac, Float_t acc, Float_t dec)
{
    const int vr = mac->GetVelRes();
    mac->SetAcceleration(acc*vr);
    mac->SetAcceleration(dec*vr);
    return !mac->IsZombieNode();
}

bool MPointing::Break()
{
    return fCosy->Break() || fCosy->HasError() || fCosy->HasZombie();
}

// --------------------------------------------------------------------------
//
// Move the telescope to the given position. The position must be given in
// a ZdAz object in rad.
//
// The first positioning is done absolutely. If we didn't reach the
// correct psotion we try to correct for this by 10 relative position
// maneuvers. If this doesn't help positioning failed.
//
// As a reference the shaftencoder values are used.
//
int MPointing::SetPosition(const ZdAz &dst, Bool_t track) // [rad]
{
    /*
    const ZdAz d = dst*kRad2Deg;

    MTime t(-1);
    lout << t << " - Target Position: " << d.Zd() << "deg, " << d.Az() << "deg (Zd/Az)" << endl;

    //
    // Calculate new target position (shortest distance to go)
    //
    const ZdAz src = fCosy->GetSePos(); // [se]

    //
    // Make sure that the motors are in sync mode (necessary if the
    // MACS has been rebooted from a Zombie state.
    //
    //InitSync();
    //if (fMac3->IsZombieNode())
    //    return false;

    //
    // Because we agreed on I don't search for the shortest move
    // anymore
    //
    // const ZdAz dest = CorrectTarget(src, dst);
    //
    ZdAz bend = fCosy->fBending(dst); // [rad]

    const ZdAz dest = bend*16384/2/TMath::Pi(); // [se]

    if (!fCosy->CheckRange(bend))
        return kFALSE;

    bend *= kRad2Deg;
    fCosy->fZdAzSoll = dst;

    cout << "Source        Zd: " << src.Zd()  << "se  Az:" << src.Az()  << "se" << endl;
    cout << "Destination   Zd: " << dst.Zd()*8192/TMath::Pi() << "se  Az:" << dst.Az()*8192/TMath::Pi()  << "se" << endl;
    cout << "Bend'd Dest   Zd: " << dest.Zd() << "se  Az:" << dest.Az() << "se" << endl;
    cout << "Bend'd Dest   Zd: " << bend.Zd() << "deg  Az:" << bend.Az() << "deg" << endl;

    int i;
    for (i=0; i<(track?1:10) && !Break(); i++)
    {

        lout << "- Step #" << i << endl;
        //
        // Get Shaft Encoder Positions
        //
        const ZdAz p=fCosy->GetSePos();

        //
        // calculate control deviation and rounded cd
        //
        ZdAz rd = dest-p; // [se]

        // ===========================================
        const ZdAz ist = dst-rd*TMath::Pi()/8192;

        const double p1 = ist.Zd()-19.0605/kRad2Deg;
        const double p2 = dst.Zd()-19.0605/kRad2Deg;

        const double f1 = (-26.0101*sin(p1)+443.761*ist.Zd())*8192/TMath::Pi();
        const double f2 = (-26.0101*sin(p2)+443.761*dst.Zd())*8192/TMath::Pi();
        // ===========================================

        ZdAz cd = rd;     // [se]
        cd.Round();

        //
        // Check if there is a control deviation on the axis
        //
        const Bool_t cdzd = (int)cd.Zd() ? kTRUE : kFALSE;
        const Bool_t cdaz = (int)cd.Az() ? kTRUE : kFALSE;

        //
        // check if we reached the correct position already
        //
        if (!cdzd && !cdaz)
        {
            t.Now();
            lout << t << " - Positioning done in " << i << (i==1?" step.":" steps.") << endl;
            fCosy->SetStatus(MDriveCom::kStopped);
            fCosy->fCom->Send("POSITION DONE");
            return TRUE;
        }

        //
        // change units from se to re
        //
        rd *= fCosy->kGearRatio; // [re]
        rd.Zd(f2-f1);

        //
        // Initialize Velocities so that we reach both positions
        // at the same time
        //
        if (i)
        {
            lout << "--- LO-SPEED ---" << endl;
            SetAccDec(fCosy->fMac1, 0.1, 0.1);
            SetAccDec(fCosy->fMac2, 0.1, 0.1);

            SetPosVelocity(1.0, 0.05);
        }
        else
        {
            const Double_t y = 15*fCosy->kGearRatio.Y();
            if (rd.Az()>-y && rd.Az()<y)
            {
                lout << "--- LO-SPEED Mac1 ---" << endl;
                SetAccDec(fCosy->fMac1, 0.05, 0.05);
            }
            else
                SetAccDec(fCosy->fMac1, fAcc, fDec);

            SetAccDec(fCosy->fMac2, fAcc, fDec);

            SetPosVelocity(fabs(rd.Ratio()), fVel);
        }

        rd.Round();

        // FIXME? Check for Error or Zombie?

        // cout << " + " << (int)cdzd << " " << (int)cdaz << endl;
        // cout << " + APOS:  Zd=" << setw(6) << p.Zd()  << "se   Az=" << setw(6) << p.Az()  << "se" << endl;
        // cout << " +       dZd=" << setw(6) << cd.Zd() << "se  dAz=" << setw(6) << cd.Az() << "se" << endl;
        // cout << " +       dZd=" << setw(6) << rd.Zd() << "re  dAz=" << setw(6) << rd.Az() << "re" << endl;
        // cout << " + Ratio: Zd=" << setw(6) << kGearRatio.X()  << "se   Az=" << setw(6) << kGearRatio.Y()  << "se" << endl;

        //
        // repositioning (relative)
        //

        lout << "- Do Relative Positioning..." << endl;
        DoRelPos(rd, cdzd, cdaz);
        lout << "- Relative Positioning Done" << endl;
    }


    return FALSE;*/

    const ZdAz d = dst*kRad2Deg;

    MTime t(-1);
    lout << t << " - Target Position: " << d.Zd() << "deg, " << d.Az() << "deg (Zd/Az)" << endl;

    //
    // Calculate new target position (shortest distance to go)
    //
    //const ZdAz src = fCosy->GetSePos(); // [se]

    //
    // Make sure that the motors are in sync mode (necessary if the
    // MACS has been rebooted from a Zombie state.
    //
    //InitSync();
    //if (fMac3->IsZombieNode())
    //    return false;

    //
    // Because we agreed on I don't search for the shortest move
    // anymore
    //
    // const ZdAz dest = CorrectTarget(src, dst);
    //
    ZdAz bend = fCosy->fBending(dst); // [rad]

    const ZdAz dest = bend*16384/2/TMath::Pi(); // [se]

    if (!fCosy->CheckRange(bend))
        return kFALSE;

    bend *= kRad2Deg;
    fCosy->fZdAzSoll = dst;

    //cout << "Source        Zd: " << src.Zd()  << "se  Az:" << src.Az()  << "se" << endl;
    //cout << "Destination   Zd: " << Rad2SE(dst.Zd()) << "se  Az:" << Rad2SE(dst.Az())  << "se" << endl;
    //cout << "Bend'd Dest   Zd: " << dest.Zd() << "se  Az:" << dest.Az() << "se" << endl;
    //cout << "Bend'd Dest   Zd: " << bend.Zd() << "deg  Az:" << bend.Az() << "deg" << endl;

    //
    // Set velocities
    //
    //const int vr = fCosy->fMac1->GetVelRes();

    int i;
    for (i=0; i<(track?1:10) && !Break()/*(fCosy->Break() || fCosy->HasError() || fCosy->HasZombie())*/; i++)
    {

        lout << "- Step #" << i << endl;
        //
        // Get Shaft Encoder Positions
        //
        const ZdAz p=fCosy->GetSePos();

        //
        // calculate control deviation and rounded cd
        //
        ZdAz rd = dest-p; // [se]

        // ===========================================
        const ZdAz ist = dst-rd*TMath::Pi()/8192;

        const double p1 = ist.Zd()-19.0605/kRad2Deg;
        const double p2 = dst.Zd()-19.0605/kRad2Deg;

        const double f1 = (-26.0101*sin(p1)+443.761*ist.Zd())*8192/TMath::Pi();
        const double f2 = (-26.0101*sin(p2)+443.761*dst.Zd())*8192/TMath::Pi();
        // ===========================================

        ZdAz cd = rd;     // [se]
        cd.Round();

        //
        // Check if there is a control deviation on the axis
        //
        const Bool_t cdzd = (int)cd.Zd() ? kTRUE : kFALSE;
        const Bool_t cdaz = (int)cd.Az() ? kTRUE : kFALSE;

        //
        // check if we reached the correct position already
        //
        if (!cdzd && !cdaz)
        {
            t.Now();
            lout << t << " - Positioning done in " << i << (i==1?" step.":" steps.") << endl;
            fCosy->SetStatus(MDriveCom::kStopped);
            fCosy->fCom->Send("POSITION DONE");
            return TRUE;
        }

        //
        // change units from se to re
        //
        rd *= fCosy->kGearRatio; // [re]
        rd.Zd(f2-f1);

        //
        // Initialize Velocities so that we reach both positions
        // at the same time
        //
/*        if (i)
        {
            fCosy->fMac1->SetAcceleration(0.1*vr);
            fCosy->fMac2->SetAcceleration(0.1*vr);

            fCosy->fMac1->SetDeceleration(0.1*vr);
            fCosy->fMac2->SetDeceleration(0.1*vr);

            fCosy->SetPosVelocity(1.0, 0.05);
        }
        else
        {
            if (rd.Az()>-15*fCosy->kGearRatio.Y() && rd.Az()<15*fCosy->kGearRatio.Y())
            {
#ifdef EXPERT
                cout << " -------------- LO ---------------- " << endl;
#endif
                fCosy->fMac1->SetAcceleration(0.05*vr);
                fCosy->fMac1->SetDeceleration(0.05*vr);
            }
            else
            {
#ifdef EXPERT
                cout << " -------------- HI ---------------- " << endl;
                fCosy->fMac1->SetAcceleration(0.4*vr);// 0.4
                fCosy->fMac1->SetDeceleration(0.4*vr);// 0.4
#else
                fCosy->fMac1->SetAcceleration(0.2*vr);
                fCosy->fMac1->SetDeceleration(0.1*vr);
#endif
            }

#ifdef EXPERT
            fCosy->fMac2->SetAcceleration(0.4*vr);// 0.4
            fCosy->fMac2->SetDeceleration(0.4*vr);// 0.4
            fCosy->SetPosVelocity(fabs(rd.Ratio()), 0.2); // fast: 0.6, slow: 0.2
#else
            fCosy->fMac2->SetAcceleration(0.2*vr);
            fCosy->fMac2->SetDeceleration(0.1*vr);
            fCosy->SetPosVelocity(fabs(rd.Ratio()), 0.1);
#endif
        }
        */
        if (i)
        {
            //lout << "--- LO-SPEED ---" << endl;
            SetAccDec(fCosy->fMac1, 0.1, 0.1);
            SetAccDec(fCosy->fMac2, 0.1, 0.1);

            SetPosVelocity(1.0, 0.05);
        }
        else
        {
            const Double_t y = 15*fCosy->kGearRatio.Y();
            if (rd.Az()>-y && rd.Az()<y)
            {
                //lout << "--- LO-SPEED Mac1 ---" << endl;
                SetAccDec(fCosy->fMac1, 0.05, 0.05);
            }
            else
                SetAccDec(fCosy->fMac1, fAcc, fDec);

            SetAccDec(fCosy->fMac2, fAcc, fDec);

            SetPosVelocity(fabs(rd.Ratio()), fVel);
        }

        rd.Round();

        // FIXME? Check for Error or Zombie?


        // cout << " + " << (int)cdzd << " " << (int)cdaz << endl;
        // cout << " + APOS:  Zd=" << setw(6) << p.Zd()  << "se   Az=" << setw(6) << p.Az()  << "se" << endl;
        // cout << " +       dZd=" << setw(6) << cd.Zd() << "se  dAz=" << setw(6) << cd.Az() << "se" << endl;
        // cout << " +       dZd=" << setw(6) << rd.Zd() << "re  dAz=" << setw(6) << rd.Az() << "re" << endl;
        // cout << " + Ratio: Zd=" << setw(6) << kGearRatio.X()  << "se   Az=" << setw(6) << kGearRatio.Y()  << "se" << endl;


        //
        // repositioning (relative)
        //
        lout << "- Do Relative Positioning..." << endl;
        DoRelPos(rd, cdzd, cdaz);
        lout << "- Relative Positioning Done" << endl;
    }
    if (i==1 && track && !Break()/*(fCosy->Break() || fCosy->HasError() || fCosy->HasZombie())*/)
    {
        t.Now();
        lout << t << " - Positioning done." << endl;
        fCosy->SetStatus(MDriveCom::kStopped);
        fCosy->fCom->Send("POSITION DONE");
        return TRUE;
    }

    if (i<10)
        fCosy->StopMovement();
    else
        fCosy->SetStatus(MDriveCom::kStopped);

    t.Now();
    lout << t << " - Warning: Requested position not reached (i=" << dec << i << ")" << endl;

    fCosy->fCom->Send("POSITION FAILED");

    return FALSE;
}
