/*
Application 6
Name: Amanda Gumbs 
Date: 4/19/2026 
Real Time Systems Spring 2026 



*/


#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"
#include "driver/gpio.h"
#include "esp_log.h"
#include "driver/i2c.h"
#include "esp_timer.h"
#include <stdbool.h>
#include "driver/uart.h"
#include <string.h>

//Theme related to video games since I can use buttons, joysticks, blinking lights, 
//similar to the PS5 dual sense controller

#define BLUE_LED GPIO_NUM_25 //RGB wire going to Port ESP35
#define GREEN_LED GPIO_NUM_33 //RGB wire going to Port ESP33
#define RED_LED GPIO_NUM_26 //RGB wire going to Port ESP26
#define PURP_LED GPIO_NUM_21 //Single LED wire going to Port ESP21

#define RED_BUTTON_PIN GPIO_NUM_18 //Red  button on a controler on port ESP18
#define BLUE_BUTTON_PIN GPIO_NUM_17 //Blue button on a controller port ESP17
#define GYR_SENSOR GPIO_NUM_14 //Using the Accelerometer and Gyroscope sensor to mimic a Contoller that takes in movement - port ESP14

// These gyroscope Axis mimix a player tilting a controller slowly or quickly (at angles or side to side) 
//to move a character or object, Z axis allows for tilt up/down quickly 
#define ACCEL_MIN_XY -2.0f    // allow near-zero X/Y at rest
#define ACCEL_MAX_XY  2.0f     //Using XY axis for angles
#define ACCEL_MIN_Z   8.0f     //Z is up and down so separate
#define ACCEL_MAX_Z 12.0f    // allow ~9.81 on Z plus some headroom

//using serial ports as if it was a wired controller, have signals over COM
#define UART_PORT UART_NUM_0
#define UART_BAUD 115200
#define UART_TX_PIN 1
#define UART_RX_PIN 3

// Event codes
#define EVENT_SUCCESS  1
#define EVENT_MISS     0

//To restore RGB colors based on game time events 
#define EVENT_SUCCESS   1
#define EVENT_MISS      0
#define EVENT_RESTORE   3

//Semaphores to synchronize and control when task can run or access shared data.
static SemaphoreHandle_t sem_binary; //Change it later to a gaming type variable

//going to incrporate a ISR triggered by button 
static SemaphoreHandle_t isr_sem;

static SemaphoreHandle_t sem_count; //Change it later to a gaming type variable

//using this Mutex for our accelerator sensor so that we can protect against race conditions
//if this sensor reads and acts on data at the same time another task might also be reading and enacting on data 

static SemaphoreHandle_t accel_mutex; //Change it later to a gaming type variable

static QueueHandle_t game_event_queue; //Keeping a queue and tracking things in game

//Setting our accelerometer values at 0  
static float accel_x = 0;
static float accel_y = 0;
static float accel_z = 0; 

//For Quick Time RED BUTTON PRESSES 
//If no input get detected, it goes into semaphore count or if input is late 
//it goes into semaphore count 
#define QUICK_TIME_WINDOW_MS 3000 //3 seconds to press button
#define MAX_MISSES 5
 


//ALL OF THIS is for the Acc+Gyro sensor, Wokwi had this code in Arduino and I asked Claude to convert it to ESPIDF
//Link for Original code given by Wokwi docs -( https://docs.wokwi.com/parts/wokwi-mpu6050 )
//https://claude.ai/chat/c02781e3-0ac5-47dc-a2f0-eac867c2da16
#define I2C_MASTER_SCL_IO       22
#define I2C_MASTER_SDA_IO       23
#define I2C_MASTER_FREQ_HZ      400000
#define I2C_MASTER_PORT         I2C_NUM_0

#define MPU6050_ADDR            0x68
#define MPU6050_PWR_MGMT_1      0x6B
#define MPU6050_ACCEL_XOUT_H    0x3B
#define ACCEL_SCALE             16384.0f
#define G_TO_MS2                9.81f

static const char *TAG = "MPU6050";

