#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), fStatusPdo3(0xff)
{
//    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;
        fSoftVersion = val;
        return;

    case 0x100b:
        // Do not display, this is used for CheckConnection
        // lout << "Node ID: " << dec << val << endl;
        return;

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

    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 << "- " << GetNodeName() << ": Velocity resolution = " << dec << val << " ticks/min" << endl;
        fVelRes = val;
        return;

    case 0x6501:
        lout << "- " << GetNodeName() << ": Encoder resolution = " << dec << val << " ticks/min" << endl;
        fRes = 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, timeval_t *tv)
{
    switch (idx)
    {
    case 0x2002:
        switch (subidx)
        {
        case 0:
            //lout << ddev(MLog::eGui);
            lout << "- " << GetNodeName() << ": Velocity set." << endl;
            //lout << edev(MLog::eGui);
            return;
        }
        break;

    case 0x2003:
        switch (subidx)
        {
        case 0:
            //lout << ddev(MLog::eGui);
            lout << "- " << GetNodeName() << ": Acceleration set." << endl;
            //lout << edev(MLog::eGui);
            return;
        case 1:
            //lout << ddev(MLog::eGui);
            lout << "- " << GetNodeName() << ": Decceleration set." << endl;
            //lout << edev(MLog::eGui);
            return;
        }
        break;

    case 0x3006:
        switch (subidx)
        {
        case 0:
            //lout << ddev(MLog::eGui);
            lout << "- " << GetNodeName() << ": RPM mode switched." << 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:
        HandleNodeguard(tv);
        return;

    case 0x6004:
        switch (subidx)
        {
        case 0:
            //lout << ddev(MLog::eGui);
            lout << "- " << GetNodeName() << ": Absolute positioning started." << endl;
            //lout << edev(MLog::eGui);
            return;

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


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


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

void Macs::ReqRes()
{
    lout << "- " << GetNodeName() << ": Requesting encoder resolution (res, 0x6501)." << endl;
    RequestSDO(0x6501);
    WaitForSdo(0x6501);
}

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

void Macs::CheckConnection()
{
    RequestSDO(0x100b);
    WaitForSdo(0x100b);
}


void Macs::Init()
{
    if (HasError())
    {
        lout << "Macs::Init: " << GetNodeName() << " has error --> ZOMBIE!" << endl;
        SetZombie();
    }

    StopHostGuarding();
    StopGuarding();

    usleep(2000*GetGuardTime());

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

    if (IsZombieNode())
    {
        lout << GetNodeName() << " - InitDevice failed!" << endl;
        return;
    }

    // FIXME! Not statically linked!
    if (fSoftVersion<0x00000042) // 00.66
    {
        lout << GetNodeName() << " - Software Version too old!" << endl;
        SetZombie();
        return;
    }

    SetRpmMode(FALSE);

    ReqRes();    // Init fRes
    ReqVelRes(); // Init fVelRes

    lout << "- " << GetNodeName() << ": Motor on." << 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);

    //    StartGuarding(500, 2);
    //    StartHostGuarding();
}

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 << "- " << GetNodeName() << ": Motor off." << 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 << "- " << GetNodeName() << ": Requesting Position." << endl;
    RequestSDO(0x6004);
    WaitForSdo(0x6004);
}

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

void Macs::SetHome(LWORDS_t pos, WORD_t maxtime)
{
    StopHostGuarding();
    StopGuarding();

    lout << "- " << GetNodeName() << ": Driving 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 << "- " << GetNodeName() << ": Home position reached. " << endl;

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

    StartGuarding();
    StartHostGuarding();
}

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

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 << "- " << GetNodeName() << ": Setting NOWAIT " << (flag?"ON":"OFF") << "." << 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 << "- " << GetNodeName() << ": Starting RPM Sync Mode." << 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 << "- " << GetNodeName() << ": Starting Position Sync Mode." << 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];

              // data[3]&0x01; // motor not moving
    fPosActive = data[3]&kPosActive; // positioning active
    fRpmActive = data[3]&kRpmActive; // RPM mode switched on
              // data[3]&0x08; //  - unused -
              // data[3]&0x10; //  - unused -
              // data[3]&0x20; //  - unused -
    fInControl = data[3]&0x40; // motor uncontrolled
              // data[3]&0x80; // axis resetted (after errclr, motor stop, motor on)

    fStatus = data[3];

    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 interrupt 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 << "- " << GetNodeName() << ": reports Error occursion." << endl;
        lout << "Macs::HandlePDO2: " << GetNodeName() << " --> ZOMBIE!" << endl;
        SetZombie();
        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 (not cleared) by software." << endl;

        //
        // If the error is unhadled and/or not cleared, don't try it again.
        //
        if (GetError()==errnum)
            return;
    }

    SetError(errnum);

    lout << GetNodeName() << " reports: ";
    switch (errnum)
    {
    case 6:
        //
        // Report the error to the user. All possible movements should have
        // been stopped anyhow. Now delete the error to prevent the system
        // from reporting this error a thousands of times.
        //
        lout << "Home position not the first positioning command." << endl;
        SetError(0);
        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;
        //EnableTimeout(false);
        return;

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

void Macs::HandlePDO3(BYTE_t *data, timeval_t *tv)
{
    // 3    5    7    9
    // 1100 1010 1110 1001
    if (fStatusPdo3 == data[3])
        return;

    lout << GetNodeName() << ": Status PDO3 = ";
    const Bool_t ready = data[3]&0x01;
    const Bool_t fuse  = data[3]&0x02;
    const Bool_t emcy  = data[3]&0x04;
    const Bool_t vltg  = data[3]&0x08;
    const Bool_t mode  = data[3]&0x10;
    const Bool_t rf    = data[3]&0x20;
    const Bool_t brake = data[3]&0x40;
    if (ready) lout << "DKC-Ready ";
    if (fuse)  lout << "FuseOk ";
    if (emcy)  lout << "EmcyOk ";
    if (vltg)  lout << "OvervoltOk ";
    if (mode)  lout << "SwitchToManualMode ";
    if (rf)    lout << "RF ";
    if (brake) lout << "BrakeOpen ";
    lout << endl;

    fStatusPdo3 = data[3];
}

// FIXME? Handling of fIsZombie?
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 #" << dec << GetError() << endl;
    switch (GetError())
    {
    case   6: // home
    case   8: // control dev
    case   9: // zero idx
    case  84: // ON TIME
        lout << "- " << GetNodeName() << ": Cannot handle error #" << GetError() << endl;
        return;

    case 11:  // software endswitch
    case 25:  // hardware endswitch
        lout << "- " << GetNodeName() << ": Cannot handle error 'Endswitch!'" << endl;
        return;

    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;
        */
    default:
        lout << "- " << GetNodeName() << ": Cannot handle error #" << GetError() << endl;
 
    }
}

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

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

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

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

/*   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::SendNodeguard()
{
    SendSDO(0x4000, 0, (LWORD_t)0, false);
}

// --------------------------------------------------------------------------
//
// This starts the host guarding. The host guarding is only available
// if the node guarding is running. The host guarding works with the
// guardtime and the lifetimefactor from the nodeguarding.
//
void Macs::StartHostGuarding()
{
    SendSDO(0x100c, 0, (LWORD_t)GetGuardTime());
    WaitForSdo(0x100c);

    SendSDO(0x100d, 0, (LWORD_t)GetLifeTimeFactor());
    WaitForSdo(0x100d);

    lout << "- " << GetNodeName() << ": Hostguarding started (" << dec;
    lout << GetLifeTimeFactor() << "*" << GetGuardTime() << "ms)" << endl;
}

// --------------------------------------------------------------------------
//
// Stop the host guarding.
//
void Macs::StopHostGuarding()
{
    SendSDO(0x100c, 0, (LWORD_t)0);
    WaitForSdo(0x100c);

    SendSDO(0x100d, 0, (LWORD_t)0);
    WaitForSdo(0x100d);

    lout << "- " << GetNodeName() << ": Hostguarding stopped." << endl;
}