#ifndef COSY_CanOpen
#define COSY_CanOpen

#ifndef ROOT_TCondition
#include <TCondition.h>
#endif

#ifdef __CINT__
typedef Byte_t   BYTE_t;
typedef UShort_t WORD_t;
typedef Short_t  WORDS_t;
typedef UInt_t   LWORD_t;
typedef Int_t    LWORDS_t;
struct Message;
struct FastMessage;
#else
#include "gendef.h"
#include "dpm.h"
#endif

#ifndef COSY_SdoList
#include "sdolist.h"
#endif

#ifndef COSY_MTimeout
#include "MTimeout.h"
#endif

//#define FALSE       0
//#define TRUE        1

// CAL Function Codes
// COB-ID = function code | node-id (5bits)
#define kNMT           0x0 // only with COB-ID 0
#define kSYNC          0x1 // only with COB-ID 0
#define kTIMESTAMP     0x2 // only with COB-ID 0

#define kEMERGENCY     0x1
#define kPDO1_TX       0x3
#define kPDO2_TX       0x5
#define kPDO3_TX       0x7
#define kPDO4_TX       0x9
#define kSDO_RX        0xb  // this is used to set data of the shaft encoder
#define kSDO_TX        0xc  // this is used to request data from the shaft encoder
#define kNodeguard     0xe

// NMT: no answer to NMT command
// cob-id=0, command (byte), id (byte)
#define kNMT_START     0x01 // change to operational state (start)
#define kNMT_STOP      0x02 // change to prepared    state (stop)
#define kNMT_PREOP     0x80 // enter pre operational state
#define kNMT_RESET     0x81 // reset node (set parameter to power on values)
#define kNMT_REINIT    0x82 // reset communication of node (set communication parameters to power on values)

// command for SDOs
#define kSDO_LEN4      0x3
#define kSDO_LEN2      0xb
#define kSDO_LEN1      0xf

#define kSDO_RXm4      0x22  // this is used with SDO_TX to send a maximum of 4 bytes
#define kSDO_RX4       0x20|kSDO_LEN4  // this is used with SDO_TX to send 4 bytes
#define kSDO_RX2       0x20|kSDO_LEN2  // this is used with SDO_TX to send 2 bytes
#define kSDO_RX1       0x20|kSDO_LEN1  // this is used with SDO_TX to send 1 byte
#define kSDO_RX_DATA   0x40            // this is used to request parameters from the encoder
#define kSDO_TX4       0x40|kSDO_LEN4  // answer to 0x40 with 4 bytes of data
#define kSDO_TX3       0x40|kSDO_LEN2  // answer to 0x40 with 2 bytes of data
#define kSDO_TX1       0x40|kSDO_LEN1  // answer to 0x40 with 1 byte  of data
#define kSDO_TX_OK     0x60            // answer to a SDO_TX message
#define kSDO_TX_ERROR  0x80            // error message (instead of 0x60)

#define kWaitUnlimited 0
#define kDontWait      (-1)

class Interface;

typedef struct timeval timeval_t;

class CanOpen
{
private:
    Interface     *fInterface;      // Handle to the I/O interface
    PendingSDOList fSdoList;        // List for pending SDOs
    TCondition     fPdoCond[32][4]; // one for every PDO of every node

protected:
    virtual void Start();           // Start CanOpen communication
    virtual void Stop();            // Stop  CanOpen communcation

    virtual bool HasError() const;

private:
    //
    // Handle can objects arrived at the CanOpen interface
    //
    virtual void HandleSDO(BYTE_t node, BYTE_t cmd, WORD_t idx, BYTE_t subidx, LWORD_t data, const timeval_t &tv)=0;
    virtual void HandlePDO1(BYTE_t node, const BYTE_t *data, const timeval_t &tv)=0;
    virtual void HandlePDO2(BYTE_t node, const BYTE_t *data, const timeval_t &tv)=0;
    virtual void HandlePDO3(BYTE_t node, const BYTE_t *data, const timeval_t &tv)=0;
    virtual void HandlePDO4(BYTE_t node, const BYTE_t *data, const timeval_t &tv)=0;
    virtual void HandleNodeguard(BYTE_t node, const timeval_t &tv)=0;
    virtual void HandleEmergency(BYTE_t node, const timeval_t &tv)=0;

    // Handle message arrived at the CanOpen interface and split into can objects
    virtual void HandleCanMessage(WORD_t cobid, const BYTE_t *data, const timeval_t &tv);

public:
    CanOpen();
    virtual ~CanOpen();

    void SetInterface(Interface *f) { fInterface=f; } // Set Handle to the interface
    Bool_t HasConnection() const;

