#include "canopen.h"

#include <iostream.h> // cout
#include <iomanip.h>  // setw, setfill

CanOpen::CanOpen(const char *dev, const int baud, ostream &out=cout) : VmodIcan(dev, baud, out)
{
    for (int i=0; i<32; i++)
        for (int j=0; j<4; j++)
        {
            pthread_cond_init(&fPdoCond[i][j], NULL);
            pthread_mutex_init(&fPdoMux[i][j], NULL);
            pthread_mutex_lock(&fPdoMux[i][j]);
        }
}

CanOpen::~CanOpen()
{
    for (int i=0; i<32; i++)
        for (int j=0; j<4; j++)
        {
            pthread_cond_destroy(&fPdoCond[i][j]);
            pthread_mutex_destroy(&fPdoMux[i][j]);
        }
}

void CanOpen::HandleCanMessage(WORD_t cobid, BYTE_t *data, struct timeval *tv)
{
    const WORD_t fcode = cobid >> 7;

    switch (fcode)
    {
    case kNMT:
        cout << "NMT: " << hex ;
        cout << "CobId: 0x" << cobid << " ";
        cout << "cmd=0x" << (int)data[0] << " ";
        cout << "node=" << dec << (int)data[1] << endl;
        return;

    case kSYNC:
        cout << "Sync" << endl;
        return;

    case kSDO_RX:
        {
            const BYTE_t  node   = cobid & 0x1f;
            const BYTE_t  cmd    = data[0];
            const LWORD_t dat    = data[4] | (data[5]<<8) | (data[6]<<16) | (data[7]<<24);
            const WORD_t  idx    = data[1] | (data[2]<<8);
            const WORD_t  subidx = data[3];

            HandleSDO(node, cmd, idx, subidx, dat, tv);

            fSdoList.Del(node, idx, subidx);
        }
        return;

    case kPDO1_TX:
        {
            const BYTE_t node = cobid & 0x1f;
            HandlePDO1(node, data, tv);
            pthread_cond_broadcast(&fPdoCond[node-1][0]);
        }
        return;

    case kPDO2_TX:
        {
            const BYTE_t node = cobid & 0x1f;
            HandlePDO2(node, data, tv);
            pthread_cond_broadcast(&fPdoCond[node-1][1]);
        }
        return;

    case kPDO3_TX:
        {
            const BYTE_t node = cobid & 0x1f;
            HandlePDO3(node, data, tv);
            pthread_cond_broadcast(&fPdoCond[node-1][2]);
        }
        return;

    case kPDO4_TX:
        {
            const BYTE_t node = cobid & 0x1f;
            HandlePDO4(node, data, tv);
            pthread_cond_broadcast(&fPdoCond[node-1][3]);
        }
        return;
    }

    const BYTE_t node = cobid & 0x1f;
    cout << "Function Code: 0x" << hex << fcode << "  Node: " << dec << (int)node << endl;
}

void CanOpen::EnableCanMsg(BYTE_t node, BYTE_t fcode, int flag)
{
    if (node>=0x20)
        return;

    EnableCobId(CobId(node, fcode), flag);
}

void CanOpen::EnableEmcy(BYTE_t node)
{
    EnableCanMsg(node, kEMERGENCY);
}


void CanOpen::EnableSdoRx(BYTE_t node)
{
    EnableCanMsg(node, kSDO_RX);
}

void CanOpen::EnablePdo1Rx(BYTE_t node)
{
    EnableCanMsg(node, kPDO1_TX);
}

void CanOpen::EnablePdo2Rx(BYTE_t node)
{
    EnableCanMsg(node, kPDO2_TX);
}

void CanOpen::EnablePdo3Rx(BYTE_t node)
{
    EnableCanMsg(node, kPDO1_TX);
}

void CanOpen::EnablePdo4Rx(BYTE_t node)
{
    EnableCanMsg(node, kPDO2_TX);
}

void CanOpen::SendPDO1(BYTE_t node, BYTE_t data[8])
{
    SendCanFrame(CobId(node, kPDO1_TX), data);
}

void CanOpen::SendPDO2(BYTE_t node, BYTE_t data[8])
{
    SendCanFrame(CobId(node, kPDO2_TX), data);
}

void CanOpen::SendPDO3(BYTE_t node, BYTE_t data[8])
{
    SendCanFrame(CobId(node, kPDO3_TX), data);
}

void CanOpen::SendPDO4(BYTE_t node, BYTE_t data[8])
{
    SendCanFrame(CobId(node, kPDO4_TX), data);
}

void CanOpen::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)
{
    BYTE_t msg[8] = { m0, m1, m2, m3, m4, m5, m6, m7 };
    SendCanFrame(CobId(node, kPDO2_TX), msg);
}

void CanOpen::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)
{
    BYTE_t msg[8] = { m0, m1, m2, m3, m4, m5, m6, m7 };
    SendCanFrame(CobId(node, kPDO2_TX), msg);
}

void CanOpen::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)
{
    BYTE_t msg[8] = { m0, m1, m2, m3, m4, m5, m6, m7 };
    SendCanFrame(CobId(node, kPDO3_TX), msg);
}

void CanOpen::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)
{
    BYTE_t msg[8] = { m0, m1, m2, m3, m4, m5, m6, m7 };
    SendCanFrame(CobId(node, kPDO4_TX), msg);
}
  
void CanOpen::SendSDO(BYTE_t node, WORD_t idx, BYTE_t subidx, BYTE_t val)
{
    fSdoList.Add(node, idx, subidx);

    SendCanFrame(CobId(node, kSDO_TX), kSDO_RX1,
                 word_to_lsb(idx), word_to_msb(idx), subidx, val);
}

void CanOpen::SendSDO(BYTE_t node, WORD_t idx, BYTE_t subidx, WORD_t val)
{
    fSdoList.Add(node, idx, subidx);

    SendCanFrame(CobId(node, kSDO_TX), kSDO_RX2,
                 word_to_lsb(idx), word_to_msb(idx), subidx,
                 word_to_lsb(val), word_to_msb(val));
}

void CanOpen::SendSDO(BYTE_t node, WORD_t idx, BYTE_t subidx, LWORD_t val)
{
    fSdoList.Add(node, idx, subidx);

    SendCanFrame(CobId(node, kSDO_TX), kSDO_RX4,
                 word_to_lsb(idx), word_to_msb(idx), subidx,
                 word_to_lsb(val&0xffff), word_to_msb(val&0xffff),
                 word_to_lsb(val>>16), word_to_msb(val>>16));
}

void CanOpen::RequestSDO(BYTE_t node, WORD_t idx, BYTE_t subidx)
{
    fSdoList.Add(node, idx, subidx);

    SendCanFrame(CobId(node, kSDO_TX), kSDO_RX_DATA, word_to_lsb(idx), word_to_msb(idx), subidx);
}

void CanOpen::SendNMT(BYTE_t node, BYTE_t cmd)
{
    SendCanFrame(CobId(0, kNMT), cmd, node);
}

WORD_t CanOpen::CobId(BYTE_t node, BYTE_t fcode)
{
    return (fcode<<7) | node&0x1f;
}