#include <Arduino.h>
#include <Whill.h>

using namespace Whill;

WhillC whill;

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

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

void parseReply()
{
    whill.parse();

    Serial.print("available : "); Serial.println(whill.available());

    while(whill.available())
    {
        Serial.println("[data received]");
        Serial.print("size  "); Serial.println(whill.size());
        Serial.print("data  ");
        for (uint8_t i = 0; i < whill.size(); ++i)
        {
            Serial.print(whill.data(i)); Serial.print(" ");
        }
        Serial.println(); Serial.println();

        if      (whill.dataset() == 0) printSpeedProfile();
        else if (whill.dataset() == 1) printSensorInfo();
        else    Serial.println("invalid dataset number");

        whill.pop();
    }
}


void setup()
{
    Serial1.begin(38400, SERIAL_8N2);
    whill.attach(Serial1);

    delay(1000);

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

    Serial.println("start streaming");

    // second, stream sensor data
    whill.streaming(true, 1000, DATASET::SENSOR_INFO);
    delay(1500);

    int count = 0;
    while(count < 10)
    {
        Serial.print("in while : ");
        Serial.println(count);
        parseReply();
        delay(1000);
        ++count;
    }

    whill.streaming(false);
    delay(1000);
    parseReply(); // maybe nothing

    Serial.println("end streaming test");
}

void loop()
{
}