static void set_rgb(int r, int g, int b) {
    gpio_set_level(RED_LED, r);
    gpio_set_level(GREEN_LED, g);
    gpio_set_level(BLUE_LED, b);
}


//SETUP TO MAKE THE ACCELEROMETER AND GRYOSCOPE SENSOR WORK 
static esp_err_t i2c_master_init(void) {
    i2c_config_t conf;
    conf.mode = I2C_MODE_MASTER;
    conf.sda_io_num = I2C_MASTER_SDA_IO;
    conf.scl_io_num = I2C_MASTER_SCL_IO;
    conf.sda_pullup_en = GPIO_PULLUP_ENABLE;
    conf.scl_pullup_en = GPIO_PULLUP_ENABLE;
    conf.master.clk_speed = I2C_MASTER_FREQ_HZ;
    i2c_param_config(I2C_MASTER_PORT, &conf);
    return i2c_driver_install(I2C_MASTER_PORT, conf.mode, 0, 0, 0);
}

static esp_err_t mpu6050_write(uint8_t reg, uint8_t data) {
    uint8_t buf[2] = {reg, data};
    return i2c_master_write_to_device(I2C_MASTER_PORT, MPU6050_ADDR,
                                      buf, sizeof(buf),
                                      pdMS_TO_TICKS(100));
}

static esp_err_t mpu6050_read(uint8_t reg, uint8_t *data, size_t len) {
    return i2c_master_write_read_device(I2C_MASTER_PORT, MPU6050_ADDR,
                                        &reg, 1, data, len,
                                        pdMS_TO_TICKS(100));
}

static esp_err_t mpu6050_init(void) {
    return mpu6050_write(MPU6050_PWR_MGMT_1, 0x00);
}

static void read_accel(float *ax, float *ay, float *az) {
    uint8_t raw[6];
    mpu6050_read(MPU6050_ACCEL_XOUT_H, raw, 6);

    int16_t x = (int16_t)((raw[0] << 8) | raw[1]);
    int16_t y = (int16_t)((raw[2] << 8) | raw[3]);
    int16_t z = (int16_t)((raw[4] << 8) | raw[5]);

    *ax = (x / ACCEL_SCALE) * G_TO_MS2;
    *ay = (y / ACCEL_SCALE) * G_TO_MS2;
    *az = (z / ACCEL_SCALE) * G_TO_MS2;
}
//End Code for Acc/Gyro sensor code conversion

//UART configuration 
//Asked Claude to help me configure my uart, I changed some of it for later task based on
//my task and my program themeing 
//https://claude.ai/chat/8846f07b-dadf-461b-8abb-afe68882e007
static void uart_init(void) {
    uart_config_t uart_config;
    uart_config.baud_rate = UART_BAUD;
    uart_config.data_bits = UART_DATA_8_BITS;
    uart_config.parity = UART_PARITY_DISABLE;
    uart_config.stop_bits = UART_STOP_BITS_1;
    uart_config.flow_ctrl = UART_HW_FLOWCTRL_DISABLE;
    uart_param_config(UART_PORT, &uart_config);
    uart_set_pin(UART_PORT, UART_TX_PIN, UART_RX_PIN, -1, -1);
    uart_driver_install(UART_PORT, 1024, 0, 0, NULL, 0);
}


//HEARTBEAT BLINK TASKS
//Period: 1000ms | Deadline: None | SOFT real-time
//Heartbeat just lets us know the system is on.
void heartbeatBlink_task(void *pvParameter) {
  while (1){
    gpio_set_level(PURP_LED, 1); //Set pin high and turn on LED 
    vTaskDelay(pdMS_TO_TICKS(1000)); //Delay for 1000ms (1 second)
    gpio_set_level(PURP_LED, 0); //Set pin low and turn off LED
    vTaskDelay(pdMS_TO_TICKS(1000)); // Delay for 1000ms (1 second)
  }
}


//ISR (Interrupt Service Routine) - Since button takes priority, we interrupt whatever is happening to ensure 
//the most resources go to said task 

