#ifndef COSY_ShafTEncoder
#define COSY_ShafTEncoder

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

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

class Macs;
class TGLabel;

enum Direction_t { kUndefined, kForward, kBackward };                                                    

class ShaftEncoder : public NodeDrv
{
private:
    LWORDS_t fPos;   // ticks
    WORDS_t  fVel;   // ticks per 5ms
    WORDS_t  fAcc;   // ticks per 25ms^2
    WORDS_t  fTurn;  // Number of turn
    LWORD_t  fTicks; // Number of ticks per turn
    WORD_t   fTurns; // Number of possible turns

    Direction_t fDirection;
  
    Float_t fHysteresisPos;
    Float_t fHysteresisNeg;

    TGLabel  *fLabel;     //
    LWORDS_t  fUpdPos;    // ticks

    bool fPosHasChanged;  //!
    bool fDirHasChanged;  //!

    MTime fTime;
    MLog *fReport;

    Macs *fMotor;     // Corresponding Motor/MACS
    Int_t fOffset;    // offset between SE and Motor
    Int_t fDirChangedPos; // Last position at which the SE changed its moving direction
    Int_t fDirChangedOffset; // Offset between SE and Motor when SE changed its moving direction last


    void HandlePDOType0(const BYTE_t *data, const timeval_t &tv);
    void HandlePDOType1(const BYTE_t *data, const timeval_t &tv);
    void HandlePDOType2(const BYTE_t *data, const timeval_t &tv);

    void ReqPos();

    void Init();
    void CheckConnection();
    // void CheckTwin(Int_t diff) const;

public:
    ShaftEncoder(const BYTE_t nodeid, const char *name=NULL, MLog &out=gLog);

    void StopDevice();

    void SetDisplay(TGLabel *label) { fLabel = label; }
    void SetMotor(Macs *m) { fMotor = m; }
    //void SetTwin(ShaftEncoder *se) { fTwin = se; }

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

    LWORDS_t GetPos() const       { return IsZombieNode() ? 0 : fPos+fTurn*fTicks; } // FIXME? 0?
    Int_t    GetDirection() const { return IsZombieNode() ? 0 : fPos-fDirChangedPos; } // FIXME? 0?
    Int_t    GetDirChangedPos() const { return IsZombieNode() ? 0 : fDirChangedPos; } // FIXME? 0?
    LWORD_t  GetPhysRes() const { return fTicks; }
    Int_t    GetOffset() const { return fOffset; }
    Int_t    GetDirChangedOffset() const { return fDirChangedOffset; }

    Int_t GetPosDirCorrected() const
    {
        return DirHasChanged() ? GetDirChangedPos() : GetPos();
    }
    Int_t GetOffsetDirCorrected() const
    {
        return DirHasChanged() ? GetDirChangedOffset() : GetOffset();
    }

    void     SetOffset(Int_t off) { fOffset = off; }

    double GetMjd();

    void SetPreset(LWORD_t pre=0);

    void DisplayVal();

    bool PosHasChanged() const { return fPosHasChanged; }
    bool DirHasChanged() const { return fDirHasChanged; }
    void ResetPosHasChanged() { fPosHasChanged = false; }
    void ResetDirHasChanged() { fDirHasChanged = false; }

    void SetReport(MLog *log) { fReport = log; }

    void SetHysteresisNeg(Float_t f) { fHysteresisNeg = f; }
    void SetHysteresisPos(Float_t f) { fHysteresisPos = f; }

    Float_t GetPosCorrected() const
    {
        switch (fDirection)
        {
        case kUndefined:
            return GetPos();
        case kForward:
            return GetPos()-fHysteresisPos;
        case kBackward:
            return GetPos()+fHysteresisNeg;
        }
         return 0;
    }

    ClassDef(ShaftEncoder, 0)
};

#endif