#include "ad_parameter_identification.h"

#include <string.h>

#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050)
#define AD_WEAK __attribute__((weak))
#elif defined(__CC_ARM)
#define AD_WEAK __weak
#elif defined(__GNUC__)
#define AD_WEAK __attribute__((weak))
#else
#define AD_WEAK
#endif

static AD_MotorParameters_t g_params;
static AD_Measurements_t g_last_meas;
static AD_PhaseCurrentPeaks_t g_phase_current_peaks;
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 float g_pwm_duty_limit;
static float g_rotation_frequency_hz;
static float g_rotation_modulation;
static uint16_t g_rotation_ramp_time_ms;
static uint16_t g_motor_control_type;

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) ||
                     (mode == AD_PARAM_ID_MODE_PWM_TEST_UH) ||
                     (mode == AD_PARAM_ID_MODE_PWM_TEST_UL) ||
                     (mode == AD_PARAM_ID_MODE_PWM_TEST_VH) ||
                     (mode == AD_PARAM_ID_MODE_PWM_TEST_VL) ||
                     (mode == AD_PARAM_ID_MODE_PWM_TEST_WH) ||
                     (mode == AD_PARAM_ID_MODE_PWM_TEST_WL) ||
                     (mode == AD_PARAM_ID_MODE_PWM_TEST_ALL) ||
                     (mode == AD_PARAM_ID_MODE_AUTO_IDENTIFICATION) ||
                     (mode == AD_PARAM_ID_MODE_ROTATION_3HZ));
}

static uint8_t mode_uses_test_timeout(uint8_t mode)
{
    return (uint8_t)((mode != AD_PARAM_ID_MODE_IDLE) &&
                     (mode != AD_PARAM_ID_MODE_DATA_LOGGING) &&
                     (mode != AD_PARAM_ID_MODE_ROTATION_3HZ));
}

static uint8_t mode_is_valid(uint8_t mode)
{
    return (uint8_t)((mode == AD_PARAM_ID_MODE_IDLE) ||
                     (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) ||
                     (mode == AD_PARAM_ID_MODE_DATA_LOGGING) ||
                     (mode == AD_PARAM_ID_MODE_PWM_TEST_UH) ||
                     (mode == AD_PARAM_ID_MODE_PWM_TEST_UL) ||
                     (mode == AD_PARAM_ID_MODE_PWM_TEST_VH) ||
                     (mode == AD_PARAM_ID_MODE_PWM_TEST_VL) ||
                     (mode == AD_PARAM_ID_MODE_PWM_TEST_WH) ||
                     (mode == AD_PARAM_ID_MODE_PWM_TEST_WL) ||
                     (mode == AD_PARAM_ID_MODE_PWM_TEST_ALL) ||
                     (mode == AD_PARAM_ID_MODE_AUTO_IDENTIFICATION) ||
                     (mode == AD_PARAM_ID_MODE_ROTATION_3HZ));
}

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 update_phase_current_peaks(const AD_Measurements_t *meas)
{
    float ia_abs;
    float ib_abs;
    float ic_abs;

    if (meas == 0) {
        return;
    }

    ia_abs = absf_local(meas->ia_A);
    ib_abs = absf_local(meas->ib_A);
    ic_abs = absf_local(meas->ic_A);

    if (ia_abs > g_phase_current_peaks.ia_peak_A) {
        g_phase_current_peaks.ia_peak_A = ia_abs;
    }
    if (ib_abs > g_phase_current_peaks.ib_peak_A) {
        g_phase_current_peaks.ib_peak_A = ib_abs;
    }
    if (ic_abs > g_phase_current_peaks.ic_peak_A) {
        g_phase_current_peaks.ic_peak_A = ic_abs;
    }
}

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_status |= AD_PARAM_ID_STATUS_SAFETY_LIMITS_UNKNOWN;
    } else {
        g_status &= ~AD_PARAM_ID_STATUS_SAFETY_LIMITS_UNKNOWN;
    }
}

