/**
 * @file test_apply_controller.c
 * @brief Unit-Tests fuer den Apply-Controller (ASIL-D Kern).
 *
 * @reqs SWE-001 SWE-002 SWE-003 SWE-004
 * @arch SWA-002
 */
#include "../unit_test_framework.h"
#include "../../src/apply_controller.h"
#include "../../src/actuator_driver.h"

TestStats g_test_stats = {0, 0};

static ApplyInputs make_idle_inputs(void)
{
    ApplyInputs in = {0};
    in.sw_state              = SWITCH_NEUTRAL;
    in.standstill            = true;
    in.engine_running        = true;
    in.brake_pedal_pressed   = true;
    in.gear_engaged          = true;
    in.safety_apply_request  = false;
    in.left_force_n          = 0;
    in.right_force_n         = 0;
    return in;
}

static void apply_full_cycle_until_state(ApplyInputs* in, EpbState target, int max_steps)
{
    for (int i = 0; i < max_steps; ++i) {
        apply_ctrl_step_50ms(in);
        if (apply_ctrl_get_state() == target) {
            return;
        }
    }
}

static void test_init(void)
{
    TEST_BEGIN("init -> RELEASED");
    (void)actuator_init();
    ASSERT_EQ(apply_ctrl_init(), EPB_OK);
    ASSERT_EQ(apply_ctrl_get_state(), EPB_STATE_RELEASED);
    TEST_END();
}

static void test_null_input(void)
{
    TEST_BEGIN("step_50ms(NULL) -> last_error=EINVAL");
    (void)actuator_init();
    (void)apply_ctrl_init();
    apply_ctrl_step_50ms(NULL);
    ASSERT_EQ(apply_ctrl_last_error(), EPB_EINVAL);
    TEST_END();
}

static void test_apply_request_starts_applying(void)
{
    TEST_BEGIN("Apply-Request bei Stillstand -> APPLYING");
    (void)actuator_init();
    (void)apply_ctrl_init();
    ApplyInputs in = make_idle_inputs();
    in.sw_state = SWITCH_APPLY;
    apply_ctrl_step_50ms(&in);
    ASSERT_EQ(apply_ctrl_get_state(), EPB_STATE_APPLYING);
    TEST_END();
}

static void test_no_apply_without_standstill(void)
{
    TEST_BEGIN("Apply-Request ohne Stillstand wird ignoriert");
    (void)actuator_init();
    (void)apply_ctrl_init();
    ApplyInputs in = make_idle_inputs();
    in.standstill = false;
    in.sw_state = SWITCH_APPLY;
    apply_ctrl_step_50ms(&in);
    ASSERT_EQ(apply_ctrl_get_state(), EPB_STATE_RELEASED);
    TEST_END();
}

static void test_applying_reaches_applied_on_target_force(void)
{
    TEST_BEGIN("APPLYING -> APPLIED wenn beide Klemmkraefte >= Target");
    (void)actuator_init();
    (void)apply_ctrl_init();
    ApplyInputs in = make_idle_inputs();
    in.sw_state = SWITCH_APPLY;
    apply_ctrl_step_50ms(&in);  /* -> APPLYING */
    in.sw_state = SWITCH_NEUTRAL;
    in.left_force_n  = 12000;
    in.right_force_n = 12000;
    apply_ctrl_step_50ms(&in);
    ASSERT_EQ(apply_ctrl_get_state(), EPB_STATE_APPLIED);
    TEST_END();
}

static void test_applying_timeout_to_error(void)
{
    TEST_BEGIN("APPLYING-Timeout (>30 Steps) -> ERROR");
    (void)actuator_init();
    (void)apply_ctrl_init();
    ApplyInputs in = make_idle_inputs();
    in.sw_state = SWITCH_APPLY;
    apply_ctrl_step_50ms(&in);  /* -> APPLYING */
    in.sw_state = SWITCH_NEUTRAL;
    /* Klemmkraefte bleiben bei 0 -> Timeout nach 30 Steps */
    apply_full_cycle_until_state(&in, EPB_STATE_ERROR, 35);
    ASSERT_EQ(apply_ctrl_get_state(), EPB_STATE_ERROR);
    ASSERT_EQ(apply_ctrl_last_error(), EPB_ETIMEOUT);
    TEST_END();
}

