#include "motor_foc.h"

#include <math.h>
#include <stdio.h>
#include <stdlib.h>

#define CHECK(condition) do { \
    if (!(condition)) { \
        fprintf(stderr, "FAIL %s:%d: %s\n", __FILE__, __LINE__, #condition); \
        exit(1); \
    } \
} while (0)

static mfoc_config test_config(void) {
    const mfoc_config config = {
        .min_bus_voltage_v = 9.0f,
        .max_bus_voltage_v = 12.6f,
        .max_phase_current_a = 2.0f,
        .phase_sum_tolerance_a = 0.10f,
        .current_kp_v_per_a = 0.5f,
        .current_ki_v_per_a_s = 100.0f,
        .max_dt_s = 0.0001f,
        .modulation_ceiling = 0.90f
    };
    return config;
}

static mfoc_input zero_input(void) {
    const mfoc_input input = {
        .phase_a_current_a = 0.0f,
        .phase_b_current_a = 0.0f,
        .phase_c_current_a = 0.0f,
        .sin_electrical_angle = 0.0f,
        .cos_electrical_angle = 1.0f,
        .bus_voltage_v = 11.1f,
        .d_current_ref_a = 0.0f,
        .q_current_ref_a = 0.0f,
        .dt_s = 0.00005f
    };
    return input;
}

static void test_config_and_zero_vector(void) {
    mfoc_controller controller;
    mfoc_config config = test_config();
    CHECK(mfoc_init(&controller, &config));
    mfoc_input input = zero_input();
    mfoc_output out = mfoc_step(&controller, &input);
    CHECK(out.valid);
    CHECK(fabsf(out.d_current_a) < 0.000001f);
    CHECK(fabsf(out.q_current_a) < 0.000001f);
    CHECK(fabsf(out.duty_a - 0.5f) < 0.000001f);
    CHECK(fabsf(out.duty_b - 0.5f) < 0.000001f);
    CHECK(fabsf(out.duty_c - 0.5f) < 0.000001f);

    config.max_phase_current_a = 2.01f;
    CHECK(!mfoc_init(&controller, &config));
    out = mfoc_step(&controller, &input);
    CHECK(!out.valid);
    CHECK(out.duty_a == 0.0f && out.duty_b == 0.0f && out.duty_c == 0.0f);
}

static void test_clarke_park_and_svpwm(void) {
    mfoc_controller controller;
    mfoc_config config = test_config();
    CHECK(mfoc_init(&controller, &config));
    mfoc_input input = zero_input();
    input.phase_a_current_a = 0.60f;
    input.phase_b_current_a = -0.30f;
    input.phase_c_current_a = -0.30f;
    mfoc_output out = mfoc_step(&controller, &input);
    CHECK(out.valid);
    CHECK(fabsf(out.d_current_a - 0.60f) < 0.00001f);
    CHECK(fabsf(out.q_current_a) < 0.00001f);
    CHECK(out.d_voltage_v < 0.0f);
    CHECK(out.duty_a < out.duty_b);
    CHECK(fabsf((out.duty_a + out.duty_b + out.duty_c) / 3.0f - 0.5f) < 0.02f);

    CHECK(mfoc_init(&controller, &config));
    input = zero_input();
    input.phase_b_current_a = 0.4330127f;
    input.phase_c_current_a = -0.4330127f;
    input.q_current_ref_a = 1.0f;
    out = mfoc_step(&controller, &input);
    CHECK(out.valid);
    CHECK(fabsf(out.q_current_a - 0.50f) < 0.0001f);
    CHECK(out.q_voltage_v > 0.0f);

    input = zero_input();
    input.q_current_ref_a = 2.0f;
    config.current_kp_v_per_a = 100.0f;
    CHECK(mfoc_init(&controller, &config));
    out = mfoc_step(&controller, &input);
    CHECK(out.valid);
    CHECK(isfinite(out.duty_a) && isfinite(out.duty_b) && isfinite(out.duty_c));
    CHECK(out.duty_a >= 0.0f && out.duty_a <= 1.0f);
    CHECK(out.duty_b >= 0.0f && out.duty_b <= 1.0f);
    CHECK(out.duty_c >= 0.0f && out.duty_c <= 1.0f);
}

static void expect_safe_reject(mfoc_controller *controller, mfoc_input input) {
    CHECK(mfoc_init(controller, &(mfoc_config){
        .min_bus_voltage_v = 9.0f,
        .max_bus_voltage_v = 12.6f,
        .max_phase_current_a = 2.0f,
        .phase_sum_tolerance_a = 0.10f,
        .current_kp_v_per_a = 0.5f,
        .current_ki_v_per_a_s = 100.0f,
        .max_dt_s = 0.0001f,
        .modulation_ceiling = 0.90f
    }));
    mfoc_output out = mfoc_step(controller, &input);
    CHECK(!out.valid);
    CHECK(out.duty_a == 0.0f && out.duty_b == 0.0f && out.duty_c == 0.0f);
    CHECK(controller->d_integral_v == 0.0f && controller->q_integral_v == 0.0f);
}

static void test_fail_closed_inputs(void) {
    mfoc_controller controller;
    mfoc_input input = zero_input();
    input.phase_a_current_a = 2.01f;
    expect_safe_reject(&controller, input);
    input = zero_input();
    input.bus_voltage_v = 8.99f;
    expect_safe_reject(&controller, input);
    input = zero_input();
    input.q_current_ref_a = 2.01f;
    expect_safe_reject(&controller, input);
    input = zero_input();
    input.d_current_ref_a = 1.5f;
    input.q_current_ref_a = 1.5f;
    expect_safe_reject(&controller, input);
    input = zero_input();
    input.sin_electrical_angle = 0.3f;
    input.cos_electrical_angle = 0.3f;
    expect_safe_reject(&controller, input);
    input = zero_input();
    input.dt_s = 0.00011f;
    expect_safe_reject(&controller, input);
    input = zero_input();
    input.phase_a_current_a = 0.11f;
    expect_safe_reject(&controller, input);
    input = zero_input();
    input.bus_voltage_v = NAN;
    expect_safe_reject(&controller, input);
}

static void test_integrator_reset(void) {
    mfoc_controller controller;
    mfoc_config config = test_config();
    config.current_kp_v_per_a = 0.5f;
    config.current_ki_v_per_a_s = 1000.0f;
    CHECK(mfoc_init(&controller, &config));
    mfoc_input input = zero_input();
    input.q_current_ref_a = 0.5f;
    mfoc_output out = mfoc_step(&controller, &input);
    CHECK(out.valid);
    CHECK(controller.q_integral_v > 0.0f);
    mfoc_reset(&controller);
    CHECK(controller.d_integral_v == 0.0f);
    CHECK(controller.q_integral_v == 0.0f);
}

int main(void) {
    test_config_and_zero_vector();
    test_clarke_park_and_svpwm();
    test_fail_closed_inputs();
    test_integrator_reset();
    puts("PASS: motor FOC current-loop transform, modulation, limits, fail-closed input and reset");
    return 0;
}
