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

MainLoop::MainLoop() noexcept:
    pwmBoard(0x40, 60),
    motors({
        AdafruitServoMotor(
            pwmBoard,
            0,
            1.0,
            1.0,
            135.0,
            -135.0,
            45.0,
            180.0,
            0,
            172,
            565
        ),
        AdafruitServoMotor(
            pwmBoard,
            1,
            1.0,
            1.0,
            38.0,
            -38.0,
            85.0,
            180.0,
            50,
            172,
            565
        ),
        AdafruitServoMotor(
            pwmBoard,
            2,
            1.0,
            1.0,
            123.0,
            -123.0,
            57.0,
            180.0,
            -123,
            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);
    for (uint8_t i = 0; i < 3; i++) {
        motors[i].commitMoveImmediately();
    }

    StateMachine::getInstance().setState(StateMachine::STATE::IDLING);
}

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

    switch (StateMachine::getInstance().getState()) {
        case StateMachine::STATE::IDLING:
        case StateMachine::STATE::MOVING:
            while(uart.getNumAvailablePackets()) {
            uint8_t packet[16];
            uart.getNextPacket(packet);
            CommandParser::Command command = CommandParser::getInstance().parsePacket(packet);

            switch (command.instruction) {
                // Scoping to keep variables local
                case CommandParser::Command::TYPE::MOVE_ABSOLUTE: {
                    AdafruitServoMotor& motor = motors[command.args[0]];
                    const float moveTarget = (float)command.args[1] / 10.0f;
                    motor.moveTo(moveTarget);
                }
                break;
                case CommandParser::Command::TYPE::MOVE_RELATIVE: {
                    AdafruitServoMotor& motor = motors[command.args[0]];
                    const float moveDelta = (float)command.args[1] / 10.0f;
                    motor.moveTo(motor.getPosition() + moveDelta);
                }
                break;
                case CommandParser::Command::TYPE::RESET_POSITIONS: {
                    for (uint8_t i = 0; i < 3; i++) {
                        motors[i].moveTo(0);
                        motors[i].commitMoveImmediately();
                    }
                }
                break;
            }

        }
    break;
    case StateMachine::STATE::FAULT:
    default:
        // Freeze in place until error clears or device reset
        yield();
        break;
    }

}