// main.c
// Starter code for CSCE 41104 Assignment 3 - RTOS & Robots
// Jonas Brown (author metadata in your JSON)
//
// Notes:
//  - This code sets up tasks, timers, semaphores, queue, and GPIO ISRs.
//  - Task bodies and state machine logic are left as skeletons for you to implement.
//  - Uses FreeRTOS software timers instead of vTaskDelay for periodic behavior.

#include <stdio.h>
#include <string.h>

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/timers.h"
#include "freertos/semphr.h"
#include "freertos/queue.h"

#include "driver/gpio.h"
#include "esp_log.h"

// #include "TM1637TinyDisplay.h" // ensure this library is available in component path

static const char *TAG = "stoplight";

// pins from your JSON/preamble
#define RED1 42
#define YELLOW1 41
#define GREEN1 40
#define RED2 37
#define YELLOW2 38
#define GREEN2 39
#define CROSSWALKLED 36
#define PIRUPDOWN 14
#define PIRLEFTRIGHT 15
#define BTNUPDOWN 12
#define BTNLEFTRIGHT 13

// TM1637 pins
const int CLK = 2;
const int DIO = 4;

// RTOS objects
static SemaphoreHandle_t xCrosswalkSemaphore = NULL;   // binary semaphore from buttons -> crosswalk
static QueueHandle_t xSerialQueue = NULL;              // queue to gatekeeper serial task
static TimerHandle_t tmr_status = NULL;                // periodic status stream timer (1s)
static TimerHandle_t tmr_traffic = NULL;               // traffic state machine timer (tick)
static TimerHandle_t tmr_display = NULL;

volatile bool crosswalk_requested = false;
volatile bool crosswalk_active = false;
static int crosswalk_countdown = 0;               // display countdown timer

// Task handles (not strictly required, but handy)
TaskHandle_t th_gatekeeper = NULL;
TaskHandle_t th_crosswalk_leds = NULL;
TaskHandle_t th_traffic_sm = NULL;
TaskHandle_t th_display = NULL;

// TM1637 display object
// TM1637TinyDisplay display(CLK, DIO);

// A little helper message type for serial queue (simple fixed-size)
typedef struct {
    char msg[128];
} serial_msg_t;

////////////////////////////////////////////////////////////////////////////////
// ISRs
////////////////////////////////////////////////////////////////////////////////

static void IRAM_ATTR pir_isr_handler(void* arg)
{
    uint32_t pin = (uint32_t) arg;
    BaseType_t xHigherPriorityWoken = pdFALSE;

    serial_msg_t s;
    snprintf(s.msg, sizeof(s.msg), "PIR triggered on pin %ld", pin);

    // Send a message to the gatekeeper queue (from ISR)
    xQueueSendFromISR(xSerialQueue, &s, &xHigherPriorityWoken);

    // Optionally signal a task to reduce green time - leave for main logic
    if (xHigherPriorityWoken == pdTRUE) {
        portYIELD_FROM_ISR();
    }
}

static void IRAM_ATTR btn_isr_handler(void* arg)
{
    uint32_t pin = (uint32_t) arg;
    BaseType_t xHigherPriorityWoken = pdFALSE;

    crosswalk_requested = true;  // <-- new

    xSemaphoreGiveFromISR(xCrosswalkSemaphore, &xHigherPriorityWoken);

    serial_msg_t s;
    snprintf(s.msg, sizeof(s.msg), "Crosswalk button pushed (pin %ld)", pin);
    xQueueSendFromISR(xSerialQueue, &s, &xHigherPriorityWoken);

    if (xHigherPriorityWoken == pdTRUE) {
        portYIELD_FROM_ISR();
    }
}

////////////////////////////////////////////////////////////////////////////////
// Gatekeeper task - single place to do serial prints
////////////////////////////////////////////////////////////////////////////////
void gatekeeper_task(void* pvParameters)
{
    serial_msg_t recv;
    while (1) {
        if (xQueueReceive(xSerialQueue, &recv, portMAX_DELAY) == pdTRUE) {
            // All serial I/O goes through here.
            // Keep it short and non-blocking – printf is okay for this classroom project.
            ESP_LOGI(TAG, "GATE: %s", recv.msg);
        }
    }
}

