/**
 * @file apply_controller.c
 * @brief Apply/Hold/Release State Machine.
 *
 * @arch SWA-002
 * @reqs SWE-001 SWE-002 SWE-003 SWE-004
 *
 * ASIL: D. This is the safety-critical core logic.
 * Changes require a technical review with 2 approvals.
 */
#include <stddef.h>

#include "apply_controller.h"
#include "actuator_driver.h"

typedef struct {
    EpbState  state;
    uint8_t   step_in_state;    /* 50ms ticks in the current state */
    EpbStatus last_error;
    uint32_t  step_count;       /* Watchdog alive counter        */
} ApplyCtx;

static ApplyCtx s_ctx;

static void enter_state(EpbState new_state)
{
    s_ctx.state         = new_state;
    s_ctx.step_in_state = 0U;
}

static bool release_preconditions_ok(const ApplyInputs* in)
{
    /* @reqs SWE-005 (Release preconditions) — consumed here */
    return in->engine_running
        && in->brake_pedal_pressed
        && in->gear_engaged;
}

static bool apply_request_present(const ApplyInputs* in)
{
    return (in->sw_state == SWITCH_APPLY) || in->safety_apply_request;
}

static bool release_request_present(const ApplyInputs* in)
{
    return in->sw_state == SWITCH_RELEASE;
}

static uint16_t min_force(const ApplyInputs* in)
{
    return (in->left_force_n < in->right_force_n)
        ? in->left_force_n : in->right_force_n;
}

EpbStatus apply_ctrl_init(void)
{
    s_ctx.state         = EPB_STATE_RELEASED;
    s_ctx.step_in_state = 0U;
    s_ctx.last_error    = EPB_OK;
    s_ctx.step_count    = 0U;
    return EPB_OK;
}

void apply_ctrl_step_50ms(const ApplyInputs* in)
{
    if (in == NULL) {
        s_ctx.last_error = EPB_EINVAL;
        return;
    }

    /* SWE-002: Watchdog alive counter erhoehen */
    ++s_ctx.step_count;

    if (s_ctx.step_in_state < UINT8_MAX) {
        ++s_ctx.step_in_state;
    }

    switch (s_ctx.state) {
    case EPB_STATE_RELEASED:
        if (apply_request_present(in) && in->standstill) {
            (void)actuator_apply(ACTUATOR_LEFT,  80U);
            (void)actuator_apply(ACTUATOR_RIGHT, 80U);
            enter_state(EPB_STATE_APPLYING);
        }
        break;

    case EPB_STATE_APPLYING:
        /* SWE-004: Check target clamping force reached */
        if (min_force(in) >= APPLY_TARGET_FORCE_N) {
            (void)actuator_stop(ACTUATOR_LEFT);
            (void)actuator_stop(ACTUATOR_RIGHT);
            enter_state(EPB_STATE_APPLIED);
        } else if (s_ctx.step_in_state >= APPLY_TIMEOUT_50MS) {
            s_ctx.last_error = EPB_ETIMEOUT;
            (void)actuator_stop(ACTUATOR_LEFT);
            (void)actuator_stop(ACTUATOR_RIGHT);
            enter_state(EPB_STATE_ERROR);
        }
        break;

    case EPB_STATE_APPLIED:
        /* SWE-001: Hold clamping force — re-apply on drop */
        if (min_force(in) < (APPLY_TARGET_FORCE_N - HOLD_TOLERANCE_N)) {
            (void)actuator_apply(ACTUATOR_LEFT,  60U);
            (void)actuator_apply(ACTUATOR_RIGHT, 60U);
            enter_state(EPB_STATE_APPLYING);
            break;
        }
        if (release_request_present(in) && release_preconditions_ok(in)) {
            (void)actuator_release(ACTUATOR_LEFT,  80U);
            (void)actuator_release(ACTUATOR_RIGHT, 80U);
            enter_state(EPB_STATE_RELEASING);
        }
        break;

    case EPB_STATE_RELEASING:
        if (min_force(in) < HOLD_TOLERANCE_N) {
            (void)actuator_stop(ACTUATOR_LEFT);
            (void)actuator_stop(ACTUATOR_RIGHT);
            enter_state(EPB_STATE_RELEASED);
        } else if (s_ctx.step_in_state >= (APPLY_TIMEOUT_50MS - 6U)) {
            s_ctx.last_error = EPB_ETIMEOUT;
            (void)actuator_stop(ACTUATOR_LEFT);
            (void)actuator_stop(ACTUATOR_RIGHT);
            enter_state(EPB_STATE_ERROR);
        }
        break;

    case EPB_STATE_ERROR:
    default:
        /* Wait for reset; safe state is Apply, hence no release */
        if (!apply_request_present(in) && !release_request_present(in)) {
            s_ctx.last_error = EPB_OK;
            enter_state(EPB_STATE_RELEASED);
        }
        break;
    }
}

EpbState apply_ctrl_get_state(void)
{
    return s_ctx.state;
}

EpbStatus apply_ctrl_last_error(void)
{
    return s_ctx.last_error;
}

uint32_t apply_ctrl_get_step_count(void)
{
    return s_ctx.step_count;
}