//I want the button to be a Hard miss type task 
//Similar to quick time actions in a video game, if you miss the presses
//it affects your characters abilities or puts you in danger

//missing too many times will kill you and make you restart progress  
void IRAM_ATTR button_isr(void *arg) {
    SemaphoreHandle_t sem = (SemaphoreHandle_t)arg;
    BaseType_t hpw = pdFALSE;
    ESP_EARLY_LOGI("ISR", "BUTTON PRESSED");
    xSemaphoreGiveFromISR(sem, &hpw);
    portYIELD_FROM_ISR(hpw);
}

//RED BUTTON TASK - LIKE A CONTROLLER
//Period 2000ms | Deadline: 3000ms | HARD real-time
//Missing deadline = player misses quick time event, they miss dodging/blocking an attack
//too many missing blocks, character progress restarts to last checkpoint 
void buttons_task(void *pvParameter) {
    while (1) {
        // Signal player a quick time event is starting
        ESP_LOGI(TAG, "======= QUICK TIME EVENT! =======");
        uart_write_bytes(UART_PORT, "\n>>> PRESS RED OR BLUE NOW! You have 3 seconds!\n", 47);
        set_rgb(1, 0, 0); // red = press now

        bool missed = false;
        int64_t start = esp_timer_get_time(); // start timer here

        //choose an binary semaphore (named isr_sem) because a button only has two states
        //0 (not available)- no button has been pressed, so button_task is blcoked and waiting
        //1 (available) - button was pressed, ISR gave the semaphore for interrupt, button_task proceeds 
        //using uart_write to write and print to cconsole through serial port 
        // 1 second warning
        if (xSemaphoreTake(isr_sem, pdMS_TO_TICKS(1000)) == pdTRUE) {
            goto success;
        }
        uart_write_bytes(UART_PORT, "\n>>> 2 seconds left!\n", 20);

        // 2 second warning
        if (xSemaphoreTake(isr_sem, pdMS_TO_TICKS(1000)) == pdTRUE) {
            goto success;
        }
        uart_write_bytes(UART_PORT, "\n>>> 1 second left!\n", 19);

        // Final second
        if (xSemaphoreTake(isr_sem, pdMS_TO_TICKS(1000)) == pdTRUE) {
            goto success;
        }

        // Timed out
        missed = true;
        goto done;

        success:
        {
          //DETERMINISM PROOF
            int64_t elapsed = esp_timer_get_time() - start;
            char tbuf[128]; //large buffer to build and hold string before moving it 
            snprintf(tbuf, sizeof(tbuf), "\n>>> Response time: %lld ms (deadline: 3000ms)\n", elapsed / 1000);
            uart_write_bytes(UART_PORT, tbuf, strlen(tbuf));



            //Sometimes use ESP_LOGI since it is the natural ESP-IDF logging/printing systems 
            int event = EVENT_SUCCESS;
            xQueueSend(game_event_queue, &event, 0);
            ESP_LOGI(TAG, "\nQuick Time Success!");
            //uart_write_bytes(UART_PORT, "\n>>> SUCCESS!\n", 13);
            vTaskDelay(pdMS_TO_TICKS(1000)); //Delay by 1 second 
            goto done;
        }

        done:
        if (missed) {
          //DETERMINISM PROOF 
            int64_t elapsed = esp_timer_get_time() - start;
            char tbuf[128];
            snprintf(tbuf, sizeof(tbuf), "\n>>> Deadline MISSED at: %lld ms (deadline: 3000ms)\n", elapsed / 1000);
            uart_write_bytes(UART_PORT, tbuf, strlen(tbuf));

            int event = EVENT_MISS;
            xQueueSend(game_event_queue, &event, 0);
            ESP_LOGI(TAG, "\nQuick Time Missed!");
            //uart_write_bytes(UART_PORT, "\n>>> MISSED!\n", 12);
            xSemaphoreGive(sem_count); //Adding to the count mutex since we want to track misses
            //we use the counting semaphore because the counting semaphore keep the thread protected against race conditions 
            //or multiple task reading at the same time and getting a corrupted value 

            int chances_left = MAX_MISSES - uxSemaphoreGetCount(sem_count);
            char buf[128];
            snprintf(buf, sizeof(buf), "\n>>> Chances left: %d/%d\n", chances_left, MAX_MISSES);
            uart_write_bytes(UART_PORT, buf, strlen(buf));


            vTaskDelay(pdMS_TO_TICKS(500));

            //Once sem count surpasses MAX_MISSES, we reset the count semaphore and restart progress 
            if (uxSemaphoreGetCount(sem_count) >= MAX_MISSES) {
                ESP_LOGI(TAG, "\nToo many misses! Restarting from last checkpoint...");
                uart_write_bytes(UART_PORT, "\n>>> CHECKPOINT RESET!\n", 22);
                vTaskDelay(pdMS_TO_TICKS(2000));
                while (uxSemaphoreGetCount(sem_count) > 0)
                    xSemaphoreTake(sem_count, 0);
            }
        }

        int restore = EVENT_RESTORE;// restore blue
        xQueueSend(game_event_queue, &restore, 0);
        vTaskDelay(pdMS_TO_TICKS(2000)); // wait before next event
    }
}
//OLD BUTTONS TASK 
/*void buttons_task(void *pvParameter) {
    while (1) {
        ESP_LOGI(TAG, "Press RED or BLUE to block!");

        bool missed = false;

        if (xSemaphoreTake(isr_sem, pdMS_TO_TICKS(QUICK_TIME_WINDOW_MS)) == pdTRUE) {
            int event = EVENT_SUCCESS;
            xQueueSend(game_event_queue, &event, 0);
            ESP_LOGI(TAG, "Quick Time Success!");
            vTaskDelay(pdMS_TO_TICKS(500));
        } else {
            missed = true;
        }

        if (missed) {
            int event = EVENT_MISS;
            xQueueSend(game_event_queue, &event, 0);
            ESP_LOGI(TAG, "Quick Time Missed!");
            xSemaphoreGive(sem_count);
            vTaskDelay(pdMS_TO_TICKS(500));

            if (uxSemaphoreGetCount(sem_count) >= MAX_MISSES) {
                ESP_LOGI(TAG, "Too many misses! Restarting from last checkpoint...");
                while (uxSemaphoreGetCount(sem_count) > 0)
                    xSemaphoreTake(sem_count, 0);
            }
        }

        vTaskDelay(pdMS_TO_TICKS(2000));
    }
}
*/

