#include "MSlewing.h"

#include "MLogManip.h"

#include "MCosy.h"
#include "dkc.h"
#include "MDriveCom.h"

#include "MString.h"
#include "MPointing.h"

ClassImp(MSlewing);

using namespace std;

//#define EXPERT
#undef EXPERT

bool MSlewing::SetAcc(Dkc *mac, Float_t acc)
{
    // FIXME: Get acceleration scale from DKC!
    mac->SetAcceleration(TMath::Nint(acc*1000000000));
    return !mac->IsZombieNode();
}

// --------------------------------------------------------------------------
//
//  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 MSlewing::SetPosVelocity(const ZdAz &res, Float_t vel)
{
    const Double_t taz = TMath::Abs(res.Az())/fCosy->fMac1->GetVelMax();
    const Double_t tzd = TMath::Abs(res.Zd())/fCosy->fMac2->GetVelMax();

    if (tzd > taz)
    {
        fCosy->fMac1->SetVelocityRel(vel*TMath::Abs(res.Az()/res.Zd()));
        fCosy->fMac2->SetVelocityRel(vel);
    }
    else
    {
        fCosy->fMac1->SetVelocityRel(vel);
        fCosy->fMac2->SetVelocityRel(vel*TMath::Abs(res.Zd()/res.Az()));
    }
}

// --------------------------------------------------------------------------
//
// Does an absolute 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 MSlewing::DoAbsPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2)
{
    if (fCosy->HasZombie())
        return;

    fCosy->SetStatus(MDriveCom::kMoving);

    if (axe1) fCosy->fMac2->StartAbsPosRev(rd.Zd());
    if (axe2) fCosy->fMac1->StartAbsPosRev(rd.Az());

    if (axe1) fCosy->fMac2->WaitForSdo(0x6004, 0);
    if (axe2) fCosy->fMac1->WaitForSdo(0x6004, 0);

    // FIXME: We need a delay here to account for the delay of the
    // toggle bit in the SPS. We need a more precise return value from
    // the SPS:
    usleep(150000);

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

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


// --------------------------------------------------------------------------
//
// Caluclate the difference between feedback 1 and feedback 2 at
// the given zenith angle (feedback 2)
//
Double_t MSlewing::GetDiff(const ZdAz &za) const
{
    const Double_t zd = za.Zd(); //[revolutions]

    const Double_t sh = -1.21  *(TMath::SinH(0.916*zd*TMath::TwoPi())-1);
    const Double_t cs =  0.667 *TMath::Cos(1.735*(zd-0.236)*TMath::TwoPi());
    const Double_t of =  0.6497;

    return (sh+cs+of)/360;      //[revolutions]
}

// --------------------------------------------------------------------------
//
// 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 MSlewing::SetPosition(const ZdAz &dst, Bool_t track) // [rad]
{
    gLog << all << MTime(-1) << " - Target Position: " << dst.Zd()*TMath::RadToDeg() << "deg, " << dst.Az()*TMath::RadToDeg() << "deg (Zd/Az)" << endl;

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

    // Check whether bending is valid!

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

    fCosy->fZdAzSoll = dst;

    int i;
    for (i=0; i<(track?1:10) && !Break(); i++)
    {
        gLog << inf2 << "- Step #" << i << endl;

        // Get feedback 2
        const ZdAz sepos = fCosy->GetSePos();

        // Calculate residual to move deviation
        const ZdAz res = dest-sepos; // [revolutions]

        gLog << inf2 << "- Shaftencoders show a residual deviation of dZd=";
        gLog << MString::Format("%.2f", res.Zd()*360*60) << "' and dAz=";
        gLog << MString::Format("%.2f", res.Az()*360*60) << "'" << endl;

        // Check which axis should still be moved
        ZdAz cd  = res;             // [revolutions]
        cd *= 1./fMaxResidual;      // Scale to units of the maximum residual
        cd.Abs();

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

        // check if we reached the correct position already
        if (!cdzd && !cdaz)
        {
            gLog << all << MTime(-1) << " - 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
#ifdef FACT
        const Double_t add = 0;
#else
        const Double_t add = GetDiff(sepos)-GetDiff(dest);
#endif

        const ZdAz dest2(dest.Zd()+add, dest.Az());
        const ZdAz res2 = dest-sepos;
        // =================================================

        //gLog << warn << "WARNING - The center of the elevation axis is taken as center of the drive bow" << endl;

        SetAcc(fCosy->fMac1, fAcc.Az());
        SetAcc(fCosy->fMac2, fAcc.Zd());

        SetPosVelocity(res2, fVel);

        gLog << inf2 << "- Do absolute positioning..." << endl;
        DoAbsPos(dest2, cdzd, cdaz);
        gLog << inf2 << "- Absolute Positioning Done" << endl;
    }
    if (i==1 && track && !Break())
    {
        gLog << all << MTime(-1) << " - 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);

    gLog << warn << MTime(-1) << " - Warning: Requested position not reached (i=" << i << ")" << endl;

    fCosy->fCom->SendStatus("Target position missed!");

    return FALSE;
}