#include "MCosy.h"

#include <iomanip.h>
#include <fstream.h>
#include <iostream.h>

#include <TROOT.h>
#include <TEnv.h>
#include <TSystem.h>
#include <TApplication.h>

#include "MGCosy.h"
#include "Slalib.h"

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

#include <sys/resource.h>  // PRIO_PROCESS

typedef struct tm tm_t;

#define GEAR_RATIO_ALT  75.55 // 75.25 VERY IMPORTANT! unit=RE/SE
#define GEAR_RATIO_AZ  179.8  // VERY IMPORTANT! unit=RE/SE

const XY kGearRatio(GEAR_RATIO_ALT, GEAR_RATIO_AZ);
const XY kGearRatio2(GEAR_RATIO_ALT*16384.0/360.0, GEAR_RATIO_AZ*16384.0/360.0);

double Rad2SE(double rad)
{
    return 16384.0/D2PI*rad;
}

double Rad2ZdRE(double rad)
{
    return 16384.0/D2PI*rad*kGearRatio.X();
}

double Rad2AzRE(double rad)
{
    return 16384.0/D2PI*rad*kGearRatio.Y();
}

double Deg2ZdRE(double rad)
{
    return rad*kGearRatio2.X();
}

double Deg2AzRE(double rad)
{
    return rad*kGearRatio2.Y();
}

//double Rad2Deg(double rad)
//{
//    return 360.0/D2PI*rad;
//}

ZdAz MCosy::CorrectTarget(const ZdAz &src, const ZdAz &dst)
{
    // CorrectTarget [se]

    // src [se]
    // dst [rad]

    // fAltMax = 70
    // fAltMin = -105/110
    // fAzMin = -355
    // fAzMax =  355

    ZdAz source = src * 360.0/16384.0;
    ZdAz dest   = dst * 360.0/D2PI;

    if (dest.Zd()>-1e-6 && dest.Zd()<1e-6)
        return dst*(16384.0/D2PI);

    const float fZdMin = -67;
    const float fZdMax =  67;
    const float fAzMin = -29;
    const float fAzMax = 423;

    //
    // This corrects to target for the shortest distance, not for the fastest move!
    //
    ZdAz s = source-dest;

    float min = s.Sqr();

    //
    // Is it enought to search inside one revolution?
    //
    ZdAz ret = dest;

    for (int i=-5; i<5+1; i++)
    {
        const ZdAz p(i%2 ? -dest.Zd() : dest.Zd(), dest.Az() - i*180);

        //
        // Range Check
        //
        if (p.Zd()<fZdMin || p.Zd()>fZdMax)
            continue;

        if (p.Az()<fAzMin || p.Az()>fAzMax)
            continue;

        //
        // Calculate distance
        //
        s = source-p;

        const float dist = s.Sqr();

        if (dist > min)
            continue;

        //
        // New shortest distance
        //
        ret = p;
        min = dist;
    }
    cout << "Shortest Zd: " << ret.Zd() << "  Az:" << ret.Az() << endl;
    return ret*(16384.0/360.0);
}


ZdAz MCosy::GetSePos()
{
    const int p0 = fAlt1->GetPos();
    const int p1 = fAlt2->GetPos();
    const int p2 = fAz->GetPos();

    const int a0 = p0; //p0>8192?p0-16384:p0;
    const int a1 = p1; //p1>8192?p1-16384:p1;
    const int a2 = p2; //p2>8192?p2-16384:p2;

    //
    // interpolate shaft encoder positions
    //
    const float a = (float)(a0-a1)/2;

    //
    // calculate 'regelabweichung'
    //
    return ZdAz(a, a2);
}

ZdAz MCosy::GetRePos()
{
    return ZdAz(fMac2->GetPos(), fMac1->GetPos());
}

