#include <stdio.h>
#include <string.h>
#include "FreeRTOS.h"
#include "audio_adc.h"
#include "queue.h"
#include "stm32f1xx.h"
#include "task.h"
#include "tusb.h"

void vApplicationStackOverflowHook(TaskHandle_t xTask, char *pcTaskName) {
    (void)xTask;
    (void)pcTaskName;
    // Мигаем LED очень быстро при переполнении стека
    while (1) {
        GPIOC->ODR ^= GPIO_ODR_ODR13;
        for (volatile int i = 0; i < 50000; i++);
    }
}

// === Структура данных для очереди ===

typedef struct {
    uint16_t min_val;
    uint16_t max_val;
    uint16_t avg_val;
    uint32_t buffer_num;
} audio_stats_packet_t;

static QueueHandle_t audio_stats_queue = NULL;

// === System Clock ===

void SystemClock_Config(void) {
    RCC->CR |= RCC_CR_HSEON;
    while (!(RCC->CR & RCC_CR_HSERDY));

    FLASH->ACR = FLASH_ACR_LATENCY_2;

    RCC->CFGR &= ~RCC_CFGR_PLLMULL;
    RCC->CFGR |= RCC_CFGR_PLLMULL9;
    RCC->CFGR |= RCC_CFGR_PLLSRC;
    RCC->CFGR &= ~RCC_CFGR_USBPRE;

    RCC->CR |= RCC_CR_PLLON;
    while (!(RCC->CR & RCC_CR_PLLRDY));

    RCC->CFGR &= ~RCC_CFGR_SW;
    RCC->CFGR |= RCC_CFGR_SW_PLL;
    while ((RCC->CFGR & RCC_CFGR_SWS) != RCC_CFGR_SWS_PLL);

    SystemCoreClock = 72000000;
}

// === Audio Callback ===

void audio_buffer_ready(audio_sample_t *buffer, uint32_t size) {
    static uint32_t buffer_counter = 0;
    buffer_counter++;

    // Мигаем LED при каждом вызове
    if (buffer_counter % 5 == 0) { GPIOC->ODR ^= GPIO_ODR_ODR13; }

    uint16_t min_val = 4095;
    uint16_t max_val = 0;
    uint32_t sum = 0;

    for (uint32_t i = 0; i < size; i++) {
        uint16_t val = buffer[i];
        if (val < min_val) min_val = val;
        if (val > max_val) max_val = val;
        sum += val;
    }

    if (buffer_counter % 10 == 0) {
        audio_stats_packet_t packet = {
            .min_val = min_val,
            .max_val = max_val,
            .avg_val = (uint16_t)(max_val - min_val),
            .buffer_num = buffer_counter};

        BaseType_t xHigherPriorityTaskWoken = pdFALSE;
        xQueueSendFromISR(
            audio_stats_queue,
            &packet,
            &xHigherPriorityTaskWoken);
        portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
    }
}

// === Tasks ===

void usb_device_task(void *param) {
    (void)param;
    while (1) {
        tud_task();
        vTaskDelay(pdMS_TO_TICKS(1));
    }
}

void cdc_task(void *param) {
    (void)param;
    char tx_buffer[256];
    uint32_t heartbeat_counter = 0;
    uint32_t last_buffer_count = 0;

    while (1) {
        heartbeat_counter++;

        // Heartbeat + ДИАГНОСТИКА регистров каждые 100 циклов
        if (heartbeat_counter % 100 == 0 && tud_cdc_connected()) {
            uint32_t current_buffer_count = audio_adc_get_buffer_count();

            int len = snprintf(
                tx_buffer,
                sizeof(tx_buffer),
                "HB:%lu Q:%u BC:%lu | "
                "TIM3_CR1:%lX TIM3_CNT:%lu | "
                "ADC1_CR2:%lX ADC1_SR:%lX | "
                "DMA_CCR:%lX DMA_CNDTR:%lu\r\n",
                heartbeat_counter,
                (unsigned)uxQueueMessagesWaiting(audio_stats_queue),
                current_buffer_count,
                TIM3->CR1,
                TIM3->CNT,
                ADC1->CR2,
                ADC1->SR,
                DMA1_Channel1->CCR,
                DMA1_Channel1->CNDTR);

            if (tud_cdc_write_available() >= len) {
                tud_cdc_write(tx_buffer, len);
                tud_cdc_write_flush();
            }

            last_buffer_count = current_buffer_count;
        }

        // Остальной код без изменений
        audio_stats_packet_t packet;
        if (xQueueReceive(audio_stats_queue, &packet, pdMS_TO_TICKS(10)) ==
            pdPASS) {
            int len = snprintf(
                tx_buffer,
                sizeof(tx_buffer),
                "Buf:%lu Min:%u Max:%u Avg:%u\r\n",
                packet.buffer_num,
                packet.min_val,
                packet.max_val,
                packet.avg_val);

            if (tud_cdc_connected() && len > 0 &&
                tud_cdc_write_available() >= len) {
                tud_cdc_write(tx_buffer, len);
                tud_cdc_write_flush();
            }
        }

        // Echo
        if (tud_cdc_available()) {
            uint8_t buf[64];
            uint32_t count = tud_cdc_read(buf, sizeof(buf));
            if (tud_cdc_connected() && count > 0) {
                tud_cdc_write(buf, count);
                tud_cdc_write_flush();
            }
        }
    }
}

