/*
  - AI Fixes: button responsiveness and heartbeat starvation

  Project demonstrates an autonomous vehicle's parking sensor using an
  ultrasonic sensor.Emergency mode is activated when vehicle is within
   30cm of an object or person
    - emergency can be overwritten by holding the push button
      - when button is released emergency mode will turn on if and
      only if vehicle is still less than 30cm away from object
  
*/

#include <stdio.h>
#include <stdint.h>
#include <string.h> 
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"
#include "driver/gpio.h"
#include "esp_timer.h"
#include "esp_log.h"
#include "esp_rom_sys.h"
#include "esp_system.h"
#include "esp_random.h"



//Pins
#define LED_HEARTBEAT GPIO_NUM_15 
#define LED_EMERGENCY GPIO_NUM_4
#define BUTTON_GPIO   GPIO_NUM_18   //Chat changed this to 18 since it didnt work on 0
#define TRIG_GPIO     GPIO_NUM_16
#define ECHO_GPIO     GPIO_NUM_17

//Timing 
#define SENSOR_PERIOD_MS 50 
#define PROCESS_PERIOD_MS 100 
#define HEARTBEAT_MS 500 
#define DISTANCE_EMERGENCY_CM 30 


SemaphoreHandle_t echo_semaphore; //protects the sensor
SemaphoreHandle_t distance_mutex; //protects all distance variables
SemaphoreHandle_t print_mutex; //protects console
volatile bool emergency_override = false;
SemaphoreHandle_t override_mutex; //protects button 



//critical section variables
volatile int64_t echo_rise_us = 0;
volatile int64_t echo_fall_us = 0;
float shared_distance_cm = -1.0f;

/* Helper prototypes made by ChatGPT*/
void gpio_init(void);
void trigger_pulse(void);
float compute_distance_cm(int64_t rise_us, int64_t fall_us);
void echo_isr_handler(void*);


void gpio_init(void)
{
   //LED
    gpio_config_t io_conf = { 
      .pin_bit_mask = (1ULL << LED_HEARTBEAT) | (1ULL << LED_EMERGENCY), 
      .mode = GPIO_MODE_OUTPUT, 
      }; 
      gpio_config(&io_conf); 

      //Button
      gpio_config_t btn_conf = { 
        .pin_bit_mask = (1ULL << BUTTON_GPIO), 
        .mode = GPIO_MODE_INPUT, 
        .pull_up_en = GPIO_PULLUP_ENABLE 
        }; 
        gpio_config(&btn_conf); 

    // Trigger
    gpio_set_direction(TRIG_GPIO, GPIO_MODE_OUTPUT);
    gpio_set_level(TRIG_GPIO, 0);

    // Echo
    gpio_set_direction(ECHO_GPIO, GPIO_MODE_INPUT);
    gpio_set_intr_type(ECHO_GPIO, GPIO_INTR_ANYEDGE);

    // Install ISR service and add handler (safe to call once)
    gpio_install_isr_service(0);
    gpio_isr_handler_add(ECHO_GPIO, echo_isr_handler, NULL);
}

/* Accurate 10us trigger for HC-SR04 by chat*/
void trigger_pulse(void)
{
    gpio_set_level(TRIG_GPIO, 0);
    esp_rom_delay_us(2);
    gpio_set_level(TRIG_GPIO, 1);
    esp_rom_delay_us(10);
    gpio_set_level(TRIG_GPIO, 0);
}

//made by chat
float compute_distance_cm(int64_t rise_us, int64_t fall_us)
{
    int64_t dt = fall_us - rise_us;
    if (dt <= 0) return -1.0f;
    // distance cm = (dt_us * 0.0343) / 2
    return ((float)dt) * 0.0343f / 2.0f;
}

//parking sensor ISR
/* Minimal ISR: timestamp edges, give semaphore on falling by chat*/
void IRAM_ATTR echo_isr_handler(void* arg)
{
    int level = gpio_get_level(ECHO_GPIO);
    int64_t now = esp_timer_get_time();

    if (level) {
        echo_rise_us = now;
    } else {
        echo_fall_us = now;
        BaseType_t woken = pdFALSE;
        if (echo_semaphore) {
            xSemaphoreGiveFromISR(echo_semaphore, &woken);
            if (woken) portYIELD_FROM_ISR();
        }
    }
}