int MCosy::SetPosition(const ZdAz &dst) // [rad]
{
    // FIXME: CORRECT BY fOffset !!!!!!!!!!

    //
    // Calculate new target position (shortest distance to go)
    //
    const ZdAz src  = GetSePos();
    const ZdAz dest = CorrectTarget(src, dst);

    cout << "Positioning to Target:" << endl;
    //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 << "Shortest Dest Zd: " << dest.Zd() << "se  Az:" << dest.Az() << "se" << endl;

    for (int i=0; i<10 && !StopWaitingForSDO(); i++)
    {
        //
        // Get Shaft Encoder Positions
        //
        const ZdAz p=GetSePos();

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

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

        //
        // check if we reached the correct position already
        //
        if (!(int)cd.Zd() && !(int)cd.Az())
        {
            cout << "Positioning done with " << i << "manuvers." << endl;
            return TRUE;
        }

        //
        // change units from se to re
        //
        rd *= kGearRatio; // [re]

        //
        // Set velocities
        //
        const int vr = fMac1->GetVelRes();
        const float maxvel = (i?0.1:0.9)*vr; // maxvel = 90%
        const float maxacc = (i?0.1:0.5)*vr; // maxacc = 50%;

        const float diff = i?1:fabs(rd.Ratio());

        cout << "Salt/Saz: " << diff << endl;

        if (diff <1)
        {
            fMac1->SetVelocity(maxvel);
            fMac1->SetAcceleration(maxacc);
            fMac1->SetDeceleration(maxacc);

            fMac2->SetVelocity(maxvel*diff);
            fMac2->SetAcceleration(maxacc*diff);
            fMac2->SetDeceleration(maxacc*diff);
        }
        else
        {
            fMac1->SetVelocity(maxvel/diff);
            fMac1->SetAcceleration(maxacc/diff);
            fMac1->SetDeceleration(maxacc/diff);

            fMac2->SetVelocity(maxvel);
            fMac2->SetAcceleration(maxacc);
            fMac2->SetDeceleration(maxacc);
        }

        rd.Round();

        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;

        //
        // repositioning (relative)
        //
        if ((int)cd.Zd()) fMac2->StartRelPos(rd.Zd());
        if ((int)cd.Az()) fMac1->StartRelPos(rd.Az());

        cout << "Waiting for positioning..." << flush;

        WaitForSdos();

        cout << "SDO..." << flush;

        while (fMac1->IsPositioning() || fMac2->IsPositioning() && !StopWaitingForSDO())
            usleep(1);

        cout << "done." << endl;
    }

    if (StopWaitingForSDO())
    {
        fMac1->HandleError();
        fMac2->HandleError();
    }

    cout << "Positioning ERROR!" << endl;
    return FALSE;
}

