#include "MCosy.h"

#include <iomanip>
#include <fstream>

#include <TEnv.h>
#include <TTimer.h>
#include <TApplication.h>

//#include "MLog.h"
#include "MLogManip.h"

#include "MEnv.h"
#include "MTime.h"
#include "MPointing.h"

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

#include "dkc.h"

ClassImp(MCosy);

using namespace std;

typedef struct tm tm_t;

//#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 * TMath::RadToDeg();

    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 Double_t pa = fMac1 ? (Double_t)fMac1->GetPdoPos2()/fMac1->GetPosRes()  : 0;
    const Double_t p1 = fMac2 ? (Double_t)fMac2->GetPdoPos2()/fMac2->GetPosRes() : 0;

    return ZdAz(p1, pa);
}

// --------------------------------------------------------------------------
//
// 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);
    gLog << inf << t << " - MCosy::WaitForEndMovement aborted...";
    if (Break())
        gLog << " Break signal...";
    if (HasError())
        gLog << " Network has error...";
    if (HasZombie())
        gLog << " Network has zombie...";
    gLog << 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())
    {
        gLog << err << "ERROR: Requested Zenith Angle below negative endswitch." << endl;
        return kFALSE;
    }

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

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

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


    return kTRUE;
}

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

    pointing *= TMath::RadToDeg();

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

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

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

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

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

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

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

    if (!rc)
        gLog << "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)/TMath::RadToDeg());

    const ZdAz diff = (dest-point)*TMath::RadToDeg();

    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]
{
    MSlewing point(this);

    // Default: point.SetPointAcc(0.03, 0.01);
    point.SetPointAcc(0.03, 0.01);
    point.SetPointVelocity(0.3);

    return point.SetPosition(dst, track);
}

void MCosy::TrackPosition(const RaDec &dst) // ra, dec [rad]
{
    MTracking track(this);
    track.SetOut(fOutRep);

    track.SetPointAcc(0.03, 0.01);
    track.SetPointVelocity(0.3);
    track.SetTrackAcc(0.01, 0.01);

    track.TrackPosition(dst);
}

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

    MTracking track(this);
    track.SetOut(fOutRep);
    track.SetPointAcc(0.09, 0.03);
    track.SetPointVelocity(1.0);
    track.SetTrackAcc(0.01, 0.01);

    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%
    //
    gLog << inf2 << "Stopping movement..." << endl;
    if (fMac1 && fMac2)
    {
        // FIXME: Makes sense?
        fMac1->SetDeceleration(TMath::Nint(0.03*1000000000));
        fMac2->SetDeceleration(TMath::Nint(0.09*1000000000));

        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()
{
    if (!HasConnection())
    {
        gLog << warn << "- No connection to network." << endl;
        return false;
    }

    CheckForError();

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

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

    CheckForError();

    return fMac1->IsOperative() && fMac2->IsOperative();
}

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

    case WM_STOP:
        //cout << "MCosy::Proc: Stop." << endl;
        if (!CheckNetwork())
            return 0xebb0;
        StopMovement();
        return 0;

    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*TMath::DegToRad())*TMath::RadToDeg();

            if (!fOutTp)
            {
                //
                // open tpoint file
                //
                const TString name = GetFileName("tpoint", "old-tpoint", "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();
            za.Set(TMath::Pi()/2-sepos.Zd(), sepos.Az());
            za *= TMath::RadToDeg();

            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 0xca1b;

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

            ZdAz dest = *((ZdAz*)mp) * TMath::DegToRad();
            //if (!SetPosition(dest, kTRUE))
            //    return 0x1234;

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

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

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

            const bool arm = mp ? *((bool*)mp) : true;
            if (arm)
            {
                fMac1->Arm();
                fMac2->Arm();
            }
            else
            {
                fMac1->Disarm();
                fMac2->Disarm();
            }
        }
        //cout << "WM_Position: done. (return 0x7777)" << endl;
        return 0x9999;

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

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

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

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

    case WM_PREPS:
        //cout << "WM_Track: START" << endl;
        {
            if (!CheckNetwork())
                return 0xebb0;

            const char *preps = (const char*)mp;
            cout << "Preposition command to " << preps << " received." << endl;

            ifstream fin("prepos.txt");
            if (!fin)
            {
                cout << "ERROR: cannot open prepos.txt." << endl;
                return 0xebb1;
            }

            while (1)
            {
                Double_t zd, az;
                fin >> zd >> az;

                TString str;
                str.ReadLine(fin);
                if (!fin)
                    break;

                str.ToLower();

                if (str.Strip(TString::kBoth)==preps)
                {
                    ZdAz dest(zd, az);
                    SetPosition(dest*TMath::DegToRad());
                    return 0x7979;
                }
                cout << "ERROR - Requested preposition not found in file..." << endl;
            }
        }
        //cout << "WM_Track: done. (return 0x8888)" << endl;
        return 0x7878;
/*
    case WM_TESTSE:
        //cout << "WM_TestSe: start." << endl;
        fBackground = mp ? kBgdSeTest : kBgdNone;
        //cout << "WM_TestSe: done. (return 0x1e51)" << endl;
        return 0x1e51;

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

    case WM_DISPLAY:
        //cout << "WM_Display: start." << endl;
        fTriggerDisplay = kTRUE;
        //cout << "WM_Disply: done. (return 0xd1e1)" << endl;
        return 0xd1e1;
 */
    case WM_TRACK:
    case WM_GRB:
        //cout << "WM_Track/GRB: START" << endl;
        {
            RaDec dest = ((RaDec*)mp)[0];
            if (fStarguider)
                fStarguider->SetPointingPosition(((RaDec*)mp)[1]);
            if (!CheckNetwork())
                return 0xebb0;

            if (msg==WM_TRACK)
                TrackPosition(dest*TMath::DegToRad());
            else
                TrackPositionGRB(dest*TMath::DegToRad());
        }
        //cout << "WM_Track/GRB: done. (return 0x8888)" << endl;
        return 0x8888;

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

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

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

    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*TMath::DegToRad()); // [rad]

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

            if (fMac1 && fMac2)
                a1 = AlignTrackingPos(a1);

            a1 = fBending(a1);
            CheckRange(a1);
            a1 *= TMath::RadToDeg();

            const ZdAz a2 = a1/360;

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

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

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

        return 0x1010;

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

