#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->SetDeceleration(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*fCosy->kResSE/TMath::TwoPi(); // [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();

    const Float_t rad2se = fCosy->kResSE.X()/TMath::TwoPi();

    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
        ZdAz rd = dest-p; // [se]
        ZdAz cd = rd;     // [se]
        // Correct for having two SE available
// FIMXE....
//       cd.Zd(cd.Zd()*2);
        // Round to check whether we are as near as possible
        // to the value we expect
        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->SendStatus("Target position reached.");
            return TRUE;
        }

        // ==============================================
        //   Estimate the noncircularity of the zd axis
        const ZdAz ist = dst-rd*TMath::TwoPi()/fCosy->kResSE;

        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())*rad2se;
        const double f2 = (-26.0101*sin(p2)+443.761*dst.Zd())*rad2se;
        // ==++=========================================

        // change units from se to re
        rd *= fCosy->kGearTot/fCosy->kResSE; // [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->kGearTot.Y()*fCosy->kResSE.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->SendStatus("Tracking preposition reached.");
        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->SendStatus("Target position missed!");

    return FALSE;
}