#include "headSystem.hpp"
#include "pico/stdlib.h"
#include "hardware/pwm.h"

#include "FreeRTOS.h"
#include "queue.h"
#include "semphr.h"

HeadSystem::HeadSystem() {
    initPins();
}

void vHeadTaskFn(void *pvParameters) {
    HeadSystem * headSystem = (HeadSystem *) pvParameters;
    headSystem->vHeadTask();
}

void HeadSystem::initPins() {
    stoneState = EMPTY;

    headStateQueue = xQueueCreate(3, sizeof(bool)); 
    headSateSemaphore = xSemaphoreCreateBinary();
    xTaskCreate(vHeadTaskFn, "Head Task", 1024, this, 1, &headTaskHandle);

    gpio_init(SENSOR_STONE_PIN);
    gpio_set_dir(SENSOR_STONE_PIN, GPIO_IN);
    gpio_pull_up(SENSOR_STONE_PIN);

    gpio_init(SENSOR_Z_AXIS_PIN);
    gpio_set_dir(SENSOR_Z_AXIS_PIN, GPIO_IN);
    gpio_pull_down(SENSOR_Z_AXIS_PIN);

    gpio_init(LED1_PIN);
    gpio_set_dir(LED1_PIN, GPIO_OUT);
    gpio_init(LED2_PIN);
    gpio_set_dir(LED2_PIN, GPIO_OUT);

    gpio_set_function(MOTOR_CHOPPER1_PIN, GPIO_FUNC_PWM);
    gpio_set_function(MOTOR_CHOPPER2_PIN, GPIO_FUNC_PWM);
    gpio_set_function(MOTOR_Z_AXIS_PIN, GPIO_FUNC_PWM);

    gpio_init(BUTTON_PIN);
    gpio_set_dir(BUTTON_PIN, GPIO_IN);
    gpio_pull_up(BUTTON_PIN);

    motor_chopper1_slice_num = pwm_gpio_to_slice_num(MOTOR_CHOPPER1_PIN);
    motor_chopper2_slice_num = pwm_gpio_to_slice_num(MOTOR_CHOPPER2_PIN);
    motor_z_axis_slice_num = pwm_gpio_to_slice_num(MOTOR_Z_AXIS_PIN);

    pwm_config config = pwm_get_default_config();
    pwm_config_set_wrap(&config, PWM_TOTAL);
    pwm_config_set_clkdiv(&config, PWM_CLK_DIV);

    pwm_init(motor_chopper1_slice_num, &config, true);
    pwm_init(motor_chopper2_slice_num, &config, true);
    pwm_init(motor_z_axis_slice_num, &config, true);

    pwm_set_gpio_level(MOTOR_CHOPPER1_PIN, PWM_INVERT(MOTOR_MS18_MID));
    pwm_set_gpio_level(MOTOR_CHOPPER2_PIN, PWM_INVERT(MOTOR_MS18_MID));
    pwm_set_gpio_level(MOTOR_Z_AXIS_PIN, PWM_INVERT(MOTOR_MS18_MID));

    setChopper(false, false);
}