void led_task(void *param) {
    (void)param;
    while (1) {
        GPIOC->BSRR = GPIO_BSRR_BR13;
        vTaskDelay(pdMS_TO_TICKS(500));
        GPIOC->BSRR = GPIO_BSRR_BS13;
        vTaskDelay(pdMS_TO_TICKS(500));
    }
}

// === Задача инициализации аудио ===
void audio_init_task(void *param) {
    (void)param;

    // Индикация старта инициализации (мигнем 3 раза быстро)
    for (int i = 0; i < 3; i++) {
        GPIOC->ODR ^= GPIO_ODR_ODR13;
        vTaskDelay(pdMS_TO_TICKS(100));
    }

    if (!audio_adc_init(audio_buffer_ready)) {
        // Ошибка: мигаем очень быстро
        while (1) {
            GPIOC->ODR ^= GPIO_ODR_ODR13;
            vTaskDelay(pdMS_TO_TICKS(50));
        }
    }

    // Запускаем ADC после старта FreeRTOS
    audio_adc_start();

    // Индикация успешного запуска (мигнем 5 раз медленно)
    for (int i = 0; i < 5; i++) {
        GPIOC->ODR ^= GPIO_ODR_ODR13;
        vTaskDelay(pdMS_TO_TICKS(200));
    }

    vTaskDelete(NULL);
}

// === USB Reset ===

void force_usb_reset(void) {
    RCC->APB2ENR |= RCC_APB2ENR_IOPAEN;
    GPIOA->CRH &= ~GPIO_CRH_CNF12;
    GPIOA->CRH |= GPIO_CRH_MODE12_1;
    GPIOA->BSRR = GPIO_BSRR_BR12;
    for (volatile int i = 0; i < 500000; i++) __NOP();
    GPIOA->CRH &= ~GPIO_CRH_MODE12;
    GPIOA->CRH |= GPIO_CRH_CNF12_0;
}

// === Main ===

int main(void) {
    SystemClock_Config();

    // LED
    RCC->APB2ENR |= RCC_APB2ENR_IOPCEN;
    GPIOC->CRH &= ~GPIO_CRH_CNF13;
    GPIOC->CRH |= GPIO_CRH_MODE13_1;

    force_usb_reset();

    // USB
    RCC->APB2ENR |= RCC_APB2ENR_IOPAEN;
    RCC->APB1ENR |= RCC_APB1ENR_USBEN;

    NVIC_SetPriority(USB_HP_CAN1_TX_IRQn, 6);
    NVIC_SetPriority(USB_LP_CAN1_RX0_IRQn, 6);
    NVIC_SetPriority(USBWakeUp_IRQn, 6);

    NVIC_EnableIRQ(USB_HP_CAN1_TX_IRQn);
    NVIC_EnableIRQ(USB_LP_CAN1_RX0_IRQn);
    NVIC_EnableIRQ(USBWakeUp_IRQn);

    tusb_init();

    // Очередь создаем ДО старта планировщика
    audio_stats_queue = xQueueCreate(10, sizeof(audio_stats_packet_t));
    if (audio_stats_queue == NULL) {
        while (1) {
            GPIOC->ODR ^= GPIO_ODR_ODR13;
            for (volatile int i = 0; i < 100000; i++);
        }
    }

    // Задачи
    xTaskCreate(
        usb_device_task,
        "usbd",
        256,
        NULL,
        configMAX_PRIORITIES - 1,
        NULL);
    xTaskCreate(cdc_task, "cdc", 320, NULL, configMAX_PRIORITIES - 2, NULL);
    xTaskCreate(led_task, "led", 128, NULL, 1, NULL);
    xTaskCreate(audio_init_task, "audio_init", 128, NULL, 2, NULL);

    vTaskStartScheduler();

    while (1);
}

// === USB Handlers ===

void USB_HP_CAN1_TX_IRQHandler(void) {
    tud_int_handler(0);
}

void USB_LP_CAN1_RX0_IRQHandler(void) {
    tud_int_handler(0);
}

void USBWakeUp_IRQHandler(void) {
    tud_int_handler(0);
}