/*Application 6
Name: Lakshmi Katravulapalli
Application Context: Theme Park Ride Control System at Disney */

#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"
#include "driver/gpio.h"
#include "driver/adc.h"
#include "esp_log.h"
#include "esp_timer.h"

#include "freertos/event_groups.h"
EventGroupHandle_t event_group;
const int ESTOP = BIT0; // ( 1 << 0 )  // 0x01  // 00000001

//Define LED's
#define LED_GREEN GPIO_NUM_2 // Green LED -> GPIO 2
#define LED_RED  GPIO_NUM_4 // Red LED -> GPIO 4
#define LED_BLUE  GPIO_NUM_5 // Red LED -> GPIO 5
#define BUTTON_PIN GPIO_NUM_14 //Push Button -> GPIO 14

//Define Ultrasonic Sensor
#define PIN_TRIG GPIO_NUM_32 //Trigger Pin -> GPIO 32
#define PIN_ECHO GPIO_NUM_33 //Echo Pin -> GPIO 33
#define MAX_DISTANCE 200 // Max distance of 1 meter for sensor

// Handles for semaphores and mutex - you'll initialize these in the main program
SemaphoreHandle_t xButtonSem;
SemaphoreHandle_t sem_sensor;
SemaphoreHandle_t print_mutex;

//Soft Task 1: Flash Green LED to make sure the ride system is alive and safe
void heartbeat_task(void *pvParameters) {
 while (true) {
    if(!(xEventGroupGetBits(event_group) & ESTOP)){
    //Turn LED on for half second
    gpio_set_level(LED_GREEN, 1);
    vTaskDelay(pdMS_TO_TICKS(500));
    //Turn the LED off for half second
    gpio_set_level(LED_GREEN, 0);
    vTaskDelay(pdMS_TO_TICKS(500));
  }
  }
  
}

//Hard Task 2: Use ultrasonic distance sensor to detect objects in its path to avoid obstacles and ensure ride safety
void distance_sensor(void *pvParameters){
  float distance = 0; //initialize distance variable
  while(true){
    if(!(xEventGroupGetBits(event_group) & ESTOP)){
      bool overtime = false; //Checks if echo signal took too long or did not arrive
      gpio_set_level(PIN_TRIG, 0); //Set trigger pin to low
      esp_rom_delay_us(2);
      gpio_set_level(PIN_TRIG, 1); //Set trigger pin to high
      esp_rom_delay_us(10); //trigger ultrasonic sensor
      gpio_set_level(PIN_TRIG, 0);

    uint32_t timeout = MAX_DISTANCE * 58 * 2; //Calculation in ms to determine if readings are overtime
    
    //Find the current time
    int64_t t0 = esp_timer_get_time();

    //Check if the echo sensor provided a reading
    while(gpio_get_level(PIN_ECHO) == 0){
      if (esp_timer_get_time() - t0 >= timeout) {
        overtime = true; //If time took longer then break
        break;
      }
    }

     //Find the current time
    int64_t t_start = esp_timer_get_time();

    //Check to see if sensor reading is overtime
    while(!overtime && gpio_get_level(PIN_ECHO) == 1){
      //Checks microseconds readings
      if (esp_timer_get_time() - t_start > timeout) {       
        overtime = true;
        break;
      }
    }   

    //Fine the end time
    int64_t t_end = esp_timer_get_time();

    //If reading is not overtime, calculate distance 
    if (!overtime) {
          float duration_us = (float)(t_end - t_start);  //calculate duration of echo pulse
          distance = duration_us / 58.0f;  // Convert to distance in centimeters
        } else {
            distance = -1.0f; //If reading is out of range set to -1
        }

   //Check distance for any obstacles in front of ride from 50cm to 200cm      
  if (distance > 50 && distance < MAX_DISTANCE) {
        xSemaphoreGive(sem_sensor);  // If detected signal sensor event
    }

    if (xSemaphoreTake(print_mutex, portMAX_DELAY)) {
          printf("Ride Sensor Reading: %f cm\n", distance); //Display distance sensor readings
          xSemaphoreGive(print_mutex); 
    }

    vTaskDelay(pdMS_TO_TICKS(100)); 
    }
  }
}

