#pragma once
#ifndef WHILL_OMNI_H
#define WHILL_OMNI_H

#include <array>
#include "Whill.h"
#include "OmniModel.h"

WHILL_NAMESPACE_BEGIN

using namespace Eigen;

class WhillOmni
{
public:

    enum class LR { L, R };

    bool open(Stream& device_l, Stream& device_r, int baud);
    void attach(Stream& device_l, Stream& device_r);
    void close();

    bool power(bool b);
    bool powerSupply(bool b);
    bool controlEnable(bool b);

    bool move(float x, float y, float r); // [mm/s], [mm/s], [rad/s]
    bool stop();
    bool 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 streaming(bool b, uint16_t interval = 1000, DATASET n = DATASET::SENSOR_INFO, SPEEDMODE mode = SPEEDMODE::RS232C);
    void parse();

    const uint8_t available(LR lr) const { return whills[(uint8_t)lr].available(); }
    const uint8_t size(LR lr) const { return whills[(uint8_t)lr].size(); }
    const char data(LR lr, char i) const { return whills[(uint8_t)lr].data(i); }
    const char* const data(LR lr) const { return whills[(uint8_t)lr].data(); }
    void pop(LR lr) { whills[(uint8_t)lr].pop(); }
    void pop() { for (auto& w : whills) w.pop(); }

    // DATASET Number
    uint8_t dataset(LR lr) { return whills[(uint8_t)lr].dataset(); }

    // DATASET::SPEED_PROFILE
    uint8_t speedMode(LR lr) { return whills[(uint8_t)lr].speedMode(); }
    uint8_t forwardSpeed(LR lr) { return whills[(uint8_t)lr].forwardSpeed(); }
    uint8_t forwardAccel(LR lr) { return whills[(uint8_t)lr].forwardAccel(); }
    uint8_t forwardDecel(LR lr) { return whills[(uint8_t)lr].forwardDecel(); }
    uint8_t reverseSpeed(LR lr) { return whills[(uint8_t)lr].reverseSpeed(); }
    uint8_t reverseAccel(LR lr) { return whills[(uint8_t)lr].reverseAccel(); }
    uint8_t reverseDecel(LR lr) { return whills[(uint8_t)lr].reverseDecel(); }
    uint8_t turnSpeed(LR lr) { return whills[(uint8_t)lr].turnSpeed(); }
    uint8_t turnAccel(LR lr) { return whills[(uint8_t)lr].turnAccel(); }
    uint8_t turnDecel(LR lr) { return whills[(uint8_t)lr].turnDecel(); }

    // DATASET::SENSOR_INFO
    uint16_t sensorAccelX(LR lr) { return whills[(uint8_t)lr].sensorAccelX(); } // [mg]
    uint16_t sensorAccelY(LR lr) { return whills[(uint8_t)lr].sensorAccelY(); } // [mg]
    uint16_t sensorAccelZ(LR lr) { return whills[(uint8_t)lr].sensorAccelZ(); } // [mg]
    uint16_t sensorGyroX(LR lr)  { return whills[(uint8_t)lr].sensorGyroX(); } // [mdps]
    uint16_t sensorGyroY(LR lr)  { return whills[(uint8_t)lr].sensorGyroY(); } // [mdps]
    uint16_t sensorGyroZ(LR lr)  { return whills[(uint8_t)lr].sensorGyroZ(); } // [mdps]
    int8_t joystickFront(LR lr) { return whills[(uint8_t)lr].joystickFront(); } // -100 - +100
    int8_t joystickSide(LR lr)  { return whills[(uint8_t)lr].joystickSide(); } // -100 - +100
    uint8_t battery(LR lr) { return whills[(uint8_t)lr].battery(); } // 0 - 100[%]
    int16_t batteryCurrent(LR lr) { return whills[(uint8_t)lr].batteryCurrent(); } // [mA], sampling rate is 4Hz
    float angleRadR(LR lr) { return whills[(uint8_t)lr].angleRadR(); } // [rad], range is (-PI, PI)
    float angleRadL(LR lr) { return whills[(uint8_t)lr].angleRadL(); } // [rad], range is (-PI, PI)
    float speedR(LR lr) { return whills[(uint8_t)lr].speedR(); } // [km/h]
    float speedL(LR lr) { return whills[(uint8_t)lr].speedL(); } // [km/h]
    bool powerState(LR lr) { return whills[(uint8_t)lr].powerState(); }
    uint8_t speedModeIndicator(LR lr) { return whills[(uint8_t)lr].speedModeIndicator(); }
    uint8_t errorCode(LR lr) { return whills[(uint8_t)lr].errorCode(); }

private:

    static constexpr int NUM_WHILLS {2};
    static constexpr float C {0.71}; // trad??
    static constexpr float RADIUS {1.0}; // from center to wheel

    struct WhillXY { float x; float y; };

    std::array<WhillModelC, NUM_WHILLS> whills;
    Work::Model::Omni <Vector3d, Vector4d, 3, 4> model;
};

WHILL_NAMESPACE_END

#endif // WHILL_OMNI_H
