#include "dkc.h"

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

#include <TGLabel.h>

#include "network.h"

#include "MLogManip.h"

#include "MString.h"

ClassImp(Dkc);

using namespace std;

#define EXPERT

Dkc::Dkc(const BYTE_t nodeid, const char *name)
    : NodeDrv(nodeid, name), fMacId(2*nodeid+1),
    fPdoPos1(0), fPdoPos2(0), fPosActive(0), fRpmActive(0),
    fStatusDKC(0), fStatusPdo3(0xff), fArmed(false),
    fReport(NULL),fLabel(NULL)
{
}

Dkc::~Dkc()
{
}

TString Dkc::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 "Position control mode with encoder 1, lagless";
    case 0xa105: return "Position control mode with encoder 2, lagless";
    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 positioning with encoder 1, lagless";
    case 0xa109: return "Drive controlled interpolated positioning with encoder 2, lagless";
    //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 0xa152: return "Drive controlled positioning with encoder 2";
    case 0xa153: return "Drive controlled positioning with encoder 2, 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 0xc300: return "Command: set absolute measure";
    case 0xc400: return "Switching to parameter mode";
    case 0xc401: return "Drive active, switching mode not allowed";
    case 0xc500: return "Error reset";
    case 0xc600: return "Drive controlled homi*ng procedure ";
    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 0xe829: return "Positive position limit exceeded";
    case 0xe830: return "Negative position limit exceeded";
    case 0xe831: return "Position limit reached during jog";
    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 position limit exceeded";
    case 0xf630: return "Negative position limit exceeded";
    case 0xf634: return "Emergency-Stop";
    case 0xf643: return "Positive end-switch activated";
    case 0xf644: return "Negative end-switch activated";
    case 0xf870: return "24V DC error";
    case 0xf878: return "Velocity loop error";
    }
    return "unknown";
}

TString Dkc::GetStatus(LWORD_t val) const
{
    const Int_t errnum = val&0xffff;
    const Int_t errinf = val>>16;

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

    // DKC offline. This is a fatal error
    if (errinf==0)
        return "offline.";

    TString str;

    const Int_t type = errinf&0xf000;
    switch (type)
    {
    case 0xf000: str += "ERROR";   break;
    case 0xe000: str += "WARNING"; break;
    case 0xa000: str += "Status";  break;
    case 0xc000: 
    case 0xd000: str += "Message"; break;
    default:     str += "Unknown"; break;
    }

    str += " (";
    str += MString::Format("%X", errinf);
    str += "): ";
    str += EvalStatusDKC(errinf);
    str += (type==0xf000 || type==0xe000 ? "!" : ".");

    return str;
}

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

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

    gLog << all << MTime(-1) << ": " << GetNodeName() << " " << GetStatus(val) << endl;

    // errinf==0: DKC offline. This is a fatal error
    return errinf==0 ? kFALSE : (errinf&0xf000)!=0xf000;
}

void Dkc::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==0)
        {
            fPdoPos1 = (LWORDS_t)val;
            fPdoTime1.Set(tv);
            fHasChangedPos1 = true;
        }
        if (subidx==1)
        {
            fPdoPos2 = (LWORDS_t)val;
            fPdoTime2.Set(tv);
            fHasChangedPos2 = true;
        }
        return;

    case 0x6002:
        gLog << inf2 << "- " << GetNodeName() << ": Velocity resolution = " << dec << val << " (1rpm)" << endl;
        fVelRes = val;
        return;

    case 0x6003:
        gLog << inf2 << "- " << GetNodeName() << ": Maximum velocity = " << dec << val << " (100%)" << endl;
        fVelMax = val;
        return;

    case 0x6501:
        gLog << inf2 << "- " << GetNodeName() << ": Position resolution = " << dec << val << " ticks/revolution" << endl;
        fPosRes = val;
        return;
    }

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

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

void Dkc::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 Dkc::ReqVelRes()
{
    gLog << inf2 << "- " << GetNodeName() << ": Requesting velocity resolution (velres, 0x6002)." << endl;
    RequestSDO(0x6002);
    WaitForSdo(0x6002);
}

void Dkc::ReqVelMax()
{
    gLog << inf2 << "- " << GetNodeName() << ": Requesting maximum velocity (velmax, 0x6003)." << endl;
    RequestSDO(0x6003);
    WaitForSdo(0x6003);
}

void Dkc::ReqPosRes()
{
    gLog << inf2 << "- " << GetNodeName() << ": Requesting position resolution (posres, 0x6501)." << endl;
    RequestSDO(0x6501);
    WaitForSdo(0x6501);
}

void Dkc::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 Dkc::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 Dkc::Arm()
{
    StartNode();
}

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

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

void Dkc::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);

    SetPDO1On(TRUE);
    /*
    if (HasError())
    {
        gLog << err << "Dkc::Init: " << GetNodeName() << " has error --> ZOMBIE!" << endl;
        SetZombie();
    }
    if (IsZombieNode())
        return;
    */

    SetRpmMode(FALSE);

    ReqPosRes(); // Init fVelRes
    ReqVelRes(); // Init fVelResVelRes
    ReqVelMax(); // Init fVelMax

#ifdef EXPERT
    StartNode();
#endif

    gLog << inf2 << "- " << GetNodeName() << ": Checking armed status." << endl;
    RequestSDO(0x1000, 1);
    WaitForSdo(0x1000, 1);
}