//OLD BLUE BUTTON TASK 
//BLUE BUTTON TASK - LIKE A CONTROLLER
//Period: 2000ms | Deadline: No deadline | SOFT real-time 
// If you don't interact it is not the end of the world, similar to open world game, 
//you can interact but really may not harm anything 
/*void blue_button_task(void *pvParameters){
  while(1){
    if (xSemaphoreTake(blue_isr_sem, portMAX_DELAY) == pdTRUE){
      int event = EVENT_INTERACT;
      xQueueSend(game_event_queue, &event, 0); // Add this line

      ESP_LOGI(TAG, "PRESS BLUE BUTTON TO INTERACT!");

      gpio_set_level(BLUE_LED, 1);
      gpio_set_level(GREEN_LED, 0);
      
      ESP_LOGI(TAG, "PRESS BLUE BUTTON TO INTERACT!");
      vTaskDelay(pdMS_TO_TICKS(500));

      gpio_set_level(GREEN_LED, 1);
      gpio_set_level(BLUE_LED, 0);
    }
  }
}
*/



//ACCELROMETER + GYROSCOPE TASKS
//Had claude do part of the gyro sensor task so I can properly incorporate the isr semaphore
//https://claude.ai/chat/c02781e3-0ac5-47dc-a2f0-eac867c2da16
//I changed a lot of it to match my desired design and variables, but it gave me a good skeleton of where to start.
//Period: 50ms | Deadline: No deadline | SOFT real-time
//Just using the gyroscope to interact and possibly move around the game
void gyroSensorTask(void *pvParameters) {
  float ax, ay, az; //Holding accelerometer readings 

  //ACCELEROMETER + GYROSCOPE GPIO sensor check 
  while (mpu6050_init() != ESP_OK) {
        ESP_LOGE(TAG, "MPU6050 not connected!");
        vTaskDelay(pdMS_TO_TICKS(1000));
    }
    ESP_LOGI(TAG, "MPU6050 ready!");

  while(1){
    //reads raw accelerometer readings from sensor and passes them into the mutex 
    read_accel(&ax, &ay, &az);
    

      if (xSemaphoreTake(accel_mutex, pdMS_TO_TICKS(50))==pdTRUE){
        accel_x = ax;
        accel_y = ay;
        accel_z = az;

        xSemaphoreGive(accel_mutex);

      }

        //Checks against the defined axis and thier ranges; bool is true if if axis is in normal range 
        bool x_ok = (ax >= ACCEL_MIN_XY && ax <= ACCEL_MAX_XY);
        bool y_ok = (ay >= ACCEL_MIN_XY && ay <= ACCEL_MAX_XY);
        bool z_ok = (az >= ACCEL_MIN_Z  && az <= ACCEL_MAX_Z);

        //If axis is passed our normal range values, it gives sem_binary to wake up uart_alert_task
        //then prints movement warning over UART serial  
        if (!x_ok || !y_ok || !z_ok) {
            xSemaphoreGive(sem_binary);
        }

      vTaskDelay(pdMS_TO_TICKS(1000)); //read every 500 ms (0.5 seconds)
    }

  }



