#include <stdio.h>
#include <string.h>
#include "FreeRTOS.h"
#include "audio_adc.h"
#include "audio_processor.h"  // НОВОЕ
#include "health.h"
#include "protocol.h"
#include "queue.h"
#include "stm32f1xx.h"
#include "task.h"
#include "tusb.h"

void vApplicationStackOverflowHook(TaskHandle_t xTask, char *pcTaskName) {
    (void)xTask;
    (void)pcTaskName;
    while (1) {
        GPIOC->ODR ^= GPIO_ODR_ODR13;
        for (volatile int i = 0; i < 50000; i++);
    }
}

void vApplicationMallocFailedHook(void) {
    taskDISABLE_INTERRUPTS();
    while (1) {
        GPIOC->ODR ^= GPIO_ODR_ODR13;
        for (volatile int i = 0; i < 200000; i++) {}
    }
}

static void panic_blink_forever(uint32_t delay_ms) {
    // PC13 уже сконфигурирован в main
    while (1) {
        GPIOC->ODR ^= GPIO_ODR_ODR13;
        vTaskDelay(pdMS_TO_TICKS(delay_ms));
    }
}

// === Структуры данных ===

typedef struct {
    float rms_dbfs;
    float peak_hz;
    float peak_mag;
    uint8_t clipped;
    uint32_t timestamp_ms;
} audio_metrics_packet_t;

static QueueHandle_t audio_metrics_queue = NULL;
static volatile uint32_t buffer_counter = 0;

// === 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 (НОВОЕ: копируем в очередь для обработки) ===

// Буфер для копирования из ISR
static audio_sample_t processing_buffer[AUDIO_BUFFER_SIZE];

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

    // Мигаем LED
    /* if (buffer_counter % 5 == 0) { GPIOC->ODR ^= GPIO_ODR_ODR13; } */

    // Копируем данные (ISR должен быть быстрым)
    memcpy(processing_buffer, buffer, size * sizeof(audio_sample_t));

    // Сигналим задаче обработки через notification
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    extern TaskHandle_t audio_process_task_handle;
    if (audio_process_task_handle != NULL) {
        vTaskNotifyGiveFromISR(
            audio_process_task_handle,
            &xHigherPriorityTaskWoken);
        portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
    }
}

// === Tasks ===

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

TaskHandle_t audio_process_task_handle = NULL;

void audio_process_task(void *param) {
    (void)param;

    if (!audio_processor_init()) {
        while (1) {
            GPIOC->ODR ^= GPIO_ODR_ODR13;
            vTaskDelay(pdMS_TO_TICKS(50));
            health_kick_watchdog();
        }
    }

    audio_metrics_t metrics;

    TickType_t last_wake = xTaskGetTickCount();
    const TickType_t period = pdMS_TO_TICKS(100);  // 10 Hz

    while (1) {
        // 1) Ждём хотя бы один новый буфер от ISR
        ulTaskNotifyTake(pdTRUE, portMAX_DELAY);

        // 2) Выкидываем накопившиеся уведомления, чтобы не пытаться "догонять"
        // прошлое
        while (ulTaskNotifyTake(pdTRUE, 0) > 0) {}

        // 3) Обрабатываем самый свежий буфер (processing_buffer
        // перезаписывается в ISR)
        if (audio_processor_process_512(processing_buffer, &metrics)) {
            health_update_led(metrics.peak_hz, metrics.rms_dbfs);

            audio_metrics_packet_t packet = {
                .rms_dbfs = metrics.rms_dbfs,
                .peak_hz = metrics.peak_hz,
                .peak_mag = metrics.peak_mag,
                .clipped = metrics.clipped,
                .timestamp_ms = xTaskGetTickCount(),
            };
            (void)xQueueSend(audio_metrics_queue, &packet, 0);
        }

        health_kick_watchdog();

        // ограничиваем частоту обработки
        vTaskDelayUntil(&last_wake, period);
    }
}

void cdc_task(void *param) {
    (void)param;
    // Buffer for packet (12 bytes)
    uint8_t tx_buffer[PACKET_TOTAL_SIZE];

    while (1) {
        // Check if USB is connected
        if (tud_cdc_connected()) {
            audio_metrics_packet_t packet;

            // Wait for data from DSP task
            if (xQueueReceive(
                    audio_metrics_queue,
                    &packet,
                    pdMS_TO_TICKS(10)) == pdPASS) {
                // Pack data according to FR-1.4 spec
                protocol_pack_v1(
                    tx_buffer,
                    packet.timestamp_ms,
                    packet.rms_dbfs,
                    packet.peak_hz);

                // Write to USB CDC
                // Check available space just in case
                if (tud_cdc_write_available() >= sizeof(tx_buffer)) {
                    tud_cdc_write(tx_buffer, sizeof(tx_buffer));
                    tud_cdc_write_flush();
                }
            }
        } else {
            // Flush queue if USB not connected to prevent stalling DSP task
            // or just sleep longer.
            vTaskDelay(pdMS_TO_TICKS(100));
        }
    }
}

void audio_init_task(void *param) {
    (void)param;

    // Индикация старта
    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));
        }
    }

    audio_adc_start();

    for (int i = 0; i < 5; i++) {
        GPIOC->ODR ^= GPIO_ODR_ODR13;
        vTaskDelay(pdMS_TO_TICKS(200));
    }

    vTaskDelete(NULL);
}

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 GPIO (will be managed by health_led_task)
    RCC->APB2ENR |= RCC_APB2ENR_IOPCEN;
    GPIOC->CRH &= ~(GPIO_CRH_MODE13 | 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();

    // Initialize watchdog BEFORE starting tasks
    health_init_watchdog();

    // FFT queue
    audio_metrics_queue = xQueueCreate(10, sizeof(audio_metrics_packet_t));
    if (audio_metrics_queue == NULL) {
        while (1) {
            GPIOC->ODR ^= GPIO_ODR_ODR13;
            for (volatile int i = 0; i < 100000; i++);
        }
    }

    // Create tasks
    xTaskCreate(
        usb_device_task,
        "usbd",
        256,
        NULL,
        configMAX_PRIORITIES - 1,
        NULL);
    xTaskCreate(cdc_task, "cdc", 320, NULL, configMAX_PRIORITIES - 2, NULL);

    xTaskCreate(health_led_task, "health_led", 128, NULL, 1, NULL);

    xTaskCreate(audio_init_task, "audio_init", 128, NULL, 2, NULL);
    xTaskCreate(
        audio_process_task,
        "audio_proc",
        512,
        NULL,
        configMAX_PRIORITIES - 2,
        &audio_process_task_handle);

    if (xTaskCreate(health_led_task, "health_led", 128, NULL, 1, NULL) !=
        pdPASS) {
        panic_blink_forever(100);
    }

    vTaskStartScheduler();

    while (1);  // Should never reach here
}

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