void MCosy::ReadConfig(MEnv &env)
{
    gLog << inf2 << "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);
    gLog << "done." << endl;

    gLog << all << flush;

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

    fMin = fBending.AddOffsets(fMin*TMath::DegToRad());
    fMax = fBending.AddOffsets(fMax*TMath::DegToRad());

    gLog << " * Min': " << fMin.Zd()*TMath::RadToDeg() << "deg  " << fMin.Az()*TMath::RadToDeg() << "deg" << endl;
    gLog << " * Max': " << fMax.Zd()*TMath::RadToDeg() << "deg  " << fMax.Az()*TMath::RadToDeg() << "deg" << endl;
}

ZdAz MCosy::GetPointingPos() const
{
    if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
        return ZdAz(0, 0);

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

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

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

    //
    // Update Gui, foremer MTGui.
    //
    if (fMac1)
        fMac1->DisplayVal();
    if (fMac2)
        fMac2->DisplayVal();

    /*
    Byte_t avail = 0;

    avail |= (fMac1 && !fMac1->IsZombieNode()) ? 0x01 : 0;
    avail |= (fMac2 && !fMac2->IsZombieNode()) ? 0x02 : 0;
//    avail |= (!(fStatus&MDriveCom::kError) && 1 ? 0x40 : 0;
  */
    Bool_t armed = kTRUE;

    armed &= fMac1 && fMac1->IsArmed();
    armed &= fMac2 && fMac2->IsArmed();

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

    const TString stataz = fMac1 ? fMac1->GetStatusDKC() : "";
    const TString statzd = fMac2 ? fMac2->GetStatusDKC() : "";

    const UInt_t stat1 = fMac1 ? fMac1->GetStatusPdo3() : 0;
    const UInt_t stat2 = fMac2 ? fMac2->GetStatusPdo3() : 0;

    ZdAz bendist = GetPointingPos();

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

    static MTimeout tout(0);
    if (tout.HasTimedOut())
    {
        tout.Start(999);
        fCom->SendReport(fStatus, fRaDec, fZdAzSoll, bendist, fTrackingError, armed);
    }

    fWin->UpdateWeather(*fCom);
    fWin->Update(bendist, fTrackingError, /*fVelocity, fOffset,*/
                 fRaDec, fZdAzSoll, fStatus, (stat1<<8)|stat2, HasConnection(), armed, statzd, stataz);

    gLog.UpdateGui();

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

    return kTRUE;
}

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

    CheckForError();

    ReadConfig(env);

    gLog << inf << "- 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()
{
    gLog << inf << "- Stopping GUI update." << endl;
    fUpdateGui->TurnOff();
    gLog << inf << "- GUI Update stopped." << endl;

    gLog << inf << "- Stopping CAN network." << endl;
    Network::Stop();
    gLog << inf << "- CAN network stopped." << endl;

    gLog << inf << "- Stopping message queue." << endl;
    CancelThread();
    gLog << inf << "- Message queue stopped." << endl;
}

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

    fMac1=new Dkc(id1, "DKC/Az");
    fMac2=new Dkc(id2, "DKC/Zd");

    fMac1->SetReport(fOutRep);
    fMac2->SetReport(fOutRep);

    gLog << inf << "- Connecting devices to network." << endl;

    //
    // Connect the devices to the network
    //
    SetNode(fMac1);
    SetNode(fMac2);

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

    gLog << all << "- Starting GUI." << endl;
    fWin=new MGCosy(fObservatory, this);
}

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

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

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

