#include "drill_smo.h"

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

#define PI 3.14159265358979323846f
#define TWO_PI 6.28318530717958647692f

static float wrap_pi_test(float x) {
    while (x > PI) x -= TWO_PI;
    while (x < -PI) x += TWO_PI;
    return x;
}

static drill_smo_config config(void) {
    return (drill_smo_config){
        .phase_resistance_ohm = 0.5f,
        .phase_inductance_h = 0.0005f,
        .sliding_gain_v = 10.0f,
        .current_error_sat_a = 0.25f,
        .bemf_filter_tau_s = 0.000012f,
        .pll_kp_per_s = 1800.0f,
        .pll_ki_per_s2 = 80000.0f,
        .min_bemf_v = 0.25f,
        .max_phase_voltage_v = 7.3f,
        .max_phase_current_a = 2.0f,
        .max_mechanical_rpm = 50000.0f,
        .max_dt_s = 0.000025f,
        .lock_phase_error_rad = 0.60f,
        .pole_pairs = 4u,
        .lock_samples = 16u,
    };
}

static void test_sincos_quadrants(void) {
    static const float angles[] = {0.0f, 0.4f, 1.2f, 2.1f, -0.7f, -2.8f,
                                  6.2831853f, -6.2831853f, 15.7f};
    for (unsigned k=0; k<sizeof(angles)/sizeof(angles[0]); ++k) {
        float sn, cs;
        drill_smo_sincos(angles[k], &sn, &cs);
        float expected_s, expected_c;
        /* independent low-cost reference series is unnecessary for these checks */
        const float a=angles[k];
        float x=a;
        float cosine_sign=1.0f;
        while(x>PI) x-=TWO_PI;
        while(x<-PI) x+=TWO_PI;
        if(x>PI/2.0f) { x=PI-x; cosine_sign=-1.0f; }
        else if(x<-PI/2.0f) { x=-PI-x; cosine_sign=-1.0f; }
        const float x2=x*x;
        expected_s=x*(1.0f-x2/6.0f+x2*x2/120.0f-x2*x2*x2/5040.0f);
        expected_c=cosine_sign*(1.0f-x2/2.0f+x2*x2/24.0f-x2*x2*x2/720.0f);
        assert(sn == sn && cs == cs);
        assert((sn-expected_s < 0.004f) && (sn-expected_s > -0.004f));
        assert((cs-expected_c < 0.004f) && (cs-expected_c > -0.004f));
    }
    float sn=1.0f, cs=1.0f;
    drill_smo_sincos(1.0e20f,&sn,&cs);
    assert(sn==0.0f && cs==0.0f);
    drill_smo_sincos(NAN,&sn,&cs);
    assert(sn==0.0f && cs==0.0f);
}

static void test_svpwm_voltage_reconstruction(void) {
    float alpha=0.0f, beta=0.0f;
    assert(drill_smo_svpwm_to_ab(0.5f,0.5f,0.5f,12.6f,&alpha,&beta));
    assert(alpha>-0.001f && alpha<0.001f);
    assert(beta>-0.001f && beta<0.001f);
    assert(drill_smo_svpwm_to_ab(0.75f,0.5f,0.25f,12.0f,&alpha,&beta));
    assert(alpha>2.99f && alpha<3.01f);
    assert(beta>1.72f && beta<1.74f);
    assert(drill_smo_svpwm_to_ab(0.85f,0.6f,0.35f,12.0f,&alpha,&beta));
    assert(alpha>2.99f && alpha<3.01f);
    assert(beta>1.72f && beta<1.74f);
    assert(!drill_smo_svpwm_to_ab(-0.1f,0.5f,0.25f,12.0f,&alpha,&beta));
    assert(alpha==0.0f && beta==0.0f);
}

static void run_rotating_bemf(int direction, float target_speed, float *rpm,
                              float *angle_error, bool *locked) {
    drill_smo_state s;
    const drill_smo_config c=config();
    assert(drill_smo_init(&s,&c));
    const float dt=0.00001f;
    const float current_alpha=0.20f, current_beta=-0.10f;
    float theta=0.7f;
    drill_smo_output o={0};
    for (unsigned n=0; n<150000u; ++n) {
        float sn,cs;
        drill_smo_sincos(theta,&sn,&cs);
        const float e_alpha=-target_speed*0.00030f*sn;
        const float e_beta= target_speed*0.00030f*cs;
        const drill_smo_input i={
            .voltage_alpha_v=c.phase_resistance_ohm*current_alpha+e_alpha,
            .voltage_beta_v=c.phase_resistance_ohm*current_beta+e_beta,
            .current_alpha_a=current_alpha,
            .current_beta_a=current_beta,
            .dt_s=dt,
            .commanded_direction=(int8_t)direction,
        };
        assert(drill_smo_step(&s,&i,&o));
        theta=wrap_pi_test(theta+target_speed*dt);
    }
    *rpm=o.mechanical_speed_rpm;
    *angle_error=wrap_pi_test(o.electrical_angle_rad-theta);
    *locked=o.observer_locked;
}

