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

#ifdef HAL_IWDG_MODULE_ENABLED
extern IWDG_HandleTypeDef hiwdg;
#endif

extern TIM_HandleTypeDef htim1;
extern UART_HandleTypeDef huart2;

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

using interval_us = uint16_t;

class mutex {
    volatile uint8_t locked;

public:
    mutex() : locked(0)
    {}

    __always_inline
    bool try_lock()
    {
        auto l = __LDREXB(&locked);

        if (l == 1) {
            return false;
        }

        return __STREXB(1, &locked) == 0;
    }

    __always_inline
    bool lock()
    {
        do {
            while(__LDREXB(&locked) == 1);
        } while(__STREXB(1, &locked) != 0);
    }

    __always_inline
    bool unlock()
    {
        locked = 0;
    }
};

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

    volatile unsigned int _size;

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

    unsigned int size()
    {
        return _size;
    }

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

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

        samples[_size++] = sample;
    }

    interval_us at(int i)
    {
        return samples[i];
    }
};

sample_buffer<10> buffer;
mutex buffer_lock;

void main_pre_init()
{
}

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

    printf("Radio Controller\n");

//    HAL_TIM_Base_Start_IT(&htim1);
    if (HAL_TIM_IC_Start_IT(&htim1, TIM_CHANNEL_1) != HAL_OK) {
        halt();
    }

    uart2.enable();

    buffer.reset();
}

static uint32_t tick_next = 0;
static uint32_t cnt_prev = 0;

static bool seen_high = false;

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

    if (now >= tick_next) {
        uint32_t cnt = __HAL_TIM_GET_COUNTER(&htim1);
        auto diff = static_cast<int>(cnt - cnt_prev);
        dbg.println("cnt=%lu, diff=%d, now=%" PRIu32 ", seen_high=%d, buffer_size=%d", cnt, diff, now, seen_high,
                    buffer.size());
        cnt_prev = cnt;

        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();

        for (unsigned int i = 0; i < buffer.size(); i++) {
            dbg.println("%d", static_cast<int>(buffer.at(i)));
        }
        dbg.println("\n");

        buffer.reset();
//        buffer_lock.unlock();
    }

    seen_high |= HAL_GPIO_ReadPin(RADIO_RX_GPIO_Port, RADIO_RX_Pin);

#ifdef HAL_IWDG_MODULE_ENABLED
    HAL_IWDG_Refresh(&hiwdg);
#endif
}

void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
{
    auto input_capture = __HAL_TIM_GET_COMPARE(htim, TIM_CHANNEL_1);

    if (buffer_lock.try_lock()) {
        buffer.append(static_cast<uint16_t>(input_capture));
        buffer_lock.unlock();
    }
}