#include "MCosy.h"
#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 "slalib/slalib.h"  // FIXME: REMOVE

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

ClassImp(MCosy);

typedef struct tm tm_t;

/*
#define GEAR_RATIO_ALT  2475.6 // [U_mot/U_tel(360deg)]
#define GEAR_RATIO_AZ   5891.7 // [U_mot/U_tel(360deg)]

#define RES_RE           500   // [re/U_mot]
#define RES_SE         16384   // [se/U_tel(360deg)]
*/
/*
 #define GEAR_RATIO_ALT (75.55*16384/1500) // 75.25 VERY IMPORTANT! unit=U_mot/U_tel
 #define GEAR_RATIO_AZ  (179.8*16384/1500)  // VERY IMPORTANT! unit=U_mot/U_tel
*/

//const XY kGearRatio (GEAR_RATIO_ALT*RES_RE/RES_SE, GEAR_RATIO_AZ*RES_RE/RES_SE);   //[re/se]
//const XY kGearRatio2(GEAR_RATIO_ALT*RES_RE/360.0,  GEAR_RATIO_AZ*RES_RE/360.0);    //[re/deg]

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

//#define EXPERT
#undef EXPERT

double MCosy::Rad2SE(double rad) const
{
    return 16384.0/k2Pi*rad;
}

double MCosy::Rad2ZdRE(double rad) const
{
    return 16384.0/k2Pi*rad*kGearRatio.X();
}

double MCosy::Rad2AzRE(double rad) const
{
    return 16384.0/k2Pi*rad*kGearRatio.Y();
}

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

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

