#include <iostream>

#include "network.h"
#include "shaftencoder.h"

using namespace std;

TROOT root("Cosy", "Magic Control System");

int main(int argc, char **argv)
{
//    TApplication* app = new TApplication("App", &argc, argv);
//    MLog *l = new MLog("log/cosy.log", kTRUE);
//    MLog &lout = *l;
//    gLog.EnableOutputDevice(MLog::eStdout);

    cout << endl;
    cout << "Usage: testse [nodeid [[speed]" << endl;
    cout << endl;
    const int baud = argc>2 ? atoi(argv[2]) : 500;
    cout << "Creating Network (" << baud << "bps)..." << endl;
    Network net("/dev/dpm_00", baud);

    const int nodeid = argc>1 ? atoi(argv[1]) : 16;

    cout << "Creating SE #" << nodeid << "..." << endl;
    ShaftEncoder se(nodeid, "SE/Zd1");

    cout << "Adding SE..." << endl;
    net.SetNode(&se);

    // Don't call this function twice!
    cout << "Start Network..." << endl;
    net.Start();

    if (se.IsZombieNode())
    {
        cout << "      /-------------------------------------\\" << endl;
        cout << " >>>>> Initialization of ShaftEncoder failed <<<<<" << endl;
        cout << "      \\-------------------------------------/" << endl;
    }
    else
    {
#ifndef TESTPDO
        while (1)
        {
            se.RequestSDO(0x6004);
            se.WaitForSdo(0x6004);
            if (se.IsZombieNode())
            {
                cout << "      /------------------------------\\" << endl;
                cout << " >>>>> Reading of ShaftEncoder failed <<<<<" << endl;
                cout << "      \\------------------------------/" << endl;
                break;
            }
            usleep(100000);
        }
#else
        for (int i=0; i<50; i++)
        {
            cout << se.GetPos() << endl;
            usleep(100000);
        }
#endif
    }

    cout << "Stop Network..." << endl;
    net.Stop();

    cout << "The End." << endl;
}