#include "stm32c0xx_hal.h"
#include "FreeRTOS.h"
#include "task.h"

// Constants for ultrasonic sensors
#define LEFT_TRIG_PIN GPIO_PIN_10
#define LEFT_ECHO_PIN GPIO_PIN_4
#define RIGHT_TRIG_PIN GPIO_PIN_2
#define RIGHT_ECHO_PIN GPIO_PIN_7
#define FRONT_TRIG_PIN GPIO_PIN_15
#define FRONT_ECHO_PIN GPIO_PIN_5
#define REAR_TRIG_PIN GPIO_PIN_10
#define REAR_ECHO_PIN GPIO_PIN_3

// Task function to blink an LED
void blinkTest(void *pvParameters) {
  while (1) {
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_8, GPIO_PIN_SET);
    vTaskDelay(pdMS_TO_TICKS(500));
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_8, GPIO_PIN_RESET);
    vTaskDelay(pdMS_TO_TICKS(500));
  }
}

// Function to initialize GPIO pins for ultrasonic sensors
void initUltrasonicPins() {
  // Enable clocks for both GPIOA and GPIOB
  __HAL_RCC_GPIOA_CLK_ENABLE();
  __HAL_RCC_GPIOB_CLK_ENABLE();

  GPIO_InitTypeDef GPIO_InitStruct = {0};

  // Configure GPIOB pins for Trig as output
  GPIO_InitStruct.Pin = LEFT_TRIG_PIN | RIGHT_TRIG_PIN | REAR_TRIG_PIN;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

  // Configure GPIOB pins for Echo as input
  GPIO_InitStruct.Pin = LEFT_ECHO_PIN | RIGHT_ECHO_PIN | REAR_ECHO_PIN;
  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

  // Configure GPIOA pins for Trig as output
  GPIO_InitStruct.Pin = FRONT_TRIG_PIN;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

  // Configure GPIOA pins for Echo as input
  GPIO_InitStruct.Pin = FRONT_ECHO_PIN;
  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
}


// Function to calculate distance from ultrasonic sensor
long getDistance(uint16_t trigPin, uint16_t echoPin) {
  // Send trigger pulse
  HAL_GPIO_WritePin(GPIOB, trigPin, GPIO_PIN_RESET);
  HAL_Delay(5);
  HAL_GPIO_WritePin(GPIOB, trigPin, GPIO_PIN_SET);
  HAL_Delay(10);
  HAL_GPIO_WritePin(GPIOB, trigPin, GPIO_PIN_RESET);

  // Wait for echo and measure pulse duration (timers required)
  // Add pulse measurement using a timer or polling the GPIO pin

  long duration = 0; // Replace this with actual pulse duration measurement
  long distance = (duration * 0.034) / 2;  // Calculate distance based on the speed of sound
  return distance;
}

// Task to read direction sensors (using ultrasonic sensors)
void readDirectionSensors(void *pvParameters) {
  while (1) {
    // Get distances from both ultrasonic sensors
    long leftDistance = getDistance(LEFT_TRIG_PIN, LEFT_ECHO_PIN);
    long rightDistance = getDistance(RIGHT_TRIG_PIN, RIGHT_ECHO_PIN);
    long frontDistance = getDistance(FRONT_TRIG_PIN, FRONT_ECHO_PIN);
    long rearDistance = getDistance(REAR_TRIG_PIN, REAR_ECHO_PIN);

    // Print distance measurements (Replace with UART or other method)
    printf("Left Distance: %ld cm\n", leftDistance);
    printf("Right Distance: %ld cm\n", rightDistance);
    printf("Front Distance: %ld cm\n", frontDistance);
    printf("Rear Distance: %ld cm\n", rearDistance);
    vTaskDelay(pdMS_TO_TICKS(1000)); // Delay for 1 second
  }
}

void setup() {
  HAL_Init();
  SystemClock_Config();  // Assuming this is configured

  // Initialize pins and peripherals
  initUltrasonicPins();

  // Create tasks
  // xTaskCreate(blinkTest, "Blink LED", configMINIMAL_STACK_SIZE, NULL, 1, NULL);
  xTaskCreate(readDirectionSensors, "Read Direction", configMINIMAL_STACK_SIZE, NULL, 1, NULL);

  // Start the scheduler
  vTaskStartScheduler();
}

void loop() {
  // Empty loop since FreeRTOS handles tasks
}