/*
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);
}

// --------------------------------------------------------------------------
//
// request the current positions from the rotary encoders.
// use GetRePos to get the psotions. If the request fails the function
// returns kFALSE, otherwise kTRUE
//
Bool_t MCosy::RequestRePos()
{
    //
    // Send request
    //
    fMac2->RequestSDO(0x6004);
    fMac1->RequestSDO(0x6004);

    //
    // Wait until the objects are received.
    //
    fMac2->WaitForSdo(0x6004);
    fMac1->WaitForSdo(0x6004);

    //
    // If waiting was not interrupted everything is ok. return.
    //
    if (!(Break() || HasError() || HasZombie()))
        return kTRUE;

    //
    // If the waiting was interrupted due to a network error,
    // print some logging message.
    //
    if (HasError())
        lout << "Error while requesting re pos from Macs (SDO #6004)" << endl;

    return kFALSE;
}

// --------------------------------------------------------------------------
//
//  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());
}

// --------------------------------------------------------------------------
//
//  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 MCosy::SetPosVelocity(const Float_t ratio, Float_t vel)
{
    //
    // Set velocities
    //
    const int vr = fMac1->GetVelRes();

    vel *= vr;

    if (ratio <1)
    {
        fMac1->SetVelocity(vel);
        fMac2->SetVelocity(vel*ratio);
    }
    else
    {
        fMac1->SetVelocity(vel/ratio);
        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 MCosy::DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2)
{
    if (HasZombie())
        return;

    SetStatus(MDriveCom::kMoving);

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

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

// --------------------------------------------------------------------------
//
// 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;
    t.Now();
    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;
}

// --------------------------------------------------------------------------
//
// 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]
{
    const ZdAz d = dst*kRad2Deg;

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

    //
    // Calculate new target position (shortest distance to go)
    //
    const ZdAz src = 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 = fBending(dst); // [rad]

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

    if (!CheckRange(bend))
        return kFALSE;

    bend *= kRad2Deg;
    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 = fMac1->GetVelRes();

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

        lout << "- Step #" << i << endl;
        //
        // Get Shaft Encoder Positions
        //
        const ZdAz p=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;
            SetStatus(MDriveCom::kStopped);
            return TRUE;
        }

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

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

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

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

#ifdef EXPERT
            fMac2->SetAcceleration(0.4*vr);// 0.4
            fMac2->SetDeceleration(0.4*vr);// 0.4
            SetPosVelocity(fabs(rd.Ratio()), 0.2); // fast: 0.6, slow: 0.2
#else
            fMac2->SetAcceleration(0.2*vr);
            fMac2->SetDeceleration(0.1*vr);
            SetPosVelocity(fabs(rd.Ratio()), 0.1);
#endif
        }

        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() || HasError() || HasZombie()))
    {
        t.Now();
        lout << t << " - Positioning done." << endl;
        SetStatus(MDriveCom::kStopped);
        return TRUE;
    }

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

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

// --------------------------------------------------------------------------
//
// Sets the tracking velocity
//
// The velocities are given in a ZdAz object in re/min. Return kTRUE
// in case of success, kFALSE in case of failure.
//
Bool_t MCosy::SetVelocity(const ZdAz &v)
{
    //
    // Send the new velocities for both axes.
    //
    fMac2->SendSDO(0x3006, 1, (LWORD_t)v.Zd());  // SetRpmVelocity [re/min]
    fMac1->SendSDO(0x3006, 1, (LWORD_t)v.Az());  // SetRpmVelocity [re/min]

    //
    // Wait for the objects to be acknoledged.
    //
    fMac2->WaitForSdo(0x3006, 1);
    fMac1->WaitForSdo(0x3006, 1);

    //
    // If the waiting for the objects wasn't interrupted return kTRUE
    //
    if (!(Break() || HasError() || HasZombie()))
        return kTRUE;

    //
    // print a message if the interruption was due to a Can-node Error
    //
    if (HasError())
        lout << "Error while setting velocity (SDO #3006)" << endl;

    return kFALSE;
}

// --------------------------------------------------------------------------
//
// Initializes Tracking mode
//
// Initializes the accelerations of both axes with 90% of the maximum
// acceleration. Set the status for moving and tracking and starts thr
// revolution mode.
//
bool MCosy::InitTracking()
{
    // FIXME? Handling of Zombie OK?
    if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
        return false;

    //
    // Start revolution mode
    //
    fMac2->SetAcceleration(0.1*fMac2->GetVelRes());
    fMac2->SetDeceleration(0.1*fMac2->GetVelRes());
    if (fMac2->IsZombieNode())
        return false;

    fMac1->SetAcceleration(0.1*fMac1->GetVelRes());
    fMac1->SetDeceleration(0.1*fMac1->GetVelRes());
    if (fMac1->IsZombieNode())
        return false;

    SetStatus(MDriveCom::kMoving | MDriveCom::kTracking);

    fMac2->SetRpmMode(TRUE);
    if (fMac2->IsZombieNode())
        return false;

    fMac1->SetRpmMode(TRUE);
    if (fMac1->IsZombieNode())
        return false;

    return true;
}

// --------------------------------------------------------------------------
//
// Limits the speed. 
//
// This function should work as a limiter. If a tracking error is too large
// to be corrected fast enough we would get enormous velocities. These
// velocities are limited to the maximum velocity.
//
void MCosy::LimitSpeed(ZdAz *vt, const ZdAz &vcalc) const
{
    //
    // How to limit the speed. If the wind comes and blowes
    // we cannot forbid changing of the sign. But on the other hand
    // we don't want fast changes!
    //
    ULong_t vrzd = fMac1->GetVelRes();
    ULong_t vraz = fMac2->GetVelRes();

#define sgn(x) (x<0?-1:1)

    //
    // When speed changes sign, the maximum allowed speed
    // is 25% of the |v|
    //
    //const Float_t limit    = 0.25;

    //
    // The maximum allowed speed while tracking is 10%
    //
    const Float_t maxtrack = 0.1;
/*
    if (sgn(vt->Az()) != sgn(vcalc.Az()))
        vt->Az(0);
//    else
    {
        if (fabs(vt->Az()) < fabs(vcalc.Az()) *0.5)
            vt->Az(0.5*vcalc.Az());

        if (fabs(vt->Az()) > fabs(vcalc.Az()) *1.5)
            vt->Az(1.5*vcalc.Az());
    }

    if (sgn(vt->Zd()) != sgn(vcalc.Zd()))
        vt->Zd(0);
//    else
    {
        if (fabs(vt->Zd()) > fabs(vcalc.Az()) *1.5)
            vt->Zd(1.5*vcalc.Zd());

        if (fabs(vt->Zd()) < fabs(vcalc.Az()) *0.5)
            vt->Zd(0.5*vcalc.Zd());
    }
    */
   /*
    if (sgn(vt->Az()) != sgn(vcalc.Az())
        && fabs(vt->Az()) < limit*fabs(vcalc.Az())
       )
        {
            lout << "Warning: Negative Azimuth speed limit (" << limit*100 << "%) exceeded... set to 0." << endl;
            vt->Az(0);
        }
    else*/
        if (fabs(vt->Az()) > maxtrack*vraz)
        {
            lout << "Warning: Azimuth speed limit (" << maxtrack*100 << "%) exceeded... limited." << endl;
            vt->Az(maxtrack*vraz*sgn(vcalc.Az()));
        }
