#include "ad_parameter_identification.h"

#include <string.h>

static AD_MotorParameters_t g_params;
static AD_Measurements_t g_last_meas;
static AD_ParamID_SafetyLimits_t g_limits;
static uint32_t g_status;
static uint32_t g_faults;
static uint32_t g_start_timestamp_us;
static uint8_t g_mode;
static uint8_t g_software_enable;
static uint8_t g_locked_rotor_allowed;

static uint8_t mode_requires_power(uint8_t mode)
{
    return (uint8_t)((mode == AD_PARAM_ID_MODE_STATOR_RESISTANCE) ||
                     (mode == AD_PARAM_ID_MODE_NO_LOAD_MAGNETIZING) ||
                     (mode == AD_PARAM_ID_MODE_LOCKED_ROTOR_LEAKAGE) ||
                     (mode == AD_PARAM_ID_MODE_INERTIA_FRICTION));
}

static uint32_t elapsed_us(uint32_t now_us, uint32_t start_us)
{
    return now_us - start_us;
}

static float absf_local(float value)
{
    return (value < 0.0f) ? -value : value;
}

static void set_fault(uint32_t fault_flags)
{
    g_faults |= fault_flags;
    if (fault_flags != 0UL) {
        g_status |= AD_PARAM_ID_STATUS_FAULT_LATCHED;
        AD_ParamID_Stop();
    }
}

static void update_safety_from_measurements(const AD_Measurements_t *meas)
{
    uint32_t faults = 0UL;

    if ((meas->status_flags & AD_MEAS_STATUS_OVERCURRENT) != 0UL) {
        faults |= AD_PARAM_ID_FAULT_OVERCURRENT;
    }
    if ((meas->status_flags & AD_MEAS_STATUS_OVERVOLTAGE) != 0UL) {
        faults |= AD_PARAM_ID_FAULT_OVERVOLTAGE;
    }
    if ((meas->status_flags & AD_MEAS_STATUS_UNDERVOLTAGE) != 0UL) {
        faults |= AD_PARAM_ID_FAULT_UNDERVOLTAGE;
    }
    if ((meas->status_flags & AD_MEAS_STATUS_OVERTEMPERATURE) != 0UL) {
        faults |= AD_PARAM_ID_FAULT_OVERTEMPERATURE;
    }
    if ((meas->status_flags & AD_MEAS_STATUS_DRIVER_FAULT) != 0UL) {
        faults |= AD_PARAM_ID_FAULT_DRIVER;
    }
    if ((meas->status_flags & AD_MEAS_STATUS_EMERGENCY_STOP) != 0UL) {
        faults |= AD_PARAM_ID_FAULT_EMERGENCY_STOP;
    }

    if (g_limits.overcurrent_A > 0.0f) {
        if ((absf_local(meas->ia_A) > g_limits.overcurrent_A) ||
            (absf_local(meas->ib_A) > g_limits.overcurrent_A) ||
            (absf_local(meas->ic_A) > g_limits.overcurrent_A)) {
            faults |= AD_PARAM_ID_FAULT_OVERCURRENT;
        }
    }

    if ((g_limits.overvoltage_V > 0.0f) && (meas->vdc_V > g_limits.overvoltage_V)) {
        faults |= AD_PARAM_ID_FAULT_OVERVOLTAGE;
    }

    if ((g_limits.undervoltage_V > 0.0f) && (meas->vdc_V < g_limits.undervoltage_V)) {
        faults |= AD_PARAM_ID_FAULT_UNDERVOLTAGE;
    }

    if ((g_limits.overtemperature_C > 0.0f) &&
        (meas->temperature_C > g_limits.overtemperature_C)) {
        faults |= AD_PARAM_ID_FAULT_OVERTEMPERATURE;
    }

    if ((g_limits.speed_limit_rpm > 0.0f) &&
        (absf_local(meas->speed_rpm) > g_limits.speed_limit_rpm)) {
        faults |= AD_PARAM_ID_FAULT_EMERGENCY_STOP;
    }

    set_fault(faults);
}

static void update_limit_status(void)
{
    if ((g_limits.overcurrent_A <= 0.0f) ||
        (g_limits.overvoltage_V <= 0.0f) ||
        (g_limits.undervoltage_V <= 0.0f) ||
        (g_limits.overtemperature_C <= 0.0f)) {
        g_status |= AD_PARAM_ID_STATUS_SAFETY_LIMITS_UNKNOWN;
    } else {
        g_status &= ~AD_PARAM_ID_STATUS_SAFETY_LIMITS_UNKNOWN;
    }
}

void AD_ParamID_Init(void)
{
    memset(&g_params, 0, sizeof(g_params));
    memset(&g_last_meas, 0, sizeof(g_last_meas));
    memset(&g_limits, 0, sizeof(g_limits));
    g_limits.timeout_us = AD_PARAM_ID_TIMEOUT_US_DEFAULT;
    g_status = AD_PARAM_ID_STATUS_SAFETY_LIMITS_UNKNOWN;
    g_faults = 0UL;
    g_start_timestamp_us = 0UL;
    g_mode = AD_PARAM_ID_MODE_IDLE;
    g_software_enable = 0U;
    g_locked_rotor_allowed = 0U;
}

void AD_ParamID_Reset(void)
{
    AD_ParamID_Stop();
    g_faults = 0UL;
    g_status &= ~(AD_PARAM_ID_STATUS_FAULT_LATCHED |
                  AD_PARAM_ID_STATUS_TIMEOUT |
                  AD_PARAM_ID_STATUS_POWER_TEST_BLOCKED |
                  AD_PARAM_ID_STATUS_LOCKED_ROTOR_BLOCKED);
    update_limit_status();
}