static void enter_data_logging_blocked(void)
{
    AD_ParamID_HardwareDisable();
    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;
}

void AD_ParamID_Init(void)
{
    memset(&g_params, 0, sizeof(g_params));
    memset(&g_last_meas, 0, sizeof(g_last_meas));
    memset(&g_phase_current_peaks, 0, sizeof(g_phase_current_peaks));
    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;
    g_pwm_duty_limit = AD_PARAM_ID_DEFAULT_PWM_DUTY_LIMIT;
    g_rotation_frequency_hz = AD_PARAM_ID_DEFAULT_ROTATION_FREQUENCY_HZ;
    g_rotation_modulation = AD_PARAM_ID_DEFAULT_ROTATION_MODULATION;
    g_rotation_ramp_time_ms = AD_PARAM_ID_DEFAULT_ROTATION_RAMP_TIME_MS;
    g_motor_control_type = (uint16_t)AD_PARAM_ID_MOTOR_CONTROL_AD;
}

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 |
                  AD_PARAM_ID_STATUS_COMPLETE |
                  AD_PARAM_ID_STATUS_PARTIAL_COMPLETE |
                  AD_PARAM_ID_STATUS_STEP_FAILED |
                  AD_PARAM_ID_STATUS_LOCKED_ROTOR_SKIPPED);
    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 |
                  AD_PARAM_ID_STATUS_COMPLETE |
                  AD_PARAM_ID_STATUS_PARTIAL_COMPLETE |
                  AD_PARAM_ID_STATUS_STEP_FAILED |
                  AD_PARAM_ID_STATUS_LOCKED_ROTOR_SKIPPED);

    if (mode_is_valid(mode) == 0U) {
        AD_ParamID_Stop();
        return;
    }

    AD_ParamID_HardwareDisable();

    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;
            enter_data_logging_blocked();
            return;
        }
    }

#if AD_PARAM_ID_ENABLE_POWER_TESTS == 0
    if (mode_requires_power(mode) != 0U) {
        enter_data_logging_blocked();
        return;
    }
#endif

    if ((mode_requires_power(mode) != 0U) &&
        ((g_status & AD_PARAM_ID_STATUS_SAFETY_LIMITS_UNKNOWN) != 0UL)) {
        enter_data_logging_blocked();
        return;
    }

    if ((mode_requires_power(mode) != 0U) && (g_software_enable == 0U)) {
        enter_data_logging_blocked();
    } 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)
{
    if ((g_mode != AD_PARAM_ID_MODE_IDLE) ||
        ((g_status & (AD_PARAM_ID_STATUS_ACTIVE | AD_PARAM_ID_STATUS_POWER_STAGE_ARMED)) != 0UL)) {
        AD_ParamID_HardwareDisable();
    }
    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_phase_current_peaks(meas);
    g_status |= AD_PARAM_ID_STATUS_DATA_VALID;
    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) &&
        (mode_uses_test_timeout(g_mode) != 0U)) {
        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);
        }
    }

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

    if (g_mode == AD_PARAM_ID_MODE_ROTATION_3HZ) {
        if ((mode_requires_power(g_mode) == 0U) ||
            (AD_ParamID_IsPowerStageAllowed() == 0U) ||
            ((g_status & AD_PARAM_ID_STATUS_SAFETY_LIMITS_UNKNOWN) != 0UL) ||
            (AD_ParamID_HardwareStep(g_mode, &g_last_meas) == 0U)) {
            enter_data_logging_blocked();
        }
    }
}

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

    if ((g_mode == AD_PARAM_ID_MODE_DATA_LOGGING) ||
        (g_mode == AD_PARAM_ID_MODE_ROTATION_3HZ)) {
        return;
    }

    if ((mode_requires_power(g_mode) == 0U) ||
        (AD_ParamID_IsPowerStageAllowed() == 0U) ||
        ((g_status & AD_PARAM_ID_STATUS_SAFETY_LIMITS_UNKNOWN) != 0UL) ||
        (AD_ParamID_HardwareStep(g_mode, &g_last_meas) == 0U)) {
        enter_data_logging_blocked();
    }
}

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

