#include "nodedrv.h"

#include <iomanip.h>
#include <iostream.h>

#include "network.h"

NodeDrv::NodeDrv(BYTE_t nodeid, ostream &out) : Log(out), fNetwork(NULL), fId(32)
{
    if (nodeid>31)
    {
        cout << "SetNode - Error: Only node Numbers < 32 are allowed"<< endl;
        return;
    }

    fId = nodeid;
}

void NodeDrv::InitDevice(Network *net)
{
    fNetwork = net;

    EnableCanMsg(kPDO1_TX);
    EnableCanMsg(kPDO2_TX);
    EnableCanMsg(kPDO3_TX);
    EnableCanMsg(kPDO4_TX);
    EnableCanMsg(kSDO_RX);
    EnableCanMsg(kSDO_TX);
}

void NodeDrv::HandleSDOOK(WORD_t idx, BYTE_t subidx)
{
    lout << hex << setfill('0');
    lout << "Sdo=" << idx  << "/" << (int)subidx << " set.";
    lout << endl;
}

void NodeDrv::HandleSDOError(LWORD_t data)
{
    lout << "Err: 0x";
    lout << hex << setfill('0') << setw(4) << data;
    lout << endl;;
}

void NodeDrv::HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, struct timeval *tv)
{
    cout << "SdoRx: Idx=0x"<< hex << idx << "/" << (int)subidx;
    cout << ", val=0x" << val << endl;
}

void NodeDrv::SendPDO1(BYTE_t data[8])
{
    fNetwork->SendPDO1(fId, data);
}

void NodeDrv::SendPDO2(BYTE_t data[8])
{
    fNetwork->SendPDO2(fId, data);
}

void NodeDrv::SendPDO1(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)
{
    fNetwork->SendPDO1(fId, m0, m1, m2, m3, m4, m5, m6, m7);
}

void NodeDrv::SendPDO2(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)
{
    fNetwork->SendPDO2(fId, m0, m1, m2, m3, m4, m5, m6, m7);
}

void NodeDrv::SendSDO(WORD_t idx, BYTE_t subidx, BYTE_t val)
{
    fNetwork->SendSDO(fId, idx, subidx, val);
}

void NodeDrv::SendSDO(WORD_t idx, BYTE_t subidx, WORD_t val)
{
    fNetwork->SendSDO(fId, idx, subidx, val);
}

void NodeDrv::SendSDO(WORD_t idx, BYTE_t subidx, LWORD_t val)
{
    fNetwork->SendSDO(fId, idx, subidx, val);
}

void NodeDrv::SendSDO(WORD_t idx, BYTE_t val)
{
    fNetwork->SendSDO(fId, idx, val);
}

void NodeDrv::SendSDO(WORD_t idx, WORD_t val)
{
    fNetwork->SendSDO(fId, idx, val);
}

void NodeDrv::SendSDO(WORD_t idx, LWORD_t val)
{
    fNetwork->SendSDO(fId, idx, val);
}

void NodeDrv::RequestSDO(WORD_t idx, BYTE_t subidx=0)
{
    fNetwork->RequestSDO(fId, idx, subidx);
}

void NodeDrv::SendNMT(BYTE_t cmd)
{
    fNetwork->SendNMT(fId, cmd);
}

void NodeDrv::EnableCanMsg(BYTE_t fcode)
{
    fNetwork->EnableCanMsg(fId, fcode, TRUE);
}

void NodeDrv::WaitForSdo(WORD_t idx, BYTE_t subidx)
{
    while (fNetwork->WaitingForSdo(fId, idx, subidx))
        usleep(1);
}

void NodeDrv::WaitForSdos()
{
    while (fNetwork->WaitingForSdo(fId))
        usleep(1);
}

void NodeDrv::WaitForNextPdo1()
{
    fNetwork->WaitForNextPdo1(fId);
}

void NodeDrv::WaitForNextPdo2()
{
    cout << "WAIT: " << (int)fId << " " << fNetwork << endl;
    fNetwork->WaitForNextPdo2(fId);
}

void NodeDrv::WaitForNextPdo3()
{
    fNetwork->WaitForNextPdo3(fId);
}

void NodeDrv::WaitForNextPdo4()
{
    fNetwork->WaitForNextPdo4(fId);
}