#pragma once

#include "../Helper/delay_helper.h"
#include "stm32h563xx.h"
#include "stm32h5xx_hal_tim.h"
#include <cstdint>
#include <stdexcept>

#include "../Helper/gpio_helper.hpp"
#include "../Helper/task_helper.hpp"

class Ultrasonic {
public:
    Ultrasonic(GPIO_TypeDef* gpio, uint16_t pin, TIM_HandleTypeDef* timer, uint32_t channel)
        : Ultrasonic({gpio, pin}, timer, channel) {}

    Ultrasonic(const GpioHelper::Gpio& trigger, TIM_HandleTypeDef* timer, uint32_t channel)
        : trigger(trigger), timer(timer), channel(channel) {
        captureFlag = GetCaptureFlag(channel);
    }

    ~Ultrasonic()                            = default;
    Ultrasonic(const Ultrasonic&)            = delete;
    Ultrasonic& operator=(const Ultrasonic&) = delete;
    Ultrasonic(Ultrasonic&&)                 = delete;
    Ultrasonic& operator=(Ultrasonic&&)      = delete;

    double GetDistance(uint32_t time = 1) {
        double distance = 0;
        for (uint32_t cnt = 0; cnt < time; ++cnt) {
            Trigger();
            auto echoTime = MeasureEchoTime();
            distance += CalculateDistance(echoTime);
        }
        return distance / time;
    }

private:
    constexpr static uint32_t kTimeout = 100;

    GpioHelper::Gpio trigger;
    TIM_HandleTypeDef* timer;
    uint32_t channel;
    uint32_t captureFlag;

    void Trigger() {
        trigger.Set();
        DelayUs(10);
        trigger.Reset();
    }

    double MeasureEchoTime() {
        timer->Instance->CNT = 0;
        uint32_t data[2];
        __HAL_TIM_CLEAR_FLAG(timer, captureFlag);
        HAL_TIM_IC_Start(timer, channel);
        try {
            for (auto& value : data) {
                TaskHelper::WaitFor(
                    [&]() {
                        if (__HAL_TIM_GET_FLAG(timer, captureFlag)) {
                            __HAL_TIM_CLEAR_FLAG(timer, captureFlag);
                            value = HAL_TIM_ReadCapturedValue(timer, channel);
                            return true;
                        }
                        return false;
                    },
                    kTimeout);
            }
            HAL_TIM_IC_Stop(timer, channel);
            return (data[1] - data[0]) / 250.0;
        } catch (const std::exception& e) {
            HAL_TIM_IC_Stop(timer, channel);
            throw;
        }
    }

    [[nodiscard]] static double CalculateDistance(double echoTime) {
        return (echoTime * 0.0343) / 2.0;
    }

    static uint32_t GetCaptureFlag(uint32_t channel) {
        switch (channel) {
        case TIM_CHANNEL_1:
            return TIM_SR_CC1IF;
        case TIM_CHANNEL_2:
            return TIM_SR_CC2IF;
        case TIM_CHANNEL_3:
            return TIM_SR_CC3IF;
        case TIM_CHANNEL_4:
            return TIM_SR_CC4IF;
        default:
            throw std::invalid_argument("Invalid timer channel");
        }
    }
};