/*
    if (sgn(vt->Zd()) != sgn(vcalc.Zd())
        && fabs(vt->Zd()) < limit*fabs(vcalc.Zd())
       )
        {
            lout << "Warning: Negative Altitude speed limit (" << limit*100 << "%) exceeded... set to 0." << endl;
            vt->Zd(0);
        }
    else
 */       if (fabs(vt->Zd()) > maxtrack*vrzd)
        {
            lout << "Warning: Altitude speed limit (" << maxtrack*100 << "%) exceeded... limited." << endl;
            vt->Zd(maxtrack*vrzd*sgn(vcalc.Zd()));
        }
}
/*
Bool_t MCosy::AlignTrackingPos(ZdAz pointing, ZdAz &za) const
{
    // pointing [deg]
    if (pointing.Zd()<0)
    {
        pointing.Zd(-pointing.Zd());
        pointing.Az(pointing.Az()+180);
    }

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

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

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

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

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

    return rc;
}
*/

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);
    }

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

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

        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 *= 16384/TMath::Pi()/2; // [se]
    dest *= -kGearRatio;         // [re]

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

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

void MCosy::TrackPosition(const RaDec &dst) // ra, dec [rad]
{
    lout << "Track Position: " << dst.Ra()*kRad2Deg/15 << "h, " << dst.Dec()*kRad2Deg << "deg (Ra/Dec)" << endl;

    SlaStars sla(fObservatory);

    //
    // Position to actual position
    //
    sla.Now();
    ZdAz dest = sla.CalcZdAz(dst);

    // FIXME: Determin tracking start point by star culmination
    if (dest.Az()<-TMath::Pi()/2)
    {
        lout << "Adding 360deg to Azimuth " << dest.Az()*kRad2Deg << endl;
        dest.Az(dest.Az() + TMath::Pi()*2);
    }

    if (dest.Az()>3*TMath::Pi()/2)
    {
        lout << "Substracting 360deg to Azimuth " << dest.Az()*kRad2Deg << endl;
        dest.Az(dest.Az() -TMath::Pi()*2);
    }

    if (!SetPosition(dest, kTRUE))
    //if (!SetPosition(dest, kFALSE))
    {
        lout << "Error: Cannot start tracking, positioning failed." << endl;
        return;
    }

    //
    // calculate offset from present se position
    //
    const ZdAz sepos = GetSePos()*kGearRatio;

    if (!RequestRePos())
        return;

    //
    // Estimate Offset before starting to track
    //
    fOffset = sepos-GetRePos();

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

    //
    // Init accelerations and Rpm Mode
    //
    if (!InitTracking())
    {
        StopMovement();
        return;
    }

    XY xy(Rad2Deg(dst.Ra())*24/360, Rad2Deg(dst.Dec()));

    sla.Now();
    lout << sla.GetTime() << " - Start tracking:";
    lout << " Ra: " << xy.X() << "h  " << "Dec: " << xy.Y() << "\xb0" << endl;
/*#ifdef EXPERT
    ofstream fout("coordinates.txt");
    fout << xy;
    fout.close();
#endif
*/    //
    // Initialize Tracker (slalib or starguider)
    //
    fRaDec = dst;
    fBackground = kBgdTracking;

//---    ofstream fout("log/cosy.pos");
//---    fout << "Tracking:";
//---    fout << " Ra: " << Rad2Deg(dst.Ra())  << "\x9c  ";
//---    fout << "Dec: " << Rad2Deg(dst.Dec()) << "\x9c" << endl << endl;
//---    fout << "     Mjd/10ms    V/re/min/4" << endl;

    //
    // We want to reach the theoretical position exactly in about 0.5s
    //
    // *OLD*const float dt = 1;  // 1 second
    const float dt = 5;//3;  // 2 second
    while (!(Break() || HasError() || HasZombie()))
    {
        //
        // Request Target position for this moment
        //
        sla.Now();

        //
        // Request theoretical Position for a time in the future (To+dt) from CPU
        //
        const Double_t mjd = sla.GetMjd()+dt/(60*60*24);
        const ZdAz pointing = sla.CalcZdAz(fRaDec, mjd); // soll pointing [deg]

        /*
        ZdAz dest;
        if (!AlignTrackingPos(pointing, dest))
            break;
            */
        ZdAz dest = AlignTrackingPos(pointing);

        ZdAz vcalc = sla.GetApproxVel(fRaDec) * kGearRatio2*4./60.;  // [re/min]

        float dtime = -1;
        if (kFALSE /*fUseStarguider*/)
            dtime = Starguider(mjd, dest);

        if (dtime<0)
        {
            dest = fBending(dest);       // [rad]

            if (!CheckRange(dest))
                break;

            dest *= 16384/TMath::Pi()/2; // [se]
            dest *= kGearRatio;          // [re]

            //
            // Request absolute position of rotary encoder from Macs
            //
            if (!RequestRePos())
                break;

            //
            // distance between (To+dt) and To [re]
            // position time difference < 5usec
            // fOffset does the synchronization between the
            // Shaft- and the rotary encoders
            dest -= GetRePos() + fOffset;

            dtime = dt;
        }

        //
        // 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/(dtime/*-(fMac2->GetTime()-sla)*/);

        //
        // calculate real velocity of future [re/min]
        // believing the Macs manual '/4' shouldn't be necessary, but it is.
        //
        ZdAz vt = v/4;
        LimitSpeed(&vt, vcalc);
        vt.Round();

        //
        // check if the drive is fast enough to follow the star
        //
        if (vt.Zd()>.9*fMac1->GetVelRes() || vt.Az()>.9*fMac2->GetVelRes())
        {
            lout << "Error: Tracking speed faster than 90% of possible maximum velocity." << endl;
            break;
        }

        //
        // Set theoretical velocity (as early after calculation as possible)
        // Maybe we should attenuate the changes
        //
        if (!SetVelocity(vt))
            break;

        //
        // Now do 'unnecessary' things
        //
        fVelocity = vt/kGearRatio2*4;

//---        const double mjd = fMac2->GetMjd();
//---        fout << setprecision(15) << setw(17) << mjd*60.*60.*24. << " ";
//---        fout << setw(4) << vt.Zd() << " ";
//---        fout << setw(4) << vt.Az() << endl;
        //
        // FIXME? Calculate an accuracy for the tracking system?
        // How good do we reach the calculated position in 'real'
        // re valus?
        //


        //
        // Update speed as often as possible.
        // make sure, that dt is around 10 times larger than the
        // update time
        //
        //
        // The loop should not be executed faster than the ramp of
        // a change in the velocity can be followed.
        // (This is important on fast machines >500MHz)
        //
        /*
        MTimeout t(1000);
        while (!t.HasTimedOut())
            usleep(1);
         */
        usleep(1000000); // 1s
        cout << "." << flush;
        //usleep(50000); // 0.05s
    }

    sla.Now();

    fBackground = kBgdNone;
    StopMovement();

    lout << sla.GetTime() << " - Tracking stopped." << endl;
}