//using UART to give feedback to player if they are moving their controller too far or too little
//Period: 50ms | Deadline: 20.0f to 50.0f | SOFT real-time 
//Not entirely missing a deadline, just going too fast or slow within movement. 
void uart_alert_task(void *pvParameters) {
    char buf[64];
    while (1) {
        if (xSemaphoreTake(sem_binary, portMAX_DELAY) == pdTRUE) {
            if (xSemaphoreTake(accel_mutex, pdMS_TO_TICKS(50)) == pdTRUE) {
                float lx = accel_x;
                float ly = accel_y;
                float lz = accel_z;
                xSemaphoreGive(accel_mutex);

                if (lx > ACCEL_MAX_XY || ly > ACCEL_MAX_XY || lz > ACCEL_MAX_Z) {
                    snprintf(buf, sizeof(buf), "\nALERT: Movement too fast! X:%.2f Y:%.2f Z:%.2f\n", lx, ly, lz);
                } else if (lx < ACCEL_MIN_XY || ly < ACCEL_MIN_XY || lz < ACCEL_MIN_Z) {
                    snprintf(buf, sizeof(buf), "\nALERT: Movement too slow! X:%.2f Y:%.2f Z:%.2f\n", lx, ly, lz);
                }

                uart_write_bytes(UART_PORT, buf, strlen(buf));
            }
            vTaskDelay(pdMS_TO_TICKS(200));
        }
    }
}

// EVENT HANDLER TASK
// Period: event driven | Deadline: none | SOFT real-time
// Processes game events and logs overall game state
void event_handler_task(void *pvParameters) {
    int event;
    //setting default RGB state to BLUE RGB 
    gpio_set_level(RED_LED, 0);
    gpio_set_level(GREEN_LED, 0);
    gpio_set_level(BLUE_LED, 1); //blue at startup
    while (1) {
      //using blocks to wait for something to arrive in game_event_queue 
      //then checking what events was received and jumps to matching case 
        if (xQueueReceive(game_event_queue, &event, portMAX_DELAY) == pdTRUE) {
            switch (event) {
                case EVENT_SUCCESS:
                    uart_write_bytes(UART_PORT, "\n>>> GAME: Player blocked successfully!\n", 38);
                    gpio_set_level(RED_LED, 0);
                    gpio_set_level(BLUE_LED, 0);
                    gpio_set_level(GREEN_LED, 1); // green only
                    break;
                case EVENT_MISS:
                    uart_write_bytes(UART_PORT, "\n>>> GAME: Player missed! Watch Out!\n", 36);
                    gpio_set_level(GREEN_LED, 0);
                    gpio_set_level(BLUE_LED, 0);
                    gpio_set_level(RED_LED, 1); // red only
                    break;
                case EVENT_RESTORE:
                    gpio_set_level(RED_LED, 0);
                    gpio_set_level(GREEN_LED, 0);
                    gpio_set_level(BLUE_LED, 1); // blue only
                    break;
                default:
                    break;
            }
        }
    }
}
 
