#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 <TTimer.h>

#include <TH2.h>
#include <TH3.h>
#include <TProfile.h>
#include <TCanvas.h>

#include "MGCosy.h"
#include "MTime.h"
#include "MDriveCom.h"
#include "MStarguider.h"
#include "SlaStars.h"
#include "MPointing.h"
#include "MTracking.h"

#include "slalib/slalib.h"  // FIXME: REMOVE

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

ClassImp(MCosy);

typedef struct tm tm_t;

/* +===================================+
    FIXME: What if fMac3 (Sync) died?
   +===================================+
*/

//#define EXPERT
#undef EXPERT

/*
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 * kRad2Deg;

    if (dest.Zd()>-3 && dest.Zd()<3)
        dest.Zd(dest.Zd()<0?-3:3);

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

    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;
    }
    return ret*(16384.0/360.0);
}
*/

// --------------------------------------------------------------------------
//
//  GetSePos, reads the Shaftencoder positions from the Can-drivers
//  for the shaftencoders. The two shaft encoders at the elevation axis
//  are avaraged. The values are returned as a ZdAz object.
//
//  If one of the two shaftencoders on the elevation axis is missing
//  the other one's position is returned.
//
//  The positions are alway up-to-date because the shaftencoders are
//  sending all changes immediatly.
//
ZdAz MCosy::GetSePos() const
{
    const int pa = fAz->GetPos();
    if (fZd1->IsZombieNode() && fZd2->IsZombieNode())
        return ZdAz(0, pa);

    //
    // Get the values
    //
    int p1 =  (fZd1->GetPos()+8192)%16384;
    int p2 = -(fZd2->GetPos()+8192)%16384;

    if (fZd1->IsZombieNode())
        return ZdAz(p2, pa);
    if (fZd2->IsZombieNode())
        return ZdAz(p1, pa);

    //
    // interpolate shaft encoder positions
    //
    float p = (float)(p1+p2)/2;

    return ZdAz(p, pa);
}

// --------------------------------------------------------------------------
//
//  reads the Rotary encoder positions from the last request of the Macs.
//
//  The positions are returned as a ZdAz object. Use RequestRePos to request
//  the current positions first.
//
ZdAz MCosy::GetRePos()
{
    return ZdAz(fMac2->GetPos(), fMac1->GetPos());
}

// --------------------------------------------------------------------------
//
//  reads the Rotary encoder positions from the Macs.
//
//  The positions are returned as a ZdAz object. The positions are the ones
//  which are send as PDOs to the computer. This is done at a given
//  frequency. Which means, that this positions are not ought to be
//  up-to-date.
//
ZdAz MCosy::GetRePosPdo()
{
    return ZdAz(fMac2->GetPdoPos(), fMac1->GetPdoPos());
}

// --------------------------------------------------------------------------
//
// check for a break-signal (from the msgqueue) and errors.
//
int MCosy::StopWaitingForSDO() const
{
    return 0/*Break() || HasError()*/;
}

// --------------------------------------------------------------------------
//
// Waits for a movement to become finished.
//
// First waits for all peding Sdos, then waits until both motors are stopped
// or waiting for SDOs was stopped (either by an error or by Break)
//
void MCosy::WaitForEndMovement()
{
    // FIXME, what when waiting times out (Zombie)
    if (!fMac1 || !fMac2)
        return;

    while ((fMac1->IsPositioning() || fMac2->IsPositioning()) &&
           !(Break() || HasError() || HasZombie()))
        usleep(1);

    if (!Break() && !HasError() && !HasZombie())
        return;

    MTime t(-1);
    lout << t << " - MCosy::WaitForEndMovement aborted...";
    if (Break())
        lout << " Break signal...";
    if (HasError())
        lout << " Network has error...";
    if (HasZombie())
        lout << " Network has zombie...";
    lout << endl;
}

// --------------------------------------------------------------------------
//
// Check for an error...
//
// This is ment for usage after the Action: All Motors Stop.
//
void MCosy::CheckForError()
{
    //
    // Check all Can-Nodes for an Error. If there is no error the motor
    // status is set to stopped.
    //
    if (HasError() || HasZombie())
    {
        SetStatus(MDriveCom::kError);
        return;
    }

    if (fMac1->IsPositioning() || fMac2->IsPositioning())
        SetStatus(MDriveCom::kMoving);
    else
        SetStatus(MDriveCom::kStopped);

    //
    // If there is an error, the error status is set to Error.
    //

    /*
     FIXME: HANDLINGE ERROR

     //
     // Now try to handle the error.
     //
     fMac1->HandleError();
     fMac2->HandleError();

     //
     // If the error couldn't get solved return
     //
     if (HasError())
     return;

     //
     // Set motor status to stopped
     //
     SetStatus(MDriveCom::kStopped);
     */
}

Bool_t MCosy::CheckRange(const ZdAz &d) const
{
    // d [rad]

    if (d.Zd()<fMin.Zd())
    {
        lout << "ERROR: Requested Zenith Angle below negative endswitch." << endl;
        return kFALSE;
    }

    if (d.Zd()>fMax.Zd())
    {
        lout << "ERROR: Requested Zenith Angle behind positive endswitch." << endl;
        return kFALSE;
    }

    if (d.Az()<fMin.Az())
    {
        lout << "ERROR: Requested Azimuth Angle below negative endswitch." << endl;
        return kFALSE;
    }

    if (d.Az()>fMax.Az())
    {
        lout << "ERROR: Requested Azimuth Angle behind positive endswitch." << endl;
        return kFALSE;
    }


    return kTRUE;
}