// --------------------------------------------------------------------------
//
// 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
}

void MCosy::StopTracking()
{
    //
    // Set status to Stopping
    //
    SetStatus(MDriveCom::kStopping);

    //
    // set deceleration to 50%
    //
    cout << "Stopping tracking (dec=20%)..." << endl;
    fMac1->SetDeceleration(0.2*fMac1->GetVelRes());
    fMac2->SetDeceleration(0.2*fMac2->GetVelRes());

    fMac2->SendSDO(0x3006, 1, (LWORD_t)0);  // SetRpmVelocity [re/min]
    fMac1->SendSDO(0x3006, 1, (LWORD_t)0);  // SetRpmVelocity [re/min]
    fMac2->WaitForSdo(0x3006, 1);
    fMac1->WaitForSdo(0x3006, 1);

    cout << "Waiting for end of movement..." << endl;
    WaitForEndMovement();

    //
    // Wait for the objects to be OKed.
    //
    fMac1->SetRpmMode(FALSE);
    fMac2->SetRpmMode(FALSE);

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

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

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;

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

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

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

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

            *tpout << 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);
            cout << dest.Zd()*180/3.1415 << " " << dest.Az()*180/3.1415 << endl;
            cout << rd.Ra()*12/3.1415 << " " << rd.Dec()*180/3.1415 << endl;
            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*16384/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::Pi()*2/16384;
            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;
    const Double_t gaz = env.GetValue("Az_GearRatio[U_mot/U_tel]", 1000.0);
    const Double_t gzd = env.GetValue("Zd_GearRatio[U_mot/U_tel]", 1000.0);

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

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

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

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

    kGearRatio.Set (gzd*resrezd*4/ressezd, gaz*resreaz*4/resseaz);  //[re/se]
    kGearRatio2.Set(gzd*resrezd*4/360.0,   gaz*resreaz*4/360.0);    //[re/deg]
    cout << "done." << endl;

    cout << " * Setting Gear Ratios:" << endl;
    cout << "   --------------------" << endl;
    cout << " *  X: " << gzd << "*" << resrezd << "/" << ressezd << "=4*" << kGearRatio.X() << endl;
    cout << " *  Y: " << gaz << "*" << resreaz << "/" << resseaz << "=4*" << kGearRatio.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::TalkThreadTracking()
{
    if (fZd1->IsZombieNode() && fZd2->IsZombieNode())
        return;

    if (fAz->IsZombieNode())
        return;

    if (!fMac1 || !fMac2)
        return;

    lout << "- Tracking Thread started..." << endl;

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

    ZdAz old;
    ZdAz ist = GetSePos();              // [se]

    ZdAz time;

    ZdAz sollzd = sla.CalcZdAz(fRaDec); // [rad]
    ZdAz sollaz = sollzd;               // [rad]

    //
    // only update fTrackingError while tracking
    //
    bool phca1=false;
    bool phca2=false;
    bool phcaz=false;

    while (fBackground==kBgdTracking)
    {
        //
        // Make changes (eg wind) smoother - attenuation of control function
        //
        const float weight = 1.; //0.3;

        //
        // This is the time constant which defines how fast
        // you correct for external influences (like wind)
        //
        fZd1->ResetPosHasChanged();
        fZd2->ResetPosHasChanged();
        fAz->ResetPosHasChanged();
        do
        {
            phca1 = fZd1->PosHasChanged();
            phca2 = fZd2->PosHasChanged();
            phcaz = fAz->PosHasChanged();
            usleep(1);
        } while (!phca1 && !phca2 && !phcaz && fBackground==kBgdTracking);

        //---usleep(100000); // 0.1s

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

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

        //
        // Get time from last shaftencoder position change (position: ist)
        // FIXME: I cannot take the avarage
        //
        // FIXME
        //time.Zd(fZd1->GetMjd());
        /* OLD* */
        if (fZd1->GetMjd()>fZd2->GetMjd())
            time.Zd(fZd1->GetMjd());
        else
            time.Zd(fZd2->GetMjd());

        //time.Zd((fZd1->GetMjd()+fZd2->GetMjd())/2.0);
        time.Az(fAz->GetMjd());

        //
        // if Shaftencoder changed position
        // calculate were we should be
        //
        if (phca1 || phca2 /*(int)ist.Zd() != (int)old.Zd()*/)
        {
            sollzd = sla.CalcZdAz(fRaDec, time.Zd()); // [rad]
            /*
            ZdAz dummy = fBending(sla.CalcZdAz(fRaDec));
            sollzd = CorrectTarget(ist, dummy); // [se]
            */
            fOffset.Zd(fOffset.Zd()*(1.-weight)+(ist.Zd()*kGearRatio.X()-istre.Zd())*weight);
        }

        if (phcaz /*(int)ist.Az() != (int)old.Az()*/)
        {
            sollaz = sla.CalcZdAz(fRaDec, time.Az()); // [rad]
            /*
            ZdAz dummy = fBending(sla.CalcZdAz(fRaDec));
            sollaz = CorrectTarget(ist, dummy); // [se]
            */
            fOffset.Az(fOffset.Az()*(1.-weight)+(ist.Az()*kGearRatio.Y()-istre.Az())*weight);
        }

        ZdAz soll(sollzd.Zd(), sollaz.Az()); // [rad]

        fZdAzSoll = AlignTrackingPos(soll);

        ist *= TMath::Pi()*2/16384;
        soll = fBending(fZdAzSoll);
        fTrackingError.Set(ist.Zd()-soll.Zd(), ist.Az()-soll.Az());

        //---            fout << setprecision(15) << setw(17) << time.Zd()*60.*60.*24. << " ";
        //---            fout << setprecision(5)  << setw(7)  << fTrackingError.Zd() << "  ";
        //---            fout << setprecision(15) << setw(17) << time.Az()*60.*60.*24. << " ";
        //---            fout << setprecision(5)  << setw(7)  << fTrackingError.Az() << endl;
    }

    lout << "- Tracking Thread done." << endl;

    //---        fout << endl << endl;
}

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::Pi()*2 / 16384;

        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>16384) // Each 2.5deg (144)
        {
            se0.Zd(se.Zd());
            re0.Zd(re.Zd());

            se -= dse/2;

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

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

            se -= dse/2;

            ZdAz bend = fBending.CorrectBack(se*2*TMath::Pi()/16384)*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;

        case kBgdTracking:
            TalkThreadTracking();
            continue;

        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()*2*TMath::Pi()/16384; // [se]

    //cout << seist.Zd()*kRad2Deg << " " << seist.Az()*kRad2Deg << endl;

    ZdAz back = fBending.CorrectBack(seist)*180/TMath::Pi();

    //cout << back.Zd() << " " << back.Az() << endl;

    return back;
}

