#pragma once
#ifndef WHILL_H
#define WHILL_H

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

WHILL_NAMESPACE_BEGIN

class WhillModelC
{
    struct SpeedProfile
    {
        float f_m1; // [m/s]
        float r_m1; // [m/s]
        float t_m1; // [m/s]?? TODO: check spec
    };
    // TODO: iranai kamo
    // TODO: only struct Status is enough??

    struct Status
    {
        // DATASET::SPEED_PROFILE
        struct Speed
        {
            uint8_t mode;
            uint8_t forward_speed;
            uint8_t forward_accel;
            uint8_t forward_decel;
            uint8_t reverse_speed;
            uint8_t reverse_accel;
            uint8_t reverse_decel;
            uint8_t turn_speed;
            uint8_t turn_accel;
            uint8_t turn_decel;
        } speed;

        // DATASET::SENSOR_INFO
        struct Info
        {
            struct Sensor
            {
                uint16_t accelX; // [mg]
                uint16_t accelY; // [mg]
                uint16_t accelZ; // [mg]
                uint16_t gyroX; // [mdps]
                uint16_t gyroY; // [mdps]
                uint16_t gyroZ; // [mdps]
            } sensor;
            struct Joystick
            {
                int8_t front; // -100 - +100
                int8_t side;  // -100 - +100
            } joystick;
            struct Battery
            {
                uint8_t  percent; // 0 - 100[%]
                uint16_t current; // [mA], sampling rate is 4Hz
                bool power_state;
            } battery;
            struct Wheel
            {
                uint8_t speed_mode_indicator;
                float angle_rad_r; // [rad], range is (-PI, PI)
                float angle_rad_l; // [rad], range is (-PI, PI)
                float speed_r; // [km/h]
                float speed_l; // [km/h]
            } wheel;
            uint8_t error_code;
        } info;
    };

    struct StreamingInfo
    {
        bool is_streaming;
        uint16_t interval;
        DATASET dataset;
        SPEEDMODE speed_mode;
    };

public:

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

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

    bool move(int8_t fb, int8_t lr);
    bool moveVel(float y, float x);
    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);
    uint16_t streamingInterval() { return streaming_info.interval; }
    void parse();

    const uint8_t available() const { return unpacker.available(); }
    const uint8_t size() const { return unpacker.size(); }
    const char data(char i) const { return unpacker.data(i); }
    const char* const data() const { return unpacker.data(); }
    void pop() { unpacker.pop(); }

    // DATASET::SPEED_PROFILE
    uint8_t speedMode() { return status.speed.mode; }
    uint8_t forwardSpeed() { return status.speed.forward_speed; }
    uint8_t forwardAccel() { return status.speed.forward_accel; }
    uint8_t forwardDecel() { return status.speed.forward_decel; }
    uint8_t reverseSpeed() { return status.speed.reverse_speed; }
    uint8_t reverseAccel() { return status.speed.reverse_accel; }
    uint8_t reverseDecel() { return status.speed.reverse_decel; }
    uint8_t turnSpeed() { return status.speed.turn_speed; }
    uint8_t turnAccel() { return status.speed.turn_accel; }
    uint8_t turnDecel() { return status.speed.turn_decel; }

    // DATASET::SENSOR_INFO
    uint16_t sensorAccelX() { return status.info.sensor.accelX; } // [mg]
    uint16_t sensorAccelY() { return status.info.sensor.accelY; } // [mg]
    uint16_t sensorAccelZ() { return status.info.sensor.accelZ; } // [mg]
    uint16_t sensorGyroX()  { return status.info.sensor.gyroX; } // [mdps]
    uint16_t sensorGyroY()  { return status.info.sensor.gyroY; } // [mdps]
    uint16_t sensorGyroZ()  { return status.info.sensor.gyroZ; } // [mdps]
    int8_t joystickFront() { return status.info.joystick.front; } // -100 - +100
    int8_t joystickSide()  { return status.info.joystick.side; } // -100 - +100
    uint8_t battery() { return status.info.battery.percent; } // 0 - 100[%]
    int16_t batteryCurrent() { return status.info.battery.current; } // [mA], sampling rate is 4Hz
    float angleRadR() { return status.info.wheel.angle_rad_r; } // [rad], range is (-PI, PI)
    float angleRadL() { return status.info.wheel.angle_rad_l; } // [rad], range is (-PI, PI)
    float speedR() { return status.info.wheel.speed_r; } // [km/h]
    float speedL() { return status.info.wheel.speed_l; } // [km/h]
    bool powerState() { return status.info.battery.power_state; }
    uint8_t speedModeIndicator() { return status.info.wheel.speed_mode_indicator; }
    uint8_t errorCode() { return status.info.error_code; }

    // 34 バッテリー通信エラー
    // 61 モーター高温
    // 62 モーター低温
    // 70 モータードライバ温度異常
    // 71 モーター電圧がover voltage
    // 72 モーター電圧がunder voltage
    // 74 モーター過電流

private:

    bool send();

    const uint32_t STATUS_RECEIVE_TIMEOUT = 2000;

    Stream* serial;
    Packer packer;
    Unpacker unpacker;
    SpeedProfile spd_profile;
    Status status;
    StreamingInfo streaming_info;

    uint32_t prev_streaming_ms;
};

WHILL_NAMESPACE_END

using WhillC = Whill::WhillModelC;

#endif // WHILL_H
