/**
 * @file safety_manager.c
 * @brief Safety Manager — Hill-Hold, Auto-Apply, Drive-Away Assist.
 *
 * @arch SWA-001
 * @reqs SWE-007 SWE-008 SWE-009 SWE-010 SWE-011 SWE-012
 *
 * ASIL: D. This component decides, when the Apply Controller receives an
 * apply or release request (hill-hold handover, auto-apply on
 * engine-off, drive-away assist).
 * Changes require a technical review with 2 approvals.
 */
#include <stddef.h>

#include "safety_manager.h"

typedef struct {
    SafetyState state;
    uint16_t    ticks_in_state;  /* 50ms ticks in the current state */
    bool        apply_requested;
    bool        release_requested;
} SafetyCtx;

static SafetyCtx s_ctx;

static void enter(SafetyState s)
{
    s_ctx.state          = s;
    s_ctx.ticks_in_state = 0U;
}

static bool standstill(const SafetyInputs* in)
{
    return in->vehicle_speed_kmh < SAFETY_STANDSTILL_KMH;
}

static bool grade_steep(const SafetyInputs* in)
{
    /* @reqs SWE-009: Hill-hold above |grade| > 5% */
    float g = in->grade_percent;
    if (g < 0.0f) {
        g = -g;
    }
    return g > SAFETY_HILLHOLD_GRADE_PCT;
}

/**
 * @reqs SWE-011 (Detect drive-away intent)
 */
static bool drive_intent(const SafetyInputs* in)
{
    return (in->gas_pedal_percent > SAFETY_DRIVE_INTENT_GAS_PCT)
        && in->gear_in_drive
        && in->engine_running;
}

/**
 * @reqs SWE-012 (Safety check before auto-release)
 */
static bool drive_away_safety_ok(const SafetyInputs* in)
{
    return in->door_closed && in->seatbelt_fastened;
}

EpbStatus safety_mgr_init(void)
{
    s_ctx.state             = SAFETY_IDLE;
    s_ctx.ticks_in_state    = 0U;
    s_ctx.apply_requested   = false;
    s_ctx.release_requested = false;
    return EPB_OK;
}

void safety_mgr_step_50ms(const SafetyInputs* in)
{
    if (in == NULL) {
        return;
    }

    if (s_ctx.ticks_in_state < UINT16_MAX) {
        ++s_ctx.ticks_in_state;
    }

    /* Default: no apply/release request unless explicitly set below. */
    s_ctx.apply_requested   = false;
    s_ctx.release_requested = false;

    switch (s_ctx.state) {
    case SAFETY_IDLE:
        /* @reqs SWE-009: Hill-hold activation */
        if (grade_steep(in) && standstill(in) && in->brake_pedal_pressed) {
            enter(SAFETY_HILL_HOLD_ARMED);
            break;
        }
        /* @reqs SWE-007: Detect engine-off condition */
        if (!in->engine_running && standstill(in)
            && in->current_state != EPB_STATE_APPLIED
            && in->current_state != EPB_STATE_APPLYING) {
            enter(SAFETY_AUTO_APPLY_ARMED);
            break;
        }
        /* @reqs SWE-011 + SWE-012: Drive-Away Assist */
        if (in->current_state == EPB_STATE_APPLIED
            && drive_intent(in) && drive_away_safety_ok(in)) {
            s_ctx.release_requested = true;
            enter(SAFETY_DRIVE_AWAY);
        }
        break;

    case SAFETY_HILL_HOLD_ARMED:
        /* @reqs SWE-010: Trigger apply on brake release */
        if (!in->brake_pedal_pressed) {
            s_ctx.apply_requested = true;
            enter(SAFETY_HILL_HOLD_ACTIVE);
            break;
        }
        /* Hill-hold condition no longer met? */
        if (!grade_steep(in) || !standstill(in)) {
            enter(SAFETY_IDLE);
        }
        break;

    case SAFETY_HILL_HOLD_ACTIVE:
        /* Ends when vehicle rolls or brake is applied */
        if (in->vehicle_speed_kmh > SAFETY_RELEASE_KMH
            || in->current_state == EPB_STATE_APPLIED) {
            enter(SAFETY_IDLE);
        } else {
            s_ctx.apply_requested = true;
        }
        break;

    case SAFETY_AUTO_APPLY_ARMED:
        /* Condition must remain continuously satisfied */
        if (in->engine_running || !standstill(in)) {
            enter(SAFETY_IDLE);
            break;
        }
        /* @reqs SWE-008: Auto-apply after 2 s (40 ticks) delay */
        if (s_ctx.ticks_in_state >= SAFETY_AUTO_APPLY_DELAY_50MS) {
            s_ctx.apply_requested = true;
            enter(SAFETY_AUTO_APPLY_TRIGGERED);
        }
        break;

    case SAFETY_AUTO_APPLY_TRIGGERED:
        if (in->current_state == EPB_STATE_APPLIED) {
            enter(SAFETY_IDLE);
        } else {
            s_ctx.apply_requested = true;
        }
        break;

    case SAFETY_DRIVE_AWAY:
        /* Ends when the brake has been released or preconditions are no longer ok. */
        if (in->current_state == EPB_STATE_RELEASED
            || in->current_state == EPB_STATE_RELEASING) {
            enter(SAFETY_IDLE);
        } else if (!drive_intent(in) || !drive_away_safety_ok(in)) {
            enter(SAFETY_IDLE);
        } else {
            s_ctx.release_requested = true;
        }
        break;

    default:
        enter(SAFETY_IDLE);
        break;
    }
}

bool safety_mgr_apply_requested(void)
{
    return s_ctx.apply_requested;
}

bool safety_mgr_release_requested(void)
{
    return s_ctx.release_requested;
}

SafetyState safety_mgr_get_state(void)
{
    return s_ctx.state;
}