#include "vacum.hpp"
#include "pinConfigNode.hpp"

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

#include "pico/stdlib.h"

void vInterruptionTaskFn(void *pvParameters) {
    VacumControl *vacumControl = (VacumControl *)pvParameters;
    vacumControl->interruptionTaskFn();
}

void vButtonTaskFn(void *pvParameters) {
    VacumControl *vacumControl = (VacumControl *)pvParameters;
    vacumControl->buttonTaskFn();
}

VacumControl::VacumControl(uint coreMask) {
    this->coreMask = coreMask;
    initVacumPins();
    state = false;
    vacumMutex = xSemaphoreCreateMutex();
    interruptionSemaphore = xSemaphoreCreateBinary();
    xTaskCreateAffinitySet(vInterruptionTaskFn, "Interruption Task", 1024, this, 3, coreMask, &interruptionTask);
    xTaskCreateAffinitySet(vButtonTaskFn, "Button Task", 1024, this, 2, coreMask, &buttonTask);
}

void VacumControl::initVacumPins() {
    gpio_init(LED1_PIN);
    gpio_set_dir(LED1_PIN, GPIO_OUT);
    gpio_put(LED1_PIN, 0);

    gpio_init(LED2_PIN);
    gpio_set_dir(LED2_PIN, GPIO_OUT);
    gpio_put(LED2_PIN, 0);

    gpio_init(RELAY_PIN);
    gpio_set_dir(RELAY_PIN, GPIO_OUT);
    gpio_put(RELAY_PIN, 0);

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

void VacumControl::setState(bool on) {
    xSemaphoreTake(vacumMutex, portMAX_DELAY);
    if(on) {
        gpio_put(RELAY_PIN, 1);
        gpio_put(LED1_PIN, 1);
        state = true;
        xSemaphoreGive(interruptionSemaphore);
    } else {
        gpio_put(RELAY_PIN, 0);
        gpio_put(LED1_PIN, 0);
        state = false;
    }
    xSemaphoreGive(vacumMutex);
}

void VacumControl::interruptionTaskFn() {
    while(true) {
        xSemaphoreTake(interruptionSemaphore, portMAX_DELAY);
        if(state) {
            while(state) {
                vTaskDelay(INTERRUPTION_DELAY / portTICK_PERIOD_MS);               
                if(state) {
                    gpio_put(RELAY_PIN, 0);
                    gpio_put(LED1_PIN, 0);
                } else
                    break;

                vTaskDelay(INTERRUPTION_DURATION / portTICK_PERIOD_MS);
                if(state) {
                    gpio_put(RELAY_PIN, 1);
                    gpio_put(LED1_PIN, 1);
                }
            }
        }
    }
}

void VacumControl::buttonTaskFn() {
    bool buttonLastState = gpio_get(BUTTON_PIN);

    while(true) {
        bool buttonState = gpio_get(BUTTON_PIN);

        if(!buttonState && buttonLastState) {
            setState(true);
            vTaskDelay(250 / portTICK_PERIOD_MS);
        } else if(buttonState && !buttonLastState) {
            setState(false);
        }

        buttonLastState = buttonState;
        vTaskDelay(250 / portTICK_PERIOD_MS);
    }
}