#include <Arduino_FreeRTOS.h>
#include <task.h>
#include <queue.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>

#define F_CPU 1000000UL

#define TRIG_PIN PB0
#define ECHO_PIN PD7
#define BUZZER_PIN PB2
#define LED_PIN PB3
#define RED_BUTTON PD2
#define GREEN_BUTTON PD3

long Duration;
int Dis;//Distance
int SafetyDistance;
volatile int FLAG = 1; 
volatile int MeasureDistanceFlag = 1;

TaskHandle_t ultrasonicTaskHandle;
TaskHandle_t safety_task_Handle;

QueueHandle_t xQueue;

void vultrasonicTask(void *pvParameters);
void vsafetyTask(void *pvParameters);

ISR(INT0_vect);
ISR(INT1_vect);

void setup() {
    DDRB |= (1 << TRIG_PIN) | (1 << BUZZER_PIN) | (1 << LED_PIN); 
    DDRD &= ~(1 << ECHO_PIN) & ~(1 << RED_BUTTON) & ~(1 << GREEN_BUTTON); 
    PORTD |= (1 << RED_BUTTON) | (1 << GREEN_BUTTON); 

    Serial.begin(9600); 

    
    EICRA = (1 << ISC01) | (1 << ISC11); 
    EIMSK = (1 << INT0) | (1 << INT1); 
    sei(); // Enable global interrupts

    xQueue = xQueueCreate(1, sizeof(int));

    PORTB |= (1 << BUZZER_PIN) | (1 << LED_PIN);

    xTaskCreate(ultrasonicTask, "ultrasonicTask", configMINIMAL_STACK_SIZE, NULL, 1, &ultrasonicTaskHandle);

    xTaskCreate(vsafetyTask, "safetyTask", configMINIMAL_STACK_SIZE, NULL, 1, &safety_task_Handle);
    vTaskStartScheduler();
}

void loop() {
    //  empty 
}

void ultrasonicTask(void *pvParameters) {
    for (;;) {
        if (MeasureDistanceFlag && FLAG) {
            PORTB &= ~(1 << TRIG_PIN);
            vTaskDelay(pdMS_TO_TICKS(2));
            PORTB |= (1 << TRIG_PIN); 
            vTaskDelay(pdMS_TO_TICKS(10));
            PORTB &= ~(1 << TRIG_PIN); 


            Duration = pulseIn(ECHO_PIN, HIGH);
            Dis = ((Duration * 0.034 / 2) + 1);
            SafetyDistance = Dis;

           
            Serial.print("Distance: ");
            Serial.println(Dis);

            if (Dis < 10) {
                PORTB |= (1 << BUZZER_PIN) ;
                Serial.println("Buzzer ON");
            } else {
                PORTB &= ~(1 << BUZZER_PIN) ;
                Serial.println("Buzzer OFF");
            }
        }

        vTaskDelay(pdMS_TO_TICKS(100));
    }
}

void vsafetyTask(void *pvParameters) {
    int SemaphoreValue;
    for (;;) {
        if (xQueueReceive(xQueue, &SemaphoreValue, portMAX_DELAY) == pdPASS) {
            if (SemaphoreValue == 0) {
                PORTB &= ~((1 << BUZZER_PIN) | (1 << LED_PIN));
                MeasureDistanceFlag = 0; 
            } else if (SemaphoreValue == 1) {
                PORTB |= (1 << BUZZER_PIN) | (1 << LED_PIN);
                MeasureDistanceFlag = 1;
            }
        }
    }
}

ISR(INT0_vect) {
    int SemaphoreValue = 0;
    Serial.println("Red button pressed");
    xQueueSendFromISR(xQueue, &SemaphoreValue, NULL);
    portYIELD_FROM_ISR();
}

ISR(INT1_vect) {
    int SemaphoreValue = 1;
    Serial.println("Green button pressed");
    xQueueSendFromISR(xQueue, &SemaphoreValue, NULL);
    portYIELD_FROM_ISR();
}