static void test_forward_and_reverse_tracking(void) {
    float rpm,error;
    bool locked;
    run_rotating_bemf(1,20525.0f,&rpm,&error,&locked);
    printf("FORWARD synthetic: %.1f rpm, angle error %.3f rad, locked=%d\n",
           rpm,error,locked);
    assert(locked);
    assert(rpm>47000.0f && rpm<51000.0f);
    assert(error<0.20f && error>-0.20f);
    run_rotating_bemf(-1,-18850.0f,&rpm,&error,&locked);
    printf("REVERSE synthetic: %.1f rpm, angle error %.3f rad, locked=%d\n",
           rpm,error,locked);
    assert(locked);
    assert(rpm< -42000.0f && rpm> -48000.0f);
    assert(error<0.20f && error>-0.20f);
}

static void test_invalid_input_fails_closed(void) {
    drill_smo_state s;
    drill_smo_config c=config();
    drill_smo_output o;
    assert(drill_smo_init(&s,&c));
    drill_smo_input i={0};
    i.dt_s=0.00001f;
    i.commanded_direction=1;
    i.current_alpha_a=2.1f;
    assert(!drill_smo_step(&s,&i,&o));
    assert(!s.initialized);
    assert(!o.valid);
    assert(drill_smo_init(&s,&c));
    i.current_alpha_a=1.5f;
    i.current_beta_a=1.5f;
    assert(!drill_smo_step(&s,&i,&o));
    assert(!s.initialized && !o.valid);
    c.pole_pairs=0u;
    assert(!drill_smo_init(&s,&c));
}

static void test_no_back_emf_cannot_lock(void) {
    drill_smo_state s;
    const drill_smo_config c=config();
    drill_smo_output o;
    assert(drill_smo_init(&s,&c));
    const drill_smo_input i={
        .voltage_alpha_v=0.0f, .voltage_beta_v=0.0f,
        .current_alpha_a=0.0f, .current_beta_a=0.0f,
        .dt_s=0.00001f, .commanded_direction=1,
    };
    for (unsigned n=0; n<400u; ++n) assert(drill_smo_step(&s,&i,&o));
    assert(!o.angle_valid && !o.observer_locked);
}

static void test_back_emf_loss_unlocks(void) {
    drill_smo_state s;
    const drill_smo_config c=config();
    drill_smo_output o={0};
    assert(drill_smo_init(&s,&c));
    const float speed=20525.0f, dt=0.00001f;
    const float ia=0.20f, ib=-0.10f;
    float theta=0.7f;
    for (unsigned n=0; n<120000u; ++n) {
        float sn,cs;
        drill_smo_sincos(theta,&sn,&cs);
        const float e_alpha=-speed*0.00030f*sn;
        const float e_beta=speed*0.00030f*cs;
        const drill_smo_input i={
            .voltage_alpha_v=c.phase_resistance_ohm*ia+e_alpha,
            .voltage_beta_v=c.phase_resistance_ohm*ib+e_beta,
            .current_alpha_a=ia, .current_beta_a=ib,
            .dt_s=dt, .commanded_direction=1,
        };
        assert(drill_smo_step(&s,&i,&o));
        theta=wrap_pi_test(theta+speed*dt);
    }
    assert(o.observer_locked);
    const drill_smo_input stopped={
        .voltage_alpha_v=0.0f, .voltage_beta_v=0.0f,
        .current_alpha_a=0.0f, .current_beta_a=0.0f,
        .dt_s=dt, .commanded_direction=1,
    };
    for (unsigned n=0; n<500u; ++n) assert(drill_smo_step(&s,&stopped,&o));
    assert(!o.angle_valid && !o.observer_locked);
}

int main(void) {
    test_sincos_quadrants();
    test_svpwm_voltage_reconstruction();
    test_forward_and_reverse_tracking();
    test_invalid_input_fails_closed();
    test_no_back_emf_cannot_lock();
    test_back_emf_loss_unlocks();
    puts("DRILL_SMO_HOST_TEST_PASS: CORDIC, forward/reverse BEMF lock, 50k-rpm range, fail-closed input");
    return 0;
}
