#include <stdio.h>
#include <stdlib.h>
#include <driver/gpio.h>
// #include "esp_err.h"
// #include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

#define LED_BLUE_PIN GPIO_NUM_21
#define LED_RED_PIN GPIO_NUM_33

#define BUTTON_BLUE_PIN GPIO_NUM_11
#define BUTTON_RED_PIN GPIO_NUM_12

#define BUTTON_BLUE_ACK_PIN GPIO_NUM_14
#define BUTTON_RED_ACK_PIN GPIO_NUM_13

#define CONTROL_TASK_BLUE_NAME       "controlTaskBlue"
#define TASK_BLUE_STACK_SIZE (configMINIMAL_STACK_SIZE * 4)
#define TASK_BLUE_PRIORITY   (tskIDLE_PRIORITY + 3)

#define CONTROL_TASK_RED_NAME       "controlTaskRed"
#define TASK_RED_STACK_SIZE (configMINIMAL_STACK_SIZE * 4)
#define TASK_RED_PRIORITY   (tskIDLE_PRIORITY + 4)

#define TASK_BLINK_NAME       "blinkTask"
#define TASK_BLINK_STACK_SIZE (configMINIMAL_STACK_SIZE * 4)
#define TASK_BLINK_PRIORITY   (tskIDLE_PRIORITY + 1)

#define TASK_ACK_NAME       "ackTask"
#define TASK_ACK_STACK_SIZE (configMINIMAL_STACK_SIZE * 4)
#define TASK_ACK_PRIORITY   (tskIDLE_PRIORITY + 2)

static void task_blue_button(void *arg);
static void task_red_button(void *arg);
static void task_blink(void *arg);
static void task_ack(void *arg);

static TaskHandle_t xtask_handle_blue = NULL;
static TaskHandle_t xtask_handle_red = NULL;
static TaskHandle_t xtask_handle_blink = NULL;
static TaskHandle_t xtask_handle_ack = NULL;

static uint8_t blink_red = 0;
static uint8_t blink_blue = 0;

typedef struct ack_task_parameters_t {
  gpio_num_t button_ack_blue;
  gpio_num_t button_ack_red;
} ack_task_parameters_t;

typedef struct control_task_parameters_t {
  gpio_num_t button;
} control_task_parameters_t;

static ack_task_parameters_t ack_buttons = {BUTTON_BLUE_ACK_PIN, BUTTON_RED_ACK_PIN};
static control_task_parameters_t blue_button = {BUTTON_BLUE_PIN};
static control_task_parameters_t red_button = {BUTTON_RED_PIN};

void app_main(void)
{
    BaseType_t xReturn = pdPASS;

    xReturn = xTaskCreate(&task_blue_button,
                          CONTROL_TASK_BLUE_NAME,
                          TASK_BLUE_STACK_SIZE,
                          &blue_button,
                          TASK_BLUE_PRIORITY,
                          &xtask_handle_blue);

    if (xReturn != pdPASS || xtask_handle_blue == NULL) {
        printf("Error creating task_blue_button\n");
    }

    xReturn = xTaskCreate(&task_red_button,
                          CONTROL_TASK_RED_NAME,
                          TASK_RED_STACK_SIZE,
                          &red_button,
                          TASK_RED_PRIORITY,
                          &xtask_handle_red);

    if (xReturn != pdPASS || xtask_handle_red == NULL) {
        printf("Error creating task_red_button\n");
    }

    xReturn = xTaskCreate(&task_blink,
                          TASK_BLINK_NAME,
                          TASK_BLINK_STACK_SIZE,
                          NULL,
                          TASK_BLINK_PRIORITY,
                          &xtask_handle_blink);

    if (xReturn != pdPASS || xtask_handle_blink == NULL) {
        printf("Error creating task_blink\n");
    }

    xReturn = xTaskCreate(&task_ack,
                          TASK_ACK_NAME,
                          TASK_ACK_STACK_SIZE,
                          &ack_buttons,
                          TASK_ACK_PRIORITY,
                          &xtask_handle_ack);

    if (xReturn != pdPASS || xtask_handle_ack == NULL) {
        printf("Error creating task_ack\n");
    }

    while (1) {
        vTaskDelay(pdMS_TO_TICKS(10000));
    }
}

static void task_blue_button(void *taskparams)
{
    control_task_parameters_t tparams = *(control_task_parameters_t *)taskparams;

    gpio_reset_pin(tparams.button);
    gpio_set_direction(tparams.button, GPIO_MODE_INPUT);

    while(1){
        int button_pressed = !gpio_get_level(tparams.button);
        if (button_pressed) {
          blink_blue = 1;
          vTaskSuspend(NULL);
        }
        vTaskDelay(50 / portTICK_RATE_MS);
    }
}

static void task_red_button(void *taskparams)
{
    control_task_parameters_t tparams = *(control_task_parameters_t *)taskparams;

    gpio_reset_pin(tparams.button);
    gpio_set_direction(tparams.button, GPIO_MODE_INPUT);

    while(1){
        int button_pressed = !gpio_get_level(tparams.button);
        if (button_pressed) {
          blink_red = 1;
          vTaskSuspend(NULL);
        }
        vTaskDelay(50 / portTICK_RATE_MS);
    }
}

static void task_blink(void *arg)
{
    gpio_pad_select_gpio(LED_BLUE_PIN);
    gpio_set_direction (LED_BLUE_PIN, GPIO_MODE_OUTPUT);
    gpio_pad_select_gpio(LED_RED_PIN);
    gpio_set_direction (LED_RED_PIN, GPIO_MODE_OUTPUT);

    while (1) {
        gpio_set_level(LED_BLUE_PIN, 0);
        gpio_set_level(LED_RED_PIN, 0);

        vTaskDelay(500 / portTICK_RATE_MS);

        if (blink_red) gpio_set_level(LED_RED_PIN, 1);

        vTaskDelay(500 / portTICK_RATE_MS);

        if (blink_blue) gpio_set_level(LED_BLUE_PIN, 1);
        gpio_set_level(LED_RED_PIN, 0);

        vTaskDelay(500 / portTICK_RATE_MS);

        if (blink_red) gpio_set_level(LED_RED_PIN, 1);

        vTaskDelay(500 / portTICK_RATE_MS);
    }
}

static void task_ack(void* taskparams)
{
    ack_task_parameters_t tparams = *(ack_task_parameters_t *)taskparams;

    gpio_reset_pin(tparams.button_ack_blue);
    gpio_set_direction(tparams.button_ack_blue, GPIO_MODE_INPUT);

    gpio_reset_pin(tparams.button_ack_red);
    gpio_set_direction(tparams.button_ack_red, GPIO_MODE_INPUT);

    while(1) {
        int pressed = !gpio_get_level(tparams.button_ack_blue);
        if (pressed && blink_blue == 1) {
            blink_blue = 0;
            vTaskResume(xtask_handle_blue);
        }

        pressed = !gpio_get_level(tparams.button_ack_red);
        if (pressed && blink_red == 1) {
            blink_red = 0;
            vTaskResume(xtask_handle_red);
        }

        vTaskDelay(100 / portTICK_RATE_MS);
    }

}

// controlTaskBlue/Red wait for button press
// if pressed, set blink_red/blue to 1 if 0 and vTaskPause self
// ackTask sets blink_red/blink_blue to 0 if 1 and button pressed