const AD_Measurements_t* AD_ParamID_GetLastMeasurements(void)
{
    return &g_last_meas;
}

const AD_PhaseCurrentPeaks_t* AD_ParamID_GetPhaseCurrentPeaks(void)
{
    return &g_phase_current_peaks;
}

void AD_ParamID_ResetPhaseCurrentPeaks(void)
{
    memset(&g_phase_current_peaks, 0, sizeof(g_phase_current_peaks));
}

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_SetParameters(const AD_MotorParameters_t *params)
{
    if (params != 0) {
        g_params = *params;
        if (g_params.valid_mask != 0UL) {
            g_status |= AD_PARAM_ID_STATUS_DATA_VALID;
        }
    }
}

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)
{
    uint8_t previous_enable = g_software_enable;

    g_software_enable = (enable != 0U) ? 1U : 0U;
    if (g_software_enable == 0U) {
        g_status &= ~AD_PARAM_ID_STATUS_POWER_STAGE_ARMED;
        if ((previous_enable != 0U) ||
            (g_mode != AD_PARAM_ID_MODE_IDLE) ||
            ((g_status & AD_PARAM_ID_STATUS_ACTIVE) != 0UL)) {
            AD_ParamID_HardwareDisable();
        }
    } else if (((g_status & AD_PARAM_ID_STATUS_FAULT_LATCHED) == 0UL) &&
               ((g_status & AD_PARAM_ID_STATUS_SAFETY_LIMITS_UNKNOWN) == 0UL) &&
               (AD_PARAM_ID_ENABLE_POWER_TESTS != 0)) {
        g_status |= AD_PARAM_ID_STATUS_POWER_STAGE_ARMED;
    } else {
        g_status &= ~AD_PARAM_ID_STATUS_POWER_STAGE_ARMED;
        if (previous_enable != 0U) {
            AD_ParamID_HardwareDisable();
        }
    }
}

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

uint8_t AD_ParamID_IsLockedRotorAllowed(void)
{
    return g_locked_rotor_allowed;
}

void AD_ParamID_SetPwmDutyLimit(float duty_limit)
{
    if (duty_limit <= 0.0f) {
        g_pwm_duty_limit = AD_PARAM_ID_DEFAULT_PWM_DUTY_LIMIT;
    } else if (duty_limit > 0.45f) {
        g_pwm_duty_limit = 0.45f;
    } else {
        g_pwm_duty_limit = duty_limit;
    }
}

float AD_ParamID_GetPwmDutyLimit(void)
{
    return g_pwm_duty_limit;
}

void AD_ParamID_SetRotationFrequencyHz(float frequency_hz)
{
    if (frequency_hz <= 0.0f) {
        g_rotation_frequency_hz = AD_PARAM_ID_DEFAULT_ROTATION_FREQUENCY_HZ;
    } else if (frequency_hz < 0.1f) {
        g_rotation_frequency_hz = 0.1f;
    } else if (frequency_hz > AD_PARAM_ID_MAX_ROTATION_FREQUENCY_HZ) {
        g_rotation_frequency_hz = AD_PARAM_ID_MAX_ROTATION_FREQUENCY_HZ;
    } else {
        g_rotation_frequency_hz = frequency_hz;
    }
}

float AD_ParamID_GetRotationFrequencyHz(void)
{
    return g_rotation_frequency_hz;
}

void AD_ParamID_SetRotationModulation(float modulation)
{
    if (modulation <= 0.0f) {
        g_rotation_modulation = AD_PARAM_ID_DEFAULT_ROTATION_MODULATION;
    } else if (modulation < 0.01f) {
        g_rotation_modulation = 0.01f;
    } else if (modulation > 0.45f) {
        g_rotation_modulation = 0.45f;
    } else {
        g_rotation_modulation = modulation;
    }
}

float AD_ParamID_GetRotationModulation(void)
{
    return g_rotation_modulation;
}

