#include <stdio.h>
#include "pico/stdlib.h"

#include "FreeRTOSConfig.h"
#include "FreeRTOS.h"
#include "task.h"
#include "hardware/pwm.h"

#include "headSystem.hpp"

void vMainTask(void *pvParameters) {
    HeadSystem * headSystem = (HeadSystem *) pvParameters;
    bool buttonState = false;
    bool buttonLastState = false;

    headSystem->setHeadUp(true);
    
    while (1) {
        buttonState = !gpio_get(BUTTON_PIN);

        if(buttonState) {
            STONE_STATE s = headSystem->dropSequence();
            printf("Stone dropped: %d\n", s);
        }

        buttonLastState = buttonState;
        vTaskDelay(pdMS_TO_TICKS(100));
    }
}

int main()
{
    stdio_init_all();

    HeadSystem headSystem;
    printf("HeadSystem initialized\n");

    xTaskCreate(vMainTask, "Button Task", 1024, &headSystem, 1, NULL);
    vTaskStartScheduler();

    while (1) {
        tight_loop_contents();
    }
    
}