void MCosy::TrackPosition(const RaDec &dst) // ra, dec [rad]
{
    Slalib sla;

    //
    // Position to actual position
    //
    Timer t;
    t.GetTime();

    sla.Set(t.GetMjd());
    ZdAz dest = sla.CalcZdAz(dst);

    if (!SetPosition(dest))
    {
        cout << "ERROR: Cannot start tracking, unable to reach requested position." << endl;
        return;
    }

    //
    // calculate offset from present se position
    //

    const ZdAz sepos = GetSePos()*kGearRatio;

    fMac1->ReqPos();
    fMac2->ReqPos();

    const ZdAz repos=GetRePos();

    fOffset = sepos-repos;

    cout << "Offset: " << sepos.Zd() << "re, " << sepos.Az() << "re" << endl;
    cout << "Offset: " << repos.Zd() << "re, " << repos.Az() << "re" << endl;
    cout << "Offset: " << fOffset.Zd() << "re, " << fOffset.Az() << "re" << endl;

    //
    // Start revolution mode
    //
    fMac2->SetAcceleration(0.90*fMac2->GetVelRes());
    fMac2->SetDeceleration(0.90*fMac2->GetVelRes());

    fMac1->SetAcceleration(0.90*fMac1->GetVelRes());
    fMac1->SetDeceleration(0.90*fMac1->GetVelRes());

    fMac2->SetRpmMode(TRUE);
    fMac1->SetRpmMode(TRUE);

    /*-*/ int s = t.GetSecs();
    cout << "Start tracking: Ra: " << Rad2Deg(dst.Ra()) << kDEG << "  Dec: ";
    cout << Rad2Deg(dst.Dec()) << kDEG << endl;

    //
    // Initialize Tracker (slalib or starguider)
    //
    fRaDec    = dst;
    fTracking = kTRUE;

    ofstream fout("cosy.pos");
    fout << "   Mjd/10ms    Offset/RE Deviation/RE V/RE/MIN/4" << endl;

    //
    // We want to reach the theoretical position exactly in about 0.5s
    //
    const float dt = 1;  // 1 second
    while (!StopWaitingForSDO())
    {
        //
        // Request Real Position from Drive
        //
        Timer t;
        t.GetTime();

        //
        // Request theoretical Position for a time in the future (To+dt) from CPU
        //
        sla.Set(t.GetMjd()+dt/(60*60*24));
        dest = CorrectTarget(GetSePos(), sla.CalcZdAz(dst));

        fMac2->RequestSDO(0x6004);
        fMac1->RequestSDO(0x6004);
        WaitForSdos();
        if (StopWaitingForSDO())
        {
            lout << "Error 6004 happened" << endl;
            SkipPendingSdos();
            break;
        }

        //
        // Copy fOffset to a local variable
        //
        ZdAz offset = fOffset;

        //
        // distance between (To+dt) and To [re]
        // position time difference < 5usec
        //
        dest *= kGearRatio;
        dest -= GetRePos() + offset;

        //
        // 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.0/(dt-(fMac2->GetTime()-t));

        //
        // calculate real velocity of future [re/min]
        //
        ZdAz vt = v/4;
        vt.Round();

        if (v.Zd()>.9*fMac1->GetVelRes() || v.Az()>.9*fMac2->GetVelRes())
        {
            cout << "Error: Tracking speed faster than possible maximum velocity." << endl;
            break;
        }

        //
        // Set theoretical velocity (as early after calculation as possible)
        //
        //
        // Maybe we should attenuate the changes
        //
        fMac2->SendSDO(0x3006, 1, (LWORD_t)vt.Zd());  // SetRpmVelocity [re/min]
        fMac1->SendSDO(0x3006, 1, (LWORD_t)vt.Az());  // SetRpmVelocity [re/min]
        WaitForSdos();
        if (StopWaitingForSDO())
        {
            lout << "Error 3006 happened" << endl;
            SkipPendingSdos();
            break;
        }

        //
        // Now do 'unnecessary' things
        // calculate control deviation - for the moment for convinience
        //

        //if (fMac1->GetTime()-s > 1)
        {
            const double mjd = fMac2->GetMjd();
            sla.Set(mjd);
            ZdAz dest0=CorrectTarget(GetSePos(), sla.CalcZdAz(dst));
            dest0 *= kGearRatio;
            dest0 -= GetRePos()+offset;
            dest0.Round();
            //cout << "Control deviation: ";
            fout << setprecision(15) << setw(15) << mjd*60.*60.*24. << " ";
            fout << setw(4) << (int)fOffset.Zd() << " ";
            fout << setw(4) << (int)fOffset.Az() << " ";

            fout << setw(4) << dest0.Zd() << " ";
            fout << setw(4) << dest0.Az() << " ";
            fout << setw(4) << vt.Zd()    << " ";
            fout << setw(4) << vt.Az() << endl;
            s = (int)fMac1->GetTime();
        }

        //
        // Update speed as often as possible.
        // make sure, that dt is around 10 times larger than the
        // update time
        //
        // usleep(50000); // 0.05s
    }

    fTracking = kFALSE;

    //
    // Stop revolution mode
    //
    fMac2->SetRpmMode(FALSE);
    fMac1->SetRpmMode(FALSE);

    cout << "Tracking stopped." << endl;

    if (StopWaitingForSDO())
    {
        fMac1->HandleError();
        fMac2->HandleError();
    }
}

