#include "freertos/idf_additions.h"
#include "freertos/projdefs.h"
#include "freertos/semphr.h"
#include "portmacro.h"
#include <driver/gpio.h>
#include <esp_rom_gpio.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <inttypes.h>
#include <stdio.h>

#include "tasks.h"

QueueHandle_t blink_cmd_q;

void blink_cmd_print(const BlinkCmd_t *cmd) {
    printf("BlinkCmd { ");

    switch (cmd->variant) {
    case BLINK_RATE:
        printf("variant: BLINK_RATE, ");
        printf("rate: %" PRIu32, cmd->rate);
        break;
    case BLINK_STATE:
        printf("variant: BLINK_STATE, ");
        printf("state: %s", cmd->state ? "ON" : "OFF");
        break;
    default:
        printf("variant: UNKNOWN (%d)", cmd->variant);
        break;
    }

    printf(" }\n");
}

void task_blink(void *pvParams) {
    esp_rom_gpio_pad_select_gpio(LED_PIN);
    gpio_set_direction(LED_PIN, GPIO_MODE_OUTPUT);

    u32 level = 1; // For blinking

    // Set this to some initial value
    BlinkCmd_t command = {
        .variant = BLINK_RATE,
        .rate = 800,
    };

    if (pvParams != NULL) {
        command = *(BlinkCmd_t *)pvParams;
        printf("Got initial configuration for leds: ");
        blink_cmd_print(&command);
    }

    while (1) {
        if (pdPASS == xQueueReceive(blink_cmd_q, &command, 0))
            blink_cmd_print(&command);

        switch (command.variant) {
        case BLINK_RATE:
            level = !level;
            gpio_set_level(LED_PIN, level);
            vTaskDelay(command.rate / portTICK_PERIOD_MS);
            break;
        case BLINK_STATE:
            gpio_set_level(LED_PIN, command.state);
            vTaskDelay(pdMS_TO_TICKS(20));
            break;
        default:
            break;
        }
    }
}

void task_blink_cycle(void *pvParams) {
    BlinkCmd_t cmd = {
        .variant = BLINK_RATE,
        .rate = 1000,
    };

    vTaskDelay(pdMS_TO_TICKS(2000));

    if (xQueueSend(blink_cmd_q, &cmd, pdMS_TO_TICKS(200)) != pdPASS) {
        printf("Error sending blink command...");
    }

    vTaskDelay(pdMS_TO_TICKS(2000));

    cmd.rate = 50;
    if (xQueueSend(blink_cmd_q, &cmd, pdMS_TO_TICKS(200)) != pdPASS) {
        printf("Error sending blink command...");
    }

    vTaskDelay(pdMS_TO_TICKS(2000));

    cmd.rate = 25;
    if (xQueueSend(blink_cmd_q, &cmd, pdMS_TO_TICKS(200)) != pdPASS) {
        printf("Error sending blink command...");
    }

    vTaskDelay(pdMS_TO_TICKS(2000));

    cmd.rate = 500;
    if (xQueueSend(blink_cmd_q, &cmd, pdMS_TO_TICKS(200)) != pdPASS) {
        printf("Error sending blink command...");
    }

    vTaskDelay(pdMS_TO_TICKS(2000));

    cmd.variant = BLINK_STATE;
    cmd.state = 1;
    if (xQueueSend(blink_cmd_q, &cmd, pdMS_TO_TICKS(200)) != pdPASS) {
        printf("Error sending blink command...");
    }

    vTaskDelay(pdMS_TO_TICKS(2000));

    cmd.variant = BLINK_RATE;
    cmd.rate = 200;
    if (xQueueSend(blink_cmd_q, &cmd, pdMS_TO_TICKS(200)) != pdPASS) {
        printf("Error sending blink command...");
    }

    vTaskDelay(pdMS_TO_TICKS(2000));

    cmd.variant = BLINK_STATE;
    cmd.state = 0;
    if (xQueueSend(blink_cmd_q, &cmd, pdMS_TO_TICKS(200)) != pdPASS) {
        printf("Error sending blink command...");
    }

    vTaskDelete(NULL);
}

void vPeriodicTask(void *pvParameters) {
    TickType_t       xLastWakeTime;
    const TickType_t xDelay3ms = pdMS_TO_TICKS(2000);

    /*
     * The xLastWakeTime variable needs to be initialized with the current tick
     * count. Note that this is the only time the variable is explicitly
     * written to. After this xLastWakeTime is managed automatically by the
     * vTaskDelayUntil() API function.
     */
    xLastWakeTime = xTaskGetTickCount();

    char *taskname = pcTaskGetName(NULL);

    /* As per most tasks, this task is implemented in an infinite loop. */
    for (;;) {
        /* Print out the name of this task. */
        printf("%s - %" PRIu32 ": Running...\n", taskname, pdTICKS_TO_MS(xLastWakeTime));

        /*
         * The task should execute every 3 milliseconds exactly – see the
         * declaration of xDelay3ms in this function.
         */
        vTaskDelayUntil(&xLastWakeTime, xDelay3ms);
    }
}

#define BUTTON_GPIO GPIO_NUM_0
SemaphoreHandle_t button_sem;

static void IRAM_ATTR button_isr_handler(void *arg) {
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    xSemaphoreGiveFromISR(button_sem, &xHigherPriorityTaskWoken);
    if (xHigherPriorityTaskWoken) {
        portYIELD_FROM_ISR(); // Request context switch if needed
    }
}

void button_task(void *pvParams) {
    while (1) {
        if (xSemaphoreTake(button_sem, portMAX_DELAY)) {
            printf("Button pressed (from ISR)\n");
        }
    }
}

void task_gpio_demo(void *pvParams) {
    button_sem = xSemaphoreCreateBinary();

    // Setup GPIO
    gpio_config_t io_conf = {
        .intr_type = GPIO_INTR_NEGEDGE,
        .mode = GPIO_MODE_INPUT,
        .pin_bit_mask = (1ULL << BUTTON_GPIO),
        .pull_up_en = 1,
    };
    gpio_config(&io_conf);

    // Install ISR handler
    gpio_install_isr_service(0);
    gpio_isr_handler_add(BUTTON_GPIO, button_isr_handler, NULL);

    // Start the task
    xTaskCreate(button_task, "button_task", 2048, NULL, 10, NULL);

    vTaskDelete(NULL);
}