////////////////////////////////////////////////////////////////////////////////
// Crosswalk LED task - waits for binary semaphore from button ISRs
////////////////////////////////////////////////////////////////////////////////
void crosswalk_led_task(void* pvParameters)
{
    (void) pvParameters;
    for (;;) {
        // Wait for crosswalk request (binary semaphore). We'll not block forever in case
        // other periodic or safety logic should run; use a timeout and re-evaluate as needed.
        if (xSemaphoreTake(xCrosswalkSemaphore, pdMS_TO_TICKS(1000)) == pdTRUE) {
            // semaphore taken -> crosswalk requested
            ESP_LOGI(TAG, "Crosswalk requested: toggle CROSSWALKLED or set state");

            // TODO: set CROSSWALKLED on, start display countdown (20s), and ensure
            // traffic lights stay red during crosswalk. Use timers/flags to implement.
            // For starter: just toggle CROSSWALKLED pin as a heartbeat
            gpio_set_level(CROSSWALKLED, 1);
            // queue a status message to gatekeeper (non-ISR context)
            serial_msg_t s;
            snprintf(s.msg, sizeof(s.msg), "Crosswalk semaphore taken - request active");
            xQueueSend(xSerialQueue, &s, 0);
        } else {
            // Timeout - you can perform periodic checks or housekeeping here.
            // For now, ensure CROSSWALKLED off (or implement your actual state machine)
            gpio_set_level(CROSSWALKLED, 0);
            // short delay handled by blocking xSemaphoreTake
        }
    }
}

////////////////////////////////////////////////////////////////////////////////
// Traffic state machine task (skeleton). Use this to implement the timing schedule.
// NOTE: per assignment, actual periodic timing should be managed via software timers.
// This task reacts to timer callbacks or uses its own internal state machine tick.
////////////////////////////////////////////////////////////////////////////////
typedef enum { BOTH_RED=0, DIR1_GREEN, DIR1_YELLOW, DIR2_GREEN, DIR2_YELLOW } traffic_state_t;
volatile traffic_state_t traffic_state = BOTH_RED;

void traffic_sm_task(void* pvParameters)
{
    (void) pvParameters;
    // initial state
    traffic_state = BOTH_RED;
    for (;;) {
        // The real transitions should be driven by the traffic timer callback.
        // Here we simply block and let your timer callback change 'traffic_state' or send
        // notifications to this task to perform GPIO updates.
        // For skeleton: read current state and set the LEDs accordingly.
        switch (traffic_state) {
            case BOTH_RED:
                gpio_set_level(RED1, 1);
                gpio_set_level(YELLOW1, 0);
                gpio_set_level(GREEN1, 0);
                gpio_set_level(RED2, 1);
                gpio_set_level(YELLOW2, 0);
                gpio_set_level(GREEN2, 0);
                break;
            case DIR1_GREEN:
                gpio_set_level(RED1, 0);
                gpio_set_level(GREEN1, 1);
                gpio_set_level(YELLOW1, 0);
                gpio_set_level(RED2, 1);
                gpio_set_level(GREEN2, 0);
                gpio_set_level(YELLOW2, 0);
                break;
            case DIR1_YELLOW:
                gpio_set_level(GREEN1, 0);
                gpio_set_level(YELLOW1, 1);
                break;
            case DIR2_GREEN:
                gpio_set_level(RED1, 1);
                gpio_set_level(GREEN1, 0);
                gpio_set_level(YELLOW1, 0);
                gpio_set_level(RED2, 0);
                gpio_set_level(GREEN2, 1);
                gpio_set_level(YELLOW2, 0);
                break;
            case DIR2_YELLOW:
                gpio_set_level(GREEN2, 0);
                gpio_set_level(YELLOW2, 1);
                break;
            default:
                break;
        }

        // Wait for a notification from timer callback or small delay to prevent busy loop.
        // For the skeleton we block for a short time and then loop; actual timing should
        // be event-driven to meet the assignment rule (use software timers).
        ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(200));
    }
}

////////////////////////////////////////////////////////////////////////////////
// Display task - updates TM1637 4-digit display (countdown)
////////////////////////////////////////////////////////////////////////////////
void display_task(void* pvParameters)
{
    (void) pvParameters;
    // Example: initialize the TM1637
    // display.setBrightness(2);
    // display.clear();

    int countdown = 0; // 20 seconds when crosswalk active; otherwise 0

    for (;;) {
        // TODO: wait for timer / event that decrements or sets countdown
        // For now we block briefly and show current countdown value
        vTaskDelay(pdMS_TO_TICKS(500));

        // convert countdown to 4-digit format on display
        // very simple convert; flesh out with mm:ss if desired
        // int v = countdown;
        // int d0 = (v / 1000) % 10;
        // int d1 = (v / 100) % 10;
        // int d2 = (v / 10) % 10;
        // int d3 = (v / 1) % 10;
        // display.clear();
        // display.setSegmentsDigit(0, d0);
        // display.setSegmentsDigit(1, d1);
        // display.setSegmentsDigit(2, d2);
        // display.setSegmentsDigit(3, d3);
    }
}

