/**
 * @file test_safety_manager.c
 * @brief Unit tests for the Safety Manager (ASIL-D).
 *
 * @reqs SWE-007 SWE-008 SWE-009 SWE-010 SWE-011 SWE-012
 * @arch SWA-001
 */
#include "../unit_test_framework.h"
#include "../../src/safety_manager.h"

TestStats g_test_stats = {0, 0};

static SafetyInputs make_driving(void)
{
    SafetyInputs in = {0};
    in.engine_running       = true;
    in.brake_pedal_pressed  = false;
    in.vehicle_speed_kmh    = 50.0f;
    in.grade_percent        = 0.0f;
    in.current_state        = EPB_STATE_RELEASED;
    return in;
}

static SafetyInputs make_standstill(void)
{
    SafetyInputs in = make_driving();
    in.vehicle_speed_kmh = 0.0f;
    in.brake_pedal_pressed = true;
    return in;
}

static void step_n(SafetyInputs* in, int n)
{
    for (int i = 0; i < n; ++i) {
        safety_mgr_step_50ms(in);
    }
}

static void test_init(void)
{
    TEST_BEGIN("init -> IDLE, no apply request");
    ASSERT_EQ(safety_mgr_init(), EPB_OK);
    ASSERT_EQ(safety_mgr_get_state(), SAFETY_IDLE);
    ASSERT_TRUE(!safety_mgr_apply_requested());
    TEST_END();
}

static void test_null_input(void)
{
    TEST_BEGIN("NULL input is a no-op");
    (void)safety_mgr_init();
    safety_mgr_step_50ms(NULL);
    ASSERT_EQ(safety_mgr_get_state(), SAFETY_IDLE);
    TEST_END();
}

/* ---- Auto-Apply (SWE-007 + SWE-008) ---- */

static void test_auto_apply_armed_on_engine_off(void)
{
    TEST_BEGIN("SWE-007: engine off + standstill -> AUTO_APPLY_ARMED");
    (void)safety_mgr_init();
    SafetyInputs in = make_driving();
    in.engine_running    = false;
    in.vehicle_speed_kmh = 0.0f;
    safety_mgr_step_50ms(&in);
    ASSERT_EQ(safety_mgr_get_state(), SAFETY_AUTO_APPLY_ARMED);
    ASSERT_TRUE(!safety_mgr_apply_requested());
    TEST_END();
}

static void test_auto_apply_triggers_after_2s(void)
{
    TEST_BEGIN("SWE-008: after ~2s -> TRIGGERED + apply request");
    (void)safety_mgr_init();
    SafetyInputs in = make_driving();
    in.engine_running    = false;
    in.vehicle_speed_kmh = 0.0f;
    /* Halfway: still armed, no request */
    step_n(&in, 20);
    ASSERT_EQ(safety_mgr_get_state(), SAFETY_AUTO_APPLY_ARMED);
    ASSERT_TRUE(!safety_mgr_apply_requested());
    /* Run up to TRIGGERED within at most 25 more steps (~2 s leeway) */
    for (int i = 0; i < 25; ++i) {
        safety_mgr_step_50ms(&in);
        if (safety_mgr_get_state() == SAFETY_AUTO_APPLY_TRIGGERED) break;
    }
    ASSERT_EQ(safety_mgr_get_state(), SAFETY_AUTO_APPLY_TRIGGERED);
    ASSERT_TRUE(safety_mgr_apply_requested());
    TEST_END();
}

static void test_auto_apply_aborts_on_engine_on(void)
{
    TEST_BEGIN("Auto-Apply aborts when engine comes back on");
    (void)safety_mgr_init();
    SafetyInputs in = make_driving();
    in.engine_running    = false;
    in.vehicle_speed_kmh = 0.0f;
    step_n(&in, 20);
    ASSERT_EQ(safety_mgr_get_state(), SAFETY_AUTO_APPLY_ARMED);
    in.engine_running = true;
    safety_mgr_step_50ms(&in);
    ASSERT_EQ(safety_mgr_get_state(), SAFETY_IDLE);
    TEST_END();
}

