#include "decoder.h"
#include "script_decoder.h"

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

using mcu::arm::mutex;
using radio_controller::sample_iterator;
using radio_controller::sample;
using radio_controller::instruction;
using radio_controller::script_decoder;

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

template<typename T>
class buffer_iterator : public sample_iterator {
    const T *values_;
    const int size_;
    int idx_;

public:
    buffer_iterator(const T *values, int size) : values_(values), size_(size), idx_(-1)
    {};

    int size() override
    {
        return size_;
    }

    void reset() override
    {
        idx_ = -1;
    }

    bool next() override
    {
        if (has_next()) {
            idx_++;
            return true;
        }
        return false;
    }

    inline
    bool has_next() const override
    {
        return idx_ < size_ - 1;
    };

    const T &value() const override
    {
        return values_[idx_];
    }
};

template<unsigned int BufferSize, typename T>
class buffer {
    T samples[BufferSize];

    volatile unsigned int size_;

    uint32_t start_;
    bool first_;
    int part_;

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

    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
    uint32_t start() const
    {
        return start_;
    }

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

        if (size_ == 0) {
            start_ = HAL_GetTick();
        }

        samples[size_++] = sample;
    }

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

    buffer_iterator<T> iterator()
    {
        return buffer_iterator<T>(this->samples, this->size_);
    }
};

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

sample_buffer<10> radio_buffer;
mutex radio_buffer_lock;

sample_buffer<100> 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 const radio_controller::start samsung_start{9000, 4500};
static const radio_controller::data samsung_data{2250, 560, 1125, 560};

static const instruction *const samsung_instructions[] = {
    &samsung_start,
    &samsung_data,
    &end,
};
*/

static const radio_controller::nop nop{};

// NEC?
static const radio_controller::start nec_start{13500, 9000};
static const radio_controller::data nec_data{2250, 560, 1125, 560};
static const radio_controller::set_field nec_manufacturer_code{1, 0, 16};
static const radio_controller::set_field nec_code{2, 16, 8};

static const instruction *const nec_instructions[] = {
    &nec_start,
    &nec_data, &nec_data, &nec_data, &nec_data,
    &nec_data, &nec_data, &nec_data, &nec_data,
    &nec_data, &nec_data, &nec_data, &nec_data,
    &nec_data, &nec_data, &nec_data, &nec_data,
    &nec_data, &nec_data, &nec_data, &nec_data,
    &nec_data, &nec_data, &nec_data, &nec_data,
    &nec_data, &nec_data, &nec_data, &nec_data,
    &nec_data, &nec_data, &nec_data, &nec_data,
    &nec_manufacturer_code,
    &nec_code,
    &nop,
    &nop,
    &nop,
    &nop,
    &nop,
};

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;
    }

    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.is_empty() && now > ir_buffer.start() + 100)) {
        ir_buffer_lock.lock();

        HAL_NVIC_DisableIRQ(TIM2_IRQn);

        /*
        samsung_decoder d;
        auto it = ir_buffer.iterator();
        auto result = d.decode(&it);

        if (result.state == decoding_state::TOO_SHORT) {
            printf("Too short\n");
        } else if (result.state == decoding_state::SHORT_BODY) {
            printf("Short body\n");
        } else if (result.state == decoding_state::BAD_START) {
            printf("Bad start\n");
        } else if (result.state == decoding_state::OK) {
            printf("OK: size=%d, value: 0x%08" PRIx32 "%08" PRIx32 "\n", result.data.size(),
                   result.data.u32(1), result.data.u32(0));
            uint32_t manufacturer = result.data.extract_bits(0, 12);
            uint32_t command = result.data.extract_bits(12, 8);
            printf("Samsung: Manufacturer=%" PRIu32 ", 0x%" PRIx32 ", command=%" PRIu32 ", 0x%" PRIx32 "\n",
                   manufacturer, manufacturer, command, command);
        }
        */

//        script_decoder sd{samsung_instructions, SizeOfArray(samsung_instructions_)};
        script_decoder sd{nec_instructions, SizeOfArray(nec_instructions)};

        auto it = ir_buffer.iterator();
        auto result = sd.decode(it);

        if (result.state == radio_controller::decoding_state::FAIL) {
            printf("FAIL\n");

            it.reset();
            while (it.next()) {
                auto s = it.value();
                printf("% 5d us % 5d us\n", s.period_us, s.pulse_us);
            }

        } else if (result.state == radio_controller::decoding_state::OK) {
            printf("OK: size=%d, value: 0x%08" PRIx32 "%08" PRIx32 "\n"
                       "field1=%d, 0x%08" PRIx32 "\n"
                       "field2=%d, 0x%08" PRIx32 "\n"
                       "field3=%d, 0x%08" PRIx32 "\n",
                   result.data.size(), result.data.u32(1), result.data.u32(0),
                   result.field1, result.field1,
                   result.field2, result.field2,
                   result.field3, result.field3);

//            uint32_t manufacturer = result.data.extract_bits(0, 12);
//            uint32_t command = result.data.extract_bits(12, 8);
//            printf("Samsung: Manufacturer=%" PRIu32 ", 0x%" PRIx32 ", command=%" PRIu32 ", 0x%" PRIx32 "\n",
//                   manufacturer, manufacturer, command, command);
        }

        ir_buffer.reset();
        ir_buffer_lock.unlock();

        HAL_NVIC_EnableIRQ(TIM2_IRQn);
    }

    /*
    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);
    }
    */

    __HAL_IWDG_RELOAD_COUNTER(&hiwdg);
}

static
void ir_rx_level(TIM_HandleTypeDef *htim)
{
    static uint16_t ir_value1, ir_value2;

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

    if (!ir_level_buffer.check_first()) {
        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));
        }
    }
}

static
void ir_rx(TIM_HandleTypeDef *htim)
{
    static uint16_t ir_value1, ir_value2;

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

    if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1 && ir_buffer.check_first()) {
//        debug_pin(10);
        return;
    }

    if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1) {
        if (ir_buffer_lock.try_lock()) {
            auto period = values::to_us<uint16_t>(ir_value1);
            auto pulse = values::to_us<uint16_t>(ir_value2);
//            if (ir_buffer.size() == 0) {
//                debug_pin(5);
//            }
            ir_buffer.append({.period_us=period, .pulse_us=pulse});

            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);
//        ir_rx_level(htim);
    }
}