#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;

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

    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);
    }

    // Serial.println("move 0-100 (y, x)");
    // Serial.print(fb);
    // Serial.print(", ");
    // Serial.println(lr);

    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)
{
    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,  50);
    r_d1 = constrain(r_d1, 40,  80);
    t_m1 = constrain(t_m1,  8,  60); // less than 60 is better
    t_a1 = constrain(t_a1, 10,  60);
    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)
{
    packer.streaming(b, interval, n, mode);
    return this->send();
}

void WhillModelC::parse()
{
    while(serial->available() > 0)
    {
        char data = serial->read();
        unpacker.feed(data);
    }
}

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

