#include "MainLoop.hpp"
#include "StateMachine.hpp"
#include "CommandParser.hpp"

MainLoop::MainLoop() noexcept:
    pwmBoard(0x40, 60),
    m0(
        pwmBoard,
        0,
        1.0,
        1.0,
        135.0,
        -135.0,
        45.0,
        180.0,
        0,
        172,
        565
    )
{
    // Initialize state machine singleton
    StateMachine::getInstance();
    //m0.setScaleCalibration(RangedMotor::calculateScaleCalibration(90, 95));
}

MainLoop::~MainLoop() noexcept
{
    UartHandler::getInstance().close();
}

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

void MainLoop::setup() noexcept
{
    UartHandler::getInstance().init(9600);
    m0.commitMoveImmediately();
}

void MainLoop::update() noexcept
{
    UartHandler& uart = UartHandler::getInstance();
    uart.poll();

    while(uart.getNumAvailablePackets()) {
        uint8_t packet[16];
        uart.getNextPacket(packet);
        CommandParser::Command command = CommandParser::getInstance().parsePacket(packet);

        if (command.instruction == CommandParser::Command::TYPE::MOVE_ABSOLUTE) {
            const float moveTarget = (float)command.args[1] / 10.0f;
            m0.moveTo(moveTarget);
        }
    }
}