#include <stdio.h>
#include "drv8701.h"
#include "freertos/idf_additions.h"
#include "portmacro.h"
#include "soc/gpio_num.h"
#include "xbox_controller.h"


drv8701_task_config_t motor1 = {
    .ph_pin = GPIO_NUM_13,
    .en_pin = GPIO_NUM_14,
    .pwm_group_num = 0,
};
drv8701_task_config_t motor2 = {
    .ph_pin = GPIO_NUM_21,
    .en_pin = GPIO_NUM_45,
    .pwm_group_num = 0,
};
drv8701_task_config_t motor3 = {
    .ph_pin = GPIO_NUM_9,
    .en_pin = GPIO_NUM_10,
    .pwm_group_num = 1,
};
drv8701_task_config_t motor4 = {
    .ph_pin = GPIO_NUM_11,
    .en_pin = GPIO_NUM_12,
    .pwm_group_num = 1,
};

void app_main(void)
{
    printf("Starting\n");

    init_controller();

    xTaskCreate(drv8701_task, "Motor1 Task", 2048, (void *)&motor1, 4, NULL);
    xTaskCreate(drv8701_task, "Motor2 Task", 2048, (void *)&motor2, 4, NULL);
    xTaskCreate(drv8701_task, "Motor3 Task", 2048, (void *)&motor3, 4, NULL);
    xTaskCreate(drv8701_task, "Motor4 Task", 2048, (void *)&motor4, 4, NULL);
    
    vTaskDelay(500 / portTICK_PERIOD_MS);
    float motor1_vel = 0.0;
    while(true) {
        print_state();
        motor1_vel = motor1_vel + 0.1;
        if (motor1_vel > 1.0)
            motor1_vel = -1.0;
        printf("Motor1 velocity: %0.2f\n", motor1_vel);
        set_motor_vel(&motor1, motor1_vel);
        set_motor_vel(&motor2, motor1_vel);
        set_motor_vel(&motor3, motor1_vel);
        set_motor_vel(&motor4, motor1_vel);
        vTaskDelay(1000 / portTICK_PERIOD_MS);
    }
}