ZdAz MCosy::AlignTrackingPos(ZdAz pointing) const
{
    // pointing [rad]
    // AlignTrackingPos [deg]

    pointing *= kRad2Deg;

    if (pointing.Zd()<0)
    {
        pointing.Zd(-pointing.Zd());
        pointing.Az(pointing.Az()+180);
        //lout << "ZD=-ZD Az+=180" << endl;
    }

    const ZdAz se = GetSePos()*TMath::TwoPi()/kResSE;   // [rad]
    const ZdAz unbendedse = fBending.CorrectBack(se)*kRad2Deg; // ist pointing

    //lout << "Unbended: " << unbendedse.Zd() << " " << unbendedse.Az() << endl;

    do
    {
        const Double_t d = unbendedse.Az() - pointing.Az();
        if (d>-180 && d<=180)
            break;

        //lout << "AZ += " << TMath::Sign(360., d) << endl;

        pointing.Az(pointing.Az()+TMath::Sign(360., d));
    } while (1);

    return pointing/kRad2Deg;
/*
    const Bool_t rc = CheckRange(pointing);
    za = pointing/kRad2Deg; // [rad]

    if (!rc)
        lout << "Error: Aligned position out of Range." << endl;

    return rc;*/
}

Double_t MCosy::Starguider(Double_t mjd, ZdAz &dest) const
{
    ifstream fin("pointingpos.txt");
    if (!fin)
        return -1;

    Double_t mjd0, zd, az;
    fin >> mjd0 >> zd >> az;

    mjd0 += 52000;

    if (mjd0+1./24/60 <mjd)
        return -1;

    ZdAz point=AlignTrackingPos(ZdAz(zd, az)/kRad2Deg);
    /*
    if (!AlignTrackingPos(ZdAz(zd, az), point))
    {
        cout << "Starguider position couldn't be aligned..." << endl;
        return -1;
    }*/

    // FIXME: Check Range missing!

    const ZdAz diff = (dest-point)*kRad2Deg;

    if (diff.Zd()>5 || diff.Az()>5)
    {
        cout << "Starguider deviation too large... dZd=" << diff.Zd() <<" dAz="<<diff.Az() << endl;
        return -1;
    }

    dest -= point;
    dest *= -kGearTot/TMath::TwoPi(); // [re]

    cout << "Using Starguider... dZd=" << dest.Zd() << " dAz=" << dest.Az() << endl;

    return (mjd-mjd0) * (24*60*60); // [s]
}

// --------------------------------------------------------------------------
//
// 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 MCosy::SetPosition(const ZdAz &dst, Bool_t track) // [rad]
{
    MPointing point(this, lout);

//#ifdef EXPERT
//    point.SetAccDec(0.4, 0.4);
//    point.SetVelocity(0.2); // fast: 0.6, slow: 0.2
//#else
    point.SetPointAccDec(0.2, 0.1);
    point.SetPointVelocity(0.1);
//#endif

    return point.SetPosition(dst, track);
}

void MCosy::SetTrackingPosRE(ZdAz za)
{
    za /= kGearTot;                        // [U_tel]
    za *= TMath::TwoPi();                  // [rad]
    //cout << "RE1:  " << za.Zd()*180/3.1415 << " " << za.Az()*180/3.1415 << endl;
    fTrackingPosRaw = za*TMath::RadToDeg();
    fTrackingPos    = fBending.CorrectBack(za)*TMath::RadToDeg();
    //cout << "RE2:  " << fTrackingPos.Zd() << " " << fTrackingPos.Az() << endl;
}

void MCosy::TrackPosition(const RaDec &dst) // ra, dec [rad]
{
    MTracking track(this, lout);
    track.SetOut(fOutRep);
//#ifdef EXPERT
//    track.SetPointAccDec(0.4, 0.4);
//    track.SetPointVelocity(0.2); // fast: 0.6, slow: 0.2
//#else
    track.SetPointAccDec(0.2, 0.1);
    track.SetPointVelocity(0.1);
//#endif
    track.SetTrackAccDec(0.1, 0.1);

    track.TrackPosition(dst);
}

// --------------------------------------------------------------------------
//
// Stops the movement of both motors.
//
// Sets the status to stopping. Sets the deceleration to 50% of the maximum.
// stops. Quits the revolution mode and wait for the end of the movement.
//
void MCosy::StopMovement()
{
    //
    // Set status to Stopping
    //
    SetStatus(MDriveCom::kStopping);

    //
    // set deceleration to 50%
    //
    cout << "Stopping movement (dec=30%)..." << endl;
    if (fMac1 && fMac2)
    {
#ifdef EXPERT
        fMac1->SetDeceleration(0.5*fMac1->GetVelRes());
        fMac2->SetDeceleration(0.5*fMac2->GetVelRes());
#else
        fMac1->SetDeceleration(0.3*fMac1->GetVelRes());
        fMac2->SetDeceleration(0.3*fMac2->GetVelRes());
#endif
        fMac1->SetRpmMode(FALSE);
        fMac2->SetRpmMode(FALSE);
    }

/*
    fMac1->SetDeceleration(0.3*fMac1->GetVelRes());
    fMac2->SetDeceleration(0.3*fMac2->GetVelRes());

    fMac2->SendSDO(0x3000, Macs::string('s','t','o','p'));
    fMac1->SendSDO(0x3000, Macs::string('s','t','o','p'));
    fMac2->WaitForSdo(0x3000, 0);
    fMac1->WaitForSdo(0x3000, 0);
    fMac1->SetRpmMode(FALSE);
    fMac2->SetRpmMode(FALSE);
    */

    //
    // Wait for the movement to really be finished.
    //
#ifdef EXPERT
    cout << "Waiting for end of movement..." << endl;
#endif
    WaitForEndMovement();

    //
    // Check whether everything works fine.
    //
    CheckForError();
#ifdef EXPERT
    cout << "Movement stopped." << endl;
#endif
}

