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

#include "FreeRTOS.h"

void HeadSystem::vHeadTask() {
    while (1) {
        bool new_state;
        xQueueReceive(headStateQueue, &new_state, portMAX_DELAY);
        
        // Units in milliseconds
        const int READ_DELAY = 50;
        const int MAX_TIME = 7000;

        if(headUp != new_state) {
            if(new_state && !Z_AXIS_IS_UP()) { // Head Up

                pwm_set_gpio_level(MOTOR_Z_AXIS_PIN, PWM_INVERT(Z_AXIS_UP));
                vTaskDelay(100 / portTICK_PERIOD_MS);

                for(int i=0; i<MAX_TIME / READ_DELAY; i++) {
                    
                    if(Z_AXIS_IS_UP())
                        break;

                    vTaskDelay(READ_DELAY / portTICK_PERIOD_MS);
                }

                pwm_set_gpio_level(MOTOR_Z_AXIS_PIN, PWM_INVERT(Z_AXIS_OFF));

        } else if(!new_state && Z_AXIS_IS_UP()) { // Head Down
                pwm_set_gpio_level(MOTOR_Z_AXIS_PIN, PWM_INVERT(Z_AXIS_DOWN));
                vTaskDelay(Z_AXIS_DOWN_TIME / portTICK_PERIOD_MS);
                pwm_set_gpio_level(MOTOR_Z_AXIS_PIN, PWM_INVERT(Z_AXIS_OFF));
            }

            headUp = new_state;
        }

        xSemaphoreGive(headSateSemaphore);
    }
}

void HeadSystem::setHeadUp(bool up) {
    bool s = up;
    xQueueSend(headStateQueue, &s, portMAX_DELAY);
    xSemaphoreTake(headSateSemaphore, portMAX_DELAY);
}