void Dkc::StopDevice()
{
    //
    // FIXME: This isn't called if the initialization isn't done completely!
    //

    SetRpmMode(FALSE);
}

void Dkc::ReqPos1()
{
    gLog << inf2 << "- " << GetNodeName() << ": Requesting position feedback 1." << endl;
    RequestSDO(0x6004, 0);
    WaitForSdo(0x6004, 0);
}

void Dkc::ReqPos2()
{
    gLog << inf2 << "- " << GetNodeName() << ": Requesting position feedback 2." << endl;
    RequestSDO(0x6004, 1);
    WaitForSdo(0x6004, 1);
}

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

void Dkc::SetVelocity(LWORD_t vel)
{
    gLog << dbg << "- Setting velocity to: " << vel << endl;
    SendSDO(0x2002, vel);     // velocity
    WaitForSdo(0x2002, 0);
}

void Dkc::SetVelocityRel(Double_t vel)
{
    gLog << dbg << "- Setting velocity to: " << vel*100 << "%" << endl;
    SendSDO(0x2002, (LWORD_t)(vel*fVelMax+0.5));     // velocity
    WaitForSdo(0x2002, 0);
}

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

void Dkc::SetDeceleration(LWORD_t dec)
{
    gLog << dbg << "- Setting deceleration to: " << dec << endl;
    SendSDO(0x2003, 1, dec);
    WaitForSdo(0x2003, 1);
}

void Dkc::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 Dkc::SetRpmVelocity(LWORDS_t cvel)
{
    SendSDO(0x3006, 1, (LWORD_t)cvel);
    WaitForSdo(0x3006, 1);
}

void Dkc::StartRelPos(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, 1, (LWORD_t)pos);
    fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO
}

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

    gLog << dbg << GetNodeName() << ": Starting absolute positioning to " << (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 Dkc::StartAbsPosRev(Double_t pos)
{
    if (!fArmed)
    {
        gLog << err << GetNodeName() << ": ERROR - Moving without being armed is not allowed." << endl;
        SetZombie();
        return;
    }

    const LWORD_t p = (LWORD_t)(pos*fPosRes+.5);

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

void Dkc::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 Dkc::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 Dkc::SendMsg(BYTE_t data[6])
{
    GetNetwork()->SendCanFrame(fMacId, 0, 0, data[0], data[1], data[2], data[3], data[4], data[5]);
}

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

    fPdoPos1    = (data[3]<<24) | (data[2]<<16) | (data[1]<<8) | data[0];
    fPdoTime1.Set(tv);
    fHasChangedPos1 = true;

    fPdoPos2    = (data[7]<<24) | (data[6]<<16) | (data[5]<<8) | data[4];
    fPdoTime2.Set(tv);
    fHasChangedPos2 = true;

    if (fReport)
    {
        fReport->Lock("ShaftEncoder::HandlePDOType0");
        *fReport << "FEEDBACK " << (int)GetId() << " " << fPdoTime1 << " PDO0 " << GetNodeName() << " " << fPdoPos1 << " " << fPdoPos2 << endl;
        fReport->UnLock("ShaftEncoder::HandlePDOType0");
    }
}

void Dkc::HandlePDO3(const BYTE_t *data, const timeval_t &tv)
{
    fArmed = data[2]&0x01;

    if (fStatus!=data[3])
    {
                  // data[3]&0x01; // motor not moving
        fPosActive = data[3]&0x02; // positioning active
        fRpmActive = data[3]&0x04; // 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];
    }

    if (fStatusPdo3!=data[0])
    {
        gLog << inf << MTime(-1) << ": " << GetNodeName() << " - PDO1 = ";
        const Bool_t ready = data[0]&0x01;
        const Bool_t fuse  = data[0]&0x02;
        const Bool_t emcy  = data[0]&0x04;
        const Bool_t vltg  = data[0]&0x08;
        const Bool_t mode  = data[0]&0x10;
        const Bool_t rf    = data[0]&0x20;
        const Bool_t brake = data[0]&0x40;
        const Bool_t power = data[0]&0x80;
        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 ";
        if (power) gLog << "PowerOn ";
        gLog << endl;

        fStatusPdo3 = data[0];
    }
}

void Dkc::CheckErrorDKC(LWORD_t val)
{
    fStatusDKC = val;

    const Bool_t rc = PrintStatus(val);
    SetError(rc ? 0 : val);
    if (!rc)
        SetZombie();
}

void Dkc::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 << "Dkc::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 0xff:
        EvalStatus(errnum, errinf);
        return;

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

// FIXME? Handling of fIsZombie?
void Dkc::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 0xff:
        gLog << err << "DKC error! Go and check what is going on!" << endl;
        return;

    default:
        gLog << "- " << GetNodeName() << ": Cannot handle error #" << GetError() << endl;
 
    }
}

void Dkc::DisplayVal()
{
    const LWORDS_t pos = GetPdoPos2();
    if (IsZombieNode())
    {
        if (fLabel)
            fLabel->SetText(new TGString(""));
        fUpdPos = ~pos;
        return;
    }

    char text[21]="";
    if (pos!=fUpdPos && fLabel)
    {
        sprintf(text, "%ld", pos);
        fLabel->SetText(new TGString(text));
        fUpdPos = pos;
    }
}