#include "macs.h"

#include <iostream.h>
#include <sys/time.h>   // timeval->tv_sec

#include "timer.h"
#include "network.h"
#include "MLogManip.h"

ClassImp(Macs);

/*
 ---------------------------
 For test purposes
 ---------------------------

class MyTimer : public TTimer
{
public:
    MyTimer(TObject *obj, Long_t ms, Bool_t mode) : TTimer(obj, ms, mode) {}
    Bool_t Notify()
    {
        cout << "Notify" << endl;
        TTimer::Notify();
        return kTRUE;
    }
};
*/

Macs::Macs(const BYTE_t nodeid, const char *name, MLog &out)
    : NodeDrv(nodeid, name, out), fMacId(2*nodeid+1),
    fPos(0), fPosTime(0.0), fPdoPos(0), fPdoTime(0.0),
    fPosActive(0), fRpmActive(0)
{
    fTimeout = new TTimer(this, 100, kFALSE); // 100ms, asynchronous
}

Macs::~Macs()
{
    fTimerOn = kFALSE;
    delete fTimeout;
}

void Macs::HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, timeval_t *tv)
{
    switch (idx)
    {
    case 0x100a:
        lout << "- " << GetNodeName() << ": Using Software Version V" << dec << (int)(val>>16) << "." << (int)(val&0xff) << endl;
        return;

    case 0x2002:
        cout << GetNodeName() << ": Actual velocity: " << dec << val << endl;
        fVel = val;
        return;

    case 0x4000:
        switch (subidx)
        {
        case 1:
            cout << GetNodeName() << ": Timeout timer is " << (val?"en":"dis") << "abled." << endl;
            return;
        case 2:
            cout << GetNodeName() << ": Actual timeout time: " << dec << val << "ms" << endl;
            return;
        }
        break;

    case 0x6004:
        if (subidx)
            return;

//        lout << "Actual Position: " << dec << (signed long)val << endl;
        fPos = (LWORDS_t)val;
        fPosTime.SetTimer(tv);
        return;
/*
    case 0x2001:
        cout << "Axe Status: 0x" << hex << val << endl;
        cout << " - Motor "             << (val&0x01?"standing":"moving") << endl;
        cout << " - Positioning "       << (val&0x02?"active":"inactive") << endl;
        cout << " - Rotary mode "       << (val&0x04?"active":"inactive") << endl;
        cout << " - Attitude control: " << (val&0x40?"off":"on") << endl;
        cout << " - Axe resetted: "     << (val&0x80?"yes":"no") << endl;
        fPosActive = val&0x02;
        fRpmActive = val&0x04;
        return;
    case 0x2003:
        if (!subidx)
        {
            cout << "Input State: ";
            for (int i=0; i<8; i++)
                cout << (int)(val&(1<<i)?1:0);
            cout <<endl;
            return;
        }
        cout << "Input No." << subidx << (val?"hi":"lo") << endl;
        return;
    case 0x2004:
        cout << "Status Value of Axis: 0x" << hex << val << endl;
        cout << " - Attitude control: "        << (val&0x800000?"off":"on") << endl;
        cout << " - Movement done: "           << (val&0x040000?"yes":"no") << endl;
        cout << " - Number of Control Units: " << (int)((val>>8)&0xff) << endl;
        cout << " - Attitude control: "        << (val&0x04?"off":"on") << endl;
        cout << " - Startswitch active: "      << (val&0x20?"yes":"no") << endl;
        cout << " - Referenceswitch active: "  << (val&0x40?"yes":"no") << endl;
        cout << " - Endswitch active: "        << (val&0x80?"yes":"no") << endl;
        return;
*/

    case 0x6002:
        lout << "- Velocity resolution " << GetNodeName() << ": " << dec << val << " ticks/min" << endl;
        fVelRes = val;
        return;
    }
    cout << "Macs: SDO, idx=0x"<< hex << idx << "/" << (int)subidx;
    cout << ", val=0x"<<val<<endl;
}

