#include "macs.h"

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

#include "network.h"

#include "MLogManip.h"

#include "MString.h"

ClassImp(Macs);

using namespace std;

/*
 ---------------------------
 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)
    : NodeDrv(nodeid, name), fMacId(2*nodeid+1),
    fPos(0), fPdoPos(0), fPosActive(0), fRpmActive(0),
    fStatusPdo3(0xff), fArmed(false)
{
//    fTimeout = new TTimer(this, 100); //, kFALSE); // 100ms, asynchronous
}

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

TString Macs::EvalStatusDKC(UInt_t stat) const
{
    switch (stat)
    {
    case 0: return "offline";
    case 0xa000:
    case 0xa001:
    case 0xa002:
    case 0xa003: return MString::Format("Communication phase %d", stat&0xf);
    case 0xa010: return "Drive HALT";
    case 0xa012: return "Control and power section ready for operation";
    case 0xa013: return "Ready for power on";
    case 0xa100: return "Drive in Torque mode";
    case 0xa101: return "Drive in Velocity mode";
    case 0xa102: return "Position control mode with encoder 1";
    case 0xa103: return "Position control mode with encoder 2";
    case 0xa104: return "Lagless position control mode with encoder 1";
    case 0xa105: return "Lagless position control mode with encoder 2";
    case 0xa106: return "Drive controlled interpolated positioning with encoder 1";
    case 0xa107: return "Drive controlled interpolated positioning with encoder 2";
    case 0xa108: return "Drive controlled interpolated absolute positioning lagless with encoder 1";
    case 0xa109: return "Drive controlled interpolated absolute positioning lagless with encoder 2";
    case 0xa146: return "Drive controlled interpolated relative positioning with encoder 1";
    case 0xa147: return "Drive controlled interpolated relative positioning with encoder 2";
    case 0xa148: return "Drive controlled interpolated relative positioning lagless with encoder 1";
    case 0xa149: return "Drive controlled interpolated relative positioning lagless with encoder 2";
    case 0xa150: return "Drive controlled positioning with encoder 1";
    case 0xa151: return "Drive controlled positioning with encoder 1, lagless";
    case 0xa208: return "Jog mode positive";
    case 0xa218: return "Jog mode negative";
    case 0xa400: return "Automatic drive check and adjustment";
    case 0xa401: return "Drive decelerating to standstill";
    case 0xa800: return "Unknown operation mode";
    case 0xc217: return "Motor encoder reading error";
    case 0xc218: return "Shaft encoder reading error";
    case 0xc220: return "Motor encoder initialization error";
    case 0xc221: return "Shaft encoder initialization error";
    case 0xc400: return "Switching to parameter mode";
    case 0xc500: return "Error reset";
    case 0xe225: return "Motor overload";
    case 0xe249: return "Positioning command velocity exceeds limit bipolar";
    case 0xe250: return "Drive overtemp warning";
    case 0xe251: return "Motor overtemp warning";
    case 0xe252: return "Bleeder overtemp warning";
    case 0xe257: return "Continous current limit active";
    case 0xe264: return "Target position out of numerical range";
    case 0xe834: return "Emergency-Stop";
    case 0xe842: return "Both end-switches activated";
    case 0xe843: return "Positive end-switch activated";
    case 0xe844: return "Negative end-switch activated";
    case 0xf218: return "Amplifier overtemp shutdown";
    case 0xf219: return "Motor overtemp shutdown";
    case 0xf220: return "Bleeder overload shutdown";
    case 0xf221: return "Motor temperature surveillance defective";
    case 0xf224: return "Maximum breaking time exceeded";
    case 0xf228: return "Excessive control deviation";
    case 0xf250: return "Overflow of target position preset memory";
    case 0xf269: return "Error during release of the motor holding brake";
    case 0xf276: return "Absolute encoder out of allowed window";
    case 0xf409: return "Bus error on Profibus interface";
    case 0xf434: return "Emergency-Stop";
    case 0xf629: return "Positive sw end-switch";
    case 0xf630: return "Negative sw end-switch";
    case 0xf634: return "Emergency-Stop";
    case 0xf643: return "Positive hardware end-switch activated";
    case 0xf644: return "Negative hardware end-switch activated";
    case 0xf870: return "24V DC error";
    case 0xf878: return "Velocity loop error";
    }
    return "unknown";
}

Bool_t Macs::EvalStatus(LWORD_t val) const
{
    const Int_t errnum = val&0xffff;
    const Int_t errinf = val>>16;

    if (errnum!=0xff)
        return errnum==0;

    const Int_t type = errinf&0xf000;

    gLog << all << MTime(-1) << ": " << GetNodeName() << " DKC ";

    switch (type)
    {
    case 0xf000: gLog << "ERROR"; break;
    case 0xe000: gLog << "WARNING"; break;
    case 0xa000: gLog << "Status"; break;
    case 0xc000: 
    case 0xd000: gLog << "Message"; break;
    default:     gLog << "Unknown"; break;
    }

    gLog << " (" << MString::Format("%X", errinf) << "): " << EvalStatusDKC(errinf);

    gLog << (type==0xf000 || type==0xe000 ? "!" : ".") << endl;

    return type!=0xf000;
}

void Macs::HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, const timeval_t &tv)
{
  // cout << "SdoRx: Idx=0x"<< hex << idx << "/" << (int)subidx;
  // cout << ", val=0x" << val << endl;
    switch (idx)
    {
    case 0x1000:
        if (subidx==1)
        {
            gLog << inf2 << "- " << GetNodeName() << ": Node is" << (val?" ":" not ") << "armed." << endl;
            fArmed = val==1;
        }
        return;

    case 0x1003:
        // FIXME, see Init
        if (subidx!=2)
            return;
        gLog << inf2 << "- " << GetNodeName() << ": Error[0]=" << hex << val << dec << endl;
        CheckErrorDKC(val);
        return;

    case 0x100a:
        gLog << inf2 << "- " << 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 0x100c:
        gLog << inf2 << "- " << GetNodeName() << ": Guard time:" << dec << val << endl;
        return;

    case 0x100d:
        gLog << inf2 << "- " << GetNodeName() << ": Life time factor:" << dec << val << endl;
        return;

    case 0x2002:
        gLog << inf2 << 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.Set(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:
        gLog << inf2 << "- " << GetNodeName() << ": Velocity resolution = " << dec << val << " (100%)" << endl;
        fVelRes = val;
        return;

    case 0x6501:
        gLog << inf2 << "- " << GetNodeName() << ": Encoder resolution = " << dec << val << " ticks" << endl;
        fRes = val;
        return;
    }

    NodeDrv::HandleSDO(idx, subidx, val, tv);

//    cout << "Macs: SDO, idx=0x"<< hex << idx << "/" << (int)subidx;
//    cout << ", val=0x"<<val<<endl;
}

void Macs::HandleSDOOK(WORD_t idx, BYTE_t subidx, LWORD_t data, const timeval_t &tv)
{
    //    cout << "Node #" << dec << (int)GetId() << ": Sdo=" << hex << idx  << "/" << (int)subidx << " set." << endl;

    // If a real drive operation is requested from the MACS and
    // the MACS is not correctly initialized the operation is
    // rejected. (This is expecially harmfull if the NoWait state
    // is set incorrectly)
    if (data)
        SetZombie();

    switch (idx)
    {
    case 0x1000:
        switch (subidx)
        {
        case 1:
            //lout << ddev(MLog::eGui);
            gLog << inf2 << "- " << GetNodeName() << ": State of node set." << endl;
            //lout << edev(MLog::eGui);
            return;
        }
        break;

    case 0x100c:
        switch (subidx)
        {
        case 0:
            //lout << ddev(MLog::eGui);
            gLog << inf2 << "- " << GetNodeName() << ": Guard time set." << endl;
            //lout << edev(MLog::eGui);
            return;
        }
        break;

    case 0x100d:
        switch (subidx)
        {
        case 0:
            //lout << ddev(MLog::eGui);
            gLog << inf2 << "- " << GetNodeName() << ": Life time factor set." << endl;
            //lout << edev(MLog::eGui);
            return;
        }
        break;

    case 0x1800:
        switch (subidx)
        {
        case 1:
            //lout << ddev(MLog::eGui);
            gLog << inf2 << "- " << GetNodeName() << ": Status of PDO1 set." << endl;
            //lout << edev(MLog::eGui);
            return;
        }
        break;

    case 0x2002:
        switch (subidx)
        {
        case 0:
            //lout << ddev(MLog::eGui);
            gLog << inf2 << "- " << GetNodeName() << ": Velocity set." << endl;
            //lout << edev(MLog::eGui);
            return;
        }
        break;

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

    case 0x3006:
        switch (subidx)
        {
        case 0:
            //lout << ddev(MLog::eGui);
            gLog << inf2 << "- " << 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 0x6000:
        //lout << ddev(MLog::eGui);
        gLog << inf2 << "- " << GetNodeName() << ": Rotation direction set." << endl;
        //lout << edev(MLog::eGui);
        return;

    case 0x6002:
        //lout << ddev(MLog::eGui);
        gLog << inf2 << "- " << GetNodeName() << ": Velocitz resolution set." << endl;
        //lout << edev(MLog::eGui);
        return;

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

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

    case 0x6004:
        switch (subidx)
        {
        case 0:
            //lout << ddev(MLog::eGui);
            gLog << inf2 << "- " << GetNodeName() << ": Absolute positioning started." << endl;
            fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO
            //lout << edev(MLog::eGui);
            return;

        case 1:
            //lout << ddev(MLog::eGui);
            gLog << inf2 << "- " << GetNodeName() << ": Relative positioning started." << endl;
            fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO
            //lout << edev(MLog::eGui);
            return;
        }
        break;


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

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

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

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

void Macs::StartNode()
{
    //
    // Switch node from pre-operational state to operational state
    // (This is not CANOpen compatible)
    // After this the MACS will react on real movement commands.
    //
    gLog << inf2 << "- " << GetNodeName() << ": Starting Node." << endl;
    SendSDO(0x1000, 1, (LWORD_t)1);
    WaitForSdo(0x1000, 1);
}

void Macs::Arm()
{
    StartNode();
}

void Macs::Disarm()
{
    gLog << inf2 << "- " << GetNodeName() << ": Stopping Node." << endl;
    SendSDO(0x1000, 1, (LWORD_t)0);
    WaitForSdo(0x1000, 1);
}

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

void Macs::Init()
{
    //
    // Request current error status (FIXME: is the first entry in the
    // error list)
    //
    gLog << inf2 << "- " << GetNodeName() << ": Requesting Error[0]." << endl;
    RequestSDO(0x1003, 2);
    WaitForSdo(0x1003, 2);
    if (HasError())
    {
        gLog << err << "Macs::Init: " << GetNodeName() << " has error --> ZOMBIE!" << endl;
        SetZombie();
    }
    if (IsZombieNode())
        return;

    StopHostGuarding();
    StopGuarding();

    gLog << inf2 << "- " << GetNodeName() << ": Requesting Mac Software Version." << endl;
    RequestSDO(0x100a);
    WaitForSdo(0x100a);
    // FIXME! Not statically linked!
    //if (fSoftVersion<0x00000044) // 00.68
    if (fSoftVersion<0x00000046) // 00.69
    {
        gLog << err << GetNodeName() << " - Software Version " << 0.01*fSoftVersion << " too old!" << endl;
        SetZombie();
        return;
    }

    SetRpmMode(FALSE);

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

    /* Should not be necessary anymore. This is done by the MACS itself.
     lout << "- " << GetNodeName() << ": Motor on." << endl;
     SendSDO(0x3000, string('o', 'n'));
     WaitForSdo(0x3000);
     */

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

    //This is now standard in the MACS
    //SetNoWait(TRUE);

    //StartGuarding(400, 1, kFALSE); // Using PDO1 @ 100ms
    //StartGuarding(250, 4);
    //StartHostGuarding();

    // REMOVE THIS AND LET CC START THE NODE
    // StartNode();

    gLog << inf2 << "- " << GetNodeName() << ": Checking armed status." << endl;
    RequestSDO(0x1000, 1);
    WaitForSdo(0x1000, 1);
}
/*
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);

    //No need to switch it off.
    //SetNoWait(FALSE);

    StopHostGuarding();
    StopGuarding();

    //
    // 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()
{
    gLog << inf2 << "- " << GetNodeName() << ": Requesting Position." << endl;
    RequestSDO(0x6004);
    WaitForSdo(0x6004);
}

void Macs::ReqVel()
{
    gLog << inf2 << "- " << 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)
{
    gLog << dbg << "- Setting velocity to: " << vel << endl;
    SendSDO(0x2002, vel);     // velocity
    WaitForSdo(0x2002, 0);
}

void Macs::SetAcceleration(LWORD_t acc)
{
    gLog << dbg << "- Setting acceleration to: " << acc << endl;
    SendSDO(0x2003, 0, acc);  // acceleration
    WaitForSdo(0x2003, 0);
}

void Macs::SetDeceleration(LWORD_t dec)
{
    gLog << dbg << "- Setting deceleration to: " << dec << endl;
    SendSDO(0x2003, 1, dec);
    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)
{
    if (!fArmed)
    {
        gLog << err << GetNodeName() << ": ERROR - Moving without being armed is not allowed." << endl;
        SetZombie();
        return;
    }

    gLog << dbg << GetNodeName() << ": Starting abolsute positioning to " << (LWORDS_t)pos << " ticks." << endl;
    SendSDO(0x6004, 1, (LWORD_t)pos);
    fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO
}

void Macs::StartAbsPos(LWORDS_t pos)
{
    if (!fArmed)
    {
        gLog << err << GetNodeName() << ": ERROR - Moving without being armed is not allowed." << endl;
        SetZombie();
        return;
    }

    gLog << dbg << GetNodeName() << ": Starting relative positioning by " << (LWORDS_t)pos << " ticks." << endl;
    SendSDO(0x6004, 0, (LWORD_t)pos);
    fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO
}

/*
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, ...)
    //
    gLog << inf2 << "- " << 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, ...)
    //
    gLog << inf2 << "- " << 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, BYTE_t d1, BYTE_t d2,
                   BYTE_t d3, BYTE_t d4, BYTE_t d5)
{
    GetNetwork()->SendCanFrame(fMacId, 0, 0, d0, d1, d2, d3, d4, d5);
}

void Macs::HandlePDO1(const BYTE_t *data, const timeval_t &tv)
{
    // FIXME!!!! Only 0x4000 should do this to be
    // CanOpen conform
    HandleNodeguard(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)

    fArmed     = data[2]&kIsArmed==kIsArmed;

    fStatus = data[3];

    fPdoTime.Set(tv);
}

void Macs::CheckErrorDKC(LWORD_t val)
{
    Bool_t rc = EvalStatus(val);
    SetError(rc ? 0 : val);
    if (!rc)
        SetZombie();
}

void Macs::HandlePDO2(const BYTE_t *data, const 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];

    // Check if the DKC changed its status message
    if (errnum==0xff && (errinf&0xf000)<=0xe000)
    {
        CheckErrorDKC(errnum, errinf);
        return;
    }

    // Check if MACS report error occursion.
    // 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)
    {
        gLog << err << "- " << GetNodeName() << ": reports Error occursion." << endl;
        gLog << "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)
    {
        gLog << warn << GetNodeName() << ": WARNING! Previous 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);

    gLog << err << GetNodeName() << " reports: ";
    switch (errnum)
    {
    case 3:
        gLog << "Axis does not existing." << endl;
        return;
    case 5:
        gLog << "Error not cleared (while trying to move axis)" << endl;
        return;
    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.
        //
        gLog << "Home position not the first positioning command." << endl;
        SetError(0);
        return;
    case 8:
        gLog << "Control deviation overflow." << endl;
        return;
    case 9:
        gLog << "Zero index not found." << endl;
        return;
    case 10:
        gLog << "Unknown command, syntax error." << endl;
        gLog << "Please recompile and reload program." << endl;
        return;
    case 11:
    case 25:
        switch (errinf)
        {
        case -1:
            gLog << "Negative";
            break;
        case 1:
            gLog << "Positive";
            break;
        default:
            gLog << "-unknown-";
        }
        switch (errnum)
        {
        case 11:
            gLog << " software";
            break;
        case 25:
            gLog << " hardware";
            break;
        }
        gLog << " endswitch activated." << endl;
        return;
    case 12:
        gLog << "Wrong parameter number used in SET command." << endl;
        return;
    case 14:
        gLog << " Too many LOOP calls." << endl;
        return;
    case 16:
        gLog << "Parameter in EEPROM broken (means: EEPROM broken, or saving not finished)" << endl;
        gLog << "Please use APOSS to 'Reset' the MACS and reload the parameters." << endl;
        return;
    case 17:
        gLog << "Program in EEPROM broken (means: EEPROM broken, or saving not finished)" << endl;
        gLog << "Please use APOSS to delete all Programs restore the programs." << endl;
        return;
    case 18:
        gLog << "Reset by CPU (reset called by Watch-dog cause of CPU halted)" << endl;
        gLog << "Possible reasons: short under-/overvoltage or shortcut." << endl;
        return;
    case 19:
        gLog << "User break (autostart program stopped by user)" << endl;
        return;
    case 51:
        gLog << "Too many (>=10) GOSUB calls." << endl;
        return;
    case 52:
        gLog << "Too many RETURN calls." << endl;
        return;
    case 62:
        gLog << "Error veryfiing EEPROM after access (Try again savaing parameters or program)" << endl;
        return;
    case 70:
        gLog << "Error in DIM call (call to DIM doesn't fit existing DIM call)" << endl;
        return;
    case 71:
        gLog << "Array out of bound." << endl;
        return;
    case 79:
        gLog << "Timeout waiting for index (WAITNDX)." << endl;
        return;
    case 84:
        gLog << "Too many (>12) ON TIME calls." << endl;
        return;
    case 87:
        gLog << "Out of memory for variables - Check APOSS predifined number of" << endl;
        gLog << "variables and try deleting the array by doing a 'Reset' from APOSS." << endl;
        return;
    case 89:
        gLog << "CAN I/O error (REOPEN=" << dec << errinf << " " << (errinf==0?"OK":"ERR") << ")" << endl;
        return;

    case 100:
        //lout << "Connection timed out." << endl;
        //EnableTimeout(false);
        return;

    case 0xff:
        EvalStatus(errnum, errinf);
        return;

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

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

    MTime time;
    time.Now();

    gLog << inf << time << ": " << GetNodeName() << " - 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) gLog << "DKC-Ready ";
    if (fuse)  gLog << "FuseOk ";
    if (emcy)  gLog << "EmcyOk ";
    if (vltg)  gLog << "OvervoltOk ";
    if (mode)  gLog << "SwitchToManualMode ";
    if (rf)    gLog << "RF ";
    if (brake) gLog << "BrakeOpen ";
    gLog << 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
    //
    gLog << inf << GetNodeName() << " Handling Error #" << dec << GetError() << endl;
    switch (GetError())
    {
    case   6: // home
    case   8: // control dev
    case   9: // zero idx
    case  84: // ON TIME
        gLog << "- " << GetNodeName() << ": Cannot handle error #" << GetError() << endl;
        return;

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

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

    case 0xff:
        gLog << err << "DKC error! Go and check what is going on!" << endl;
        //DelError();
        return;
/*
    case 101:
        //lout << "Warning: " << GetNodeName() << " didn't respond in timeout window - try again." << endl;
        DelError();
        return;
        */
    default:
        gLog << "- " << GetNodeName() << ": Cannot handle error #" << GetError() << endl;
 
    }
}

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

    gLog << inf2 << "- " << GetNodeName() << ": Hostguarding started (" << dec;
    gLog << 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);

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