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

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);
    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 Number
    uint8_t dataset() { return unpacker.dataset(); }

    // DATASET::SPEED_PROFILE
    uint8_t speedMode() { return unpacker.speedMode(); }
    uint8_t forwardSpeed() { return unpacker.forwardSpeed(); }
    uint8_t forwardAccel() { return unpacker.forwardAccel(); }
    uint8_t forwardDecel() { return unpacker.forwardDecel(); }
    uint8_t reverseSpeed() { return unpacker.reverseSpeed(); }
    uint8_t reverseAccel() { return unpacker.reverseAccel(); }
    uint8_t reverseDecel() { return unpacker.reverseDecel(); }
    uint8_t turnSpeed() { return unpacker.turnSpeed(); }
    uint8_t turnAccel() { return unpacker.turnAccel(); }
    uint8_t turnDecel() { return unpacker.turnDecel(); }

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

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

private:

    bool send();

    Stream* serial;
    Packer packer;
    Unpacker unpacker;
    SpeedProfile spd_profile;
};

WHILL_NAMESPACE_END

using WhillC = Whill::WhillModelC;

#endif // WHILL_H