void *MCosy::Proc(int msg, void *mp)
{
    switch (msg)
    {
    case WM_WAIT:
        cout << "Wait for execution of Proc: done." << endl;
        return NULL;

    case WM_STOP:
        cout << "Stopping  positioning." << endl;
        fMac1->SetDeceleration(0.5*fMac1->GetVelRes());
        fMac2->SetDeceleration(0.5*fMac2->GetVelRes());
        cout << "Stoping" << endl;
        fMac1->SetRpmMode(FALSE);
        fMac2->SetRpmMode(FALSE);
        cout << "Done." << endl;
        while (fMac1->IsPositioning() || fMac2->IsPositioning())
            usleep(1);
        cout << "Positioning stopped." << endl;
        return NULL;

    case WM_PRESET:
        cout << "WM_PRESET: START" << endl;
        fAlt1->SetPreset();
        fAlt2->SetPreset();
        fAz->SetPreset();
        cout << "WM_PRESET: DONE (return 0xaffe)" << endl;
        return (void*)0xaffe;

    case WM_POLARIS:
        {
            cout << "WM_POLARIS: START" << endl;
            Slalib sla;

            Timer t;
            t.GetTime();

            sla.Set(t.GetMjd());

            RaDec rd(37.94, 89.2644);
            ZdAz za=sla.CalcZdAz(rd*D2PI/360.0)*16384.0/D2PI;

            cout << "Calc Zd: " << za.Zd() << " Az: " << za.Az() << endl;

            ZdAz sepos = GetSePos();
            cout << "Got  Zd: " << sepos.Zd() << " Az: " << sepos.Az() << endl;

            fAlt1->SetPreset(za.Zd());
            fAlt2->SetPreset(-za.Zd());
            fAz->SetPreset(za.Az());

            cout << "WM_PRESET: DONE (return 0xaffe)" << endl;
        }
        return (void*)0xaffe;

    case WM_POSITION:
        cout << "WM_POSITION: START" << endl;
        {
            ZdAz dest = *((ZdAz*)mp);

            SetPosition(dest*D2PI/360.0);
        }
        cout << "WM_POSITION: DONE (return 0x7777)" << endl;
        return (void*)0x7777;

    case WM_TRACK:
        cout << "WM_TRACK: START" << endl;
        {
            RaDec dest = *((RaDec*)mp);
            TrackPosition(dest*D2PI/360.0);
        }
        cout << "WM_TRACK: DONE (return 0x8888)" << endl;
        return (void*)0x8888;
    }
    cout << "Unknown Msg" << endl;
    return (void*)0xffffffff;
}

void MCosy::TalkThread()
{
    //
    // Start the Network
    //
    Network::Start();

    const int res = fMac3->GetVelRes();

    fMac3->SetVelocity(res);
    fMac3->SetAcceleration(res);
    fMac3->SetDeceleration(res);

    fMac3->SetSyncMode();

    fMac1->SetHome(250000);
    fMac2->SetHome(250000);
    PostMsg(WM_PRESET, 0, 0);
    PostMsg(WM_WAIT,   0, 0);

    fMac1->ReqPos();
    fMac2->ReqPos();

    const ZdAz repos=GetRePos();
    cout << "APOS: " << repos.Zd() << "re, " << repos.Az() << "re" << endl;

    TEnv env(".cosyrc");

    cout << Deg2AzRE(env.GetValue("MinAz[Deg]", -1.0)) << " < Az < "
        << Deg2AzRE(env.GetValue("MaxAz[Deg]", +1.0)) << "RE" << endl;
    cout << env.GetValue("MinAz[Deg]", -1.0) << " < Az < "
        << env.GetValue("MaxAz[Deg]", +1.0) << kDEG << endl;
    cout << Deg2ZdRE(env.GetValue("MinZd[Deg]", -1.0)) << "RE < Zd < "
        << Deg2ZdRE(env.GetValue("MaxZd[Deg]", +1.0)) << "RE" << endl;

    fMac1->SetNegEndswitch(Deg2AzRE(env.GetValue("MinAz[Deg]", -1.0)));
    fMac1->SetPosEndswitch(Deg2AzRE(env.GetValue("MaxAz[Deg]", +1.0)));

/*
    fMac2->SetNegEndswitch(Deg2ZdRE(env.GetValue("MinZd[Deg]", -1.0)));
    fMac2->SetPosEndswitch(Deg2ZdRE(env.GetValue("MaxZd[Deg]", +1.0)));
*/
    fMac3->SetSyncMode();
/*
    cout << "PostMsg(WM_PRESET)" << endl;
    void *rc =
    cout << hex << "WM_PRESET: ret=" << rc << endl;

    RaDec dest = RaDec(45.0, 30.0)*D2PI/360.0;

    cout << "PostMsg(WM_TRACK)" << endl;
    cout << sizeof(RaDec) << "==" << sizeof(dest) << endl;
    rc=PostMsg(WM_TRACK, &dest, sizeof(dest));
    cout << "DEST killed." << endl;
*/
    // AltAz dest = AltAz(45.0, 30.0);
    // double ra, dec;
    // slaDaf2r( 71, 0, 0, &ra,  &status); // 0 WARNING: RANGE
    // slaDaf2r( 89, 0, 0, &dec, &status); // 49
    // cout << "Start tracking: Ra: " << Rad2Deg(ra) << kDEG << "  Dec: " << Rad2Deg(dec) << kDEG << endl;

    // dest = AltAz(-46.0, 210);
    // SetPosition(dest);
    setpriority(PRIO_PROCESS, 0, 10);

    Slalib sla;
    while (1)
    {
        //
        // wait until a tracking session is started
        //
        while (!fTracking)
            usleep(1);

        ZdAz sollalt; // [se]
        ZdAz sollaz;  // [se]
        ZdAz old;

        ZdAz ist=fOffset/kGearRatio;     // [se]
        //
        // only update fOffset while tracking
        //
        while (fTracking)
        {
            usleep(100000/*00*/); // 0.1s

            //
            // Make changes (eg wind) smoother - attenuation of control function
            //

            ZdAz offset(fOffset.Zd()*9.0/10.0+((ist.Zd()-sollalt.Zd())*kGearRatio.X())/10.0,
                        fOffset.Az()*9.0/10.0+((ist.Az()-sollaz.Az()) *kGearRatio.Y())/10.0);

            fOffset = offset;
            // fOffset.Zd(((offset.Zd()>1000)||(offset.Zd()<-1000))?0:offset.Zd());
            // fOffset.Az(((offset.Az()>1000)||(offset.Az()<-1000))?0:offset.Az());

            //
            // get position, where we are
            //
            ist = GetSePos(); // [se]

            //
            // if the position didn't change continue
            //
            if ((int)ist.Zd() == (int)old.Zd() &&
                (int)ist.Az() == (int)old.Az())
                continue;

            //
            // if Alt Shaftencoder changed position
            //
            if ((int)ist.Zd() != (int)old.Zd())
            {
                //
                // Get time from last shaftencoder position change
                //
                const double t = (fAlt1->GetMjd()+fAlt2->GetMjd())/2.0;

                //
                // calculate were we should be
                //
                sla.Set(t);
                sollalt = CorrectTarget(ist, sla.CalcZdAz(fRaDec));

                old.Zd(ist.Zd());
            }

            //
            // if Alt Shaftencoder changed position
            //
            if ((int)ist.Az() != (int)old.Az())
            {
                //
                // Get time from last shaftencoder position change
                //
                const double t = fAz->GetMjd();

                //
                // calculate were we should be
                //
                sla.Set(t);
                sollaz = CorrectTarget(ist, sla.CalcZdAz(fRaDec));

                old.Az(ist.Az());
            }
        }
    }
}

