#include "motor_foc.h"

#define MFOC_SQRT3_OVER_2 0.86602540378443864676f
#define MFOC_INV_SQRT3   0.57735026918962576451f
#define MFOC_INV_SQRT2   0.70710678118654752440f
#define MFOC_FLOAT_MAX   3.402823466e+38f

static mfoc_output invalid_output(void) {
    const mfoc_output out = {0};
    return out;
}

static bool finite_positive(float value) {
    return value == value && value <= MFOC_FLOAT_MAX &&
           value >= -MFOC_FLOAT_MAX &&
           value > 0.0f;
}

static bool finite_value(float value) {
    return value == value && value <= MFOC_FLOAT_MAX &&
           value >= -MFOC_FLOAT_MAX;
}

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

static bool config_valid(const mfoc_config *cfg) {
    if (!cfg || !finite_positive(cfg->min_bus_voltage_v) ||
        !finite_positive(cfg->max_bus_voltage_v) ||
        cfg->max_bus_voltage_v <= cfg->min_bus_voltage_v ||
        !finite_positive(cfg->max_phase_current_a) ||
        cfg->max_phase_current_a > MFOC_BOARD_CURRENT_LIMIT_A ||
        !finite_positive(cfg->phase_sum_tolerance_a) ||
        cfg->phase_sum_tolerance_a > cfg->max_phase_current_a ||
        !finite_positive(cfg->current_kp_v_per_a) ||
        !finite_value(cfg->current_ki_v_per_a_s) ||
        cfg->current_ki_v_per_a_s < 0.0f ||
        !finite_positive(cfg->max_dt_s) || cfg->max_dt_s > 0.001f ||
        !finite_positive(cfg->modulation_ceiling) ||
        cfg->modulation_ceiling > 0.95f) {
        return false;
    }
    return true;
}

bool mfoc_init(mfoc_controller *controller, const mfoc_config *config) {
    if (!controller) return false;
    controller->cfg = (mfoc_config){0};
    controller->d_integral_v = 0.0f;
    controller->q_integral_v = 0.0f;
    controller->initialized = false;
    if (!config_valid(config)) return false;
    controller->cfg = *config;
    controller->initialized = true;
    return true;
}

void mfoc_reset(mfoc_controller *controller) {
    if (!controller) return;
    controller->d_integral_v = 0.0f;
    controller->q_integral_v = 0.0f;
}

static bool input_valid(const mfoc_controller *controller,
                        const mfoc_input *input) {
    const mfoc_config *cfg = &controller->cfg;
    if (!input || !finite_value(input->phase_a_current_a) ||
        !finite_value(input->phase_b_current_a) ||
        !finite_value(input->phase_c_current_a) ||
        !finite_value(input->sin_electrical_angle) ||
        !finite_value(input->cos_electrical_angle) ||
        !finite_value(input->bus_voltage_v) ||
        !finite_value(input->d_current_ref_a) ||
        !finite_value(input->q_current_ref_a) || !finite_value(input->dt_s)) {
        return false;
    }
    if (input->bus_voltage_v < cfg->min_bus_voltage_v ||
        input->bus_voltage_v > cfg->max_bus_voltage_v ||
        input->dt_s <= 0.0f || input->dt_s > cfg->max_dt_s) {
        return false;
    }
    if (absolute(input->phase_a_current_a) > cfg->max_phase_current_a ||
        absolute(input->phase_b_current_a) > cfg->max_phase_current_a ||
        absolute(input->phase_c_current_a) > cfg->max_phase_current_a ||
        absolute(input->d_current_ref_a) > cfg->max_phase_current_a ||
        absolute(input->q_current_ref_a) > cfg->max_phase_current_a) {
        return false;
    }
    const float max_current_squared = cfg->max_phase_current_a *
                                       cfg->max_phase_current_a;
    const float reference_current_squared =
        input->d_current_ref_a * input->d_current_ref_a +
        input->q_current_ref_a * input->q_current_ref_a;
    if (reference_current_squared > max_current_squared) return false;
    if (absolute(input->phase_a_current_a + input->phase_b_current_a +
                 input->phase_c_current_a) > cfg->phase_sum_tolerance_a) {
        return false;
    }
    const float angle_norm = input->sin_electrical_angle *
                             input->sin_electrical_angle +
                             input->cos_electrical_angle *
                             input->cos_electrical_angle;
    if (!finite_value(angle_norm) || angle_norm < 0.9025f ||
        angle_norm > 1.1025f) return false;
    return true;
}

static float clamp_symmetric(float value, float limit) {
    if (value > limit) return limit;
    if (value < -limit) return -limit;
    return value;
}