void app_main(void) {
  ESP_LOGI(TAG, "System starting...");
  //Semaphores
  // sem_binary used between gyroSensorTask and uart_alert_task for movement alerts
  sem_binary = xSemaphoreCreateBinary();

  //isr_sem is used between button_isr and button_task for the button presses
  isr_sem = xSemaphoreCreateBinary();

  //protects the shared accelerometer globals from being read and written at the same time by two task 
  accel_mutex = xSemaphoreCreateMutex();

  //counting semaphore to track player misses, max of 5, starting at 0
  sem_count = xSemaphoreCreateCounting(5, 0); //Keeping track of button/quick time misses 

  //Creating queue that button_task sends events and event_handler_task reads from. Holds up to 10
  game_event_queue = xQueueCreate(10, sizeof(int));


  //APP Main code for the Acc/Gyro sensor 
    uart_init();
    //initializing the I2C bus that the accelerometer sensor communicates
    ESP_ERROR_CHECK(i2c_master_init());
    vTaskDelay(pdMS_TO_TICKS(100)); //let I2C bus settle in Wokwi

    //resets and configures each LED pin as an output 
    //GPIO for LEDs
    gpio_reset_pin(PURP_LED);
    gpio_set_direction(PURP_LED, GPIO_MODE_OUTPUT);

    gpio_reset_pin(BLUE_LED);
    gpio_set_direction(BLUE_LED, GPIO_MODE_OUTPUT);

    gpio_reset_pin(GREEN_LED);
    gpio_set_direction(GREEN_LED, GPIO_MODE_OUTPUT);

    gpio_reset_pin(RED_LED);
    gpio_set_direction(RED_LED, GPIO_MODE_OUTPUT);

    gpio_set_level(BLUE_LED, 1); //set blue at startup

    //Button GPIO and attach ISR
    //configures both button pins together as inputs 
    gpio_config_t io_conf = {
      .pin_bit_mask = (1ULL << RED_BUTTON_PIN) | (1ULL << BLUE_BUTTON_PIN),
      .mode = GPIO_MODE_INPUT,
      .pull_up_en = GPIO_PULLUP_ENABLE,
      .intr_type = GPIO_INTR_NEGEDGE,

    };
    //installs GPIO interrupt service and attaches button_isr to both button pins
    //both buttons pass isr_sem to the argument so pressing either gives the same semaphore 
    gpio_config(&io_conf);
    ESP_ERROR_CHECK(gpio_install_isr_service(ESP_INTR_FLAG_LEVEL1));
    gpio_isr_handler_add(RED_BUTTON_PIN, button_isr, (void *)isr_sem);
    gpio_isr_handler_add(BLUE_BUTTON_PIN, button_isr, (void *)isr_sem);


    //ALL TASK
    //creating all five FreeRTOS task. Each call takes the same parameters
    //each of these is also set in the priority in which most CPU resources would need to go to
    xTaskCreate(gyroSensorTask, "gyroSensorTask", 4096, NULL, 3, NULL);
    xTaskCreate(uart_alert_task, "uart_alert_task", 4096, NULL, 2, NULL);
    xTaskCreate(buttons_task, "buttons_task", 4096, NULL, 6, NULL);
    xTaskCreate(heartbeatBlink_task, "heartbeatBlink_task", 4096, NULL, 1, NULL);
    xTaskCreate(event_handler_task, "event_handler_task", 4096, NULL, 5, NULL);
    //xTaskCreate(red_button_task, "red_button_task", 4096, NULL, 5, NULL);
    //xTaskCreate(blue_button_task, "blue_button_task", 4096, NULL, 4, NULL);
   
    
}