void Macs::HandleSDOOK(WORD_t idx, BYTE_t subidx)
{
    switch (idx)
    {
    case 0x2002:
        switch (subidx)
        {
        case 0:
            lout << ddev(MLog::eGui);
            lout << "- Velocity set (" << GetNodeName() << ")" << endl;
            lout << edev(MLog::eGui);
            return;
        }
        break;
    case 0x2003:
        switch (subidx)
        {
        case 0:
            lout << ddev(MLog::eGui);
            lout << "- Acceleration set (" << GetNodeName() << ")" << endl;
            lout << edev(MLog::eGui);
            return;
        case 1:
            lout << ddev(MLog::eGui);
            lout << "- Deceleration set (" << GetNodeName() << ")" << endl;
            lout << edev(MLog::eGui);
            return;
        }
        break;
    case 0x3006:
        switch (subidx)
        {
        case 0:
            lout << ddev(MLog::eGui);
            lout << "- RPM mode switched (" << GetNodeName() << ")" << endl;
            lout << edev(MLog::eGui);
            return;

        case 1:
            /*
             lout << ddev(MLog::eGui);
             lout << "- Velocity set (" << GetNodeName() << ")" << endl;
             lout << edev(MLog::eGui);
             */
            return;
        }
        break;
    case 0x4000:
        if (subidx==0 && fTimerOn)
        {
            ResetTimeout();
            return;
        }
        break;
    case 0x6004:
        switch (subidx)
        {
        case 0:
            lout << ddev(MLog::eGui);
            lout << "- Relative positioning started (" << GetNodeName() << ")" << endl;
            lout << edev(MLog::eGui);
            return;

        case 1:
            lout << ddev(MLog::eGui);
            lout << "- Absolute positioning started (" << GetNodeName() << ")" << endl;
            lout << edev(MLog::eGui);
            return;
        }
        break;


    }
    NodeDrv::HandleSDOOK(idx, subidx);
}


void Macs::ReqVelRes()
{
    lout << "- Requesting velocity resolution (velres, 0x6002) of " << GetNodeName() << endl;
    RequestSDO(0x6002);
    WaitForSdo(0x6002);
}

void Macs::SetPDO1On(BYTE_t flag)
{
    lout << "- " << (flag?"Enable":"Disable") << " PDO1 of " << GetNodeName() << endl;
    SendSDO(0x1800, 1, (LWORD_t)(flag?0:1)<<31); 
    WaitForSdo(0x1800, 1);           
}

void Macs::InitDevice(Network *net)
{
    NodeDrv::InitDevice(net);

//    SendSDO(0x4003, (LWORD_t)('E'<<24 | 'X'<<16 | 'I'<<8 'T'));
//    WaitForSdo(0x4003, 0);

/*
    lout << "- Requesting SDO 0x2002 (vel) of " << (int)GetId() << endl;
    RequestSDO(0x2002);
    WaitForSdo(0x2002);

    lout << "- Requesting SDO 0x2003 of " << (int)GetId() << endl;
    RequestSDO(0x2003);
    WaitForSdo(0x2003);

    lout << "- Requesting SDO 0x2004 of " << (int)GetId() << endl;
    RequestSDO(0x2004);
    WaitForSdo(0x2004);
    */
    EnableTimeout(kFALSE);

    lout << "- Requesting Mac Software Version of " << GetNodeName() << endl;
    RequestSDO(0x100a);
    WaitForSdo(0x100a);

    SetRpmMode(FALSE);

    ReqVelRes(); // Init fVelRes

    lout << "- Motor on of " << GetNodeName() << endl;
    SendSDO(0x3000, string('o', 'n'));
    WaitForSdo(0x3000);


//    SetHome(250000);

//    lout << "- Requesting SDO 0x2001 of " << (int)GetId() << endl;
//    RequestSDO(0x2001);
//    WaitForSdo(0x2001);

    SetPDO1On(FALSE); // this is a workaround for the Macs
    SetPDO1On(TRUE);

    SetNoWait(TRUE);
}

void Macs::StopMotor()
{
    //
    // Stop the motor and switch off the position control unit
    //
    SendSDO(0x3000, string('s','t','o','p'));
    WaitForSdo(0x3000);
}

void Macs::StopDevice()
{
    EnableTimeout(kFALSE);

    SetNoWait(FALSE);

    //
    // FIXME: This isn't called if the initialization isn't done completely!
    //

    SetRpmMode(FALSE);

    SetPDO1On(FALSE);

    lout << "- Motor off of " << GetNodeName() << endl;
    SendSDO(0x3000, string('o', 'f', 'f'));
    WaitForSdo(0x3000);

    /*
     lout << "- Stopping Program of " << (int)GetId() << endl;
     SendSDO(0x4000, (LWORD_t)0xaffe);
     WaitForSdo();
    */
}

