#include <Arduino.h>
#include <math.h>

#define PI 3.14159265359
#define PI_2 1.57079632679
#define PI_3 1.0471975512
#define _SQRT3 1.73205080757
#define voltage_power_supply 12.0

float normalizeAngle(float angle) {
    float a = fmod(angle, 2 * PI);
    return a >= 0 ? a : (a + 2 * PI);
}

void setPwm(float Ua, float Ub, float Uc) {
    Serial.print(Ua);
    Serial.print(",");
    Serial.print(Ub);
    Serial.print(",");
    Serial.println(Uc);
}

void setTorque(float Uq, float angle_el) {
 if (Uq < 0)
    angle_el += PI;
  Uq = abs(Uq);

  angle_el = normalizeAngle(angle_el + PI_2);
  int sector = floor(angle_el / PI_3) + 1;
  // calculate the duty cycles
  float T1 = _SQRT3 * sin(sector * PI_3 - angle_el) * Uq / voltage_power_supply;
  float T2 = _SQRT3 * sin(angle_el - (sector - 1.0) * PI_3) * Uq / voltage_power_supply;
  float T0 = 1 - T1 - T2;

  float Ta, Tb, Tc;
  switch (sector)
  {
  case 1:
    Ta = T1 + T2 + T0 / 2;
    Tb = T2 + T0 / 2;
    Tc = T0 / 2;
    break;
  case 2:
    Ta = T1 + T0 / 2;
    Tb = T1 + T2 + T0 / 2;
    Tc = T0 / 2;
    break;
  case 3:
    Ta = T0 / 2;
    Tb = T1 + T2 + T0 / 2;
    Tc = T2 + T0 / 2;
    break;
  case 4:
    Ta = T0 / 2;
    Tb = T1 + T0 / 2;
    Tc = T1 + T2 + T0 / 2;
    break;
  case 5:
    Ta = T2 + T0 / 2;
    Tb = T0 / 2;
    Tc = T1 + T2 + T0 / 2;
    break;
  case 6:
    Ta = T1 + T2 + T0 / 2;
    Tb = T0 / 2;
    Tc = T1 + T0 / 2;
    break;
  default:
    Ta = 0;
    Tb = 0;
    Tc = 0;
  }

  float Ua = Ta * voltage_power_supply;
  float Ub = Tb * voltage_power_supply;
  float Uc = Tc * voltage_power_supply;

  setPwm(Ua, Ub, Uc);
}

void setup() {
    Serial.begin(115200);  // Make sure to match the baud rate with Serial Monitor
}

void loop() {
    float Uq = 1/_SQRT3;  // Test value for voltage
    float angle_el = 0;  // Test value for angle

    // Test values across a full circle
    for (int i = 0; i < 360; i++) {
        angle_el = i * PI / 180;
        setTorque(Uq, angle_el);
        delay(20);  // Delay for visibility in plotter
    }
}