/*
 * safety.c — SaltyLab Safety Systems
 *
 * IWDG: 40kHz LSI, prescaler 32 → tick = 0.8ms.
 * WATCHDOG_TIMEOUT_MS from config.h (default 50ms → reload = 62).
 * Formula: reload = (timeout_ms / (prescaler / 40000)) - 1
 *          reload = (50 * 40000 / 32) - 1 = 62499 → clamp to 4095 (12-bit max)
 * At PSC=256: tick = 6.4ms, reload = ceil(50/6.4)-1 = 7 → ~57.6ms
 * Using PSC=32: tick = 0.8ms, reload = ceil(50/0.8)-1 = 62 → 50ms ✓
 */

#include "safety.h"
#include "config.h"
#include "crsf.h"
#include "stm32f7xx_hal.h"

/* IWDG prescaler 32 → LSI(40kHz)/32 = 1250 ticks/sec → 0.8ms/tick */
#define IWDG_PRESCALER   IWDG_PRESCALER_32
/* Integer formula: timeout_ms * LSI_HZ / (prescaler * 1000)
 * = WATCHDOG_TIMEOUT_MS * 40000 / (32 * 1000) = WATCHDOG_TIMEOUT_MS * 40 / 32 */
#define IWDG_RELOAD      (WATCHDOG_TIMEOUT_MS * 40UL / 32UL)

#if IWDG_RELOAD > 4095
#  error "WATCHDOG_TIMEOUT_MS too large for IWDG_PRESCALER_32 — increase prescaler"
#endif

static IWDG_HandleTypeDef hiwdg;

/* Arm interlock */
static uint32_t s_arm_start_ms = 0;
static bool     s_arm_pending  = false;

/* Tilt fault alert state — edge-detect to fire buzzer once */
static bool s_was_faulted = false;

static EstopSource s_estop_source = ESTOP_CLEAR;

void safety_init(void) {
    hiwdg.Instance       = IWDG;
    hiwdg.Init.Prescaler = IWDG_PRESCALER;
    hiwdg.Init.Reload    = IWDG_RELOAD;
    hiwdg.Init.Window    = IWDG_WINDOW_DISABLE;
    HAL_IWDG_Init(&hiwdg);  /* Starts watchdog immediately */
}

void safety_refresh(void) {
    if (hiwdg.Instance) HAL_IWDG_Refresh(&hiwdg);
}

bool safety_rc_alive(uint32_t now) {
    /* If crsf_state has never received a frame, last_rx_ms == 0 */
    if (crsf_state.last_rx_ms == 0) return false;
    return (now - crsf_state.last_rx_ms) < RC_TIMEOUT_MS;
}

void safety_alert_tilt_fault(bool faulted) {
    if (faulted && !s_was_faulted) {
        /* Rising edge: single buzzer burst to alert rider */
        HAL_GPIO_WritePin(BEEPER_PORT, BEEPER_PIN, GPIO_PIN_SET);
        HAL_Delay(200);
        HAL_GPIO_WritePin(BEEPER_PORT, BEEPER_PIN, GPIO_PIN_RESET);
    }
    s_was_faulted = faulted;
}

void safety_arm_start(uint32_t now) {
    if (!s_arm_pending) {
        s_arm_start_ms = now;
        s_arm_pending  = true;
    }
}

bool safety_arm_ready(uint32_t now) {
    if (!s_arm_pending) return false;
    return (now - s_arm_start_ms) >= ARMING_HOLD_MS;
}

void safety_arm_cancel(void) {
    s_arm_pending = false;
}

void safety_remote_estop(EstopSource src) { s_estop_source = src; }
void safety_remote_estop_clear(void) { s_estop_source = ESTOP_CLEAR; }
EstopSource safety_get_estop(void) { return s_estop_source; }
bool safety_remote_estop_active(void) { return s_estop_source >= ESTOP_REMOTE; }