void AD_ParamID_Start(uint8_t mode)
{
    g_status &= ~(AD_PARAM_ID_STATUS_POWER_TEST_BLOCKED |
                  AD_PARAM_ID_STATUS_LOCKED_ROTOR_BLOCKED |
                  AD_PARAM_ID_STATUS_TIMEOUT);

    if ((g_status & AD_PARAM_ID_STATUS_FAULT_LATCHED) != 0UL) {
        g_mode = AD_PARAM_ID_MODE_IDLE;
        return;
    }

    if (mode == AD_PARAM_ID_MODE_LOCKED_ROTOR_LEAKAGE) {
        if (g_locked_rotor_allowed == 0U) {
            g_status |= AD_PARAM_ID_STATUS_LOCKED_ROTOR_BLOCKED;
            g_mode = AD_PARAM_ID_MODE_DATA_LOGGING;
            return;
        }
    }

#if AD_PARAM_ID_ENABLE_POWER_TESTS == 0
    if (mode_requires_power(mode) != 0U) {
        g_status |= AD_PARAM_ID_STATUS_POWER_TEST_BLOCKED;
        g_mode = AD_PARAM_ID_MODE_DATA_LOGGING;
        g_status |= AD_PARAM_ID_STATUS_ACTIVE;
        g_start_timestamp_us = g_last_meas.timestamp_us;
        return;
    }
#endif

    if ((mode_requires_power(mode) != 0U) && (g_software_enable == 0U)) {
        g_status |= AD_PARAM_ID_STATUS_POWER_TEST_BLOCKED;
        g_mode = AD_PARAM_ID_MODE_DATA_LOGGING;
    } else {
        g_mode = mode;
    }

    if (g_mode == AD_PARAM_ID_MODE_IDLE) {
        AD_ParamID_Stop();
    } else {
        g_status |= AD_PARAM_ID_STATUS_ACTIVE;
        g_start_timestamp_us = g_last_meas.timestamp_us;
    }
}

void AD_ParamID_Stop(void)
{
    g_mode = AD_PARAM_ID_MODE_IDLE;
    g_status &= ~(AD_PARAM_ID_STATUS_ACTIVE | AD_PARAM_ID_STATUS_POWER_STAGE_ARMED);
}

void AD_ParamID_StepFast(const AD_Measurements_t *meas)
{
    uint32_t timeout_us;

    if (meas == 0) {
        set_fault(AD_PARAM_ID_FAULT_NULL_INPUT);
        return;
    }

    g_last_meas = *meas;
    update_safety_from_measurements(meas);
    update_limit_status();

    if ((g_status & AD_PARAM_ID_STATUS_FAULT_LATCHED) != 0UL) {
        return;
    }

    if ((g_status & AD_PARAM_ID_STATUS_ACTIVE) != 0UL) {
        timeout_us = (g_limits.timeout_us != 0UL) ?
                     g_limits.timeout_us :
                     AD_PARAM_ID_TIMEOUT_US_DEFAULT;
        if (elapsed_us(meas->timestamp_us, g_start_timestamp_us) > timeout_us) {
            g_status |= AD_PARAM_ID_STATUS_TIMEOUT;
            set_fault(AD_PARAM_ID_FAULT_TIMEOUT);
        }
    }
}

void AD_ParamID_StepSlow(void)
{
    if (g_mode == AD_PARAM_ID_MODE_DATA_LOGGING) {
        return;
    }

    /*
     * Parameter estimation commands are intentionally not implemented here.
     * Hardware-specific PWM/ADC stimulus must be added in the STM32G474
     * motor-driver layer after pinout and protections are verified.
     */
}

const AD_MotorParameters_t* AD_ParamID_GetParameters(void)
{
    return &g_params;
}

uint32_t AD_ParamID_GetStatus(void)
{
    return g_status;
}

uint8_t AD_ParamID_GetMode(void)
{
    return g_mode;
}

uint32_t AD_ParamID_GetFaults(void)
{
    return g_faults;
}

void AD_ParamID_SetSafetyLimits(const AD_ParamID_SafetyLimits_t *limits)
{
    if (limits != 0) {
        g_limits = *limits;
    }
    update_limit_status();
}

void AD_ParamID_SetSoftwareEnable(uint8_t enable)
{
    g_software_enable = (enable != 0U) ? 1U : 0U;
    if (g_software_enable == 0U) {
        g_status &= ~AD_PARAM_ID_STATUS_POWER_STAGE_ARMED;
    } else if (((g_status & AD_PARAM_ID_STATUS_FAULT_LATCHED) == 0UL) &&
               (AD_PARAM_ID_ENABLE_POWER_TESTS != 0)) {
        g_status |= AD_PARAM_ID_STATUS_POWER_STAGE_ARMED;
    }
}

void AD_ParamID_SetLockedRotorAllowed(uint8_t enable)
{
    g_locked_rotor_allowed = (enable != 0U) ? 1U : 0U;
}

uint8_t AD_ParamID_IsPowerStageAllowed(void)
{
    if ((AD_PARAM_ID_ENABLE_POWER_TESTS == 0) ||
        (g_software_enable == 0U) ||
        ((g_status & AD_PARAM_ID_STATUS_FAULT_LATCHED) != 0UL)) {
        return 0U;
    }

    return 1U;
}

void AD_ParamID_EmergencyStop(uint32_t fault_flags)
{
    if (fault_flags == 0UL) {
        fault_flags = AD_PARAM_ID_FAULT_EMERGENCY_STOP;
    }
    set_fault(fault_flags);
}
