#include "stm32f7xx_hal.h"
#include "config.h"
#include "status.h"

void status_init(void) {
    __HAL_RCC_GPIOB_CLK_ENABLE();
    __HAL_RCC_GPIOC_CLK_ENABLE();

    GPIO_InitTypeDef gpio = {0};
    gpio.Mode = GPIO_MODE_OUTPUT_PP;
    gpio.Pull = GPIO_NOPULL;
    gpio.Speed = GPIO_SPEED_FREQ_LOW;

    /* LED1 - PC15 */
    gpio.Pin = LED1_PIN;
    HAL_GPIO_Init(LED1_PORT, &gpio);

    /* LED2 - PC14 */
    gpio.Pin = LED2_PIN;
    HAL_GPIO_Init(LED2_PORT, &gpio);

    /* Buzzer - PB2 (inverted: HIGH = off) */
    gpio.Pin = BEEPER_PIN;
    HAL_GPIO_Init(BEEPER_PORT, &gpio);

    /* Start with both LEDs ON (active low) */
    HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_RESET);

    /* Buzzer off */
    HAL_GPIO_WritePin(BEEPER_PORT, BEEPER_PIN, GPIO_PIN_RESET);
}

void status_boot_beep(void) {
    /* Two quick beeps */
    HAL_GPIO_WritePin(BEEPER_PORT, BEEPER_PIN, GPIO_PIN_SET);
    HAL_Delay(80);
    HAL_GPIO_WritePin(BEEPER_PORT, BEEPER_PIN, GPIO_PIN_RESET);
    HAL_Delay(80);
    HAL_GPIO_WritePin(BEEPER_PORT, BEEPER_PIN, GPIO_PIN_SET);
    HAL_Delay(80);
    HAL_GPIO_WritePin(BEEPER_PORT, BEEPER_PIN, GPIO_PIN_RESET);
}

void status_update(uint32_t tick, int imu_ok, int armed, int tilt_fault, int remote_estop) {
    GPIO_PinState fast_blink = ((tick / 200) % 2) ? GPIO_PIN_RESET : GPIO_PIN_SET;
    GPIO_PinState blink = ((tick / 500) % 2) ? GPIO_PIN_RESET  /* ON half */
                                              : GPIO_PIN_SET;   /* OFF half */
    if (remote_estop) {
        HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, fast_blink);
        HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, fast_blink);
    } else if (!imu_ok) {
        /* IMU error: LED1 blinking (attention), LED2 solid ON */
        HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, blink);
        HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_RESET);
    } else if (tilt_fault) {
        /* Tilt fault: both LEDs slow blink */
        HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, blink);
        HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, blink);
    } else if (armed) {
        /* Armed: both LEDs solid ON */
        HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_RESET);
    } else {
        /* Normal disarmed: LED1 solid ON, LED2 off */
        HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_SET);
    }
}