void AD_ParamID_SetRotationRampTimeMs(uint16_t ramp_time_ms)
{
    if (ramp_time_ms == 0U) {
        g_rotation_ramp_time_ms = AD_PARAM_ID_DEFAULT_ROTATION_RAMP_TIME_MS;
    } else if (ramp_time_ms < (uint16_t)AD_PARAM_ID_MIN_ROTATION_RAMP_TIME_MS) {
        g_rotation_ramp_time_ms = (uint16_t)AD_PARAM_ID_MIN_ROTATION_RAMP_TIME_MS;
    } else if (ramp_time_ms > (uint16_t)AD_PARAM_ID_MAX_ROTATION_RAMP_TIME_MS) {
        g_rotation_ramp_time_ms = (uint16_t)AD_PARAM_ID_MAX_ROTATION_RAMP_TIME_MS;
    } else {
        g_rotation_ramp_time_ms = ramp_time_ms;
    }
}

uint16_t AD_ParamID_GetRotationRampTimeMs(void)
{
    return g_rotation_ramp_time_ms;
}

void AD_ParamID_SetMotorControlType(uint16_t motor_control_type)
{
    if (motor_control_type == (uint16_t)AD_PARAM_ID_MOTOR_CONTROL_BLDC) {
        g_motor_control_type = (uint16_t)AD_PARAM_ID_MOTOR_CONTROL_BLDC;
    } else {
        g_motor_control_type = (uint16_t)AD_PARAM_ID_MOTOR_CONTROL_AD;
    }
}

uint16_t AD_ParamID_GetMotorControlType(void)
{
    return g_motor_control_type;
}

void AD_ParamID_SetPolePairs(uint16_t pole_pairs)
{
    if (pole_pairs == 0U) {
        g_params.pole_pairs = (float)AD_PARAM_ID_DEFAULT_POLE_PAIRS;
    } else if (pole_pairs > (uint16_t)AD_PARAM_ID_MAX_POLE_PAIRS) {
        g_params.pole_pairs = (float)AD_PARAM_ID_MAX_POLE_PAIRS;
    } else {
        g_params.pole_pairs = (float)pole_pairs;
    }

    g_params.valid_mask |= AD_MOTOR_PARAM_VALID_POLE_PAIRS;
    g_status |= AD_PARAM_ID_STATUS_DATA_VALID;
}

uint16_t AD_ParamID_GetPolePairs(void)
{
    if (g_params.pole_pairs <= 0.0f) {
        return (uint16_t)AD_PARAM_ID_DEFAULT_POLE_PAIRS;
    }
    if (g_params.pole_pairs > (float)AD_PARAM_ID_MAX_POLE_PAIRS) {
        return (uint16_t)AD_PARAM_ID_MAX_POLE_PAIRS;
    }

    return (uint16_t)(g_params.pole_pairs + 0.5f);
}

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) ||
        ((g_status & AD_PARAM_ID_STATUS_SAFETY_LIMITS_UNKNOWN) != 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);
}

void AD_ParamID_MarkComplete(void)
{
    g_status |= AD_PARAM_ID_STATUS_COMPLETE;
    AD_ParamID_Stop();
}

void AD_ParamID_MarkPartialComplete(void)
{
    g_status |= AD_PARAM_ID_STATUS_PARTIAL_COMPLETE;
}

void AD_ParamID_MarkStepFailed(void)
{
    g_status |= AD_PARAM_ID_STATUS_STEP_FAILED;
}

void AD_ParamID_MarkLockedRotorSkipped(void)
{
    g_status |= AD_PARAM_ID_STATUS_LOCKED_ROTOR_SKIPPED |
                AD_PARAM_ID_STATUS_PARTIAL_COMPLETE;
}

AD_WEAK uint8_t AD_ParamID_HardwareStep(uint8_t mode, const AD_Measurements_t *meas)
{
    (void)mode;
    (void)meas;
    return 0U;
}

AD_WEAK void AD_ParamID_HardwareDisable(void)
{
}