void Macs::ReqPos()
{
    lout << "- Requesting Position of " << GetNodeName() << endl;
    RequestSDO(0x6004);
    WaitForSdo(0x6004);
}

void Macs::ReqVel()
{
    lout << "- Requesting Velocity of " << GetNodeName() << endl;
    RequestSDO(0x2002);
    WaitForSdo(0x2002);
}

void Macs::SetHome(LWORDS_t pos, WORD_t maxtime)
{
    lout << "- Driving " << GetNodeName() << " to home position, Offset=" << dec << pos << endl;
    SendSDO(0x6003, 2, (LWORD_t)pos);       // home
    WaitForSdo(0x6003, 2);

    // home also defines the zero point of the system
    // maximum time allowd for home drive: 25.000ms
    SendSDO(0x3001, string('h','o','m','e'));       // home
    WaitForSdo(0x3001, 0, maxtime*1000);
    lout << "- Home position of " << GetNodeName() << " reached. " << endl;

    SendSDO(0x6003, 0, string('s','e','t'));       // home
    WaitForSdo(0x6003, 0);
}

void Macs::SetVelocity(LWORD_t vel)
{
    SendSDO(0x2002, vel);     // velocity
    WaitForSdo(0x2002);
}

void Macs::SetAcceleration(LWORD_t acc)
{
    SendSDO(0x2003, 0, acc);  // acceleration
    WaitForSdo(0x2003, 0);
}

void Macs::SetDeceleration(LWORD_t dec)
{
    SendSDO(0x2003, 1, dec);  // acceleration
    WaitForSdo(0x2003, 1);
}

void Macs::SetRpmMode(BYTE_t mode)
{
    //
    // SetRpmMode(FALSE) stop the motor, but lets the position control unit on
    //
    SendSDO(0x3006, 0, mode ? string('s','t','r','t') : string('s','t','o','p'));
    WaitForSdo(0x3006, 0);
}

void Macs::SetRpmVelocity(LWORDS_t cvel)
{
    SendSDO(0x3006, 1, (LWORD_t)cvel);
    WaitForSdo(0x3006, 1);
}

void Macs::StartRelPos(LWORDS_t pos)
{
    SendSDO(0x6004, 1, (LWORD_t)pos);
}

void Macs::StartAbsPos(LWORDS_t pos)
{
    SendSDO(0x6004, 0, (LWORD_t)pos);
}

void Macs::SetNoWait(BYTE_t flag)
{
    lout << "- Setting NOWAIT " << (flag?"ON ":"OFF ") << GetNodeName() << endl;
    SendSDO(0x3008, flag ? string('o', 'n') : string('o', 'f', 'f'));
    WaitForSdo(0x3008);
}

void Macs::StartVelSync()
{
    //
    // The syncronization mode is disabled by a 'MOTOR STOP'
    // or by a positioning command (POSA, ...)
    //
    lout << "- Setting Vel Sync Mode of " << GetNodeName() << endl;
    SendSDO(0x3007, 0, string('s', 'y', 'n', 'c'));
    WaitForSdo(0x3007, 0);
}

void Macs::StartPosSync()
{
    //
    // The syncronization mode is disabled by a 'MOTOR STOP'
    // or by a positioning command (POSA, ...)
    //
    lout << "- Setting Pos Sync Mode of " << GetNodeName() << endl;
    SendSDO(0x3007, 1, string('s', 'y', 'n', 'c'));
    WaitForSdo(0x3007, 1);
}
/*
void Macs::ReqAxEnd()
{
    RequestSDO(0x2001);
    WaitForSdo(0x2001);
}
*/
void Macs::SendMsg(BYTE_t data[6])
{
    GetNetwork()->SendCanFrame(fMacId, 0, 0, data[0], data[1], data[2], data[3], data[4], data[5]);
}

