#include <Arduino.h>
#include <pwmRite.h>

// PWM output pins for 3-phase
#define U_PHASE_HIGH PA6
#define V_PHASE_HIGH PA7
#define W_PHASE_HIGH PB0
#define U_PHASE_LOW PB1
#define V_PHASE_LOW PB10
#define W_PHASE_LOW PB11

// Speed control pin
#define SPEED_POT PA0

// SPWM parameters
#define PWM_FREQUENCY 20000  // 20kHz PWM frequency
#define PWM_RESOLUTION 255   // 8-bit resolution
#define SPWM_FREQ 50         // 50Hz fundamental frequency


// Variables
volatile uint32_t pwmPeriod;
volatile float phaseAngle = 0;
volatile float frequency = SPWM_FREQ;
volatile uint32_t lastTime = 0;
volatile uint8_t speedSetpoint = 0;

// Sine wave table (128 points for half wave)
const uint8_t sineTable[128] = {
  128,131,134,137,140,143,146,149,152,155,158,162,165,167,170,173,
  176,179,182,185,188,190,193,196,198,201,203,206,208,211,213,215,
  218,220,222,224,226,228,230,232,234,235,237,238,240,241,243,244,
  245,246,247,248,249,250,250,251,251,252,252,252,253,253,253,253,
  253,253,253,252,252,252,251,251,250,250,249,248,247,246,245,244,
  243,241,240,238,237,235,234,232,230,228,226,224,222,220,218,215,
  213,211,208,206,203,201,198,196,193,190,188,185,182,179,176,173,
  170,167,165,162,158,155,152,149,146,143,140,137,134,131,128,124
};

void setup() {
  Serial.begin(115200);
  
  // Configure PWM outputs
  pinMode(U_PHASE_HIGH, OUTPUT);
  pinMode(V_PHASE_HIGH, OUTPUT);
  pinMode(W_PHASE_HIGH, OUTPUT);
  pinMode(U_PHASE_LOW, OUTPUT);
  pinMode(V_PHASE_LOW, OUTPUT);
  pinMode(W_PHASE_LOW, OUTPUT);
  
  // Initialize PWM
  setupPWM();
  
  // Set all low-side MOSFETs to LOW initially
  digitalWrite(U_PHASE_LOW, LOW);
  digitalWrite(V_PHASE_LOW, LOW);
  digitalWrite(W_PHASE_LOW, LOW);
  
  Serial.println("3-Phase SPWM Motor Controller Ready");
}

void loop() {
  // Read speed potentiometer (0-1023) and map to frequency (0-100Hz)
  int potValue = analogRead(SPEED_POT);
  frequency = map(potValue, 0, 1023, 5, 100);  // 5-100Hz range
  
  // Update SPWM every 10ms
  if (millis() - lastTime > 10) {
    updateSPWM();
    lastTime = millis();
    
    // Optional: Print debug information
    Serial.print("Frequency: ");
    Serial.print(frequency);
    Serial.print("Hz, Pot: ");
    Serial.println(potValue);
  }
}

void setupPWM() {
  // Configure timer for PWM generation
  HardwareTimer *timer = new HardwareTimer(TIM1);
  
  // Set PWM frequency
  pwmPeriod = timer->getTimerClkFreq() / PWM_FREQUENCY;
  timer->setPrescaleFactor(1);
  timer->setOverflow(pwmPeriod, TICK_FORMAT);
  
  // Configure PWM channels
  timer->setMode(1, TIMER_OUTPUT_COMPARE_PWM1, U_PHASE_HIGH);
  timer->setMode(2, TIMER_OUTPUT_COMPARE_PWM1, V_PHASE_HIGH);
  timer->setMode(3, TIMER_OUTPUT_COMPARE_PWM1, W_PHASE_HIGH);
  
  // Set PWM duty cycle to 0 initially
  timer->setCaptureCompare(1, 0, TICK_COMPARE_FORMAT);
  timer->setCaptureCompare(2, 0, TICK_COMPARE_FORMAT);
  timer->setCaptureCompare(3, 0, TICK_COMPARE_FORMAT);
  
  // Start timer
  timer->resume();
}

void updateSPWM() {
  static uint32_t phaseIndex = 0;
  
  // Calculate phase increment based on desired frequency
  float phaseIncrement = (frequency * 256.0 * 10.0) / 1000.0;  // 10ms update rate
  
  phaseIndex += (uint32_t)phaseIncrement;
  if (phaseIndex >= 256) phaseIndex -= 256;
  
  // Calculate three phases (120 degrees apart)
  uint32_t phaseU = phaseIndex;
  uint32_t phaseV = (phaseIndex + 85) % 256;  // 120° = 256/3 ≈ 85
  uint32_t phaseW = (phaseIndex + 170) % 256; // 240° = 256*2/3 ≈ 170
  
  // Get sine values and set PWM duty cycles
  uint8_t dutyU = getSineValue(phaseU);
  uint8_t dutyV = getSineValue(phaseV);
  uint8_t dutyW = getSineValue(phaseW);
  
  // Update PWM outputs
  pwmWrite(U_PHASE_HIGH, dutyU);
  pwmWrite(V_PHASE_HIGH, dutyV);
  pwmWrite(W_PHASE_HIGH, dutyW);
  
  // Control low-side MOSFETs (complementary with dead time)
  digitalWrite(U_PHASE_LOW, (dutyU < 10) ? HIGH : LOW);
  digitalWrite(V_PHASE_LOW, (dutyV < 10) ? HIGH : LOW);
  digitalWrite(W_PHASE_LOW, (dutyW < 10) ? HIGH : LOW);
}

uint8_t getSineValue(uint32_t angle) {
  // Normalize angle to 0-255
  angle = angle % 256;
  
  if (angle < 128) {
    return sineTable[angle];
  } else {
    return 255 - sineTable[angle - 128];
  }
}