//Hard Task 3: Trigger Emergency Stop Alert if button is pressed
//Stop Ride and all other functionalities for 5 seconds, restart after that time
void alertButton_task(void *pvParameters) {
    while (1) {
        int state = gpio_get_level(BUTTON_PIN); //check state of button
        static uint32_t last_press_tick = 0;  //Save the last time the button was pressed
        uint32_t current_tick = xTaskGetTickCount(); 

        // TODO 4a: Add addtional logic to prevent bounce effect (ignore multiple events for 'single press')
        if(state == 0 && (current_tick - last_press_tick) > pdMS_TO_TICKS(300))
        {
	        if (xSemaphoreTake(xButtonSem, portMAX_DELAY) == pdTRUE) {
            xEventGroupSetBits(event_group, ESTOP);

            if (xSemaphoreTake(print_mutex, portMAX_DELAY)) {
                printf("Emergency Stop Button Pressed: STOP RIDE\n"); //Print message if button is pressed
                xSemaphoreGive(print_mutex);
            }

            gpio_set_level(LED_RED, 1);
            vTaskDelay(pdMS_TO_TICKS(5000)); //Turn on red for 5 seconds
            xEventGroupClearBits(event_group, ESTOP);
            gpio_set_level(LED_RED, 0);

        }
            
        vTaskDelay(pdMS_TO_TICKS(10)); // Do Not Modify This Delay!
    }
  }
}

// === ISR: Triggered on button press ===
//Ememrgency Stop Button ISR
void IRAM_ATTR button_isr_handler(void *arg) {
      
      BaseType_t xHigherPrioTaskWoken = pdFALSE;
      xSemaphoreGiveFromISR(xButtonSem, &xHigherPrioTaskWoken);
      portYIELD_FROM_ISR(xHigherPrioTaskWoken);

}
//Soft Task 3: Detect if any obstacles are close to the ride vehicle and inform owner
void detectionHandler_task(void *pvParameters) {
    while (1) {
        //Semphore is called if obstacle is detected within distance limit
        if (xSemaphoreTake(sem_sensor, 0) && !(xEventGroupGetBits(event_group) & ESTOP)) {

            xSemaphoreTake(print_mutex, portMAX_DELAY);
            vTaskDelay(pdMS_TO_TICKS(200));
            printf("Ride Obstacle Detected: Take Action!\n"); 
            xSemaphoreGive(print_mutex);

            gpio_set_level(LED_BLUE, 1);
            vTaskDelay(pdMS_TO_TICKS(100)); //Flash Blue LED
            gpio_set_level(LED_BLUE, 0);
        }

        vTaskDelay(pdMS_TO_TICKS(10));  
    }
}

void app_main() {
  // Configure output LEDs
    gpio_config_t io_conf = {
        .pin_bit_mask = (1ULL << LED_GREEN) | (1ULL << LED_RED) | (1ULL << LED_BLUE),
        .mode = GPIO_MODE_OUTPUT,
    };
    gpio_config(&io_conf);

    //Configure Ultrasonic sensor
    gpio_reset_pin(PIN_TRIG);
    gpio_reset_pin(PIN_ECHO);
    gpio_set_direction(PIN_TRIG, GPIO_MODE_OUTPUT);
    gpio_set_direction(PIN_ECHO, GPIO_MODE_INPUT);


  //Install GPIO ISR Service Globally
    gpio_install_isr_service(0);

    //Configure Push Button
    gpio_reset_pin(BUTTON_PIN);
    gpio_set_direction(BUTTON_PIN, GPIO_MODE_INPUT);
    gpio_pullup_en(BUTTON_PIN);
    gpio_set_intr_type(BUTTON_PIN, GPIO_INTR_NEGEDGE);
    gpio_isr_handler_add(BUTTON_PIN, button_isr_handler, NULL);

    // Create synchronization primitives
    print_mutex = xSemaphoreCreateMutex(); 
    xButtonSem = xSemaphoreCreateBinary();
    sem_sensor = xSemaphoreCreateBinary();
    event_group = xEventGroupCreate();

  //Create tasks in Main function
    xTaskCreate(heartbeat_task, "Blink Task", 2048, NULL, 1, NULL);
    xTaskCreate(distance_sensor, "Display Sensor Task", 2048, NULL, 2, NULL);
    xTaskCreate(alertButton_task, "Emergency Stop Button Task", 2048, NULL, 2, NULL);
    xTaskCreate(detectionHandler_task, "Object Detection Task", 2048, NULL, 2, NULL); //Event Handler Task

}