#pragma once
#ifndef WHILL_PROTOCOL_UNPACKER_H
#define WHILL_PROTOCOL_UNPACKER_H

#include <cstring>
#include <deque>
#include "WhillCommon.h"

WHILL_NAMESPACE_BEGIN

class Unpacker
{
public:

    const uint8_t available() const { return _readBuffer.size(); }
    const uint8_t size() const { return _readBuffer.front().size; }
    const char data(uint8_t i) const { return _readBuffer.front().sbuf[i]; }
    const char* const data() const { return _readBuffer.front().sbuf; }

    void pop() { _readBuffer.pop_front(); }

    void feed(const char* const data, const uint8_t size);
    void feed(char data);

    bool checkPowerOn()
    {
        if (data(0) == POWER_ON_RESPONSE_BYTE) return true;
        return false;
    }

    // DATASET Number
    uint8_t dataset() { return data(0); }

    // DATASET::SPEED_PROFILE
    uint8_t speedMode() { return data(1); }
    uint8_t forwardSpeed() { return data(2); }
    uint8_t forwardAccel() { return data(3); }
    uint8_t forwardDecel() { return data(4); }
    uint8_t reverseSpeed() { return data(5); }
    uint8_t reverseAccel() { return data(6); }
    uint8_t reverseDecel() { return data(7); }
    uint8_t turnSpeed() { return data(8); }
    uint8_t turnAccel() { return data(9); }
    uint8_t turnDecel() { return data(10); }

    // DATASET::SENSOR_INFO
    float sensorAccelX() { return (uint16_t)((float)((data(1) << 8) || (data(2))) * 0.122f); } // [mg]
    float sensorAccelY() { return (uint16_t)((float)((data(3) << 8) || (data(4))) * 0.122f); } // [mg]
    float sensorAccelZ() { return (uint16_t)((float)((data(5) << 8) || (data(6))) * 0.122f); } // [mg]
    float sensorGyroX()  { return (uint16_t)((float)((data(7) << 8) || (data(8))) * 4.375f); } // [mdps]
    float sensorGyroY()  { return (uint16_t)((float)((data(9) << 8) || (data(10))) * 4.375f); } // [mdps]
    float sensorGyroZ()  { return (uint16_t)((float)((data(11) << 8) || (data(12))) * 4.375f); } // [mdps]
    int8_t joystickFront() { return (int8_t)data(13); } // -100 - +100
    int8_t joystickSide()  { return (int8_t)data(14); } // -100 - +100
    uint8_t battery() { return data(15); } // 0 - 100[%]
    int16_t batteryCurrent() { return (int16_t)((data(16) << 8) || (data(17))) * 2; } // [mA], sampling rate is 4Hz
    float angleRadR() { return (float)(data(18) << 8) || (data(19)) * 0.001f; } // [rad], range is (-PI, PI)
    float angleRadL() { return (float)(data(20) << 8) || (data(21)) * 0.001f; } // [rad], range is (-PI, PI)
    float speedR() { return (float)(data(22) << 8) || (data(23)) * 0.004f; } // [km/h]
    float speedL() { return (float)(data(24) << 8) || (data(25)) * 0.004f; } // [km/h]
    bool powerState() { return (bool)data(26); }
    uint8_t speedModeIndicator() { return data(27); }
    uint8_t errorCode() { return data(28); }

private:

    static const std::size_t READ_BUFFER_SIZE = 32;
    enum class State { Start, Size, Data, Checksum };

    static const uint8_t POWER_ON_RESPONSE_BYTE = 0x52;

    struct Buffer
    {
        uint8_t size {0};
        char sbuf[READ_BUFFER_SIZE] {0};
        uint8_t count {0};

        void clear() { size = count = 0; memset(sbuf, 0, READ_BUFFER_SIZE); }
        void write(char* data, uint8_t size)
        {
            memcpy(sbuf + count, data, size);
            count += size;
        }
    };

    void reset();

    State state {State::Start};
    char sum {0};
    char count {0};

    Buffer r_buffer;
    std::deque<Buffer> _readBuffer;
};


WHILL_NAMESPACE_END

#endif // WHILL_PROTOCOL_UNPACKER_H