////////////////////////////////////////////////////////////////////////////////
// Software timer callbacks
////////////////////////////////////////////////////////////////////////////////

// status timer: fired every 1 second to push a periodic status to the gatekeeper queue
void tmr_status_cb(TimerHandle_t xTimer)
{
    serial_msg_t s;
    snprintf(s.msg, sizeof(s.msg), "STATUS TICK: traffic_state=%d", (int)traffic_state);
    xQueueSend(xSerialQueue, &s, 0);
}

// traffic timer: this callback should implement the transition schedule and
// control durations. Keep callbacks short — ideally signal a task to do heavier work.
void tmr_traffic_cb(TimerHandle_t xTimer)
{
    static bool from_dir1 = true;

    // *** Crosswalk override ***
    if (crosswalk_active) {
        if (crosswalk_countdown > 0) {
            crosswalk_countdown--;
            if (crosswalk_countdown == 0) {
                crosswalk_active = false;
                gpio_set_level(CROSSWALKLED, 0);  // turn off LED
            }
        }

        traffic_state = BOTH_RED;   // force red
        if (th_traffic_sm) xTaskNotifyGive(th_traffic_sm);
        return;
    }

    // *** Normal rotation logic ***
    switch (traffic_state) {
        case BOTH_RED:
            if (crosswalk_requested) {
                crosswalk_requested = false;
                crosswalk_active = true;
                crosswalk_countdown = 20; // 20 seconds
                gpio_set_level(CROSSWALKLED, 1);
                serial_msg_t s;
                snprintf(s.msg, sizeof(s.msg), "Crosswalk Active: holding red for 20s");
                xQueueSend(xSerialQueue, &s, 0);
                break;
            }
            traffic_state = (from_dir1) ? DIR1_GREEN : DIR2_GREEN;
            xTimerChangePeriod(tmr_traffic, pdMS_TO_TICKS(18000), 0);
            
            break;

        case DIR1_GREEN:
            traffic_state = DIR1_YELLOW;
            xTimerChangePeriod(tmr_traffic, pdMS_TO_TICKS(4000), 0);
            break;

        case DIR1_YELLOW:
            from_dir1 = false;
            traffic_state = BOTH_RED;
            xTimerChangePeriod(tmr_traffic, pdMS_TO_TICKS(2000), 0);
            break;

        case DIR2_GREEN:
            traffic_state = DIR2_YELLOW;
            xTimerChangePeriod(tmr_traffic, pdMS_TO_TICKS(4000), 0);
            break;

        case DIR2_YELLOW:
            from_dir1 = true;
            traffic_state = BOTH_RED;
            xTimerChangePeriod(tmr_traffic, pdMS_TO_TICKS(2000), 0);
            break;
    }

    if (th_traffic_sm) xTaskNotifyGive(th_traffic_sm);
}



// display timer: used to decrement countdown when crosswalk active (skeleton)
void tmr_display_cb(TimerHandle_t xTimer)
{
    // TODO: decrement countdown and notify display task / update display state
    // For skeleton: send a quick status message
    serial_msg_t s;
    snprintf(s.msg, sizeof(s.msg), "Display timer tick");
    xQueueSend(xSerialQueue, &s, 0);
}