bool MCosy::CheckNetwork()
{
    //return kTRUE;
    //CheckConnections();

    CheckForError();

    if (HasZombie())
    {
        lout << "- Found Zombies in Network..." << endl;
        if (!RebootZombies())
            return false;
    }

    /*
     FIXME HANDLING ERROR
     */
    if (HasError())
    {
        fMac1->HandleError();
        fMac2->HandleError();
        fMac3->HandleError();
        if (HasError() || HasZombie())
            return false;
    }

    CheckForError();
    return true;
}

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

    case WM_STOP:
        //cout << "MCosy::Proc: Stop." << endl;
        if (!CheckNetwork())
            return (void*)0xebb0;
        StopMovement();
        return NULL;
/*
    case WM_PRESET:
        cout << "WM_Preset: start." << endl;
        if (!CheckNetwork())
            return (void*)0xebb0;
        fZd1->SetPreset();
        fZd2->SetPreset();
        fAz->SetPreset();
        cout << "WM_Preset: done. (return 0xaffe)" << endl;
        return (void*)0xaffe;
*/
        /*
    case WM_CALIB:
        {
            cout << "WM_Calib: start." << endl;
            if (!CheckNetwork())
                return (void*)0xebb0;

            SlaStars sla(fObservatory);
            sla.Now();

            RaDec rd = *((RaDec*)mp);

            //RaDec rd(37.94, 89.2644);      // POLARIS
            //RaDec rd(213.915417, 19.1825); // ARCTURUS

            cout << "Calibrating to: " << rd.Ra()*24/360 << "h " << rd.Dec() << "°" << endl;

            ZdAz za=sla.CalcZdAz(rd*kDeg2Rad)*16384.0/k2Pi;

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

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

            fZd1->SetPreset(za.Zd());
            fZd2->SetPreset(-za.Zd());
            fAz->SetPreset(za.Az());

            cout << "WM_Calib: done. (return 0xaffe)" << endl;
        }
        return (void*)0xaffe;
        */
    case WM_TPOINT:
        {
            //cout << "WM_TPoint: start." << endl;
            SlaStars sla(fObservatory);
            sla.Now();

            RaDec rd = *((RaDec*)mp);
            cout << "TPoint Star: " << rd.Ra()/15 << "h " << rd.Dec() << "°" << endl;

            AltAz za=sla.CalcAltAz(rd*kDeg2Rad)*kRad2Deg;

            if (!fOutTp)
            {
                //
                // open tpoint file
                //
                const TString name = GetFileName("tpoint/tpoint_%s.txt");
                cout << "TPoint-Cosy File ********* " << name << " ********** " << endl;

                fOutTp = new ofstream(name);
                *fOutTp << "Magic Model  TPOINT data file" << endl;
                *fOutTp << ": ALTAZ" << endl;
                *fOutTp << "49 48 0 ";
                *fOutTp << sla.GetTime().Year() << " " << sla.GetTime().Month() << " " << sla.GetTime().Day() << " ";
                *fOutTp << /*"20 1013.25 300 0.5 0.55 0.0065" <<*/ endl;
                // temp(°C) pressure(mB) height(m) humidity(1) wavelength(microm) troplapserate(K/m)
            }

            cout << "     Alt/Az: " << za.Alt() << "° " << za.Az() << "°" << endl;
            *fOutTp << setprecision(7) << za.Az() << " " << za.Alt() << " ";

            ZdAz sepos = GetSePos()*TMath::TwoPi()/kResSE;
            za.Set(TMath::Pi()/2-sepos.Zd(), sepos.Az());
            za *= kRad2Deg;

            cout << "     SE-Pos: " << za.Alt() << "° " << za.Az() << "°" << endl;
            *fOutTp << fmod(za.Az()+360, 360) << " " << za.Alt() << " ";

            if (fStarguider)
            {
                XY tp = fStarguider->GetCoordinates();
                *fOutTp << 90-tp.X() << " " << tp.Y() << " ";
            }

            *fOutTp << rd.Ra()/15 << " " << rd.Dec() << " " << setprecision(11) << sla.GetMjd() << endl;

            //cout << "WM_TPoint: done. (return 0xaffe)" << endl;
        }
        return (void*)0xca1b;

    case WM_TRACKPOS:
        //cout << "WM_TrackPosition: start." << endl;
        {
            if (!CheckNetwork())
                return (void*)0xebb0;

            ZdAz dest = *((ZdAz*)mp) * kDeg2Rad;
            if (!SetPosition(dest))
                return (void*)0x1234;

            SlaStars sla(fObservatory);
            sla.Now();

            RaDec rd = sla.CalcRaDec(dest);
            TrackPosition(rd);
        }
        //cout << "WM_TrackPosition: done. (return 0xabcd)" << endl;
        return (void*)0xabcd;

    case WM_POSITION:
        //cout << "WM_Position: start." << endl;
        {
            if (!CheckNetwork())
                return (void*)0xebb0;

            ZdAz dest = *((ZdAz*)mp);
            SetPosition(dest*kDeg2Rad);
        }
        //cout << "WM_Position: done. (return 0x7777)" << endl;
        return (void*)0x7777;

    case WM_POSITION1:
        //cout << "WM_Position1: start." << endl;
        {
            if (!CheckNetwork())
                return (void*)0xebb0;

            ZdAz dest = *((ZdAz*)mp);
            SetPosition(dest*kDeg2Rad, kTRUE);
        }
        //cout << "WM_Position: done. (return 0x7777)" << endl;
        return (void*)0x7777;

    case WM_TESTSE:
        //cout << "WM_TestSe: start." << endl;
        fBackground = mp ? kBgdSeTest : kBgdNone;
        //cout << "WM_TestSe: done. (return 0x1e51)" << endl;
        return (void*)0x1e51;

    case WM_GEAR:
        //cout << "WM_Gear: start." << endl;
        fBackground = mp ? kBgdGear : kBgdNone;
        //cout << "WM_Gear: done. (return 0xfeaf)" << endl;
        return (void*)0xfeaf;

    case WM_DISPLAY:
        //cout << "WM_Display: start." << endl;
        fTriggerDisplay = kTRUE;
        //cout << "WM_Disply: done. (return 0xd1e1)" << endl;
        return (void*)0xd1e1;

    case WM_TRACK:
        //cout << "WM_Track: START" << endl;
        {
            RaDec dest = ((RaDec*)mp)[0];
            if (fStarguider)
                fStarguider->SetPointingPosition(((RaDec*)mp)[1]);
            if (!CheckNetwork())
                return (void*)0xebb0;
            TrackPosition(dest*kDeg2Rad);
        }
        //cout << "WM_Track: done. (return 0x8888)" << endl;
        return (void*)0x8888;

    case WM_NEWTRACK:
        //cout << "WM_NewTrack: START" << endl;
        fRaDec = *((RaDec*)mp);
        //cout << "WM_NewTrack: done. (return 0x9999)" << endl;
        return (void*)0x9999;

    case WM_LOADBENDING:
        //cout << "WM_LoadBending: START" << endl;
        fBending.Load("bending.txt");
        //cout << "WM_LoadBending: done. (return 0xbe0d)" << endl;
        return (void*)0xbe0d;

    case WM_RESETBENDING:
        //cout << "WM_ResetBending: START" << endl;
        fBending.Reset();
        //cout << "WM_ResetBending: done. (return 0xbe0e)" << endl;
        return (void*)0xbe0e;

    case WM_HOME:
        //cout << "WM_Home: START" << endl;
        if (!CheckNetwork())
            return (void*)0xebb0;
        else
        {
            cout << "HOME NOT ALLOWED... for Magic." << endl;
            /*
            cout << "Going Home..." << endl;
            TEnv env(".cosyrc");

            SetStatus(MDriveCom::kMoving);

            fMac1->SetHome(250000, env.GetValue("Az_MaxTime2ReachHome[s]", 100));
            fMac2->SetHome(250000, env.GetValue("Zd_MaxTime2ReachHome[s]", 100));

            lout << "SETHOME DONE" << endl;

            SetStatus(HasError() ? MDriveCom::kError : MDriveCom::kStopped);

            fAz->SetPreset();
            fZd1->SetPreset();
            fZd2->SetPreset();

            fMac1->ReqPos();
            fMac2->ReqPos();
            fMac3->StopMotor();
            */
        }
        //cout << "WM_Home: done. (return 0x403e)" << endl;
        return (void*)0x403e;

    case WM_CALCALTAZ:
        {
            cout << endl;

            SlaStars sla(fObservatory);
            sla.Now();

            XY xy = *((XY*)mp);
            RaDec rd(xy.X()*15., xy.Y()); // [deg]

            ZdAz a1 = sla.CalcZdAz(rd*kDeg2Rad); // [rad]

            cout << "Ra/Dec source: " << xy.X()  << "h " << xy.Y()  << "°" << endl;
            cout << "Zd/Az target:  " << a1.Zd()*kRad2Deg << "° " << a1.Az()*kRad2Deg << "°" << endl;

            if (fZd1 && fZd2 && fAz)
                a1 = AlignTrackingPos(a1);

            a1 = fBending(a1);
            CheckRange(a1);
            a1 *= kRad2Deg;

            const ZdAz a2 = a1*kResSE/360;

            cout << "Zd/Az bended:  " << a1.Zd() << "° " << a1.Az() << "°" << endl;
            cout << "SE bended:     " << a2.Zd() << "  " << a2.Az() << endl;
        }
        return (void*)0xa17a;

    case WM_ENDSWITCH:
        {
            ZdAz pos = GetSePos()*TMath::TwoPi()/kResSE;
            pos = fBending.SubtractOffsets(pos)*kRad2Deg;

            cout << "Endswitch Position:  Zd=" << pos.Zd() << "°  Az=";
            cout << pos.Az() << "°" << endl;
        }

        return (void*)0x1010;

    case WM_QUIT:
        cout << "WM_Quit: now." << endl;
        if (!CheckNetwork())
        {
            lout << "ERROR: Cannot shutdown CANbus network." << endl;
            return (void*)0xebb0;
        }
        TerminateApp();
        cout << "WM_Quit: done." << endl;
        return (void*)0xaaaa;
    }
    cout << "MCosy::Proc: Unknown message 0x" << msg << endl;
    return (void*)0xffffffff;
}

