#include <usbd_cdc_if.h>
#include <cinttypes>
#include "radio-controller.h"
#include "mcu/arm/mutex.h"
#include "mcu/arm/semihosting.h"
#include "mcu/stm32cube/uart.h"
#include "mcu/stm32cube/debug.h"

using mcu::arm::mutex;

extern IWDG_HandleTypeDef hiwdg;
extern TIM_HandleTypeDef htim1;
extern UART_HandleTypeDef huart2;

mcu::stm32cube::uart::uart_port uart2(&huart2);
mcu::stm32cube::debug::dbg<100> dbg(uart2);

struct sample {
    uint16_t period_us;
    uint16_t pulse_us;
};

template<unsigned int BufferSize>
class sample_buffer {
    sample samples[BufferSize];

    volatile unsigned int _size;

    bool _first;

public:
    void reset()
    {
        _size = 0;
        _first = true;
    }

    bool check_first()
    {
        if (_first) {
            _first = false;
            return true;
        }
        return false;
    }

    __always_inline
    unsigned int size() const
    {
        return _size;
    }

    __always_inline
    bool is_full() const
    {
        return _size == BufferSize;
    }

    __always_inline
    void append(sample sample)
    {
        if (_size == BufferSize) {
            return;
        }

        samples[_size++] = sample;
    }

    __always_inline
    sample at(int i) const
    {
        return samples[i];
    }
};

volatile int sample_index;
sample_buffer<10> buffer;
mutex buffer_lock;

void main_pre_init()
{
}

inline void hal_ok(HAL_StatusTypeDef status)
{
    if (__predict_true(status == HAL_OK)) {
        return;
    }

    halt();
}

void main_post_init()
{
    bool debugger_connected = (CoreDebug->DHCSR & CoreDebug_DHCSR_C_DEBUGEN_Msk) != 0;
    if (debugger_connected) {
        semihosting::enable();
    }

    printf("Radio Controller\n");

    buffer.reset();
    sample_index = 0;
    uart2.enable();

    hal_ok(HAL_TIM_IC_Start_IT(&htim1, TIM_CHANNEL_1));
}

static uint32_t tick_next = 0;

//static bool seen_high = false;

#include <chrono>
#include <stm32f1xx_hal_tim.h>
#include <stm32f103x6.h>

template<unsigned Fcpu, typename SecondsPerTimerTick>
struct from_f_cpu_and_time_per_timer_tick {
    template<std::intmax_t Num, std::intmax_t Den>
    using r = std::ratio<Num, Den>;

    using f_cpu = r<Fcpu, 1>;
    using seconds_per_cpu_tick = r<1, f_cpu::num>;
    using seconds_per_timer_tick = SecondsPerTimerTick;
    using timer_frequency = r<SecondsPerTimerTick::den, SecondsPerTimerTick::num>;

//    static constexpr std::intmax_t prescaler = std::ratio_divive<f_cpu, timer_frequency>::value;
    using prescaler_t = std::ratio_divide<f_cpu, timer_frequency>;
    static_assert(prescaler_t::den == 1, "Could not represent the prescaler as an integer");
    static constexpr std::intmax_t prescaler = prescaler_t::num;

    // us_per_timer_tick = seconds_per_timer_tick * 1'000'000
    using us_per_timer_tick_t = typename std::ratio_multiply<seconds_per_timer_tick, r<1'000'000, 1>>::type;

    static constexpr uint16_t us_per_timer_tick = static_cast<uint16_t>(us_per_timer_tick_t::num);

    template<typename T>
    __always_inline constexpr
    static T to_us(unsigned int count)
    {
        return T(count * us_per_timer_tick);
    }
};

using values = from_f_cpu_and_time_per_timer_tick<72'000'000, std::ratio_multiply<std::ratio<5, 1>, std::micro>>;

//static_assert(values::timer_frequency::num == 200'000, "timer_frequency::num");
static_assert(values::timer_frequency::den == 1, "timer_frequency::den");
//static_assert(std::is_same<
//    values::timer_frequency,
//    std::ratio<200'000, 1>>::value, "Timer frequency is not 200kHz");
static_assert(values::prescaler == 360, "Timer frequency is not 200kHz");
static_assert(values::prescaler - 1 == RADIO_RX_TIMER_PRESCALER, "Bad prescaler from STM32CubeMX");
static_assert(values::us_per_timer_tick == 5, "bad us_per_timer_tick");

void main_loop()
{
    auto now = HAL_GetTick();

    if (now >= tick_next) {
        printf("now=%" PRIu32 "\n", now);

//        auto *str = "1234567890\n";
//        CDC_Transmit_FS(const_cast<uint8_t *>(reinterpret_cast<const uint8_t *>(str)), 5);

        tick_next += 1000;
//        seen_high = false;
    }

    if (buffer.is_full()) {
        buffer_lock.lock();

        hal_ok(HAL_TIM_IC_Stop_IT(&htim1, TIM_CHANNEL_1));

//            printf("Period:");
//            for (unsigned int i = 0; i < buffer.size(); i++) {
//                printf(" %u", buffer.at(i));
//            }
//            printf("\n");

//            dbg.print("Pulse: ");
//            for (unsigned int i = 0; i < buffer.size(); i++) {
//                auto p = values::to_us(buffer_pulse.at(i));
//                dbg.print(" %05u", p);
//            }
//            dbg.print("\n");
//
//            dbg.print("Period:");
//            for (unsigned int i = 0; i < buffer.size(); i++) {
//                auto p = values::to_us(buffer_period.at(i));
//                dbg.print(" %05u", p);
//            }
//            dbg.print("\n");

        dbg.println("Samples");
        for (unsigned int i = 0; i < buffer.size(); i++) {
            sample s = buffer.at(i);
            auto pulse = s.pulse_us;
            auto period = s.period_us;
            dbg.println("%d %d, %d%%", period, pulse, int(pulse / double(period) * 100));
        }
        dbg.println();

        buffer.reset();
        sample_index = 0;
        hal_ok(HAL_TIM_IC_Start_IT(&htim1, TIM_CHANNEL_1));
        buffer_lock.unlock();
    }

//    seen_high |= HAL_GPIO_ReadPin(RADIO_RX_GPIO_Port, RADIO_RX_Pin);

    HAL_IWDG_Refresh(&hiwdg);
}

//static uint16_t start = 0;

void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
{
    if (htim->Channel != HAL_TIM_ACTIVE_CHANNEL_1) {
        return;
    }

//    if (buffer.check_first()) {
//        return;
//    }

    static uint16_t value1, value2;

    if (sample_index == 0) {
//        value1 = static_cast<uint16_t>(HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1));
        value1 = static_cast<uint16_t>(htim->Instance->CCR1);
        sample_index = 1;
    } else {
//        value2 = static_cast<uint16_t>(HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1));
        value2 = static_cast<uint16_t>(htim->Instance->CCR1);
        sample_index = 0;

        uint16_t diff;
        if (value2 > value1) {
            diff = value2 - value1;
        } else if (value1 > value2) {
            diff = (static_cast<uint16_t>(0xffff) - value1) + value2 + static_cast<uint16_t>(1);
        } else {
            return;
        }

        if (buffer_lock.try_lock()) {
            buffer.append({values::to_us<uint16_t>(0), values::to_us<uint16_t>(diff)});

            buffer_lock.unlock();
        }
    }
}