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

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

using namespace std;
using namespace Whill;

WhillC whill;

void printSpeedProfile()
{
    cout << "SPEED_PROFILE" << endl;
    cout << "dataset : " << (int)whill.dataset() << endl;
    cout << "mode    : " << (int)whill.speedMode() << endl;
    cout << "fwd spd : " << (int)whill.forwardSpeed() << endl;
    cout << "fwd acc : " << (int)whill.forwardAccel() << endl;
    cout << "fwd dec : " << (int)whill.forwardDecel() << endl;
    cout << "bwd spd : " << (int)whill.reverseSpeed() << endl;
    cout << "bwd acc : " << (int)whill.reverseAccel() << endl;
    cout << "bwd dec : " << (int)whill.reverseDecel() << endl;
    cout << "trn spd : " << (int)whill.turnSpeed() << endl;
    cout << "trn acc : " << (int)whill.turnAccel() << endl;
    cout << "trn dec : " << (int)whill.turnDecel() << endl;
}

void printSensorInfo()
{
    cout << "SENSOR_INFO" << endl;
    cout << "sensor acc x : " << (float)whill.sensorAccelX() << endl;
    cout << "sensor acc y : " << (float)whill.sensorAccelY() << endl;
    cout << "sensor acc z : " << (float)whill.sensorAccelZ() << endl;
    cout << "sensor gyr x : " << (float)whill.sensorGyroX() << endl;
    cout << "sensor gyr y : " << (float)whill.sensorGyroY() << endl;
    cout << "sensor gyr z : " << (float)whill.sensorGyroZ() << endl;
    cout << "joystick fb  : " << (int)whill.joystickFront() << endl;
    cout << "joystick lr  : " << (int)whill.joystickSide() << endl;
    cout << "battery      : " << (int)whill.battery() << endl;
    cout << "battery crnt : " << (int)whill.batteryCurrent() << endl;
    cout << "angle rad r  : " << (float)whill.angleRadR() << endl;
    cout << "angle rad l  : " << (float)whill.angleRadL() << endl;
    cout << "speed r      : " << (float)whill.speedR() << endl;
    cout << "speed l      : " << (float)whill.speedL() << endl;
    cout << "power state  : " << (int)whill.powerState() << endl;
    cout << "speed mode   : " << (int)whill.speedModeIndicator() << endl;
    cout << "error code   : " << (int)whill.errorCode() << endl;
}

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;

        if      (whill.dataset() == 0) printSpeedProfile();
        else if (whill.dataset() == 1) printSensorInfo();
        else    cout << "invalid dataset number" << endl;

        whill.pop();
    }
}


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

    cout << "power off and wait for more than 5 sec..." << endl;

    // power off WHILL
    whill.power(false);
    sleep(5); // wait for more than 5s

    // set speed profile
    whill.speed(SPEEDMODE::RS232C, 40, 40, 40, 40, 40, 40, 40, 40, 40);
    sleep(1);

    // first, check speed profile
    whill.streaming(true, 1000, DATASET::SPEED_PROFILE);
    sleep(1.5);
    parseReply();

    // second, stream sensor data
    whill.streaming(true, 1000, DATASET::SENSOR_INFO);
    sleep(1.5);
    parseReply();

    cout << "start" << endl;

    // power on WHILL
    while (1)
    {
        whill.power(true);
        sleep(1.5); // wait for reply (max 1s)

        // after 1s, reply will come if power on success
        whill.parse();
        if (whill.available())
        {
            if (whill.checkPowerOn()) break;
        }
        cout << "re-issue power on command..." << endl;
    }

    cout << "whill power on" << endl;


    // do what we want
    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++;
    }




    whill.streaming(false);

    cout << "end test" << endl;
    whill.close();
}




