#include "network.h"

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

void Network::Start()
{
    StartReceiver();
    InitNodes();
}

void Network::Stop()
{
    StopNodes();
    StopReceiver();

    lout << "- Network stopped." << endl;
}

Network::Network(const char *dev, const int baud, ostream &out) : CanOpen(dev, baud, out)
{
    for (int i=0; i<32; i++)
        fNodes[i] = NULL;
}

void Network::HandleSDO(BYTE_t node, BYTE_t cmd, WORD_t idx, BYTE_t subidx, LWORD_t data, struct timeval *tv)
{
    switch (cmd)
    {
    case kSDO_TX4:       // answer to 0x40 with 4 bytes of data
        if (fNodes[node])
        {
            fNodes[node]->HandleSDO(idx, subidx, data, tv);
            return;
        }
        break;

    case kSDO_TX3:       // answer to 0x40 with 2 bytes of data
        if (fNodes[node])
        {
            fNodes[node]->HandleSDO(idx, subidx, data>>16, tv);
            return;
        }
        break;

    case kSDO_TX1:       // answer to 0x40 with 1 byte  of data
        if (fNodes[node])
        {
            fNodes[node]->HandleSDO(idx, subidx, data>>24, tv);
            return;
        }
        break;

    case kSDO_TX_OK:     // answer to a SDO_TX message
        if (fNodes[node])
        {
            fNodes[node]->HandleSDOOK(idx, subidx);
            return;
        }
        break;

    case kSDO_TX_ERROR:  // error message (instead of 0x60)
        if (fNodes[node])
        {
            fNodes[node]->HandleSDOError(data);
            return;
        }
        break;
    }
    cout << dec << setfill('0');
    cout << "Node=" << (int)node  << " Cmd=0x" << hex << (int)cmd << ": ";
    cout << "Sdo=" << idx  << "/" << (int)subidx << ": 0x" << setw(8) << data;
    cout << endl;
}

void Network::HandlePDO1(BYTE_t node, BYTE_t *data, struct timeval *tv)
{
    if (!fNodes[node])
    {
        cout << "Node " << dec << (int)node << ", PDO1: " << hex;
        for (int i=0; i<8; i++)
            cout << " 0x" << (int)data[i];
        cout << endl;
        return;
    }

    fNodes[node]->HandlePDO1(data, tv);
}

void Network::HandlePDO2(BYTE_t node, BYTE_t *data, struct timeval *tv)
{
    if (!fNodes[node])
    {
        cout << "Node " << dec << (int)node << ", PDO2: " << hex;
        for (int i=0; i<8; i++)
            cout << " 0x" << (int)data[i];
        cout << endl;
        return;
    }

    fNodes[node]->HandlePDO2(data, tv);
}

void Network::HandlePDO3(BYTE_t node, BYTE_t *data, struct timeval *tv)
{
    if (!fNodes[node])
    {
        cout << "Node " << dec << (int)node << ", PDO3: " << hex;
        for (int i=0; i<8; i++)
            cout << " 0x" << (int)data[i];
        cout << endl;
        return;
    }

    fNodes[node]->HandlePDO3(data, tv);
}

void Network::HandlePDO4(BYTE_t node, BYTE_t *data, struct timeval *tv)
{
    if (!fNodes[node])
    {
        cout << "Node " << dec << (int)node << ", PDO4: " << hex;
        for (int i=0; i<8; i++)
            cout << " 0x" << (int)data[i];
        cout << endl;
        return;
    }

    fNodes[node]->HandlePDO4(data, tv);
}

void Network::SetNode(NodeDrv *drv)
{
    const BYTE_t nodeid = drv->GetId();

    if (nodeid>31)
    {
        cout << "SetNode - Error: Only node Numbers < 32 are allowed"<< endl;
        return;
    }

    fNodes[nodeid] = drv;
}

void Network::InitNodes()
{
    for (int i=0; i<32; i++)
        if (fNodes[i])
        {
            lout << "- Initializing Node #" << dec << i << endl;
            fNodes[i]->InitDevice(this);
            fNodeInitialized[i] = TRUE;
        }
    lout << "- All Nodes initialized." << endl;
}

void Network::StopNodes()
{
    for (int i=0; i<32; i++)
        if (fNodes[i] && fNodeInitialized[i])
        {
            lout << "- Stopping Node #" << dec << i << endl;
            fNodes[i]->StopDevice();
        }
    lout << "- All Nodes stopped." << endl;
}