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

// Constants
#define PI 3.14159265358979323846

// Function prototypes
void smo(double ia, double ib, double v_alpha, double v_beta, double *theta, double *omega, double Ts, double K_slide);
void calculate_voltages(double id, double iq, double Rs, double Ld, double Lq, double lambda_m, double omega_e, double *vd, double *vq);
void inverse_park_transform(double vd, double vq, double theta, double *valpha, double *vbeta);
void svpwm(double valpha, double vbeta, double Vdc, double Ts, double *dutyA, double *dutyB, double *dutyC, int *sector);
double pid_controller(double setpoint, double measured_value, double *prev_error, double *integral, double Kp, double Ki, double Kd, double dt);

int main() {
// Example parameters
double id = 5.0; // d-axis current in amperes
double iq = 10.0; // q-axis current in amperes
double ia = 5.0, ib = 5.0; // Phase currents
double Rs = 0.5; // Stator resistance in ohms
double Ld = 0.001; // d-axis inductance in henries
double Lq = 0.001; // q-axis inductance in henries
double lambda_m = 0.01;// Permanent magnet flux linkage in Wb
double theta = PI/4; // Rotor angle in radians (initial estimate)
double Vdc = 300.0; // DC bus voltage in volts
double Ts = 0.0001; // Sampling time in seconds (100 μs, corresponding to a 10 kHz PWM frequency)

// PID controller parameters
double Kp_pid = 1.0, Ki_pid = 0.1, Kd_pid = 0.01;
double setpoint = 200.0; // Desired speed in rad/s
double prev_error = 0.0, integral_pid = 0.0;

// SMO parameters
double K_slide = 0.1;

// Variables for voltages
double vd, vq, valpha, vbeta;

// Variables for SVPWM duty cycles and sector
double dutyA, dutyB, dutyC;
int sector;

// Main control loop (simplified)
for (int i = 0; i < 1000; i++) {
// SMO to estimate theta and omega
smo(ia, ib, valpha, vbeta, &theta, &omega_e, Ts, K_slide);

// PID controller to regulate speed
double iq_ref = pid_controller(setpoint, omega_e, &prev_error, &integral_pid, Kp_pid, Ki_pid, Kd_pid, Ts);

// Update iq with iq_ref
iq = iq_ref;

// Calculate voltages in d-q frame
calculate_voltages(id, iq, Rs, Ld, Lq, lambda_m, omega_e, &vd, &vq);

// Perform inverse Park transformation to get alpha-beta voltages
inverse_park_transform(vd, vq, theta, &valpha, &vbeta);

// Calculate SVPWM duty cycles and sector
svpwm(valpha, vbeta, Vdc, Ts, &dutyA, &dutyB, &dutyC, &sector);

// Print results
printf("Duty cycles: A = %f, B = %f, C = %f, Sector = %d\n", dutyA, dutyB, dutyC, sector);
printf("Estimated theta: %f, omega: %f\n", theta, omega_e);
}

return 0;
}

void smo(double ia, double ib, double v_alpha, double v_beta, double *theta, double *omega, double Ts, double K_slide) {
static double x_alpha = 0.0, x_beta = 0.0;
static double z_alpha = 0.0, z_beta = 0.0;

// Sliding Mode Observer equations
double x_alpha_dot = v_alpha - K_slide * (x_alpha - z_alpha);
double x_beta_dot = v_beta - K_slide * (x_beta - z_beta);

// Integrate
x_alpha += x_alpha_dot * Ts;
x_beta += x_beta_dot * Ts;

// Calculate z_alpha and z_beta
z_alpha = x_alpha - K_slide * (x_alpha - z_alpha);
z_beta = x_beta - K_slide * (x_beta - z_beta);

// Calculate estimated theta and omega
*theta = atan2(z_beta, z_alpha);
*omega = (*theta - z_alpha) / Ts;

// Keep theta within 0 to 2*PI
if (*theta >= 2 * PI) {
*theta -= 2 * PI;
} else if (*theta < 0) {
*theta += 2 * PI;
}
}

void calculate_voltages(double id, double iq, double Rs, double Ld, double Lq, double lambda_m, double omega_e, double *vd, double *vq) {
*vd = Rs * id - omega_e * Lq * iq;
*vq = Rs * iq + omega_e * (Ld * id + lambda_m);
}

void inverse_park_transform(double vd, double vq, double theta, double *valpha, double *vbeta) {
*valpha = vd * cos(theta) - vq * sin(theta);
*vbeta = vd * sin(theta) + vq * cos(theta);
}

void svpwm(double valpha, double vbeta, double Vdc, double Ts, double *dutyA, double *dutyB, double *dutyC, int *sector) {
double Vref = sqrt(valpha * valpha + vbeta * vbeta);
double angle = atan2(vbeta, valpha);

// Determine sector
if (angle >= 0 && angle < PI/3) {
*sector = 1;
} else if (angle >= PI/3 && angle < 2*PI/3) {
*sector = 2;
} else if (angle >= 2*PI/3 && angle < PI) {
*sector = 3;
} else if (angle >= -PI && angle < -2*PI/3) {
*sector = 4;
} else if (angle >= -2*PI/3 && angle < -PI/3) {
*sector = 5;
} else {
*sector = 6;
}

// Calculate times for switching states
double T1 = (Vref * sin(PI / 3 - angle)) / (sqrt(3) * Vdc);
double T2 = (Vref * sin(angle)) / (sqrt(3) * Vdc);
double T0 = Ts - T1 - T2;

// Calculate duty cycles for each phase
*dutyA = (T1 + T2 + T0 / 2) / Ts;
*dutyB = (T2 + T0 / 2) / Ts;
*dutyC = (T0 / 2) / Ts;
}

double pid_controller(double setpoint, double measured_value, double *prev_error, double *integral, double Kp, double Ki, double Kd, double dt) {
double error = setpoint - measured_value;
*integral += error * dt;
double derivative = (error - *prev_error) / dt;
double output = Kp * error + Ki * (*integral) + Kd * derivative;
*prev_error = error;
return output;
}