void Macs::SendMsg(BYTE_t d0=0, BYTE_t d1=0, BYTE_t d2=0,
                   BYTE_t d3=0, BYTE_t d4=0, BYTE_t d5=0)
{
    GetNetwork()->SendCanFrame(fMacId, 0, 0, d0, d1, d2, d3, d4, d5);
}

void Macs::HandlePDO1(BYTE_t *data, timeval_t *tv)
{
    fPdoPos    = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7];

    fPosActive = data[3]&0x02;
    fRpmActive = data[3]&0x04;

    fPdoTime.SetTimer(tv);
}

void Macs::HandlePDO2(BYTE_t *data, timeval_t *tv)
{
    LWORDS_t errnum = (data[0]<<24) | (data[1]<<16) | (data[2]<<8) | data[3];
    LWORDS_t errinf = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7];

    //
    // errnum==0 gives a sudden information that something happened. Now the
    // microcontroller is running inside its interrup procedure which
    // stopped the normal program. The interrupt procedure should try to clear
    // the error state of the hardware. This should never create a new error!
    //
    if (!errnum)
    {
        lout << "Mac " << GetNodeName() << " reports Error occursion." << endl;
        SetError(-1);
        return;
    }

    //
    // Now the error is handled by the hardware now it is the software part
    // to react on it. The Error flag now is set to the correct value.
    //
    if (GetError()>0)
        lout << GetNodeName() << " WARNING! Error #" << GetError() << " unhandled by software." << endl;

    SetError(errnum);

    lout << GetNodeName() << " reports: ";
    switch (errnum)
    {
    case 6:
        lout << "Home position not the first positioning command." << endl;
        return;

    case 8:
        lout << "Control deviation overflow." << endl;
        return;

    case 9:
        lout << "Zero index not found." << endl;
        return;

    case 11:
    case 25:
        switch (errinf)
        {
        case -1:
            lout << "Negative";
            break;
        case 1:
            lout << "Positive";
            break;
        default:
            lout << "-unknown-";
        }
        switch (errnum)
        {
        case 11:
            lout << " software endswitch activated." << endl;
            break;
        case 25:
            lout << " hardware endswitch activated." << endl;
            break;
        }
        return;

    case 84:
        lout << "Too many (>12) ON TIME calls." << endl;
        return;

    case 100:
        lout << "Connection timed out." << endl;
        return;

    default:
        lout << "Error Nr. " << errnum << ", " << errinf << endl;
    }
}

void Macs::HandleError()
{
    //
    // If there is no error we must not handle anything
    //
    if (!HasError())
        return;

    //
    // If the program got into the: HandleError state before the hardware
    // has finished handeling the error we have to wait for the hardware
    // handeling the error
    //
    // FIXME: Timeout???
    //
    while (GetError()<0)
        usleep(1);

    //
    // After this software and hardware should be in a state so that
    // we can go on working 'as usual' Eg. Initialize a Display Update
    //
    cout << GetNodeName() << " Handling Error #" << GetError() << endl;
    switch (GetError())
    {
    case   6: // home
    case   8: // control dev
    case   9: // zero idx
    case  84: // ON TIME
        // Stop program?
        return;

    case 11:  // software endswitch
    case 25:  // hardware endswitch
    case 100: // timeout (movement has been stopped, so we can go on)
        DelError();
        return;

    case 101:
        lout << "Warning: " << GetNodeName() << " didn't respond in timeout window - try again." << endl;
        DelError();
        return;
    }
}

double Macs::GetTime()
{
    return fPosTime.Now();
}

double Macs::GetMjd()
{
    return fPosTime.CalcMjd();
}

double Macs::GetPdoTime()
{
    return fPdoTime.Now();
}

double Macs::GetPdoMjd()
{
    return fPdoTime.CalcMjd();
}

/*   0x2000 0 rw Maximum positioning error     */
/*          1 rw Negative Software Endswitch   */
/*          2 rw Positive Software Endswitch   */
void Macs::SetNegEndswitch(LWORDS_t val)
{
    SendSDO(0x2000, 1, (LWORD_t)val);
    WaitForSdo(0x2000, 1);
}

void Macs::SetPosEndswitch(LWORDS_t val)
{
    SendSDO(0x2000, 2, (LWORD_t)val);
    WaitForSdo(0x2000, 2);
}

