/*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
#define PIN_ECHO GPIO_NUM_33
#define MAX_DISTANCE 200 // Max distance of 1 meter

#define MAX_COUNT_SEM 10 
#define SENSOR_THRESHOLD 3000

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

bool led_state = true;
bool emergency_reset = false;

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

//Hard Task 3: Distance detection sensor to avoid vehicle collision
void distance_sensor(void *pvParameters){
  float distance = 0;
  while(true){
    if(!(xEventGroupGetBits(event_group) & ESTOP)){
      bool overtime = false;
      gpio_set_level(PIN_TRIG, 0);
      esp_rom_delay_us(2);
      gpio_set_level(PIN_TRIG, 1);
      esp_rom_delay_us(10);
      gpio_set_level(PIN_TRIG, 0);

    uint32_t start_tick = xTaskGetTickCount(); 

    // Unit of the following is in microseconds NOT ticks!
    uint32_t timeout = MAX_DISTANCE * 58 * 2;
    
    // Let's grab the current time
    int64_t t0 = esp_timer_get_time();

    while(gpio_get_level(PIN_ECHO) == 0){
      if (esp_timer_get_time() - t0 >= timeout) {
      //if(xTaskGetTickCount() - start_tick >= timeout){
        overtime = true;
        break;
      }
    }

   start_tick = xTaskGetTickCount(); 

     // Let's grab the current time
    int64_t t_start = esp_timer_get_time();

    // added the overtime check here 
    while(!overtime && gpio_get_level(PIN_ECHO) == 1){
      // modified to use ms instead of ticks
      if (esp_timer_get_time() - t_start > timeout) {       
        overtime = true;
        break;
      }
    }

    uint32_t end_tick = xTaskGetTickCount();     

    // Let's grab the end time
    int64_t t_end = esp_timer_get_time();

    // added else
    if (!overtime) {
          float duration_us = (float)(t_end - t_start);
          distance = duration_us / 58.0f;  // cm
        } else {
            distance = -1.0f;
        }

        
  if (distance > 50 && distance < MAX_DISTANCE) {
        xSemaphoreGive(sem_sensor);  // Signal sensor event
    }

    if (xSemaphoreTake(print_mutex, portMAX_DELAY)) {
          printf("sensor reading: %f cm\n", distance);
          xSemaphoreGive(print_mutex); 
    }

    vTaskDelay(pdMS_TO_TICKS(100)); 
  }
  }
}

//Hard Task 2: Trigger Emergency Stop Alert if button is pressed
void alertButton_task(void *pvParameters) {
    while (1) {
        int state = gpio_get_level(BUTTON_PIN);
        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) {
           // emergency_reset = true;
            //if(emergency_reset == true){
            xEventGroupSetBits(event_group, ESTOP);

            // Lock mutex to read from buffer
            if (xSemaphoreTake(print_mutex, portMAX_DELAY)) {
                printf("Emergency Stop Button Pressed\n");
                xSemaphoreGive(print_mutex);
            }

            gpio_set_level(LED_RED, 1);
            vTaskDelay(pdMS_TO_TICKS(5000)); 
            xEventGroupClearBits(event_group, ESTOP);
            gpio_set_level(LED_RED, 0);


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

// === ISR: Triggered on button press ===
//Implement simple software debounce
void IRAM_ATTR button_isr_handler(void *arg) {
      
      BaseType_t xHigherPrioTaskWoken = pdFALSE;
      xSemaphoreGiveFromISR(xButtonSem, &xHigherPrioTaskWoken);
      portYIELD_FROM_ISR(xHigherPrioTaskWoken);

}

//Soft Task 4: Send data or response from detection data 
void distanceHandler_task(void *pvParameters) {
  while(true){ // if this is not true, then we would end the task and return -- BAD :)

        if (xSemaphoreTake(sem_sensor, 0) && !(xEventGroupGetBits(event_group) & ESTOP)) {
            xSemaphoreTake(print_mutex, portMAX_DELAY);
            vTaskDelay(pdMS_TO_TICKS(200));
            printf("Object Intrusion Alert\n");
            xSemaphoreGive(print_mutex);

            gpio_set_level(LED_BLUE, 1);
            vTaskDelay(pdMS_TO_TICKS(100));
            gpio_set_level(LED_BLUE, 0);
        }

        vTaskDelay(pdMS_TO_TICKS(10)); // Idle delay to yield CPU
    }
}

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);

    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, "Alert Button Task", 2048, NULL, 2, NULL);
    xTaskCreate(distanceHandler_task, "intrusion_event_handler", 2048, NULL, 2, NULL); //Event Handler Task

}