#include "ultrasonic.h"
#include "stm32f7xx_hal.h"
#include "config.h"

/* ================================================================
 * HC-SR04 Ultrasonic Sensor Parameters
 * ================================================================ */

#define TRIGGER_PIN         GPIO_PIN_0
#define TRIGGER_PORT        GPIOA
#define ECHO_PIN            GPIO_PIN_1
#define ECHO_PORT           GPIOA
#define ECHO_TIM            TIM1
#define ECHO_TIM_CHANNEL    TIM_CHANNEL_2

/* Trigger pulse duration (10µs recommended) */
#define TRIGGER_PULSE_US    10

/* Echo timeout: max 30ms (corresponds to ~5m distance) */
#define ECHO_TIMEOUT_MS     30

/* Speed of sound: ~343 m/s @ 20°C
 * Distance = (pulse_time_us / 2) / (1000000 / 343000) mm
 * Distance = (pulse_time_us / 2) / 2.914 mm ≈ pulse_time_us / 5.828 mm
 * Or: distance_mm = (pulse_us * 343) / 2000000 = pulse_us / 5.828
 * Using approximation: distance_mm ≈ (pulse_us * 1000) / 5830
 */
#define US_TO_MM_NUMERATOR  1000
#define US_TO_MM_DENOMINATOR 5830

/* ================================================================
 * Internal State Machine
 * ================================================================ */

typedef struct {
    UltrasonicState state;
    uint32_t trigger_time_ms;           /* When trigger pulse was sent */
    uint32_t echo_start_ticks;          /* TIM1 counter at rising edge */
    uint32_t echo_end_ticks;            /* TIM1 counter at falling edge */
    uint32_t echo_width_us;             /* Calculated pulse width in µs */
    uint16_t distance_mm;               /* Last measured distance */
    bool last_valid;                    /* Was last measurement valid */
    ultrasonic_callback_t callback;     /* Result callback (optional) */
} UltrasonicState_t;

static UltrasonicState_t s_ultrasonic = {
    .state = ULTRASONIC_IDLE,
    .callback = NULL
};

static TIM_HandleTypeDef s_htim1 = {0};  /* Timer handle for IRQ handler */

/* ================================================================
 * Hardware Initialization
 * ================================================================ */

void ultrasonic_init(void)
{
    /* Enable GPIO and timer clocks */
    __HAL_RCC_GPIOA_CLK_ENABLE();
    __HAL_RCC_TIM1_CLK_ENABLE();

    /* Configure PA0 as trigger output (push-pull, fast slew) */
    GPIO_InitTypeDef gpio_init = {0};
    gpio_init.Pin = TRIGGER_PIN;
    gpio_init.Mode = GPIO_MODE_OUTPUT_PP;
    gpio_init.Pull = GPIO_NOPULL;
    gpio_init.Speed = GPIO_SPEED_HIGH;
    HAL_GPIO_Init(TRIGGER_PORT, &gpio_init);
    HAL_GPIO_WritePin(TRIGGER_PORT, TRIGGER_PIN, GPIO_PIN_RESET);

    /* Configure PA1 as alternate function (TIM1_CH2) */
    gpio_init.Pin = ECHO_PIN;
    gpio_init.Mode = GPIO_MODE_AF_PP;
    gpio_init.Pull = GPIO_PULLDOWN;
    gpio_init.Speed = GPIO_SPEED_HIGH;
    gpio_init.Alternate = GPIO_AF1_TIM1;
    HAL_GPIO_Init(ECHO_PORT, &gpio_init);

    /* Configure TIM1 for input capture on PA1 (TIM1_CH2)
     * Clock: 216MHz / PSC = 216 counts/µs (PSC=1 gives 1 count per ~4.6ns)
     * Use PSC=216 to get 1MHz clock → 1 count = 1µs
     * ARR=0xFFFF for 16-bit capture (max 65535µs ≈ 9.6m)
     */
    s_htim1.Instance = ECHO_TIM;
    s_htim1.Init.Prescaler = 216 - 1;        /* 216MHz / 216 = 1MHz (1µs per count) */
    s_htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
    s_htim1.Init.Period = 0xFFFF;            /* 16-bit counter */
    s_htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
    s_htim1.Init.RepetitionCounter = 0;
    HAL_TIM_IC_Init(&s_htim1);

    /* Configure input capture: CH2 on PA1, both rising and falling edges
     * TIM1_CH2 captures on both edges to measure echo pulse width
     */
    TIM_IC_InitTypeDef ic_init = {0};
    ic_init.ICPolarity = TIM_ICPOLARITY_RISING;  /* Start with rising edge */
    ic_init.ICSelection = TIM_ICSELECTION_DIRECTTI;
    ic_init.ICPrescaler = TIM_ICPSC_DIV1;        /* No prescaler */
    ic_init.ICFilter = 0;                         /* No filter */
    HAL_TIM_IC_ConfigChannel(&s_htim1, &ic_init, ECHO_TIM_CHANNEL);
    HAL_TIM_IC_Start_IT(&s_htim1, ECHO_TIM_CHANNEL);

    /* Enable input capture interrupt */
    HAL_NVIC_SetPriority(TIM1_CC_IRQn, 6, 0);
    HAL_NVIC_EnableIRQ(TIM1_CC_IRQn);

    /* Start the timer */
    HAL_TIM_Base_Start(&s_htim1);

    s_ultrasonic.state = ULTRASONIC_IDLE;
}

/* ================================================================
 * Public API
 * ================================================================ */