void Macs::EnableEndswitches(bool neg, bool pos)
{
    SendSDO(0x2000, 3, (LWORD_t)(neg|(pos<<1)));
    WaitForSdo(0x2000, 3);
}

void Macs::ReqTimeoutTime()
{
    RequestSDO(0x4000, 2);
    WaitForSdo(0x4000, 2);
}

void Macs::EnableTimeout(bool enable, LWORDS_t ms)
{
    if (!enable)
    {
        SendSDO(0x4000, 1, string('o', 'f', 'f'));
        WaitForSdo(0x4000, 1);

        fTimeout->Stop(); //kTRUE);

        fTimerOn = kFALSE;
    }
    else
    {
        if (ms>0)
            SetTimeoutTime(ms);

        ResetTimeout();

        fTimerOn = kTRUE;
        fTimeout->Start(fGuardTime/*/3*2*/, kTRUE); //kFALSE); //TRUE);
        //
        // Start with kFALSE would be a continous timer, but this
        // timer seems to stop it's activity at some stage without
        // any reason
        //

//        SendSDO(0x4000, 1, string('o', 'n'));
//        WaitForSdo(0x4000, 1);
    }
    lout << "- Timeout timer of " << GetNodeName() << " turned "
        << (enable?"on.":"off.") << endl;
}

Bool_t Macs::HandleTimer(TTimer *t)
{
    /*
     Fons:
     -----

     timers never trigger at the same time or when in a TTimer::Notify.
     Little explanation:

     - there are two types of timers synchronous and a-synchronous.
     - synchronous timers are only handled via the ROOT eventloop
       (see TUnixSystem::DispatchOneEvent()). If there are no mouse/keyboard
       events then the synchronous timer queue is checked. So if the processing
       of a mouse/keyboard event takes a long time synchronous timers are not
       called for a while. To prevent this from happening one can call in long
       procedures gSystem->ProcessEvents(). The system schedules only the
       next timer in the queue when the current one's Notify() has finished.
     - a-synchronous timers are triggered via SIGALARM, i.e. the program is
       interupted and execution jumps to the Notify() function. When the
       notify is finished the next a-sync timer is scheduled and the system
       resumes from the place where it was initially interrupted. One of the
       things to remember when using a-sync timers is don't make any graphics
       calls in them. X11 is not re-entrant and it might be that the SIGALARM
       signal interrupted the system while being in X11. A-sync timers are best
       used to set flags that you can test at a convenient and controlled
       time.
       */

    //
    //  FIXME! Use NMT!
    //
    // --- lout << ddev(MLog::eGui);
    // --- lout << "Send 0x4000: " << GetNodeName() << endl;

    SendSDO(0x4000, 0, (LWORD_t)0, false);

    // --- lout << "Done 0x4000: " << GetNodeName() << endl;

    Timer time;

    // --- lout << "dT " << GetNodeName() << ": " <<  dec <<(int)((fTimeoutTime-time.Now())*1000) << endl;
    // --- lout << edev(MLog::eGui);

    if (time.Now() > fTimeoutTime+0.5)
    {
        lout << ddev(MLog::eGui);
        lout << "Warning: " << GetNodeName() << " didn't respond in timeout window." << endl;
        lout << edev(MLog::eGui);
        SetError(101);
    }

    //WaitForSdo(0x4000, 0, kDontWait);
    //
    // Would not be necessary if I would Start the timer with
    // kFALSE. This would be a continous timer, but this
    // timer seems to stop it's activity at some stage without
    // any reason
    //
    if (fTimerOn)
        fTimeout->Start(fGuardTime/*/3*2*/, kTRUE);

    return kTRUE;
}

void Macs::ResetTimeout()
{
    Timer time;

    // --- lout << ddev(MLog::eGui);
    // --- lout << "Reset " << GetNodeName() << ": " << dec << (int)((fTimeoutTime-time.Now())*1000) << endl;
    // --- lout << edev(MLog::eGui);

    fTimeoutTime = time.Now() + 3.*fGuardTime/1000.;
}

void Macs::SetTimeoutTime(LWORD_t ms)
{
    // FIXME: Is '/2' the best choose?
    fGuardTime = ms/3;      // how often do we send/request the handshake

    SendSDO(0x4000, 2, ms); // How often do we check for the handshake
    WaitForSdo(0x4000, 2);
}