TString MCosy::GetFileName(const char *path, const char *name, const char *ext)
{
    // FIXME: Timeout missing

    while (1)
    {
        MTime time(-1);

        // This is the full qualified date which is part of the name
        const TString clock = time.GetStringFmt("%Y%m%d_%H%M%S");

        // This gives the night in which the date belongs to
        time.SetMjd(TMath::Nint(time.GetMjd()));

        const TString night = time.GetStringFmt("%Y_%m_%d");

        const TString dir   = Form("%s/%s", path, night.Data());
        const TString fname = Form("%s_%s.%s", name, clock.Data(), ext);

        const TString full  = Form("%s/%s", dir.Data(), fname.Data());

        gSystem->mkdir(dir, kTRUE);

        if (gSystem->AccessPathName(full, kFileExists))
            return full;

        break;// !!!!!!!!!!!!!!!!!!!!!!!

        usleep(1000);
    }
    return "";
}

MCosy::MCosy(MEnv &env, MDriveCom *com, const char *pointing)
: Network(), fObservatory(MObservatory::kMagic1), fStarguider(NULL),
fMac1(0), fMac2(0), fStatus(MDriveCom::kStopped), fOutTp(0), fOutRep(0)
{
    const Int_t id1 = env.GetValue("Az_Id", 1);
    const Int_t id2 = env.GetValue("Zd_Id", 3);

    TString name = GetFileName("rep", "cosy", "rep");
    gLog << inf << "Open Repfile: " << name << endl;
    fOutRep = new MLog(name, kTRUE);
    *fOutRep << all << "[Drive Report File]" << endl;
    *fOutRep << "Version <cvs>" << endl;
    *fOutRep << "Date " << MTime(-1) << endl;
    *fOutRep << "[Reports]" << endl;

/*
    gLog << "- Program in ";
    switch (mode)
    {
    case 0:
    gLog << "<<Standard mode>>" << endl;*/
        gLog << all << "Reading pointing model from " << pointing << "..." << endl;
        if (fBending.Load(pointing))
            gLog << all << "Reading pointing model from " << pointing << " successfull." << endl;
        else
            gLog << err << "ERROR - Reading pointing model from " << pointing << endl;
        Constructor(id1, id2);/*
        break;
    case 1:
        gLog << "<<SE mode>>" << endl;
        fBending.Load("bending.txt");
        ConstructorSE(id4, id5, id6);
        break;
    default:
        gLog << "<<Demo mode>>" << endl;
        ConstructorDemo();
    }
*/

    gLog.SetOutputGui(fWin->GetLog(), kTRUE);

    fMac2->SetDisplay(fWin->GetLabel2());
    fMac1->SetDisplay(fWin->GetLabel1());

    fCom = com;//new MDriveCom(this, addr, tx, rx, fOutRep);
    fCom->SetOutRep(fOutRep);
    //    fCom->Start();
}

void MCosy::TerminateApp()
{
    gLog << inf2 << "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);
*/

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

    gApplication->Terminate(0);
}

MCosy::~MCosy()
{
    if(fCom)
    {
        fCom->SetMsgQueue(NULL);
        fCom->SetOutRep(NULL);
    }

    gLog << inf2 << "Deleting GUI timer." << endl;
    // FIXME: Wait until last Update was finished!!!
    delete fUpdateGui;

    //fMutexGui.Lock();

    // Now the files can safely be closed
    gLog << inf2 << "Closing output files." << endl;
    if (fOutTp)
    {
        *fOutTp << "END" << endl;
        delete fOutTp;
    }

    delete fOutRep;

    //gLog << inf2 << "Deleting CC communication." << endl;
    //delete fCom;

    gLog << inf2 << "Deleting Nodes." << endl;
    fMac1->SetReport(0);
    fMac2->SetReport(0);

    delete fMac1;
    delete fMac2;

    gLog << inf2 << "Deleting MGCosy." << endl;

    gLog.DisableOutputDevice(MLog::eGui);

    delete fWin;

    gLog << inf2 << "MGCosy destructed." << endl;
}