#ifndef COSY_Macs
#define COSY_Macs

#ifndef COSY_NodeDrv
#include "nodedrv.h"
#endif

#ifndef MARS_MTime
#include "MTime.h"
#endif

class Macs : public NodeDrv
{
private:
    BYTE_t   fMacId;

    LWORD_t  fSoftVersion;

    LWORD_t  fVelRes;
    LWORDS_t fVel;

    LWORDS_t fPos;
    MTime    fPosTime;

    LWORD_t  fRes;      // Encoder resolution

    LWORDS_t fPdoPos;
    MTime    fPdoTime;

    BYTE_t   fPosActive;
    BYTE_t   fRpmActive;
    BYTE_t   fInControl;
    BYTE_t   fStatus;

    BYTE_t   fStatusPdo3;

    void Init();
    void StopDevice();

    void HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, timeval_t *tv);
    void HandleSDOOK(WORD_t idx, BYTE_t subidx, LWORD_t data, timeval_t *tv);
    //void HandleSDOError(LWORD_t data) { NodeDrv::HandleSDOError(data); }

    void HandlePDO1(BYTE_t *data, timeval_t *tv);
    void HandlePDO2(BYTE_t *data, timeval_t *tv);
    void HandlePDO3(BYTE_t *data, timeval_t *tv);
    //void HandleNodeguard(timeval_t *tv);

    void SendNodeguard();

    void CheckConnection();

public:
    enum
    {
        kNoSync = BIT(0),
        kPosSync = BIT(1),
        kVelSync = BIT(2)
    };
    enum
    {
        kNotMoving = BIT(0),    // motor not moving
        kPosActive = BIT(1),    // positioning active
        kRpmActive = BIT(2),    // RPM mode switched on
        // BIT(3-5) unsused
        kOutOfControl = BIT(6), // motor uncontrolled
        kAxisReset = BIT(7)     // axis resetted (after errclr, motor stop, motor on)
    };

    Macs(const BYTE_t nodeid, const char *name=NULL, MLog &out=gLog);
    virtual ~Macs();

    static LWORD_t string(BYTE_t b0=0, BYTE_t b1=0, BYTE_t b2=0, BYTE_t b3=0)
    {
        return (LWORD_t)(b0<<24 | b1<<16 | b2<<8 | b3);
    }

    void SendMsg(BYTE_t data[6]);
    void 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);

    void ReqPos();
    void ReqVel();
    //void ReqAxEnd();
    void ReqVelRes();
    void ReqRes();
    //void SetHome(LWORDS_t pos=0, WORD_t maxtime=25);
    void SetAcceleration(LWORD_t acc);
    void SetDeceleration(LWORD_t dec);
    void SetVelocity(LWORD_t vel);
    //void SetNoWait(BYTE_t flag=TRUE);
    void SetRpmMode(BYTE_t mode=TRUE);
    void SetRpmVelocity(LWORDS_t cvel);
    void SetPDO1On(BYTE_t flag=TRUE);
    void SetPosEndswitch(LWORDS_t val);
    void SetNegEndswitch(LWORDS_t val);

    void EnableEndswitches(bool neg=true, bool pos=true);

    void StartVelSync();
    void StartPosSync();

    void StartRelPos(LWORDS_t pos);
    void StartAbsPos(LWORDS_t pos);

    //void StopMotor();

    void StartNode();

    int IsPositioning() const { return fPosActive; }
    BYTE_t GetStatus() const { return fStatus; }

    LWORDS_t GetPdoPos() const { return fPdoPos; }

    LWORDS_t GetPos()    const { return fPos; }
    LWORDS_t GetVel()    const { return fVel; }
    LWORD_t  GetVelRes() const { return fVelRes; } // Velocity units (would be 100 for %)
    LWORD_t  GetRes()    const { return fRes; }    // Encoder resolution

    void StartHostGuarding();
    void StopHostGuarding();

    void HandleError();

    Double_t GetPosTime() const { return fPosTime; }
    Double_t GetMjd() const { return fPosTime.GetMjd(); }
    Double_t GetPdoMjd() const { return fPdoTime.GetMjd(); }

    ClassDef(Macs, 0)
};

#endif