void *MTTalk::Thread()
{
    fCosy->TalkThread();
    return NULL;
}

void MCosy::ReadConfig()
{
    cout << "Reading configuration file..." << flush;
    TEnv env(".cosyrc");
    cout << "done." << endl;

    cout << "Reading telescope range..." << flush;
    const Double_t amin = env.GetValue("Az_Min[deg]", -95.0);
    const Double_t zmin = env.GetValue("Zd_Min[deg]", -75.0);
    fMin.Set(zmin, amin);

    const Double_t amax = env.GetValue("Az_Max[deg]", 305.0);
    const Double_t zmax = env.GetValue("Zd_Max[deg]", 98.25);
    fMax.Set(zmax, amax);
    cout << "done." << endl;

    cout << " * Min:  " << zmin << "deg  " << amin << "deg" << endl;
    cout << " * Max:  " << zmax << "deg  " << amax << "deg" << endl;

    fMin = fBending.AddOffsets(fMin/kRad2Deg);
    fMax = fBending.AddOffsets(fMax/kRad2Deg);

    cout << " * Min': " << fMin.Zd()*kRad2Deg << "deg  " << fMin.Az()*kRad2Deg << "deg" << endl;
    cout << " * Max': " << fMax.Zd()*kRad2Deg << "deg  " << fMax.Az()*kRad2Deg << "deg" << endl;

    cout << "Reading gear ratios..." << flush;
    kGear.X(env.GetValue("Zd_GearRatio[U_mot/U_tel]", 1000.0));
    kGear.Y(env.GetValue("Az_GearRatio[U_mot/U_tel]", 1000.0));

    kResRE.Y(0);
    if (fMac1 && !fMac1->IsZombieNode())
        kResRE.Y(fMac1->GetRes());
    else
        if (fMac3 && !fMac3->IsZombieNode())
            kResRE.Y(fMac3->GetRes());
        else
            kResRE.Y(env.GetValue("Az_ResRE[re/U_mot]", 1500));

    kResRE.X(0);
    if (fMac2 && !fMac2->IsZombieNode())
        kResRE.X(fMac2->GetRes());
    else
        kResRE.X(env.GetValue("Zd_ResRE[re/U_mot]", 1500));

    kResSE.X(0);
    if (fZd1 && !fZd1->IsZombieNode())
        kResSE.X(fZd1->GetPhysRes());
    else
        if (fZd2 && !fZd2->IsZombieNode())
            kResSE.X(fZd2->GetPhysRes());
        else
            kResSE.X(env.GetValue("Zd_ResSE[se/U_mot]", 16384));

    kResSE.Y(0);
    if (fAz && !fAz->IsZombieNode())
        kResSE.Y(fAz->GetPhysRes());
    else
        kResSE.Y(env.GetValue("Az_ResSE[se/U_mot]", 16384));

    // believing the Macs manual '*4' shouldn't be necessary, but it is.
    // Because the a RE is 4 quad counts.
    // Calculating speeds we have to convert back to qc
    kResRE  *= 4;
    kGearTot = kResRE*kGear;

    cout << "done." << endl;

    cout << " * Setting Gear Ratios:" << endl;
    cout << "   --------------------" << endl;
    cout << " *  X: " << kGear.X() << "*" << kResRE.X()/4 << "/" << kResSE.X() << "=4*" << kGearTot.X() << "/" << kResSE.X() << endl;
    cout << " *  Y: " << kGear.Y() << "*" << kResRE.Y()/4 << "/" << kResSE.Y() << "=4*" << kGearTot.Y() << "/" << kResSE.Y() << endl;
}
/*
void MCosy::InitSync()
{
    if (!fMac3)
    {
        lout << "Unable to Init Sync! Mac3 not available." << endl;
        return;
    }

    const int res = fMac3->GetVelRes();

    fMac3->SetVelocity(0.3*res);
    fMac3->SetAcceleration(0.2*res);
    fMac3->SetDeceleration(0.2*res);
    fMac3->StartPosSync();
}
*/
void MCosy::TalkThreadSeTest()
{
//    if (fZd1->IsZombieNode() || fZd2->IsZombieNode())
    //        return;

    if (fHist)
    {
        lout << "You are much too fast... try again." << endl;
        return;
    }

    fHist = new TH2F("Diff", "Difference of SE values",
                           201, fMin.Zd(), fMax.Zd(), 41, -10.5, 10.5);
    fHist->SetXTitle("ZA [\\circ]");
    fHist->SetYTitle("\\Delta SE");

    Double_t offset = 0;

    int cnt = 0;

    lout << "Starting Shaftencoder Test..." << endl;

    while (fBackground==kBgdSeTest)
    {
        fZd1->ResetPosHasChanged();
        fZd2->ResetPosHasChanged();

        while (!fZd1->PosHasChanged() && !fZd2->PosHasChanged() &&
               fBackground==kBgdSeTest)
            usleep(1);

        const Double_t pos[3] = {
            (fZd1->GetPos()+8192)%16384,
            (fZd2->GetPos()+8192)%16384,
            fAz->GetPos() };

        //
        //  Estimate Offset from the first ten positions
        //
        if (cnt++<10)
        {
            offset += pos[0]+pos[1];
            continue;
        }
        if (cnt==11)
        {
            offset /= 10;
            cnt++;
        }

        Double_t apos = (pos[0]-pos[1])/2 * TMath::TwoPi() / kResSE.X();

        ZdAz bend = fBending.CorrectBack(ZdAz(apos, pos[2]))*kRad2Deg;
        fHist->Fill(bend.Zd(), pos[0]+pos[1]-offset);
    }

    lout << "Shaftencoder Test Stopped... displaying Histogram." << endl;

    fBackground=kBgdSeTestDispl;
}

