#include <Arduino.h>
#include <cstdint>
#include "ProtocolPacker.h"
#include "ProtocolUnpacker.h"
#include "Whill.h"

using namespace Whill;

bool WhillModelC::open(Stream& device, int baud)
{
    return true;
}

void WhillModelC::attach(Stream& device)
{
    serial = &device;
}

void WhillModelC::close()
{
    // serial->end();
}

bool WhillModelC::power(bool b)
{
    packer.power(b);
    return this->send();
}

bool WhillModelC::powerSupply(bool b)
{
    packer.powerSupply(b);
    return this->send();
}

bool WhillModelC::controlEnable(bool b)
{
    packer.controlEnable(b);
    return this->send();
}

bool WhillModelC::move(int8_t fb, int8_t lr) // fb: 100 f, -100 b, lr -100 = L, 100 = R
{
    packer.joystick(fb, lr);
    return this->send();
}

bool WhillModelC::moveVel(float y, float x) // [m/s]
{
    float fb, lr;

    // Serial.print("vel cmd [m/s] (y, x) = ");
    // Serial.print(y);
    // Serial.print(", ");
    // Serial.println(x);

    // Serial.print("spd prf [m/s] (y, x) = ");
    // Serial.print(spd_profile.f_m1);
    // Serial.print(", ");
    // Serial.println(spd_profile.t_m1);

    fb = y / spd_profile.f_m1 * 100.f;
    lr = x / spd_profile.t_m1 * 100.f;

    // Serial.println("joystick");
    // Serial.print(fb);
    // Serial.print(", ");
    // Serial.println(lr);

    if (abs(fb) > 100)
    {
        Serial.print("Warning! Excess WHILL speed Limit : "); Serial.println(fb);
        fb = constrain(fb, -100, 100);
    }
    if (abs(lr) > 100)
    {
        Serial.print("Warning! Excess WHILL speed Limit : "); Serial.println(lr);
        lr = constrain(lr, -100, 100);
    }

    return this->move((int8_t)fb, (int8_t)lr);
}

bool WhillModelC::stop()
{
    return this->move(0, 0);
}

bool WhillModelC::speed(SPEEDMODE mode, char f_m1, char f_a1, char f_d1, char r_m1, char r_a1, char r_d1, char t_m1, char t_a1, char t_d1)
{
    // WHILL New Firmware
    f_m1 = constrain(f_m1,  8,  60); // less than 60 is better
    f_a1 = constrain(f_a1, 10,  90);
    f_d1 = constrain(f_d1, 40, 160);
    r_m1 = constrain(r_m1,  8,  60); // less than 60 is better
    r_a1 = constrain(r_a1, 10,  90);
    r_d1 = constrain(r_d1, 40, 160);
    t_m1 = constrain(t_m1,  8,  60); // less than 60 is better
    t_a1 = constrain(t_a1, 10,  90);
    t_d1 = constrain(t_d1, 40, 160);

    // save speed as [m/s]
    spd_profile.f_m1 = f_m1 / 10.f * 1000.f / 60.f / 60.f;
    spd_profile.r_m1 = r_m1 / 10.f * 1000.f / 60.f / 60.f;
    spd_profile.t_m1 = t_m1 / 10.f * 1000.f / 60.f / 60.f;

    packer.speed(mode, f_m1, f_a1, f_d1, r_m1, r_a1, r_d1, t_m1, t_a1, t_d1);
    return this->send();
}

bool WhillModelC::streaming(bool b, uint16_t interval, DATASET n, SPEEDMODE mode)
{
    streaming_info.is_streaming = b;
    streaming_info.interval = interval;
    streaming_info.dataset = n;
    streaming_info.speed_mode = mode;
    packer.streaming(b, interval, n, mode);
    return this->send();
}

void WhillModelC::parse()
{
    if (streaming_info.is_streaming)
    {
        if ((millis() - prev_streaming_ms) > STATUS_RECEIVE_TIMEOUT)
        {
            status.info.battery.percent = 0;
            status.info.error_code = 253;
        }
    }

    while(serial->available() > 0)
    {
        prev_streaming_ms = millis();

        char data = serial->read();
        unpacker.feed(data);

        while (unpacker.available())
        {
            if (unpacker.dataset() == (uint8_t)DATASET::SPEED_PROFILE)
            {
#ifdef DEBUG_INFO
                printSpeedProfile();
#endif
                status.speed.mode = unpacker.speedMode();
                status.speed.forward_speed = unpacker.forwardSpeed();
                status.speed.forward_accel = unpacker.forwardAccel();
                status.speed.forward_decel = unpacker.forwardDecel();
                status.speed.reverse_speed = unpacker.reverseSpeed();
                status.speed.reverse_accel = unpacker.reverseAccel();
                status.speed.reverse_decel = unpacker.reverseDecel();
                status.speed.turn_speed = unpacker.turnSpeed();
                status.speed.turn_accel = unpacker.turnAccel();
                status.speed.turn_decel = unpacker.turnDecel();
            }
            else if (unpacker.dataset() == (uint8_t)DATASET::SENSOR_INFO)
            {
#ifdef DEBUG_INFO
                printSensorInfo();
#endif
                status.info.sensor.accelX = unpacker.sensorAccelX();
                status.info.sensor.accelY = unpacker.sensorAccelY();
                status.info.sensor.accelZ = unpacker.sensorAccelZ();
                status.info.sensor.gyroX = unpacker.sensorGyroX();
                status.info.sensor.gyroY = unpacker.sensorGyroY();
                status.info.sensor.gyroZ = unpacker.sensorGyroZ();

                status.info.joystick.front = unpacker.joystickFront();
                status.info.joystick.side = unpacker.joystickSide();

                status.info.battery.percent = unpacker.battery();
                status.info.battery.current = unpacker.batteryCurrent();
                status.info.battery.power_state = unpacker.powerState();

                status.info.wheel.speed_mode_indicator = unpacker.speedModeIndicator();
                status.info.wheel.angle_rad_r = unpacker.angleRadL();
                status.info.wheel.angle_rad_l = unpacker.angleRadR();
                status.info.wheel.speed_r = unpacker.speedR();
                status.info.wheel.speed_l = unpacker.speedL();

                status.info.error_code = unpacker.errorCode();

                // Serial.println("=================================");
                // Serial.print("Detected Wheel Spd [km/h] = ");
                // Serial.print(status.info.wheel.speed_l);
                // Serial.print(", ");
                // Serial.print(status.info.wheel.speed_r);
                // Serial.println();
                // Serial.print("Detected Wheel Anguler Spd[rad/s] = ");
                // Serial.print(status.info.wheel.speed_l * 1000. / 60. /60. / 0.125);
                // Serial.print(", ");
                // Serial.print(status.info.wheel.speed_r * 1000. / 60. /60. / 0.125);
                // Serial.println();
                // Serial.println("=================================");
            }
            pop();
        }
    }
}

bool WhillModelC::send()
{
    int n = serial->write(packer.data(), packer.size());
    packer.clear(); // TODO: smarter way...
    return (n == packer.size());
}