Bool_t MCosy::HandleTimer(TTimer *t)
{
    //
    // Update Gui, foremer MTGui.
    //
    if (fZd1)
        fZd1->DisplayVal();
    if (fZd2)
        fZd2->DisplayVal();
    if (fAz)
        fAz->DisplayVal();

    ZdAz bendist = GetPointingPos();

    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);

    lout.UpdateGui();

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

    const Bool_t trigger = fTriggerDisplay;
    fTriggerDisplay = kFALSE;

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

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

    // FIXME: Not thread safe!
    static int i=0;
    if (i++==7)
    {
        fCom->SendReport(fStatus, fRaDec, fZdAzSoll, bendist, fTrackingError);
        i=0;
    }
    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);

    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);
}
*/
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)
{
    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
/*
    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());

    //
    // open tpoint file
    //
    MTime time;
    TString name;
    while (1)
    {
        time.Now();
        name = Form("tpoint/tpoint_%s.txt", (const char*)time.GetFileName());
        if (gSystem->AccessPathName(name, kFileExists))
            break;
    }

    cout << "TPoint File ********* " << name << " ********** " << endl;

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

    fCom = new MDriveCom(this, out);
    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()
{
    *tpout << "END" << endl;
    //streampos size = tpout.tellp();
    delete tpout;

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

    delete fUpdateGui;
    delete fCom;

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

    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;
}