static float current_pi(float error_a, float dt_s, float vector_voltage_limit,
                        float kp, float ki, float *integral_v) {
    const float axis_limit = vector_voltage_limit * MFOC_INV_SQRT2;
    float candidate_integral = *integral_v + ki * error_a * dt_s;
    candidate_integral = clamp_symmetric(candidate_integral, axis_limit);
    float unsaturated = kp * error_a + candidate_integral;
    const bool saturated = unsaturated > axis_limit ||
                           unsaturated < -axis_limit;
    const bool drives_further_into_saturation =
        (unsaturated > axis_limit && error_a > 0.0f) ||
        (unsaturated < -axis_limit && error_a < 0.0f);
    if (saturated && drives_further_into_saturation) {
        candidate_integral = *integral_v;
        candidate_integral = clamp_symmetric(candidate_integral, axis_limit);
        unsaturated = kp * error_a + candidate_integral;
    }
    *integral_v = candidate_integral;
    return clamp_symmetric(unsaturated, axis_limit);
}

static bool duty_valid(float duty) {
    return finite_value(duty) && duty >= -0.000001f && duty <= 1.000001f;
}

static float duty_clamp(float duty) {
    if (duty < 0.0f) return 0.0f;
    if (duty > 1.0f) return 1.0f;
    return duty;
}

mfoc_output mfoc_step(mfoc_controller *controller, const mfoc_input *input) {
    mfoc_output out = invalid_output();
    if (!controller || !controller->initialized ||
        !input_valid(controller, input)) {
        mfoc_reset(controller);
        return out;
    }

    /* Three-shunt Clarke transform; samples must be phase-synchronous. */
    const float alpha_current = input->phase_a_current_a;
    const float beta_current =
        (input->phase_b_current_a - input->phase_c_current_a) *
        MFOC_INV_SQRT3;
    const float measured_current_squared = alpha_current * alpha_current +
                                           beta_current * beta_current;
    if (measured_current_squared >
        controller->cfg.max_phase_current_a *
        controller->cfg.max_phase_current_a) {
        mfoc_reset(controller);
        return invalid_output();
    }
    out.d_current_a = alpha_current * input->cos_electrical_angle +
                      beta_current * input->sin_electrical_angle;
    out.q_current_a = -alpha_current * input->sin_electrical_angle +
                      beta_current * input->cos_electrical_angle;

    const float vector_voltage_limit = input->bus_voltage_v *
                                       MFOC_INV_SQRT3 *
                                       controller->cfg.modulation_ceiling;
    out.d_voltage_v = current_pi(input->d_current_ref_a - out.d_current_a,
                                 input->dt_s, vector_voltage_limit,
                                 controller->cfg.current_kp_v_per_a,
                                 controller->cfg.current_ki_v_per_a_s,
                                 &controller->d_integral_v);
    out.q_voltage_v = current_pi(input->q_current_ref_a - out.q_current_a,
                                 input->dt_s, vector_voltage_limit,
                                 controller->cfg.current_kp_v_per_a,
                                 controller->cfg.current_ki_v_per_a_s,
                                 &controller->q_integral_v);

    /* Inverse Park. */
    out.alpha_voltage_v = out.d_voltage_v * input->cos_electrical_angle -
                          out.q_voltage_v * input->sin_electrical_angle;
    out.beta_voltage_v = out.d_voltage_v * input->sin_electrical_angle +
                         out.q_voltage_v * input->cos_electrical_angle;

    /* Inverse Clarke plus common-mode injection for centered SVPWM. */
    const float phase_a_v = out.alpha_voltage_v;
    const float phase_b_v = -0.5f * out.alpha_voltage_v +
                            MFOC_SQRT3_OVER_2 * out.beta_voltage_v;
    const float phase_c_v = -0.5f * out.alpha_voltage_v -
                            MFOC_SQRT3_OVER_2 * out.beta_voltage_v;
    float maximum = phase_a_v;
    float minimum = phase_a_v;
    if (phase_b_v > maximum) maximum = phase_b_v;
    if (phase_c_v > maximum) maximum = phase_c_v;
    if (phase_b_v < minimum) minimum = phase_b_v;
    if (phase_c_v < minimum) minimum = phase_c_v;
    const float common_mode_v = -0.5f * (maximum + minimum);
    out.duty_a = 0.5f + (phase_a_v + common_mode_v) / input->bus_voltage_v;
    out.duty_b = 0.5f + (phase_b_v + common_mode_v) / input->bus_voltage_v;
    out.duty_c = 0.5f + (phase_c_v + common_mode_v) / input->bus_voltage_v;
    if (!duty_valid(out.duty_a) || !duty_valid(out.duty_b) ||
        !duty_valid(out.duty_c)) {
        mfoc_reset(controller);
        return invalid_output();
    }
    out.duty_a = duty_clamp(out.duty_a);
    out.duty_b = duty_clamp(out.duty_b);
    out.duty_c = duty_clamp(out.duty_c);
    out.valid = true;
    return out;
}
