#pragma once
#include "pinConfigNode.hpp"
#include "FreeRTOS.h"
#include "queue.h"
#include "semphr.h"

enum STONE_STATE {
    EMPTY = 0b00, FULL = 0b10, LOW = 0b01
};

void vHeadTaskFn(void *pvParameters);

class HeadSystem {
private:
    unsigned int motor_chopper1_slice_num;
    unsigned int motor_chopper2_slice_num;
    unsigned int motor_z_axis_slice_num;

    bool headUp = false;

    STONE_STATE stoneState;
    xQueueHandle headStateQueue;
    xSemaphoreHandle headSateSemaphore;
    xTaskHandle headTaskHandle;

public:
    HeadSystem();

    void initPins();

    void setChopper(bool open1, bool open2);
    void setHeadUp(bool up);

    bool preLoad();
    STONE_STATE dropStone();
    
    STONE_STATE dropSequence();

    void vHeadTask();  
};