void MCosy::TalkThreadGear()
{
//    if (fZd1->IsZombieNode() || fZd2->IsZombieNode())
    //        return;

    if (fHist)
    {
        lout << "You are much too fast... try again." << endl;
        return;
    }

    fHist = new TH3F("Gear", "Gear Ratio Re/Se",
                     (int)((fMax.Zd()-fMin.Zd())/2.5+1), fMin.Zd(), fMax.Zd(),
                     (int)((fMax.Az()-fMin.Az())/2.5+1), fMin.Az(), fMax.Az(),
                     61, 349.5, 500.5);

    fHist->SetXTitle("Zd [\\circ]");
    fHist->SetYTitle("Az [\\circ]");
    fHist->SetZTitle("Re/Se");

    lout << "Starting Gear determination..." << endl;

    ZdAz se0 = GetSePos();
    ZdAz re0 = GetRePosPdo();

    while (fBackground==kBgdGear)
    {
        fZd1->ResetPosHasChanged();
        fZd2->ResetPosHasChanged();
        fAz->ResetPosHasChanged();

        while (!fZd1->PosHasChanged() && !fZd2->PosHasChanged() &&
               !fAz->PosHasChanged() && fBackground==kBgdGear)
            usleep(1);

        ZdAz se = GetSePos();
        ZdAz re = GetRePosPdo();

        ZdAz dse = se-se0;
        ZdAz dre = re-re0;

        if (fabs(dse.Zd())*144>kResSE.X()) // Each 2.5deg (144)
        {
            se0.Zd(se.Zd());
            re0.Zd(re.Zd());

            se -= dse/2;

            ZdAz bend = fBending.CorrectBack(se*TMath::TwoPi()/kResSE)*kRad2Deg;
            ((TH3*)fHist)->Fill(bend.Zd(), bend.Az(), dre.Zd()/dse.Zd());
        }

        if (fabs(dse.Az())*144>kResSE.Y()) // Each 2.5deg (144)
        {
            se0.Az(se.Az());
            re0.Az(re.Az());

            se -= dse/2;

            ZdAz bend = fBending.CorrectBack(se*TMath::TwoPi()/kResSE)*kRad2Deg;
            ((TH3*)fHist)->Fill(bend.Az(), bend.Az(), dre.Az()/dse.Az());
        }
    }
    lout << "Gear Test Stopped... displaying Histogram." << endl;

    fBackground=kBgdGearDispl;
}

