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

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

using namespace std;
using namespace Whill;

WhillC whill;

void parseReply()
{
    whill.parse();

    cout << "available : " << (int)whill.available() << endl;

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

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

    sleep(1);
    parseReply();

    int count = 0;
    while(count < 5000)
    {
        whill.move(20, 0);
        sleep(0.1);
        count++;
    }
    count = 0;
    while(count < 5000)
    {
        whill.move(-20, 0);
        sleep(0.1);
        count++;
    }
    count = 0;
    while(count < 5000)
    {
        whill.move(0, 20);
        sleep(0.1);
        count++;
    }
    count = 0;
    while(count < 5000)
    {
        whill.move(0, -20);
        sleep(0.1);
        count++;
    }

    sleep(1);
    parseReply();

    whill.close();
}
