#ifndef DRILL_SMO_H
#define DRILL_SMO_H

#include <stdbool.h>
#include <stdint.h>

typedef struct {
    float phase_resistance_ohm;
    float phase_inductance_h;
    float sliding_gain_v;
    float current_error_sat_a;
    float bemf_filter_tau_s;
    float pll_kp_per_s;
    float pll_ki_per_s2;
    float min_bemf_v;
    float max_phase_voltage_v;
    float max_phase_current_a;
    float max_mechanical_rpm;
    float max_dt_s;
    float lock_phase_error_rad;
    uint8_t pole_pairs;
    uint8_t lock_samples;
} drill_smo_config;

typedef struct {
    float voltage_alpha_v;
    float voltage_beta_v;
    float current_alpha_a;
    float current_beta_a;
    float dt_s;
    int8_t commanded_direction;
} drill_smo_input;

typedef struct {
    float electrical_angle_rad;
    float electrical_speed_rad_s;
    float mechanical_speed_rpm;
    float bemf_alpha_v;
    float bemf_beta_v;
    float phase_error_rad;
    float sine;
    float cosine;
    bool angle_valid;
    bool observer_locked;
    bool valid;
} drill_smo_output;

typedef struct {
    drill_smo_config config;
    float estimated_current_alpha_a;
    float estimated_current_beta_a;
    float estimated_bemf_alpha_v;
    float estimated_bemf_beta_v;
    float electrical_angle_rad;
    float electrical_speed_rad_s;
    uint8_t lock_count;
    uint8_t unlock_count;
    bool initialized;
    bool angle_valid;
    bool observer_locked;
} drill_smo_state;

bool drill_smo_init(drill_smo_state *state, const drill_smo_config *config);
void drill_smo_reset(drill_smo_state *state);
bool drill_smo_step(drill_smo_state *state, const drill_smo_input *input,
                    drill_smo_output *output);
bool drill_smo_svpwm_to_ab(float duty_a, float duty_b, float duty_c,
                           float bus_voltage_v, float *voltage_alpha_v,
                           float *voltage_beta_v);

/* Shared with the sensorless unit test and the FOC Park transform. */
void drill_smo_sincos(float angle_rad, float *sine, float *cosine);

#endif