void MCosy::TalkThread()
{
    /* ========== FIXME? =============
     if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
        return;
        */

    if (fMac1 && fMac2)
    {
        fMac1->ReqPos();
        fMac2->ReqPos();
    }

    //InitSync();

    /*** FOR DEMO MODE ***/
    if (!fZd1 || !fZd2 || !fAz)
        return;
    /*** FOR DEMO MODE ***/

    //
    // Start the Network
    //
    while (1)
    {
        //
        // wait until a tracking session is started
        //
        while (fBackground==kBgdNone)
            usleep(1);

        switch (fBackground)
        {
        case kBgdNone:
            continue;
/*#ifndef NEWALGO
        case kBgdTracking:
            TalkThreadTracking();
            continue;
#endif*/
        case kBgdSeTest:
            TalkThreadSeTest();
            continue;

        case kBgdGear:
            TalkThreadGear();
            continue;

        default:
            continue;
        }
    }
}

ZdAz MCosy::GetPointingPos() const
{
    if (fZd1->IsZombieNode() || fZd2->IsZombieNode() || fAz->IsZombieNode())
        return ZdAz(0, 0);

    // GetPointingPos [deg]
    const ZdAz seist = GetSePos()*TMath::TwoPi()/kResSE; // [rad]
    return fBending.CorrectBack(seist)*TMath::RadToDeg();
}

Bool_t MCosy::HandleTimer(TTimer *t)
{
    const Int_t rc = fMutexGui.TryLock();
    if (rc==13)
        cout << "MCosy::HandleTimer - mutex is already locked by this thread" << endl;

    if (rc)
    {
        lout << "* GUI update skipped due to locked mutex." << endl;
        return kTRUE;
    }

    //
    // Update Gui, foremer MTGui.
    //
    if (fZd1)
        fZd1->DisplayVal();
    if (fZd2)
        fZd2->DisplayVal();
    if (fAz)
        fAz->DisplayVal();

    Byte_t avail = 0;

    avail |= (fMac1 && !fMac1->IsZombieNode()) ? 0x01 : 0;
    avail |= (fMac2 && !fMac2->IsZombieNode()) ? 0x02 : 0;
    avail |= (fMac3 && !fMac3->IsZombieNode()) ? 0x04 : 0;
    avail |= (fZd1  && !fZd1->IsZombieNode())  ? 0x08 : 0;
    avail |= (fZd2  && !fZd2->IsZombieNode())  ? 0x10 : 0;
    avail |= (fAz   && !fAz->IsZombieNode())   ? 0x20 : 0;

    if (HasError())
        SetStatus(MDriveCom::kError);


    ZdAz bendist = fStatus&MDriveCom::kTracking ? fTrackingPos : GetPointingPos();

    //cout << (fStatus&MDriveCom::kTracking?"TRA: ":"POS: ") << bendist.Zd() << " " << bendist.Az() << endl;

    fCom->SendReport(fStatus, fRaDec, fZdAzSoll, bendist, fTrackingError);

    fWin->UpdateWeather(*fCom);
    fWin->Update(bendist, fTrackingError, fVelocity, /*fOffset,*/
                 fRaDec, fZdAzSoll, fStatus, avail);

    lout.UpdateGui();

    const Bool_t trigger = fTriggerDisplay;
    fTriggerDisplay = kFALSE;

    if (fBackground==kBgdSeTestDispl || (trigger&&fBackground==kBgdSeTest))
        DisplayHistTestSe(!trigger);

    if (fBackground==kBgdGearDispl || (trigger&&fBackground==kBgdGear))
        DisplayHistGear(!trigger);

    if (fMutexGui.UnLock()==13)
        cout << "MCosy::HandleTimer - tried to unlock mutex locked by other thread." << endl;

    return kTRUE;
}

