#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 << endl << "----------------------------------------" << endl;
    cout << "first, check current speed profile" << endl;

    whill.streaming(true, 1000, DATASET::SPEED_PROFILE);
    sleep(1.5);
    parseReply();

    cout << endl << "----------------------------------------" << endl;
    cout << "second, change speed profile to MAX speed" << endl;

    whill.speed(SPEEDMODE::RS232C, 60, 90, 160, 30, 50, 90, 35, 60, 160);
    // whill.speed(SPEEDMODE::RS232C, 59, 89, 159, 29, 49, 89, 34, 59, 159);
    // whill.speed(SPEEDMODE::RS232C, 50, 50, 60, 60, 60, 60, 60, 60, 60); // bug??
    sleep(1.5);
    parseReply();

    cout << endl << "----------------------------------------" << endl;
    cout << "third, change speed profile to MIN speed" << endl;

    whill.speed(SPEEDMODE::RS232C, 8, 10, 40, 8, 10, 40, 8, 10, 40);
    sleep(1.5);
    parseReply();

    cout << endl << "----------------------------------------" << endl;
    cout << "fourth, back to standard speed profile" << endl;

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

    cout << endl << "----------------------------------------" << endl;
    cout << "finally, disable streaming" << endl;

    whill.streaming(false);
    sleep(1.5);
    parseReply(); // maybe nothing

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




