#include "include/io.h"

#include <pico/stdlib.h>
#include <time.h>
#include <hardware/pwm.h>
#include "include/gyro.h"

void motor_control_activated(bool activated)
{
    const uint SLICE_NUM = pwm_gpio_to_slice_num(PIN_MOTOR1);

    pwm_set_enabled(SLICE_NUM, activated);
}

static void init_tirette_and_color(void)
{
    gpio_init(PIN_TIRETTE);
    gpio_init(PIN_BUTTON_COLOR);

    gpio_set_dir(PIN_TIRETTE, GPIO_OUT);
    gpio_set_dir(PIN_BUTTON_COLOR, GPIO_OUT);

    gpio_pull_up(PIN_TIRETTE);
    gpio_pull_up(PIN_BUTTON_COLOR);

}

static void init_motors(void)
{
    gpio_set_function(PIN_MOTOR1, GPIO_FUNC_PWM);
    gpio_set_function(PIN_MOTOR2, GPIO_FUNC_PWM);

    const uint SLICE_NUM = pwm_gpio_to_slice_num(PIN_MOTOR1);
    pwm_set_wrap(SLICE_NUM, 4096);

    pwm_set_gpio_level(PIN_MOTOR1, 0);
    pwm_set_gpio_level(PIN_MOTOR2, 0);

    pwm_set_enabled(SLICE_NUM, false);
}

static void init_servo(void)
{
    const uint SLICE_NUM = pwm_gpio_to_slice_num(PIN_SERVO);

    gpio_set_function(PIN_SERVO, GPIO_FUNC_PWM);

    pwm_set_wrap(SLICE_NUM, 25000);
    pwm_set_clkdiv(SLICE_NUM, 100);

    pwm_set_gpio_level(PIN_SERVO, 12500);

    pwm_set_enabled(SLICE_NUM, false);
}

static void init_i2c(void)
{
    gpio_set_function(I2C_MASTER_SDA_PIN, GPIO_FUNC_I2C);
    gpio_set_function(I2C_MASTER_SCL_PIN, GPIO_FUNC_I2C);

    gpio_pull_up(I2C_MASTER_SDA_PIN);
    gpio_pull_up(I2C_MASTER_SCL_PIN);

    i2c_init(I2C_MASTER_INSTANCE, 100 * 1000);
}

static void init_screen(void)
{
    io.screen->external_vcc=false;
    ssd1306_init(io.screen, 128, 64, 0x3C, i2c1);
    ssd1306_clear(io.screen);
    ssd1306_show(io.screen);
}

void init_io(void)
{
    init_tirette_and_color();

    init_i2c();

    init_gyro();
    gyro_calibrate();

    init_motors();
    init_servo();
}

static void update_motion_control(void)
{
    float actual_angle = io.gyro_data.z_angle;
    float error = io.target_dir - actual_angle;

    float correction = error * GAIN_KD;

    int m1_speed = io.target_speed + correction;
    int m2_speed = io.target_speed - correction;

    if(m1_speed < 0)
    {
      m1_speed = 0;
    }
    else if(m2_speed < 0)
    {
      m2_speed = 0;
    }

    pwm_set_gpio_level(PIN_MOTOR1, m1_speed);
    pwm_set_gpio_level(PIN_MOTOR2, m2_speed);
}

static void update_dancing_action(void)
{
    static double prev_time = 0;
    static double delta_time = 0;
    static int actual_angle = 87;
    static int8_t actual_dir = -1;

    delta_time += io.time_ms - prev_time;

    if(delta_time >= 10)
    {
      delta_time = 0;

      actual_angle += actual_dir * DANCING_ACTION_DELTA_ANGLE;
    }

    if(actual_angle <= 43)
    {
      actual_dir = 1;
    }
    else if(actual_angle >= 130)
    {
      actual_dir = -1;
    }

    pwm_set_gpio_level(PIN_SERVO, actual_angle);

    prev_time = io.time_ms;
}

void update_io(void)
{
    io.time_ms = (double)clock() * 1000.0 / CLOCKS_PER_SEC;

    static double last_color_change_time = 0;
    static bool has_color_changed = true;
    static bool is_color_blue = true;

    if(io.is_color_blue != is_color_blue)
    {
        ssd1306_clear(io.screen);

        if(is_color_blue)
        {
            ssd1306_draw_string(io.screen, 0, 20, 1, "Blue\nTeam");
        }
        else
        {
            ssd1306_draw_string(io.screen, 0, 20, 1, "Yellow\nTeam");
        }

        last_color_change_time = io.time_ms;

        has_color_changed = true;
    }
    /*
    if(io.time_ms - last_color_change_time > 1000 && has_color_changed)
    {
        ssd1306_clear(io.screen);
        m_screen.drawBitmap(38, 0, Res::Imgs::riombotique, 52, 64, WHITE);
        //m_screen.drawBitmap(75, 0, Imgs::poivron_robotique, 52, 64, WHITE);
        //m_screen.drawBitmap(27, 0, Res::Imgs::diable_gaga, 75, 64, WHITE);

        has_color_changed = false;
    }
    */

    io.is_tirette_pulled = gpio_get(PIN_TIRETTE);
    io.is_color_blue = is_color_blue;

    is_color_blue = !gpio_get(PIN_BUTTON_COLOR);

    if(io.motion_control_activated)
    {
        update_motion_control();
    }

    if(io.is_dancing)
    {
        update_dancing_action();
    }

    ssd1306_show(io.screen);
}

void set_dir_with_angular_speed(float dir)
{}

void io_deinit(void)
{}
