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

#include "FreeRTOS.h"
#include "task.h"

void HeadSystem::setChopper(bool open1, bool open2) {
    unsigned setpoint1 = open1 ? CHOPPER1_OPEN : CHOPPER1_CLOSE;
    unsigned setpoint2 = open2 ? CHOPPER2_OPEN : CHOPPER2_CLOSE;

    setpoint1 = PWM_INVERT(setpoint1);
    setpoint2 = PWM_INVERT(setpoint2);

    if(MOTOR_CHOPPER1) {
        pwm_set_gpio_level(MOTOR_CHOPPER2_PIN, setpoint1);
    } else {
        pwm_set_gpio_level(MOTOR_CHOPPER1_PIN, setpoint1);
    }

    if(MOTOR_CHOPPER2) {
        pwm_set_gpio_level(MOTOR_CHOPPER2_PIN, setpoint2);
    } else {
        pwm_set_gpio_level(MOTOR_CHOPPER1_PIN, setpoint2);
    }
}

STONE_STATE HeadSystem::dropStone() {

    if(!gpio_get(SENSOR_STONE_PIN)) {
        setChopper(true, false);
        vTaskDelay(250 / portTICK_PERIOD_MS);
        setChopper(false, false);
        vTaskDelay(100 / portTICK_PERIOD_MS);

        if(!gpio_get(SENSOR_STONE_PIN)) {
            return EMPTY;
        }
    }

    setChopper(false, true);
    vTaskDelay(250 / portTICK_PERIOD_MS);
    setChopper(false, false);
    vTaskDelay(100 / portTICK_PERIOD_MS);
    setChopper(true, false);
    vTaskDelay(250 / portTICK_PERIOD_MS);
    setChopper(false, false);
    vTaskDelay(100 / portTICK_PERIOD_MS);

    
    return gpio_get(SENSOR_STONE_PIN) ? FULL : LOW;
}

bool HeadSystem::preLoad() {
    if(gpio_get(SENSOR_STONE_PIN)) {
        return true;
    }
    
    setChopper(true, false);
    vTaskDelay(250 / portTICK_PERIOD_MS);
    setChopper(false, false);
    vTaskDelay(100 / portTICK_PERIOD_MS);

    return gpio_get(SENSOR_STONE_PIN);
}