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

using namespace Whill;

void Packer::power(bool b)
{
    SW sw = b ? SW::ENABLE : SW::DISABLE;
    pack((char)CMD::PWR_WHILL, (char)sw);
}

void Packer::powerSupply(bool b)
{
    SW sw = b ? SW::ENABLE : SW::DISABLE;
    pack((char)CMD::PWR_SUPPLY, (char)sw);
}

void Packer::controlEnable(bool b)
{
    USR enable = b ? USR::DISABLE : USR::ENABLE;
    pack((char)CMD::JOYSTICK, (char)enable, 0, 0);
}

void Packer::joystick(int8_t fb, int8_t lr)
{
    pack((char)CMD::JOYSTICK, (char)USR::DISABLE, (char)fb, (char)lr);
}

void Packer::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)
{
    pack (
        (char)CMD::SPEED, (char)mode,
        f_m1, f_a1, f_d1,
        r_m1, r_a1, r_d1,
        t_m1, t_a1, t_d1
    );
}

void Packer::streaming(bool b, uint16_t interval, DATASET n, SPEEDMODE mode)
{
    if (b) pack((char)CMD::STREAMING_START, (char)n, (char)((interval >> 8) & 0xFF), (char)(interval & 0xFF), (char)mode);
    else   pack((char)CMD::STREAMING_STOP);
}