static void test_auto_apply_returns_idle_when_applied(void)
{
    TEST_BEGIN("AUTO_APPLY_TRIGGERED -> IDLE when Apply Controller is done");
    (void)safety_mgr_init();
    SafetyInputs in = make_driving();
    in.engine_running    = false;
    in.vehicle_speed_kmh = 0.0f;
    /* Drive to TRIGGERED */
    for (int i = 0; i < 50; ++i) {
        safety_mgr_step_50ms(&in);
        if (safety_mgr_get_state() == SAFETY_AUTO_APPLY_TRIGGERED) break;
    }
    ASSERT_EQ(safety_mgr_get_state(), SAFETY_AUTO_APPLY_TRIGGERED);
    in.current_state = EPB_STATE_APPLIED;
    safety_mgr_step_50ms(&in);
    ASSERT_EQ(safety_mgr_get_state(), SAFETY_IDLE);
    ASSERT_TRUE(!safety_mgr_apply_requested());
    TEST_END();
}

/* ---- Hill-Hold (SWE-009 + SWE-010) ---- */

static void test_hillhold_arms_on_grade_brake_standstill(void)
{
    TEST_BEGIN("SWE-009: grade >5% + standstill + brake -> HILL_HOLD_ARMED");
    (void)safety_mgr_init();
    SafetyInputs in = make_standstill();
    in.grade_percent = 8.0f;
    safety_mgr_step_50ms(&in);
    ASSERT_EQ(safety_mgr_get_state(), SAFETY_HILL_HOLD_ARMED);
    ASSERT_TRUE(!safety_mgr_apply_requested());
    TEST_END();
}

static void test_hillhold_negative_grade_also_triggers(void)
{
    TEST_BEGIN("Hill-hold also on negative grade (downhill)");
    (void)safety_mgr_init();
    SafetyInputs in = make_standstill();
    in.grade_percent = -7.0f;
    safety_mgr_step_50ms(&in);
    ASSERT_EQ(safety_mgr_get_state(), SAFETY_HILL_HOLD_ARMED);
    TEST_END();
}

static void test_hillhold_below_threshold_no_arm(void)
{
    TEST_BEGIN("Grade < 5% -> no hill-hold");
    (void)safety_mgr_init();
    SafetyInputs in = make_standstill();
    in.grade_percent = 4.0f;
    safety_mgr_step_50ms(&in);
    ASSERT_EQ(safety_mgr_get_state(), SAFETY_IDLE);
    TEST_END();
}

static void test_hillhold_active_on_brake_release(void)
{
    TEST_BEGIN("SWE-010: brake released -> HILL_HOLD_ACTIVE + apply request");
    (void)safety_mgr_init();
    SafetyInputs in = make_standstill();
    in.grade_percent = 8.0f;
    safety_mgr_step_50ms(&in);  /* -> HILL_HOLD_ARMED */
    in.brake_pedal_pressed = false;
    safety_mgr_step_50ms(&in);
    ASSERT_EQ(safety_mgr_get_state(), SAFETY_HILL_HOLD_ACTIVE);
    ASSERT_TRUE(safety_mgr_apply_requested());
    TEST_END();
}

static void test_hillhold_active_ends_on_vehicle_rolling(void)
{
    TEST_BEGIN("HILL_HOLD_ACTIVE ends when vehicle rolls (v > 2 km/h)");
    (void)safety_mgr_init();
    SafetyInputs in = make_standstill();
    in.grade_percent = 8.0f;
    safety_mgr_step_50ms(&in);
    in.brake_pedal_pressed = false;
    safety_mgr_step_50ms(&in);
    ASSERT_EQ(safety_mgr_get_state(), SAFETY_HILL_HOLD_ACTIVE);
    in.vehicle_speed_kmh = 3.0f;
    safety_mgr_step_50ms(&in);
    ASSERT_EQ(safety_mgr_get_state(), SAFETY_IDLE);
    TEST_END();
}

static void test_hillhold_armed_to_idle_if_grade_drops(void)
{
    TEST_BEGIN("HILL_HOLD_ARMED -> IDLE when grade < 5%");
    (void)safety_mgr_init();
    SafetyInputs in = make_standstill();
    in.grade_percent = 8.0f;
    safety_mgr_step_50ms(&in);
    ASSERT_EQ(safety_mgr_get_state(), SAFETY_HILL_HOLD_ARMED);
    in.grade_percent = 1.0f;
    safety_mgr_step_50ms(&in);
    ASSERT_EQ(safety_mgr_get_state(), SAFETY_IDLE);
    TEST_END();
}

/* ---- Mutually exclusive: nicht in beiden Modi gleichzeitig ---- */

/* ---- Drive-Away-Assist (SWE-011 + SWE-012) ---- */

static SafetyInputs make_applied_at_rest(void)
{
    SafetyInputs in = {0};
    in.engine_running       = true;
    in.brake_pedal_pressed  = false;
    in.vehicle_speed_kmh    = 0.0f;
    in.grade_percent        = 0.0f;
    in.current_state        = EPB_STATE_APPLIED;
    in.gas_pedal_percent    = 0.0f;
    in.gear_in_drive        = false;
    in.door_closed          = true;
    in.seatbelt_fastened    = true;
    return in;
}