static void test_applied_holds_force(void)
{
    TEST_BEGIN("APPLIED haelt Klemmkraft (SWE-001)");
    (void)actuator_init();
    (void)apply_ctrl_init();
    ApplyInputs in = make_idle_inputs();
    in.sw_state = SWITCH_APPLY;
    apply_ctrl_step_50ms(&in);
    in.sw_state = SWITCH_NEUTRAL;
    in.left_force_n  = 12000;
    in.right_force_n = 12000;
    apply_ctrl_step_50ms(&in);  /* -> APPLIED */

    /* Klemmkraft sinkt auf 10000 (unter Toleranz-Schwelle 10800) */
    in.left_force_n  = 10000;
    in.right_force_n = 10000;
    apply_ctrl_step_50ms(&in);
    /* Apply-Controller muss nachregeln -> APPLYING */
    ASSERT_EQ(apply_ctrl_get_state(), EPB_STATE_APPLYING);
    TEST_END();
}

static void test_release_requires_preconditions(void)
{
    TEST_BEGIN("Release ohne Engine wird abgelehnt");
    (void)actuator_init();
    (void)apply_ctrl_init();
    ApplyInputs in = make_idle_inputs();
    in.sw_state = SWITCH_APPLY;
    apply_ctrl_step_50ms(&in);
    in.sw_state = SWITCH_NEUTRAL;
    in.left_force_n  = 12000;
    in.right_force_n = 12000;
    apply_ctrl_step_50ms(&in);  /* -> APPLIED */

    in.sw_state = SWITCH_RELEASE;
    in.engine_running = false;
    apply_ctrl_step_50ms(&in);
    ASSERT_EQ(apply_ctrl_get_state(), EPB_STATE_APPLIED);
    TEST_END();
}

static void test_release_with_preconditions(void)
{
    TEST_BEGIN("Release mit allen Voraussetzungen -> RELEASING -> RELEASED");
    (void)actuator_init();
    (void)apply_ctrl_init();
    ApplyInputs in = make_idle_inputs();
    in.sw_state = SWITCH_APPLY;
    apply_ctrl_step_50ms(&in);
    in.sw_state = SWITCH_NEUTRAL;
    in.left_force_n  = 12000;
    in.right_force_n = 12000;
    apply_ctrl_step_50ms(&in);  /* APPLIED */

    in.sw_state = SWITCH_RELEASE;
    apply_ctrl_step_50ms(&in);
    ASSERT_EQ(apply_ctrl_get_state(), EPB_STATE_RELEASING);

    in.sw_state = SWITCH_NEUTRAL;
    in.left_force_n  = 0;
    in.right_force_n = 0;
    apply_ctrl_step_50ms(&in);
    ASSERT_EQ(apply_ctrl_get_state(), EPB_STATE_RELEASED);
    TEST_END();
}

static void test_safety_apply_request(void)
{
    TEST_BEGIN("Safety-Manager Apply-Request (Hill-Hold/Auto-Apply)");
    (void)actuator_init();
    (void)apply_ctrl_init();
    ApplyInputs in = make_idle_inputs();
    in.sw_state = SWITCH_NEUTRAL;
    in.safety_apply_request = true;
    apply_ctrl_step_50ms(&in);
    ASSERT_EQ(apply_ctrl_get_state(), EPB_STATE_APPLYING);
    TEST_END();
}

static void test_watchdog_alive_counter(void)
{
    TEST_BEGIN("Step-Counter steigt monoton (SWE-002 Watchdog-Alive)");
    (void)actuator_init();
    (void)apply_ctrl_init();
    ApplyInputs in = make_idle_inputs();
    uint32_t before = apply_ctrl_get_step_count();
    apply_ctrl_step_50ms(&in);
    apply_ctrl_step_50ms(&in);
    apply_ctrl_step_50ms(&in);
    ASSERT_EQ(apply_ctrl_get_step_count(), before + 3);
    TEST_END();
}

static void test_error_state_recoverable(void)
{
    TEST_BEGIN("ERROR -> RELEASED bei neutralem Switch");
    (void)actuator_init();
    (void)apply_ctrl_init();
    ApplyInputs in = make_idle_inputs();
    in.sw_state = SWITCH_APPLY;
    apply_ctrl_step_50ms(&in);
    in.sw_state = SWITCH_NEUTRAL;
    apply_full_cycle_until_state(&in, EPB_STATE_ERROR, 35);
    ASSERT_EQ(apply_ctrl_get_state(), EPB_STATE_ERROR);
    apply_ctrl_step_50ms(&in);
    ASSERT_EQ(apply_ctrl_get_state(), EPB_STATE_RELEASED);
    TEST_END();
}

int main(void)
{
    printf("== test_apply_controller ==\n");
    test_init();
    test_null_input();
    test_apply_request_starts_applying();
    test_no_apply_without_standstill();
    test_applying_reaches_applied_on_target_force();
    test_applying_timeout_to_error();
    test_applied_holds_force();
    test_release_requires_preconditions();
    test_release_with_preconditions();
    test_safety_apply_request();
    test_watchdog_alive_counter();
    test_error_state_recoverable();
    TEST_SUMMARY();
}