void MCosy::DisplayHistTestSe(Bool_t del)
{
    lout << "Displaying histogram..." << endl;

    TH2F &hist = *(TH2F*)fHist;

    if (del)
    {
        fHist = NULL;
        fBackground = kBgdNone;
    }

    TCanvas *c=new TCanvas("c1", "", 1000, 1000);
    c->Divide(1,2);

    c->cd(1);
    TH2 *h=(TH2*)hist.DrawCopy();

    TProfile *p = h->ProfileX("_pfx", -1, 9999, "s");
    p->SetLineColor(kBlue);
    p->Draw("same");
    p->SetBit(kCanDelete);

    c->cd(2);

    TH1F p2("spread", "Spread of the differences", hist.GetNbinsX(), hist.GetBinLowEdge(1),
            hist.GetBinLowEdge(hist.GetNbinsX()+1));
    p2.SetXTitle("Zd [\\circ]");
    for (int i=0; i<hist.GetNbinsX(); i++)
        p2.SetBinError(i, p->GetBinError(i));
    p2.SetLineColor(kRed);
    p2.SetStats(0);
    p2.DrawCopy();

    if (del)
        delete &hist;
}

void MCosy::DisplayHistGear(Bool_t del)
{
    lout << "Displaying histogram..." << endl;

    TH3F &hist = *(TH3F*)fHist;

    if (del)
    {
        fHist = NULL;
        fBackground = kBgdNone;
    }

    TCanvas *c=new TCanvas("c1", "", 1000, 1000);
    c->Divide(2,2);

    // ----------

    c->cd(1);
    TH2D &h1=*(TH2D*)hist.Project3D("zx"); // Zd
    h1.SetTitle(" Gear Ratio Zenith Distance [re/se]  ");
    h1.SetXTitle("Zd [\\circ]");
    h1.Draw();
    h1.SetBit(kCanDelete);

    TProfile *p1 = h1.ProfileX("_pfx", -1, 9999, "s");
    p1->SetLineColor(kBlue);
    p1->Draw("same");
    p1->SetBit(kCanDelete);

    // ----------

    c->cd(2);
    TH2D &h2=*(TH2D*)hist.Project3D("zy"); // Az
    h2.SetTitle(" Gear Ratio Azimuth [re/se]  ");
    h2.SetXTitle("Zd [\\circ]");
    h2.Draw();
    h2.SetBit(kCanDelete);

    TProfile *p2 = h2.ProfileX("_pfx", -1, 9999, "s");
    p2->SetLineColor(kBlue);
    p2->Draw("same");
    p2->SetBit(kCanDelete);

    // ----------

    c->cd(3);

    TAxis &axe1 = *h1.GetXaxis();

    TH1F f1("spreadzd", " Spread Zenith Distance ",
            axe1.GetNbins(), axe1.GetXmin(), axe1.GetXmax());
    f1.SetXTitle("Zd [\\circ]");
    for (int i=0; i<axe1.GetNbins(); i++)
        f1.SetBinError(i, p1->GetBinError(i));
    f1.SetLineColor(kRed);
    f1.SetStats(0);
    f1.DrawCopy();

    c->cd(4);

    // ----------

    TAxis &axe2 = *h2.GetXaxis();

    TH1F f2("spreadaz", " Spread Azimuth ",
            axe2.GetNbins(), axe2.GetXmin(), axe2.GetXmax());
    f2.SetXTitle("Az [\\circ]");
    for (int i=0; i<axe2.GetNbins(); i++)
        f2.SetBinError(i, p2->GetBinError(i));
    f2.SetLineColor(kRed);
    f2.SetStats(0);
    f2.DrawCopy();

    // ----------

    if (del)
        delete &hist;
}

// --------------------------------------------------------------------------
//
// Start the work of the application:
//
// Start the Can-Network.
// Start the MCosy::TalkThread thread.
// turn on the gui update
//
void MCosy::Start()
{
    // Don't call this function twice!
    Network::Start();

    CheckForError();

    ReadConfig();

    lout << "- Starting TX Thread." << endl;
    fTTalk = new MTTalk(this);

    lout << "- Starting GUI update." << endl;
    fUpdateGui->TurnOn();
}

// --------------------------------------------------------------------------
//
// Start the work of the application:
//
// Turn of the gui update
// stop the MCosy::TalkThread thread.
// Stop the network
//
void MCosy::Stop()
{
    lout << "- Stopping GUI update." << endl;
    fUpdateGui->TurnOff();
    lout << "- GUI Update stopped." << endl;

    delete fTTalk;
    lout << "- TX Thread stopped." << endl;

    Network::Stop();
}

