#include "health.h"
#include <math.h>
#include "FreeRTOS.h"
#include "stm32f1xx.h"
#include "task.h"

// Default to "Heartbeat" mode (1 Hz)
static volatile uint32_t led_period_ms = 3000;

void health_init_watchdog(void) {
    IWDG->KR = 0x5555;
    IWDG->PR = 0x04;   // Prescaler /64 -> 625 Hz
    IWDG->RLR = 1875;  // ~3 seconds
    IWDG->KR = 0xCCCC;
    IWDG->KR = 0xAAAA;
}

void health_kick_watchdog(void) {
    IWDG->KR = 0xAAAA;
}

void health_update_led(float freq_hz, float rms_dbfs) {
    if (rms_dbfs < -35.0f) {
        led_period_ms = 3000;
        return;
    }

    if (freq_hz < 100.0f) freq_hz = 100.0f;
    if (freq_hz > 6000.0f) freq_hz = 6000.0f;

    // 100..8000 Hz -> 1..5 Hz blink  (period 1000..200 ms)
    float t = (log10f(freq_hz) - log10f(100.0f)) /
              (log10f(6000.0f) - log10f(100.0f));      // 0..1
    float blink_hz = 1.0f + t * 20.0f;                  // 1..5
    uint32_t period = (uint32_t)(3000.0f / blink_hz);  // 1000..200 ms

    if (period < 200) period = 50;
    if (period > 3000) period = 3000;
    led_period_ms = period;
}

void health_led_task(void *param) {
    (void)param;

    // 1) Включаем тактирование GPIOC и настраиваем PC13 как выход push-pull
    // 2MHz
    RCC->APB2ENR |= RCC_APB2ENR_IOPCEN;
    GPIOC->CRH &= ~(GPIO_CRH_MODE13 | GPIO_CRH_CNF13);
    GPIOC->CRH |= GPIO_CRH_MODE13_1;  // 2 MHz output, push-pull (CNF=00)

    // LED off initially (Blue Pill LED is active-low)
    GPIOC->ODR |= GPIO_ODR_ODR13;

    while (1) {
        uint32_t period = led_period_ms;
        uint32_t on_ms = 40;
        if (on_ms > period / 2) on_ms = period / 2;
        uint32_t off_ms = period - on_ms;

        // LED active-low: 0 = ON, 1 = OFF
        GPIOC->BSRR = GPIO_BSRR_BR13;       // ON
        vTaskDelay(pdMS_TO_TICKS(on_ms));   // держим заметный импульс
        GPIOC->BSRR = GPIO_BSRR_BS13;       // OFF
        vTaskDelay(pdMS_TO_TICKS(off_ms));  // пауза

        health_kick_watchdog();
    }
}