#define TRIG_PIN GPIO_PIN_5
#define ECHO_PIN GPIO_PIN_6
#define LED_PIN GPIO_PIN_7
#define ULTRASONIC_PORT GPIOA

#include "stm32c0xx_hal.h"

void delay_us(uint32_t us) {
  uint32_t start = micros();
  while ((micros() - start) < us);
}

void setup() {
  Serial.begin(115200);
  __HAL_RCC_GPIOA_CLK_ENABLE();

  GPIO_InitTypeDef GPIO_InitStruct = {0};

  // TRIG pin as output
  GPIO_InitStruct.Pin = TRIG_PIN;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(ULTRASONIC_PORT, &GPIO_InitStruct);

  // ECHO pin as input
  GPIO_InitStruct.Pin = ECHO_PIN;
  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  HAL_GPIO_Init(ULTRASONIC_PORT, &GPIO_InitStruct);

  // LED pin as output
  GPIO_InitStruct.Pin = LED_PIN;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(ULTRASONIC_PORT, &GPIO_InitStruct);

  // Start with LED off
  HAL_GPIO_WritePin(ULTRASONIC_PORT, LED_PIN, GPIO_PIN_RESET);
}

long readUltrasonicDistance() {
  HAL_GPIO_WritePin(ULTRASONIC_PORT, TRIG_PIN, GPIO_PIN_RESET);
  delay_us(2);
  HAL_GPIO_WritePin(ULTRASONIC_PORT, TRIG_PIN, GPIO_PIN_SET);
  delay_us(10);
  HAL_GPIO_WritePin(ULTRASONIC_PORT, TRIG_PIN, GPIO_PIN_RESET);

  uint32_t timeout = 30000;
  uint32_t startTime = micros();
  while(HAL_GPIO_ReadPin(ULTRASONIC_PORT, ECHO_PIN) == GPIO_PIN_RESET) {
    if (micros() - startTime > timeout) return -1;
  }

  uint32_t echoStart = micros();
  while(HAL_GPIO_ReadPin(ULTRASONIC_PORT, ECHO_PIN) == GPIO_PIN_SET) {
    if (micros() - echoStart > timeout) return -1;
  }
  uint32_t echoEnd = micros();

  uint32_t pulseDuration = echoEnd - echoStart;
  return pulseDuration / 58;  // distance in cm
}

void loop() {
  long distance = readUltrasonicDistance();

  if (distance == -1) {
    Serial.println("Out of range or no response");
    HAL_GPIO_WritePin(ULTRASONIC_PORT, LED_PIN, GPIO_PIN_RESET);
  } else {
    Serial.print("Distance: ");
    Serial.print(distance);
    Serial.println(" cm");

    if (distance < 100) {
      HAL_GPIO_WritePin(ULTRASONIC_PORT, LED_PIN, GPIO_PIN_SET);  // LED ON
    } else {
      HAL_GPIO_WritePin(ULTRASONIC_PORT, LED_PIN, GPIO_PIN_RESET);  // LED OFF
    }
  }

  delay(500);
}