// --------------------------------------------------------------------------
//
// Disable the synchronization by using a negative CAN Id for id2.
//
void MCosy::Constructor(Int_t id1, Int_t id2, Int_t id3,
                        Int_t id4, Int_t id5, Int_t id6)
{
    //
    // Create Nodes
    //
    lout << "- Setting up network." << endl;

    fMac1=new Macs(id1, "Mac/Az", lout);
    fMac2=new Macs(id3, "Mac/Zd", lout);
    if (id2>=0)
        fMac3=new Macs(id2, "Mac/Az-Sync", lout);

    fZd1=new ShaftEncoder(id4, "SE/Zd1", lout);
    fZd2=new ShaftEncoder(id5, "SE/Zd2", lout);
    fAz =new ShaftEncoder(id6, "SE/Az",  lout);

    fZd1->SetReport(fOutRep);
    fZd2->SetReport(fOutRep);
    fAz->SetReport(fOutRep);

    fAz->SetMotor(fMac1);
    fZd1->SetMotor(fMac2);
    fZd2->SetMotor(fMac2);
 
    lout << "- Connecting devices to network." << endl;

    //
    // Connect the devices to the network
    //
    SetNode(fMac1);
    SetNode(fMac2);
    if (id2>=0)
        SetNode(fMac3);
    SetNode(fZd1);
    SetNode(fZd2);
    SetNode(fAz);

    //
    // Create Gui Event timer and Gui
    //
    lout << "- Initializing GUI Timer." << endl;
    fUpdateGui = new TTimer(this, 100); // 100ms

    lout << "- Starting GUI." << endl;
    fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
}
/*
void MCosy::ConstructorSE(Int_t id4, Int_t id5, Int_t id6)
{
    //
    // Create Nodes
    //
    lout << "- Setting up network." << endl;

    fZd1=new ShaftEncoder(id4, "SE/Zd1", lout);
    fZd2=new ShaftEncoder(id5, "SE/Zd2", lout);
    fAz =new ShaftEncoder(id6, "SE/Az",  lout);

    lout << "- Connecting devices to network." << endl;

    //
    // Connect the devices to the network
    //
    SetNode(fZd1);
    SetNode(fZd2);
    SetNode(fAz);

    //
    // Create Gui Event timer and Gui
    //
    lout << "- Initializing GUI Timer." << endl;
    fUpdateGui = new TTimer(this, 100); // 100ms

    lout << "- Starting GUI." << endl;
    fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
}

void MCosy::ConstructorDemo()
{
    //
    // Create Nodes
    //
    lout << "- Setting up network." << endl;

    //
    // Create Gui Event timer and Gui
    //
    lout << "- Initializing GUI Timer." << endl;
    fUpdateGui = new TTimer(this, 100); // 100ms

    lout << "- Starting GUI." << endl;
    fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
}
*/

TString MCosy::GetFileName(const char *fmt)
{
    // FIXME: Timeout missing
    while (1)
    {
        MTime time(-1);
        const TString name = Form(fmt, (const char*)time.GetFileName());
        if (gSystem->AccessPathName(name, kFileExists))
            return name;
            break;

        usleep(1000);
    }
    return "";
}

MCosy::MCosy(/*int mode,*/ const char *dev, const int baud, MLog &out)
: Network(dev, baud, out), fObservatory(MObservatory::kMagic1), fStarguider(NULL), fZd1(0), fZd2(0), fAz(0), fMac1(0), fMac2(0), fMac3(0), fBackground(kBgdNone), fStatus(MDriveCom::kStopped), fOutTp(0), fOutRep(0)
{
    TEnv env(".cosyrc");
    const Int_t id1 = env.GetValue("Az_Id-MAC1", 1); //1
    const Int_t id2 = env.GetValue("Az_Id-MAC2", 2); //2
    const Int_t id3 = env.GetValue("Zd_Id-MAC",  3); //3
    const Int_t id4 = env.GetValue("Zd_Id-SE1",  4); //4
    const Int_t id5 = env.GetValue("Zd_Id-SE2",  5); //5
    const Int_t id6 = env.GetValue("Az_Id-SE",   6); //6

    TString name = GetFileName("rep/cosy_%s.rep");
    cout << "Open Repfile: " << name << endl;
    fOutRep = new MLog(name, kTRUE);
    *fOutRep << "[Drive Report File]" << endl;
    *fOutRep << "Version <cvs>" << endl;
    *fOutRep << "Date " << MTime(-1) << endl;
    *fOutRep << "[Reports]" << endl;

/*
    lout << "- Program in ";
    switch (mode)
    {
    case 0:
        lout << "<<Standard mode>>" << endl;*/
        fBending.Load("bending.txt");
        Constructor(id1, id2, id3, id4, id5, id6);/*
        break;
    case 1:
        lout << "<<SE mode>>" << endl;
        fBending.Load("bending.txt");
        ConstructorSE(id4, id5, id6);
        break;
    default:
        lout << "<<Demo mode>>" << endl;
        ConstructorDemo();
    }
*/
    lout.SetOutputGui(fWin->GetLog(), kTRUE);

    fZd1->SetDisplay(fWin->GetLabel2());
    fZd2->SetDisplay(fWin->GetLabel3());
    fAz->SetDisplay(fWin->GetLabel1());

    fCom = new MDriveCom(this, *fOutRep);
    fCom->Start();
}

void MCosy::TerminateApp()
{
    cout << "MCosy::TerminateApp()" << endl;
/*
    Int_t rc;
    TGMessageBox msg(this, gClient->GetRoot(),
                     "Information",
                     "Cosy is shutting down the system - this may take wa while!",
                     kMBIconExclamation,
                     kMBOK, //kMBClose
                     &rc, 0);
*/

    lout.DisableOutputDevice(MLog::eGui);
    // FIXME: WHY DOES THIS CRASH THE APPLICATIOn WHILE TRAKING?
    // lout.SetOutputGui(NULL, kFALSE);

    gApplication->Terminate(0);
}

MCosy::~MCosy()
{
    if (fOutTp)
    {
        *fOutTp << "END" << endl;
        delete fOutTp;
    }
    delete fOutRep;

    cout << "Deleting GUI timer." << endl;

    delete fUpdateGui;
    delete fCom;

    cout << "Deleting Nodes." << endl;

    fZd1->SetReport(0);
    fZd2->SetReport(0);
    fAz->SetReport(0);

    delete fAz;
    delete fZd1;
    delete fZd2;
    delete fMac1;
    delete fMac2;
    if (fMac3)
        delete fMac3;

    cout << "Deleting MGCosy." << endl;

    lout.DisableOutputDevice(MLog::eGui);

    delete fWin;

    cout << "MGCosy destructed." << endl;
}