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

using namespace Whill;

bool WhillOmni::open(Stream& device_l, Stream& device_r, int baud)
{
    if(!whills[(uint8_t)LR::L].open(device_l, baud)) return false;
    if(!whills[(uint8_t)LR::R].open(device_r, baud)) return false;
    return true;
}

void WhillOmni::attach(Stream& device_l, Stream& device_r)
{
    whills[(uint8_t)LR::L].attach(device_l);
    whills[(uint8_t)LR::R].attach(device_r);
}

void WhillOmni::close()
{
    for (auto& w : whills) w.close();
}

bool WhillOmni::power(bool b)
{
    bool r = true;
    for (auto& w : whills) r &= w.power(b);
    return r;
}

bool WhillOmni::powerSupply(bool b)
{
    bool r = true;
    for (auto& w : whills) r &= w.powerSupply(b);
    return r;
}

bool WhillOmni::controlEnable(bool b)
{
    bool r = true;
    for (auto& w : whills) r &= w.controlEnable(b);
    return r;
}

bool WhillOmni::move(float x, float y, float r) // [mm/s], [mm/s], [rad/s]
{
    model.updateJacobian(0.0);

    // 1: L = 1R, 2: T = 0L, 3: R = 0R, 4: B = 1L
    VectorXd v_omni(3);
    VectorXd w_wheel(4);
    VectorXd whill_l(2);
    VectorXd whill_r(2);

    // TODO: force conversion (direction)
    // v_omni << x / 1000.f, -y / 1000.f, -r; // [m/s], [m/s], [rad/s]
    v_omni << x / 1000.f, y / 1000.f, r; // [m/s], [m/s], [rad/s]
    w_wheel = model.Jacobian_inv * v_omni;

    // convert motors -> WHILL's XY
    // 0: whill[0] L, 1: whill[0] R, 2: whill[1] L, 3: whill[2] R

    // Serial.println("wheel speed");
    // Serial.print(w_wheel[0]); Serial.print(", ");
    // Serial.print(w_wheel[1]); Serial.print(", ");
    // Serial.print(w_wheel[2]); Serial.print(", ");
    // Serial.print(w_wheel[3]); Serial.println();

    // convert motor direction to whill
    w_wheel[0] = -w_wheel[0];
    w_wheel[2] = -w_wheel[2];

    // calcurate whill's translation speed
    whill_l.y() = (w_wheel[1] + w_wheel[0]) / 2.f * model.getWheelRadius();
    whill_l.x() = (w_wheel[1] - w_wheel[0]) / 1.1f * model.getWheelRadius();
    whill_r.y() = (w_wheel[3] + w_wheel[2]) / 2.f * model.getWheelRadius();
    whill_r.x() = (w_wheel[3] - w_wheel[2]) / 1.1f * model.getWheelRadius();

    // convert whill direction
    whill_l.x() = -whill_l.x();
    whill_r.x() = -whill_r.x();

    // Serial.println("move [m/s] (y, x) :");
    // Serial.print(whill_l.y()); Serial.print(" "); Serial.println(whill_l.x());
    // Serial.print(whill_r.y()); Serial.print(" "); Serial.println(whill_r.x());

    bool ret = true;
    ret &= whills[0].moveVel(whill_l.y(), whill_l.x());
    ret &= whills[1].moveVel(whill_r.y(), whill_r.x());
    return ret;
}

bool WhillOmni::stop()
{
    bool r = true;
    for (auto& w : whills) r &= w.stop();
    return r;
}

bool WhillOmni::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)
{
    bool r = true;
    for (auto& w : whills) r &= w.speed(mode, f_m1, f_a1, f_d1, r_m1, r_a1, r_d1, t_m1, t_a1, t_d1);
    return r;
}

bool WhillOmni::streaming(bool b, uint16_t interval, DATASET n, SPEEDMODE mode)
{
    bool r = true;
    for (auto& w : whills) r &= w.streaming(b, interval, n, mode);
    return r;
}

void WhillOmni::parse()
{
    for (auto& w : whills) w.parse();
}








