#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"
#include "misc.h"

using mcu::arm::mutex;

extern IWDG_HandleTypeDef hiwdg;
extern TIM_HandleTypeDef htim1;
extern TIM_HandleTypeDef htim2;
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, typename T>
class buffer {
    T samples[BufferSize];

    volatile unsigned int size_;

    bool first_;
    int part_;

public:
    void reset()
    {
        size_ = 0;
        first_ = true;
        part_ = 1;
    }

    bool check_first_part()
    {
        bool first = part_ == 1;
        part_ = first ? 2 : 1;
        return first;
    }

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

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

    __always_inline
    bool is_empty() const
    {
        return size_ == 0;
    }

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

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

        samples[size_++] = sample;
    }

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

template<size_t BufferSize>
using sample_buffer = buffer<BufferSize, sample>;

sample_buffer<10> radio_buffer;
mutex radio_buffer_lock;

sample_buffer<30> ir_buffer;
buffer<100, uint16_t> ir_level_buffer;
mutex ir_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");

    radio_buffer.reset();
    radio_buffer_lock.unlock();
    ir_buffer.reset();
    ir_level_buffer.reset();
    ir_buffer_lock.unlock();
    uart2.enable();

    hal_ok(HAL_TIM_IC_Start_IT(&htim1, TIM_CHANNEL_1));
    hal_ok(HAL_TIM_Base_Start(&htim2));
    hal_ok(HAL_TIM_IC_Start_IT(&htim2, TIM_CHANNEL_1));
    hal_ok(HAL_TIM_IC_Start_IT(&htim2, TIM_CHANNEL_2));
}

static uint32_t tick_next = 0;

//static bool seen_high = false;

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 (radio_buffer.is_full()) {
        radio_buffer_lock.lock();

        hal_ok(HAL_TIM_IC_Stop_IT(&htim1, TIM_CHANNEL_1));

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

        radio_buffer.reset();
        hal_ok(HAL_TIM_IC_Start_IT(&htim1, TIM_CHANNEL_1));
        radio_buffer_lock.unlock();
    }

    /*
    if (ir_buffer.is_full()) {
        ir_buffer_lock.lock();

        hal_ok(HAL_TIM_IC_Stop_IT(&htim2, TIM_CHANNEL_1));
        hal_ok(HAL_TIM_IC_Stop_IT(&htim2, TIM_CHANNEL_2));
        dbg.println("IR");
        for (unsigned int i = 0; i < ir_buffer.size(); i++) {
            sample s = ir_buffer.at(i);
            auto pulse = s.pulse_us;
            auto period = s.period_us;
            dbg.println("% 5d us % 5d us, %.02d%%", period, pulse, int(pulse / double(period) * 100));
        }
        dbg.println();

        ir_buffer.reset();
        hal_ok(HAL_TIM_IC_Start_IT(&htim2, TIM_CHANNEL_1));
        hal_ok(HAL_TIM_IC_Start_IT(&htim2, TIM_CHANNEL_2));
        ir_buffer_lock.unlock();
    }
    */

    if (ir_level_buffer.is_full()) {
        ir_buffer_lock.lock();

        HAL_NVIC_DisableIRQ(TIM2_IRQn);

        dbg.println("IR");
        for (unsigned int i = 0; i < ir_level_buffer.size(); i++) {
            uint16_t s = ir_level_buffer.at(i);
            dbg.println("% 5d us", s);
        }
        dbg.println();

        ir_level_buffer.reset();
        ir_buffer_lock.unlock();
        HAL_NVIC_EnableIRQ(TIM2_IRQn);
    }

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

    HAL_IWDG_Refresh(&hiwdg);
}

//static uint16_t start = 0;

uint16_t ir_value1, ir_value2;

static
void ir_rx(TIM_HandleTypeDef *htim)
{
    if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1) {
        debug_pin(2);
    }
    if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_2) {
        debug_pin(3);
    }

    if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1) {
        ir_value1 = static_cast<uint16_t>(htim->Instance->CCR1);
    } else if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_2) {
        ir_value2 = static_cast<uint16_t>(htim->Instance->CCR2);
    }

    if (ir_level_buffer.check_first()) {
        return;
    }

    if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1) {
        ir_level_buffer.append(values::to_us<uint16_t>(ir_value1 - ir_value2));
    } else if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_2) {
        ir_level_buffer.append(values::to_us<uint16_t>(ir_value2));
    }

    /*
    if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1 && !ir_buffer.check_first()) {
        if (ir_buffer_lock.try_lock()) {
            ir_buffer.append({values::to_us<uint16_t>(ir_value1), values::to_us<uint16_t>(ir_value2)});

            ir_buffer_lock.unlock();
        }
    }
    */
}

static
void radio_rx(TIM_HandleTypeDef *htim)
{
    if (htim->Channel != HAL_TIM_ACTIVE_CHANNEL_1) {
        return;
    }
//    if (radio_buffer.check_first()) {
//        return;
//    }

    static uint16_t value1, value2;

    if (radio_buffer.check_first_part()) {
//        value1 = static_cast<uint16_t>(HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1));
        value1 = static_cast<uint16_t>(htim->Instance->CCR1);
    } else {
//        value2 = static_cast<uint16_t>(HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1));
        value2 = static_cast<uint16_t>(htim->Instance->CCR2);

        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 (radio_buffer_lock.try_lock()) {
            radio_buffer.append({values::to_us<uint16_t>(0), values::to_us<uint16_t>(diff)});

            radio_buffer_lock.unlock();
        }
    }
}

void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
{
    if (htim == &htim1) {
        radio_rx(htim);
    }
    if (htim == &htim2) {
        ir_rx(htim);
    }
}