#pragma once
#ifndef BORDERXBEEPACKETIZER_H
#define BORDERXBEEPACKETIZER_H

#include <Arduino.h>
#include <queue>
#include <array>

namespace BorderXBeeProtocol
{
    static constexpr uint8_t BYTE_HEDADER = 0xFF;
    static constexpr uint8_t BYTE_FOOTER = 0xFE;
}

class BorderXBeeUnpacker
{
    struct WhillXBeeData
    {
        uint8_t id;
        uint8_t size;
        std::array<uint8_t, 8> data;
        uint8_t sum;

        std::array<float, 3> vel;
        void clear() { id = 0xFF; size = 0; data.fill(0); sum = 0; vel.fill(0); }
    };

    enum class WhillXBeeState { Header, ID, DATA };

public:

    void attach(Stream& stream) { serial = &stream; }
    uint8_t available() { return packets.size(); }

    uint8_t size() { return packets.front().size; }
    uint8_t data(uint8_t i) { return packets.front().data[i]; }
    uint32_t errorcount() { return error_count; }

    uint8_t id() { return packets.front().id; }
    float velocityX() { return packets.front().vel[0]; }
    float velocityY() { return packets.front().vel[1]; }
    float velocityW() { return packets.front().vel[2]; }

    bool isStatusReportChanged()
    {
        bool b = b_status_report_changed;
        b_status_report_changed = false;
        return b;
    }

    uint32_t statusReportDurationMs()
    {
        if      (status_report_duration == 1) return 50;
        else if (status_report_duration == 2) return 100;
        else if (status_report_duration == 3) return 500;
        else if (status_report_duration == 4) return 1000;
        return 0xFFFFFFFF;
    }

    void pop() { packets.pop(); }


    void parse()
    {
        while (serial->available())
        {
            uint8_t data = serial->read();

            switch(state)
            {
                case WhillXBeeState::Header:
                {
                    clearPacket();
                    if (data == BorderXBeeProtocol::BYTE_HEDADER)
                        state = WhillXBeeState::ID;
                    break;
                }
                case WhillXBeeState::ID:
                {
                    buffer.id = data;
                    state = WhillXBeeState::DATA;
                    break;
                }
                case WhillXBeeState::DATA:
                {
                    if ((buffer.id > 0) && (buffer.id < 8)) // for additional omni
                    {
                        if (data == BorderXBeeProtocol::BYTE_FOOTER)
                        {
                            if (buffer.size != 7)
                            {
                                Serial.print("VEL CMD recv data size error!!!! size = ");
                                Serial.println(buffer.size);
                                ++error_count;
                            }
                            else
                            {
                                for (uint8_t i = 0; i < 6; ++i) buffer.sum += buffer.data[i];
                                if (buffer.sum == buffer.data[buffer.size - 1])
                                {
                                    calcurateVel();
                                    packets.push(buffer);
                                }
                                else
                                {
                                    ++error_count;
                                }
                            }
                            clearPacket();
                            break;
                        }
                        buffer.data[buffer.size] = data;
                        ++buffer.size;
                    }
                    else if (buffer.id == 253) // change status report mode
                    {
                        if (data == BorderXBeeProtocol::BYTE_FOOTER)
                        {
                            if (buffer.size != 1)
                            {
                                Serial.print("STATUS RPT recv data size error!!!! size = ");
                                Serial.println(buffer.size);
                                ++error_count;
                            }
                            else
                            {
                                status_report_duration = buffer.data[0];
                                b_status_report_changed = true;
                            }
                            clearPacket();
                            break;
                        }
                        buffer.data[buffer.size] = data;
                        ++buffer.size;
                    }
                    else
                    {
                        Serial.print("undefined CMD error!!!! CMD ID = ");
                        Serial.println(buffer.id);
                        ++error_count;
                        clearPacket();
                    }
                    break;
                }
                default:
                {
                    clearPacket();
                    break;
                }
            }
        }

    }

private:

    void calcurateVel()
    {
        float spd = (float)(((uint16_t)buffer.data[1] << 8) | (uint16_t)buffer.data[0]); // [mm/s]
        float rad = (float)(((uint16_t)buffer.data[3] << 8) | (uint16_t)buffer.data[2]) / 10000.f; // [rad]
        float rot = ((float)(((uint16_t)buffer.data[5] << 8) | (uint16_t)buffer.data[4]) - 1000.f) / 100.f; // [rad/s]

        // convert coordinate
        rad += HALF_PI;

        buffer.vel[0] = spd * cos(rad); // [mm/s]
        buffer.vel[1] = spd * sin(rad); // [mm/s]
        buffer.vel[2] = rot; // [rad/s]
    }

    void clearPacket() { buffer.clear(); state = WhillXBeeState::Header; }

    Stream* serial;
    WhillXBeeData buffer;
    std::queue<WhillXBeeData> packets;
    WhillXBeeState state;
    uint32_t error_count;

    bool b_status_report_changed {false};
    uint8_t status_report_duration;
};


class BorderXBeePacker
{
public:

    void pack(uint8_t id, uint8_t battery_l, uint8_t battery_r, uint8_t error_l, uint8_t error_r)
    {
        buffer[0] = BorderXBeeProtocol::BYTE_HEDADER;
        buffer[1] = id;
        buffer[2] = battery_l;
        buffer[3] = battery_r;
        buffer[4] = error_l;
        buffer[5] = error_r;
        buffer[6] = BorderXBeeProtocol::BYTE_FOOTER;
    }

    uint8_t* data() { return buffer.data(); }
    uint8_t size() { return buffer.size(); }

private:

    std::array<uint8_t, 7> buffer;
};

#endif // BORDERXBEEPACKETIZER_H
