#ifndef MACS_H
#define MACS_H

#include "timer.h"
#include "nodedrv.h"

class Macs : public NodeDrv
{
private:
    BYTE_t   fMacId;

    LWORD_t  fVelRes;
    LWORDS_t fVel;

    LWORDS_t fPos;
    Timer    fPosTime;

    LWORDS_t fPdoPos;
    Timer    fPdoTime;

    BYTE_t   fPosActive;
    BYTE_t   fRpmActive;

    int      fError;      // Indicater for error state

    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);
    }

public:
    Macs(BYTE_t nodeid, ostream &out=cout);
    virtual ~Macs();

    void InitDevice(Network *);

    void StartDevice();
    void StopDevice();

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

    void HandlePDO1(BYTE_t *data, struct timeval *tv);
    void HandlePDO2(BYTE_t *data, struct timeval *tv);

    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 SetHome(LWORDS_t pos=0);
    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 StartRelPos(LWORDS_t pos);
    void StartAbsPos(LWORDS_t pos);

    void StopMotor();

    int IsPositioning() { return fPosActive; }

    double GetTime();
    double GetMjd();

    double GetPdoTime();
    double GetPdoMjd();

    LWORDS_t GetPdoPos() { return fPdoPos; }

    LWORDS_t GetPos()    { return fPos; }
    LWORDS_t GetVel()    { return fVel; }
    LWORD_t  GetVelRes() { return fVelRes; }

    int      HasError() { return fError; }
};

#endif