//hard deadline
/* Sensor task: triggers sensor, waits for ISR, updates shared_distance */
void parking_task(void* arg)
{
    const TickType_t period = pdMS_TO_TICKS(SENSOR_PERIOD_MS);
    TickType_t last = xTaskGetTickCount();

    for (;;) {
        vTaskDelayUntil(&last, period);

        trigger_pulse();

        // wait short time for echo (blocking so this task yields while waiting)
        if (xSemaphoreTake(echo_semaphore, pdMS_TO_TICKS(20)) == pdTRUE) {
            int64_t r = echo_rise_us;
            int64_t f = echo_fall_us;
            float d = compute_distance_cm(r, f);

            if (xSemaphoreTake(distance_mutex, pdMS_TO_TICKS(5)) == pdTRUE) {
                shared_distance_cm = d;
                xSemaphoreGive(distance_mutex);
            }
            xSemaphoreGive(echo_semaphore);
        } else {
            if (xSemaphoreTake(print_mutex, pdMS_TO_TICKS(100))){ 
              printf("sensor: echo timeout"); 
              xSemaphoreGive(print_mutex); 
            } 
            
        }
    }
}

//soft task
/* Processing task: simulate variable work but yield periodically */
void processing_task(void* arg)
{
    const TickType_t period = pdMS_TO_TICKS(PROCESS_PERIOD_MS);
    TickType_t last = xTaskGetTickCount(); 

    for (;;) {
        vTaskDelayUntil(&last, period);

        // read button (active-low)
        int btn = gpio_get_level(BUTTON_GPIO);

        // pick a workload size; heavy if pressed
        int workload = (btn == 0) ? 6000 : 1000 + (esp_random() % 2000);

        if (xSemaphoreTake(override_mutex, pdMS_TO_TICKS(5))) {
          emergency_override = (btn == 0);  // pressed = override
          xSemaphoreGive(override_mutex);
      }

        float local_d = -1.0f;
        if (xSemaphoreTake(distance_mutex, pdMS_TO_TICKS(5)) == pdTRUE) {
            local_d = shared_distance_cm;
            xSemaphoreGive(distance_mutex);
        }

        // simulated compute that cooperatively yields occasionally
        volatile uint32_t acc = 0;
        for (int i = 0; i < workload * 50; ++i) {
            acc += i ^ (acc << 1);
            // every 256 iterations yield to allow lower priority tasks to run (heartbeat)
            if ((i & 0xFF) == 0) {
                taskYIELD(); // cheap cooperative yield
            }
        }
        (void)acc;
       
        if (xSemaphoreTake(print_mutex, pdMS_TO_TICKS(100))){ 
          printf("process: dist=%.2fcm \n", local_d);
          xSemaphoreGive(print_mutex); 
        } 
       
    }
}

//hard task
/* Emergency: fast poll and set LED by chat */
void emergency_task(void* arg)
{
    for (;;) {
        vTaskDelay(pdMS_TO_TICKS(10));

        float local_d = -1.0f;
        if (xSemaphoreTake(distance_mutex, pdMS_TO_TICKS(2))) {
            local_d = shared_distance_cm;
            xSemaphoreGive(distance_mutex);
        }

        bool override = false;
        if (xSemaphoreTake(override_mutex, pdMS_TO_TICKS(2))) {
            override = emergency_override;
            xSemaphoreGive(override_mutex);
        }

        if (!override && local_d > 0 && local_d < DISTANCE_EMERGENCY_CM) {
            gpio_set_level(LED_EMERGENCY, 1); // emergency on
        } else {
            gpio_set_level(LED_EMERGENCY, 0); // off if override or safe distance
        }
    }
}

//soft deadline
//heartbeat task
void heartbeat_task(void* pvParameters){
    int state = 0;
    for(;;) {
        gpio_set_level(LED_HEARTBEAT, state);
        state = !state;
        vTaskDelay(pdMS_TO_TICKS(500));
    } 
}


void app_main(void)
{

    // create mutexes and semaphores
    echo_semaphore = xSemaphoreCreateBinary();
    distance_mutex = xSemaphoreCreateMutex();
    print_mutex = xSemaphoreCreateMutex();
    override_mutex = xSemaphoreCreateMutex();


    if (!echo_semaphore || !distance_mutex) {
        if (xSemaphoreTake(print_mutex, pdMS_TO_TICKS(100))){ 
          printf("Failed to create RTOS primitives");
          xSemaphoreGive(print_mutex); 
        } 
        while (1) vTaskDelay(pdMS_TO_TICKS(1000));
    }

    gpio_init();
    shared_distance_cm = -1.0f;

    // task creation
    xTaskCreatePinnedToCore(parking_task, "parking_task", 4096, NULL, 6, NULL, 1);
    xTaskCreatePinnedToCore(emergency_task, "emergency_task", 2048, NULL, 6, NULL, 1);
    xTaskCreatePinnedToCore(processing_task, "processing_task", 8192, NULL, 4, NULL, 1);
    xTaskCreatePinnedToCore(heartbeat_task, "heartbeat_task", 2048, NULL, 2, NULL, 0);


}