#include "stm32c0xx.h"
#include <stdint.h>

/* This example assumes the STM32C0 core clock is configured to 48 MHz. */
#define CORE_CLOCK_HZ 48000000UL
#define CYCLES_PER_US (CORE_CLOCK_HZ / 1000000UL)
#define SYSTICK_MASK  0x00FFFFFFUL

#define RED_PIN    11U  /* PA11 */
#define GREEN_PIN  12U  /* PA12 */
#define BLUE_PIN   10U  /* PA10 */
#define TRIG_PIN    3U  /* PB3 */
#define ECHO_PIN    4U  /* PB4 */

#define ECHO_TIMEOUT_US      30000UL
#define ECHO_TIMEOUT_CYCLES  (ECHO_TIMEOUT_US * CYCLES_PER_US)

static void systick_init(void);
static uint32_t systick_elapsed_cycles(uint32_t start_value);
static void delay_us(uint32_t us);
static void delay_ms(uint32_t ms);
static void gpio_init(void);
static void set_led(uint8_t red, uint8_t green, uint8_t blue);
static float measure_distance_cm(void);

/* Use SysTick as a continuously running 24-bit down-counter. */
static void systick_init(void)
{
    SysTick->CTRL = 0U;
    SysTick->LOAD = SYSTICK_MASK;
    SysTick->VAL = 0U;
    SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk | SysTick_CTRL_ENABLE_Msk;
}

/* Handles one SysTick wrap; each measurement timeout is less than one wrap. */
static uint32_t systick_elapsed_cycles(uint32_t start_value)
{
    uint32_t current_value = SysTick->VAL;
    return (start_value - current_value) & SYSTICK_MASK;
}

static void delay_us(uint32_t us)
{
    uint32_t required_cycles = us * CYCLES_PER_US;
    uint32_t start_value = SysTick->VAL;

    while (systick_elapsed_cycles(start_value) < required_cycles) {
    }
}

static void delay_ms(uint32_t ms)
{
    while (ms-- > 0U) {
        delay_us(1000U);
    }
}

static void gpio_init(void)
{
    RCC->IOPENR |= RCC_IOPENR_GPIOAEN | RCC_IOPENR_GPIOBEN;

    GPIOA->MODER &= ~((3U << (RED_PIN * 2U)) |
                      (3U << (GREEN_PIN * 2U)) |
                      (3U << (BLUE_PIN * 2U)));
    GPIOA->MODER |=  ((1U << (RED_PIN * 2U)) |
                      (1U << (GREEN_PIN * 2U)) |
                      (1U << (BLUE_PIN * 2U)));

    GPIOB->MODER &= ~(3U << (TRIG_PIN * 2U));
    GPIOB->MODER |=  (1U << (TRIG_PIN * 2U));
    GPIOB->MODER &= ~(3U << (ECHO_PIN * 2U));

    /* The original set_led logic is active-low (for a common-anode RGB LED). */
    GPIOA->BSRR = (1U << RED_PIN) | (1U << GREEN_PIN) | (1U << BLUE_PIN);
    GPIOB->BSRR = 1U << (TRIG_PIN + 16U);
}

static void set_led(uint8_t red, uint8_t green, uint8_t blue)
{
    GPIOA->BSRR = red ? (1U << (RED_PIN + 16U)) : (1U << RED_PIN);
    GPIOA->BSRR = green ? (1U << (GREEN_PIN + 16U)) : (1U << GREEN_PIN);
    GPIOA->BSRR = blue ? (1U << (BLUE_PIN + 16U)) : (1U << BLUE_PIN);
}

/* Returns 0 for timeout/error; a valid HC-SR04 reading is at least 2 cm. */
static float measure_distance_cm(void)
{
    const uint32_t echo_mask = 1U << ECHO_PIN;
    uint32_t wait_start;
    uint32_t pulse_start;
    uint32_t pulse_cycles;

    GPIOB->BSRR = 1U << (TRIG_PIN + 16U); /* TRIG low */
    delay_us(2U);
    GPIOB->BSRR = 1U << TRIG_PIN;         /* TRIG high */
    delay_us(10U);
    GPIOB->BSRR = 1U << (TRIG_PIN + 16U); /* TRIG low */

    wait_start = SysTick->VAL;
    while ((GPIOB->IDR & echo_mask) == 0U) {
        if (systick_elapsed_cycles(wait_start) >= ECHO_TIMEOUT_CYCLES) {
            return 0.0f;
        }
    }

    pulse_start = SysTick->VAL;
    while ((GPIOB->IDR & echo_mask) != 0U) {
        if (systick_elapsed_cycles(pulse_start) >= ECHO_TIMEOUT_CYCLES) {
            return 0.0f;
        }
    }

    pulse_cycles = systick_elapsed_cycles(pulse_start);

    /* HC-SR04 approximation: distance_cm = echo_high_time_us / 58. */
    return (float)pulse_cycles / ((float)CYCLES_PER_US * 58.0f);
}

int main(void)
{
    systick_init();
    gpio_init();
    set_led(0U, 0U, 0U);

    while (1) {
        float distance_cm = measure_distance_cm();

        if (distance_cm >= 2.0f && distance_cm <= 25.0f) {
            set_led(1U, 0U, 0U); /* Red */
        } else if (distance_cm > 25.0f && distance_cm <= 50.0f) {
            set_led(0U, 1U, 0U); /* Green */
        } else if (distance_cm > 50.0f && distance_cm <= 400.0f) {
            set_led(0U, 0U, 1U); /* Blue */
        } else {
            set_led(0U, 0U, 0U); /* Off: timeout or out of range */
        }

        delay_ms(100U);
    }
}