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

#include "../libs/Whill/WhillOmni.h"

using namespace std;
using namespace Whill;

WhillOmni whill;

void parseReply()
{
    whill.parse();

    for (uint8_t i = 0; i < 2; ++i)
    {
        WhillOmni::LR lr = (i == 0) ? WhillOmni::LR::L : WhillOmni::LR::R;
        cout << "available : " << (int)whill.available(lr) << endl;
        while(whill.available(lr))
        {
            cout << "[data received]" << endl;
            cout << "size  " << to_string((int)whill.size(lr)) << endl;
            cout << "data  ";
            for (uint8_t i = 0; i < whill.size(lr); ++i)
                cout << hex << setw(2) << setfill('0') << (int)whill.data(lr, i) << " ";
            cout << dec << endl << endl;
            whill.pop(lr);
        }
    }
}

int main()
{
    if (whill.open("/dev/ttyUSB0", "/dev/ttyUSB1", 38400)) // USB
        cout << "serial open success" << endl;
    else
        cout << "serial open failed" << endl;

    whill.speed(SPEEDMODE::RS232C, 60, 40, 40, 60, 40, 40, 60, 40, 40);
    sleep(1);
    parseReply();

    whill.streaming(true);
    // whill.streaming(true, 1000, NUM_0);
    sleep(1);
    parseReply();

    int count = 0;
    while(count < 10)
    {
        sleep(1);
        cout << "in while : " << count << endl;

        switch(count)
        {
            case 0: whill.power(true); break;
            case 1: whill.power(false); break;
            case 2: whill.powerSupply(true); break;
            case 3: whill.powerSupply(false); break;
            case 4: whill.controlEnable(true); break;
            case 5: whill.controlEnable(false); break;
            case 6: whill.stop(); break;
            case 7: whill.move(10, 0, 0); break;
            case 8: whill.move(0, 10, 0); break;
            case 9: whill.move(0, 0, 10); break;
            case 10: whill.move(-10, 0, 0); break;
            case 11: whill.move(0, -10, 0); break;
            case 12: whill.move(0, 0, -10); break;
            default: break;
        }

        sleep(2);

        parseReply();

        ++count;
    }

    whill.streaming(false);
    sleep(1);
    parseReply();

    whill.close();
}


