#include "include/inputs.h"

#include <hardware/adc.h>
#include "include/controller.h"

#include <stdio.h>

const button_def_t BUTTONS_DEFS[NB_BUTTONS] = {
    {13},
    {9},
    {1},
    {5},
    {6},
    {2}
};

void inputs_init(void)
{
    adc_init();
    adc_gpio_init(JOYSTICK_X_AXIS_PIN);
    adc_gpio_init(JOYSTICK_Y_AXIS_PIN);

    for(buttons_t actual_button = BUTTON_L; actual_button < NB_BUTTONS; actual_button++)
    {
        const button_def_t *BUTTON_DEF = &BUTTONS_DEFS[actual_button];

        gpio_init(BUTTON_DEF->pin);
        gpio_set_dir(BUTTON_DEF->pin, GPIO_OUT);
        gpio_pull_up(BUTTON_DEF->pin);

        controller.inputs_buffer.buttons_states[actual_button] = 0;
    }

    controller.inputs_buffer.x_axis_speed = 0;
    controller.inputs_buffer.y_axis_speed = 0;
    controller.inputs_buffer.robot_angle = 0;
}

void get_inputs(void)
{
    adc_select_input(JOYSTICK_X_AXIS_ADC_INPUT);
    uint16_t joystick_x_axis = adc_read();

    adc_select_input(JOYSTICK_Y_AXIS_ADC_INPUT);
    uint16_t joystick_y_axis = adc_read();

    controller.inputs_buffer.x_axis_speed = (joystick_x_axis / 16) - 128;
    controller.inputs_buffer.y_axis_speed = (joystick_y_axis / 16) - 128;

    for(buttons_t actual_button = BUTTON_L; actual_button < NB_BUTTONS; actual_button++)
    {
        const button_def_t *BUTTON_DEF = &BUTTONS_DEFS[actual_button];

        controller.inputs_buffer.buttons_states[actual_button] = gpio_get(BUTTON_DEF->pin);
    }

    if(controller.inputs_buffer.buttons_states[BUTTON_L])
        controller.inputs_buffer.robot_angle += controller.delta_time_ms / 1000.0f * ANGLE_PER_SECOND;

    if(controller.inputs_buffer.buttons_states[BUTTON_R])
        controller.inputs_buffer.robot_angle -= controller.delta_time_ms / 1000.0f * ANGLE_PER_SECOND;
}