/**
 * @file test_safety_manager.c
 * @brief Unit-Tests fuer den Safety Manager (ASIL-D).
 *
 * @reqs SWE-007 SWE-008 SWE-009 SWE-010
 * @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, kein 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 ist 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: Motor aus + Stillstand -> 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: nach ca. 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 bricht ab wenn Motor wieder an");
    (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 wenn Apply Controller fertig");
    (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: Grad >5% + Stillstand + Bremse -> 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 auch bei negativem Grad (abschuessig)");
    (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("Grad < 5% -> kein 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: Bremse losgelassen -> 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 endet wenn Fahrzeug rollt (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 wenn Grad < 5% wird");
    (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 ---- */

static void test_already_applied_does_not_arm_auto_apply(void)
{
    TEST_BEGIN("Bereits Applied: kein 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_already_applied_does_not_arm_auto_apply();
    TEST_SUMMARY();
}