bool ultrasonic_trigger(void)
{
    /* Only trigger if not currently measuring */
    if (s_ultrasonic.state != ULTRASONIC_IDLE && s_ultrasonic.state != ULTRASONIC_COMPLETE) {
        return false;
    }

    /* Send 10µs trigger pulse on PA0 */
    HAL_GPIO_WritePin(TRIGGER_PORT, TRIGGER_PIN, GPIO_PIN_SET);

    /* Busy-wait for 10µs (non-ideal but simple and precise)
     * At 216MHz: 1 clock = ~4.6ns, so ~2160 clocks for 10µs
     */
    uint32_t start = HAL_GetTick();
    while ((HAL_GetTick() - start) < 1) {
        /* Wait at least 10µs — use sysclock counter for precision */
        for (volatile int i = 0; i < 2200; i++) __NOP();
    }
    HAL_GPIO_WritePin(TRIGGER_PORT, TRIGGER_PIN, GPIO_PIN_RESET);

    s_ultrasonic.state = ULTRASONIC_TRIGGERED;
    s_ultrasonic.trigger_time_ms = HAL_GetTick();
    s_ultrasonic.echo_start_ticks = 0;
    s_ultrasonic.echo_end_ticks = 0;

    return true;
}

void ultrasonic_set_callback(ultrasonic_callback_t callback)
{
    s_ultrasonic.callback = callback;
}

UltrasonicState ultrasonic_get_state(void)
{
    return s_ultrasonic.state;
}

bool ultrasonic_get_result(uint16_t *distance_mm, bool *is_valid)
{
    if (s_ultrasonic.state != ULTRASONIC_COMPLETE) {
        return false;
    }

    if (distance_mm) *distance_mm = s_ultrasonic.distance_mm;
    if (is_valid) *is_valid = s_ultrasonic.last_valid;

    s_ultrasonic.state = ULTRASONIC_IDLE;
    return true;
}

void ultrasonic_tick(uint32_t now_ms)
{
    /* Timeout detection: if measurement takes too long, mark as error */
    if (s_ultrasonic.state == ULTRASONIC_MEASURING) {
        if ((now_ms - s_ultrasonic.trigger_time_ms) > ECHO_TIMEOUT_MS) {
            s_ultrasonic.state = ULTRASONIC_COMPLETE;
            s_ultrasonic.distance_mm = 0;
            s_ultrasonic.last_valid = false;

            if (s_ultrasonic.callback) {
                s_ultrasonic.callback(0, false);
            }
        }
    }
}

/* ================================================================
 * TIM1 Input Capture Interrupt Handler
 * ================================================================ */

void TIM1_CC_IRQHandler(void)
{
    /* Check if capture interrupt on CH2 */
    if (__HAL_TIM_GET_FLAG(&s_htim1, TIM_FLAG_CC2) != RESET) {
        __HAL_TIM_CLEAR_FLAG(&s_htim1, TIM_FLAG_CC2);

        uint32_t capture_value = HAL_TIM_ReadCapturedValue(&s_htim1, ECHO_TIM_CHANNEL);

        if (s_ultrasonic.state == ULTRASONIC_TRIGGERED || s_ultrasonic.state == ULTRASONIC_MEASURING) {
            if (s_ultrasonic.echo_start_ticks == 0) {
                /* Rising edge: mark start of echo pulse */
                s_ultrasonic.echo_start_ticks = capture_value;
                s_ultrasonic.state = ULTRASONIC_MEASURING;

                /* Switch to falling edge detection for end of echo */
                TIM_IC_InitTypeDef ic_init = {0};
                ic_init.ICPolarity = TIM_ICPOLARITY_FALLING;
                ic_init.ICSelection = TIM_ICSELECTION_DIRECTTI;
                ic_init.ICPrescaler = TIM_ICPSC_DIV1;
                ic_init.ICFilter = 0;
                HAL_TIM_IC_ConfigChannel(&s_htim1, &ic_init, ECHO_TIM_CHANNEL);
            } else {
                /* Falling edge: mark end of echo pulse and calculate distance */
                s_ultrasonic.echo_end_ticks = capture_value;

                /* Calculate pulse width (accounting for timer overflow) */
                uint32_t width = (s_ultrasonic.echo_end_ticks >= s_ultrasonic.echo_start_ticks)
                    ? (s_ultrasonic.echo_end_ticks - s_ultrasonic.echo_start_ticks)
                    : (0xFFFF - s_ultrasonic.echo_start_ticks + s_ultrasonic.echo_end_ticks + 1);

                s_ultrasonic.echo_width_us = width;

                /* Convert pulse width to distance: distance_mm = pulse_us / 5.828
                 * Using integer arithmetic: (pulse_us * 1000) / 5830
                 */
                s_ultrasonic.distance_mm = (width * US_TO_MM_NUMERATOR) / US_TO_MM_DENOMINATOR;

                /* Validate range: 20-5000mm */
                s_ultrasonic.last_valid = (s_ultrasonic.distance_mm >= 20 && s_ultrasonic.distance_mm <= 5000);
                if (!s_ultrasonic.last_valid) {
                    s_ultrasonic.distance_mm = 0;
                }

                s_ultrasonic.state = ULTRASONIC_COMPLETE;

                /* Call callback if registered */
                if (s_ultrasonic.callback) {
                    s_ultrasonic.callback(s_ultrasonic.distance_mm, s_ultrasonic.last_valid);
                }

                /* Reset for next measurement */
                s_ultrasonic.echo_start_ticks = 0;
                s_ultrasonic.echo_end_ticks = 0;
            }
        }
    }

    HAL_TIM_IRQHandler(&s_htim1);
}