////////////////////////////////////////////////////////////////////////////////
// GPIO setup helper
////////////////////////////////////////////////////////////////////////////////
static void init_gpio(void)
{
    // Configure LED outputs
    gpio_config_t io_conf = {
        .intr_type = GPIO_INTR_DISABLE,
        .mode = GPIO_MODE_OUTPUT,
        .pin_bit_mask = (1ULL<<RED1) | (1ULL<<YELLOW1) | (1ULL<<GREEN1) |
                        (1ULL<<RED2) | (1ULL<<YELLOW2) | (1ULL<<GREEN2) |
                        (1ULL<<CROSSWALKLED),
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
        .pull_up_en = GPIO_PULLUP_DISABLE
    };
    gpio_config(&io_conf);

    // Configure PIR inputs (edge triggered)
    gpio_config_t pir_conf = {
        .intr_type = GPIO_INTR_POSEDGE,
        .mode = GPIO_MODE_INPUT,
        .pin_bit_mask = (1ULL<<PIRUPDOWN) | (1ULL<<PIRLEFTRIGHT),
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
        .pull_up_en = GPIO_PULLUP_DISABLE
    };
    gpio_config(&pir_conf);

    // Configure button inputs with pull-up (adjust depending on wiring)
    gpio_config_t btn_conf = {
        .intr_type = GPIO_INTR_NEGEDGE, // assume active-low pushbutton (pressed -> falling edge)
        .mode = GPIO_MODE_INPUT,
        .pin_bit_mask = (1ULL<<BTNUPDOWN) | (1ULL<<BTNLEFTRIGHT),
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
        .pull_up_en = GPIO_PULLUP_ENABLE
    };
    gpio_config(&btn_conf);

    // Install ISR service and add handlers
    gpio_install_isr_service(0);
    gpio_isr_handler_add((gpio_num_t)PIRUPDOWN, pir_isr_handler, (void*)PIRUPDOWN);
    gpio_isr_handler_add((gpio_num_t)PIRLEFTRIGHT, pir_isr_handler, (void*)PIRLEFTRIGHT);
    gpio_isr_handler_add((gpio_num_t)BTNUPDOWN, btn_isr_handler, (void*)BTNUPDOWN);
    gpio_isr_handler_add((gpio_num_t)BTNLEFTRIGHT, btn_isr_handler, (void*)BTNLEFTRIGHT);

    // Initialize outputs to known state
    gpio_set_level(RED1, 1);
    gpio_set_level(YELLOW1, 0);
    gpio_set_level(GREEN1, 0);
    gpio_set_level(RED2, 1);
    gpio_set_level(YELLOW2, 0);
    gpio_set_level(GREEN2, 0);
    gpio_set_level(CROSSWALKLED, 0);
}

////////////////////////////////////////////////////////////////////////////////
// app_main
////////////////////////////////////////////////////////////////////////////////
void app_main(void)
{
    ESP_LOGI(TAG, "Smart Stoplight starter - initializing");

    // Create binary semaphore for crosswalk requests
    xCrosswalkSemaphore = xSemaphoreCreateBinary();
    if (xCrosswalkSemaphore == NULL) {
        ESP_LOGE(TAG, "Failed to create xCrosswalkSemaphore");
        abort();
    }

    // Create serial queue (gatekeeper)
    xSerialQueue = xQueueCreate(16, sizeof(serial_msg_t));
    if (xSerialQueue == NULL) {
        ESP_LOGE(TAG, "Failed to create xSerialQueue");
        abort();
    }

    // Initialize GPIOs and ISRs
    init_gpio();

    // Create software timers
    tmr_status = xTimerCreate("tmr_status", pdMS_TO_TICKS(1000), pdTRUE, NULL, tmr_status_cb);
    tmr_traffic = xTimerCreate("tmr_traffic", pdMS_TO_TICKS(1000), pdTRUE, NULL, tmr_traffic_cb); // tick every 1s, you will change durations by logic
    tmr_display = xTimerCreate("tmr_display", pdMS_TO_TICKS(1000), pdTRUE, NULL, tmr_display_cb);

    if (tmr_status) xTimerStart(tmr_status, 0);
    if (tmr_traffic) xTimerStart(tmr_traffic, 0);
    if (tmr_display) xTimerStart(tmr_display, 0);

    // Create Gatekeeper task (serial)
    xTaskCreatePinnedToCore(gatekeeper_task, "gatekeeper", 4096, NULL, 4, &th_gatekeeper, tskNO_AFFINITY);

    // Create Crosswalk LED task
    xTaskCreatePinnedToCore(crosswalk_led_task, "crosswalk_leds", 2048, NULL, 3, &th_crosswalk_leds, tskNO_AFFINITY);

    // Create Traffic State Machine task
    xTaskCreatePinnedToCore(traffic_sm_task, "traffic_sm", 4096, NULL, 3, &th_traffic_sm, tskNO_AFFINITY);

    // Create display task (TM1637)
    xTaskCreatePinnedToCore(display_task, "display", 4096, NULL, 2, &th_display, tskNO_AFFINITY);

    ESP_LOGI(TAG, "Initialization complete - tasks and timers started");
    // app_main returns; FreeRTOS tasks run in background
}