static void test_drive_away_armed_on_intent(void)
{
    TEST_BEGIN("SWE-011 + SWE-012: intent + safety -> DRIVE_AWAY + release request");
    (void)safety_mgr_init();
    SafetyInputs in = make_applied_at_rest();
    in.gas_pedal_percent = 25.0f;
    in.gear_in_drive     = true;
    safety_mgr_step_50ms(&in);
    ASSERT_EQ(safety_mgr_get_state(), SAFETY_DRIVE_AWAY);
    ASSERT_TRUE(safety_mgr_release_requested());
    TEST_END();
}

static void test_drive_away_blocked_without_safety(void)
{
    TEST_BEGIN("SWE-012: door open blocks drive-away");
    (void)safety_mgr_init();
    SafetyInputs in = make_applied_at_rest();
    in.gas_pedal_percent = 25.0f;
    in.gear_in_drive     = true;
    in.door_closed       = false;  /* door open */
    safety_mgr_step_50ms(&in);
    ASSERT_EQ(safety_mgr_get_state(), SAFETY_IDLE);
    ASSERT_TRUE(!safety_mgr_release_requested());
    TEST_END();
}

static void test_drive_away_blocked_without_seatbelt(void)
{
    TEST_BEGIN("SWE-012: seatbelt not fastened blocks drive-away");
    (void)safety_mgr_init();
    SafetyInputs in = make_applied_at_rest();
    in.gas_pedal_percent = 25.0f;
    in.gear_in_drive     = true;
    in.seatbelt_fastened = false;
    safety_mgr_step_50ms(&in);
    ASSERT_EQ(safety_mgr_get_state(), SAFETY_IDLE);
    TEST_END();
}

static void test_drive_away_blocked_below_gas_threshold(void)
{
    TEST_BEGIN("SWE-011: throttle < 10% does not trigger drive-away");
    (void)safety_mgr_init();
    SafetyInputs in = make_applied_at_rest();
    in.gas_pedal_percent = 5.0f;
    in.gear_in_drive     = true;
    safety_mgr_step_50ms(&in);
    ASSERT_EQ(safety_mgr_get_state(), SAFETY_IDLE);
    TEST_END();
}

static void test_drive_away_ends_when_released(void)
{
    TEST_BEGIN("DRIVE_AWAY -> IDLE when Apply Controller has released");
    (void)safety_mgr_init();
    SafetyInputs in = make_applied_at_rest();
    in.gas_pedal_percent = 25.0f;
    in.gear_in_drive     = true;
    safety_mgr_step_50ms(&in);  /* -> DRIVE_AWAY */
    in.current_state = EPB_STATE_RELEASED;
    safety_mgr_step_50ms(&in);
    ASSERT_EQ(safety_mgr_get_state(), SAFETY_IDLE);
    ASSERT_TRUE(!safety_mgr_release_requested());
    TEST_END();
}

static void test_already_applied_does_not_arm_auto_apply(void)
{
    TEST_BEGIN("Already applied: no auto-apply arming");
    (void)safety_mgr_init();
    SafetyInputs in = make_driving();
    in.engine_running    = false;
    in.vehicle_speed_kmh = 0.0f;
    in.current_state     = EPB_STATE_APPLIED;
    safety_mgr_step_50ms(&in);
    ASSERT_EQ(safety_mgr_get_state(), SAFETY_IDLE);
    TEST_END();
}

int main(void)
{
    printf("== test_safety_manager ==\n");
    test_init();
    test_null_input();
    test_auto_apply_armed_on_engine_off();
    test_auto_apply_triggers_after_2s();
    test_auto_apply_aborts_on_engine_on();
    test_auto_apply_returns_idle_when_applied();
    test_hillhold_arms_on_grade_brake_standstill();
    test_hillhold_negative_grade_also_triggers();
    test_hillhold_below_threshold_no_arm();
    test_hillhold_active_on_brake_release();
    test_hillhold_active_ends_on_vehicle_rolling();
    test_hillhold_armed_to_idle_if_grade_drops();
    test_drive_away_armed_on_intent();
    test_drive_away_blocked_without_safety();
    test_drive_away_blocked_without_seatbelt();
    test_drive_away_blocked_below_gas_threshold();
    test_drive_away_ends_when_released();
    test_already_applied_does_not_arm_auto_apply();
    TEST_SUMMARY();
}