    // Send a Process Data Object (PDO) to the CanOpen bus
    void SendPDO1(BYTE_t node, BYTE_t data[8]);
    void SendPDO2(BYTE_t node, BYTE_t data[8]);
    void SendPDO3(BYTE_t node, BYTE_t data[8]);
    void SendPDO4(BYTE_t node, BYTE_t data[8]);
    void SendPDO1(BYTE_t node,
                  BYTE_t m0=0, BYTE_t m1=0, BYTE_t m2=0, BYTE_t m3=0,
                  BYTE_t m4=0, BYTE_t m5=0, BYTE_t m6=0, BYTE_t m7=0);
    void SendPDO2(BYTE_t node,
                  BYTE_t m0=0, BYTE_t m1=0, BYTE_t m2=0, BYTE_t m3=0,
                  BYTE_t m4=0, BYTE_t m5=0, BYTE_t m6=0, BYTE_t m7=0);
    void SendPDO3(BYTE_t node,
                  BYTE_t m0=0, BYTE_t m1=0, BYTE_t m2=0, BYTE_t m3=0,
                  BYTE_t m4=0, BYTE_t m5=0, BYTE_t m6=0, BYTE_t m7=0);
    void SendPDO4(BYTE_t node,
                  BYTE_t m0=0, BYTE_t m1=0, BYTE_t m2=0, BYTE_t m3=0,
                  BYTE_t m4=0, BYTE_t m5=0, BYTE_t m6=0, BYTE_t m7=0);

    // Send a Service Data Object (SDO) to a CanOpen device (aka. write parameter)
    void SendSDO(BYTE_t node, WORD_t idx, BYTE_t subidx, BYTE_t val, bool store);
    void SendSDO(BYTE_t node, WORD_t idx, BYTE_t subidx, WORD_t val, bool store);
    void SendSDO(BYTE_t node, WORD_t idx, BYTE_t subidx, LWORD_t val, bool store);

    void SendSDO(BYTE_t node, WORD_t idx, BYTE_t val, bool store)   { SendSDO(node, idx, 0, val, store); }
    void SendSDO(BYTE_t node, WORD_t idx, WORD_t val, bool store)   { SendSDO(node, idx, 0, val, store); }
    void SendSDO(BYTE_t node, WORD_t idx, LWORD_t val, bool store)  { SendSDO(node, idx, 0, val, store); }

    // Send other objects
    void SendNMT(BYTE_t node, BYTE_t cmd);
    void SendNodeguard(BYTE_t node);

    // Request a Service Data Object (SDO) from a CanOpen device (aka. read parameter)
    void RequestSDO(BYTE_t node, WORD_t idx, BYTE_t subidx=0);

    // Enable interface for pass-through of a kind of can-object
    void EnableSdoRx(BYTE_t node);
    void EnablePdo1Rx(BYTE_t node);
    void EnablePdo2Rx(BYTE_t node);
    void EnablePdo3Rx(BYTE_t node);
    void EnablePdo4Rx(BYTE_t node);
    void EnableEmcy(BYTE_t node);
    void EnableNodeguard(BYTE_t node);

    // Enable interface for pass-through of a can-object
    void EnableCanMsg(BYTE_t node, BYTE_t fcode, int flag=TRUE);

    // Wait until the next Pdo object has arrived
    void WaitForNextPdo1(BYTE_t node) { node -= 1; fPdoCond[node][0].Wait(); }
    void WaitForNextPdo2(BYTE_t node) { node -= 1; fPdoCond[node][1].Wait(); }
    void WaitForNextPdo3(BYTE_t node) { node -= 1; fPdoCond[node][2].Wait(); }
    void WaitForNextPdo4(BYTE_t node) { node -= 1; fPdoCond[node][3].Wait(); }

    // Wait for arrival of a return to a given Sdo (or all axpected Sdos)
    bool WaitForSdos(WORDS_t ms=500);
    bool WaitForSdo(BYTE_t node, WORD_t idx, BYTE_t subidx, WORDS_t ms=500);

    virtual int StopWaitingForSDO() const { return FALSE; }

    // Low level function to send rawcan frames
    void SendCanFrame(WORD_t cobid, BYTE_t m[8], BYTE_t rtr=0);
    void SendCanFrame(WORD_t cobid,
                      BYTE_t m0=0, BYTE_t m1=0, BYTE_t m2=0, BYTE_t m3=0,
                      BYTE_t m4=0, BYTE_t m5=0, BYTE_t m6=0, BYTE_t m7=0);

    // Extract the CanOpen message from the message delivered by the interface
    void HandleMessage(const Message &msg, const timeval_t &tv);

    // Compile CobId from node and function code
    WORD_t CobId(BYTE_t node, BYTE_t fcode) const;

    ClassDef(CanOpen, 0) // implementation of the can open layer
};

#endif