#include "CommandParser.hpp"
#include "UartHandler.hpp"
#include "StateMachine.hpp"

CommandParser &CommandParser::getInstance() noexcept
{
    static CommandParser instance;
    return instance;
}

CommandParser::Command CommandParser::parsePacket(const uint8_t *packet) noexcept
{
    Command command;
    command.instruction = (Command::TYPE)packet[0];
    // Fetch args by big endian byte ordering
    command.args[0] = ((int16_t)packet[1] << 8) | packet[2];
    command.args[1] = ((int16_t)packet[3] << 8) | packet[4];

    // Validate instruction
    if ((uint8_t)command.instruction > 0x02) {
        StateMachine::getInstance().raiseError("Received unknown instruction");
    }

    return command;
}