#pragma once
#ifndef WHILL_PROTOCOL_PACKER_H
#define WHILL_PROTOCOL_PACKER_H

#include <cstdint>
#include <deque>
#include <vector>
#include "WhillCommon.h"

WHILL_NAMESPACE_BEGIN

class Packer
{
public:

    const char* data() { return packet.data(); }
    const char size() { return packet.size(); }
    void clear() { packet.clear(); } // TODO: smarter way...

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

    // fb, lr : -100 - +100
    void joystick(int8_t fb, int8_t lr);

    // mode : SPEEDMODE { JOYSTICK_1, JOYSTICK_2, JOYSTICK_3, JOYSTICK_4, RS232C, IPHONE };
    // f_m1 : max speed    at forward movement. range is ( 8 -  80), unit of value is 0.1km/h, special range for RZM, less than 60 is better
    // f_a1 : acceleration at forward movement. range is (10 -  90)
    // f_d1 : deceleration at forward movement. range is (40 - 160)
    // r_m1 : max speed    at reverse movement. range is ( 8 -  80), unit of value is 0.1km/h, special range for RZM, less than 60 is better
    // r_a1 : acceleration at reverse movement. range is (10 -  50)
    // r_d1 : deceleration at reverse movement. range is (40 -  80)
    // t_m1 : max speed    at turn    movement. range is ( 8 -  80), unit of value is 0.1km/h, special range for RZM, less than 60 is better
    // t_a1 : acceleration at turn    movement. range is (10 -  60)
    // t_d1 : deceleration at turn    movement. range is (40 - 160)
    void 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);

    void streaming(bool b, uint16_t interval, DATASET n, SPEEDMODE mode);

private:

    enum class CMD { STREAMING_START = 0, STREAMING_STOP, PWR_WHILL, JOYSTICK, SPEED, PWR_SUPPLY };
    enum class USR { DISABLE = 0, ENABLE };
    enum class SW  { DISABLE = 0, ENABLE };

    template <typename First, typename... Rest>
    void pack(First&& first, Rest&&... rest)
    {
        cmd.push_back(std::move(first));
        pack(std::forward<Rest>(rest)...);
    }

    void pack()
    {
        char checksum = 0;

        packet.emplace_back(PROTOCOL_SIGN);
        checksum ^= packet.back();
        packet.emplace_back(cmd.size() + 1); // length of data + 1(=checksum)
        checksum ^= packet.back();

        for(uint8_t i = 0; i < cmd.size(); i++)
        {
            packet.emplace_back(cmd[i]);
            checksum ^= packet.back();
        }
        packet.emplace_back(checksum);

        cmd.clear();
    }

    std::deque<char> cmd;
    std::vector<char> packet;
};


WHILL_NAMESPACE_END

#endif // WHILL_PROTOCOL_PACKER_H