void *MCosy::MapTalkThread(void *arg)
{
    pthread_detach(pthread_self());

    MCosy *cosy = (MCosy*)arg;

    cosy->TalkThread();

    cosy->lout << "- Sending Thread done." << endl;

    return NULL;
}

int MCosy::StopWaitingForSDO()
{
    return Break() || fMac1->HasError() || fMac2->HasError();
}

void MCosy::Start()
{
    if (fTxThrd)
    {
        cout << "Error: tx thread already started." << endl;
        return;
    }

    lout << "- Starting sending Thread." << endl;

    fTxThrd = new pthread_t;
    pthread_create(fTxThrd, NULL, MapTalkThread, this);
}

void MCosy::Stop()
{
    if (!fTxThrd)
        return;

    pthread_cancel(*fTxThrd);

    delete fTxThrd;
    fTxThrd = NULL;

    lout << "- Sending Thread stopped." << endl;

    SkipPendingSdos();

    Network::Stop();
}

MCosy::MCosy(const char *dev, const int baud, ostream &out)
: Network(dev, baud, out), fTxThrd(NULL), fTracking(kFALSE)
{
    //
    // Create Nodes
    //
    fMac1=new Macs(1, lout);
    fMac2=new Macs(2, lout);
    fMac3=new Macs(3, lout);
    fAlt1=new ShaftEncoder(4, lout);
    fAlt2=new ShaftEncoder(5, lout);
    fAz  =new ShaftEncoder(6, lout);

    //
    // Connect the devices to the network
    //
    SetNode(fMac1);
    SetNode(fMac2);
    SetNode(fMac3);
    SetNode(fAlt1);
    SetNode(fAlt2);
    SetNode(fAz);

    MGCosy *fWin=new MGCosy(this, gClient->GetRoot(), 1, 1);

    fAz->SetDisplay(fWin->GetLabel1());
    fAlt1->SetDisplay(fWin->GetLabel2());
    fAlt2->SetDisplay(fWin->GetLabel3());
}

void MCosy::TerminateApp()
{
   gSystem->ExitLoop();
}

MCosy::~MCosy()
{
    delete fAz;
    delete fAlt2;
    delete fAlt1;
    delete fMac1;
    delete fMac2;
    delete fMac3;

    delete fWin;
}