// 3-Phase SPWM Motor Driver with Hall Sensors and Current Sensing
// Features: Speed control, current monitoring, serial display

// Pin Definitions
// PWM Output Pins for 3-Phase Bridge
#define PHASE_U_HIGH 9
#define PHASE_U_LOW 10
#define PHASE_V_HIGH 5
#define PHASE_V_LOW 6
#define PHASE_W_HIGH 3
#define PHASE_W_LOW 11

// Hall Sensor Pins
#define HALL_U 2
#define HALL_V 7
#define HALL_W 8

// Current Sensor Pins (ACS712 or similar)
#define CURRENT_U A0
#define CURRENT_V A1

// Speed Control
#define SPEED_POT A2

// Variables
volatile int hallState = 0;
volatile unsigned long lastHallTime = 0;
volatile unsigned long hallPeriod = 0;
volatile int electricalAngle = 0;
volatile int motorSpeed = 0;

// SPWM Parameters
const int PWM_FREQUENCY = 20000;  // 20kHz PWM frequency
const int SPWM_FREQUENCY = 50;    // 50Hz electrical frequency
const int SPWM_RESOLUTION = 256;  // 8-bit resolution
const int DEAD_TIME = 10;         // Dead time in microseconds

// Motor Control Variables
int targetSpeed = 0;
int currentSpeed = 0;
float phaseUCurrent = 0;
float phaseVCurrent = 0;
float busVoltage = 24.0;  // DC bus voltage

// SPWM Lookup Table
int spwmTable[360];
int currentSector = 0;

// Timing Variables
unsigned long lastSPWMUpdate = 0;
unsigned long lastSerialUpdate = 0;
unsigned long lastSpeedUpdate = 0;

void setup() {
  Serial.begin(115200);
  
  // Initialize PWM pins
  pinMode(PHASE_U_HIGH, OUTPUT);
  pinMode(PHASE_U_LOW, OUTPUT);
  pinMode(PHASE_V_HIGH, OUTPUT);
  pinMode(PHASE_V_LOW, OUTPUT);
  pinMode(PHASE_W_HIGH, OUTPUT);
  pinMode(PHASE_W_LOW, OUTPUT);
  
  // Initialize Hall sensor pins with interrupts
  pinMode(HALL_U, INPUT_PULLUP);
  pinMode(HALL_V, INPUT_PULLUP);
  pinMode(HALL_W, INPUT_PULLUP);
  
  attachInterrupt(digitalPinToInterrupt(HALL_U), hallInterrupt, CHANGE);
  attachInterrupt(digitalPinToInterrupt(HALL_V), hallInterrupt, CHANGE);
  attachInterrupt(digitalPinToInterrupt(HALL_W), hallInterrupt, CHANGE);
  
  // Initialize current sensor pins
  pinMode(CURRENT_U, INPUT);
  pinMode(CURRENT_V, INPUT);
  pinMode(SPEED_POT, INPUT);
  
  // Generate SPWM lookup table (sine wave)
  generateSPWMTable();
  
  // Set PWM frequency for better motor control
  setPwmFrequency(PHASE_U_HIGH, 8);  // ~20kHz
  setPwmFrequency(PHASE_V_HIGH, 8);
  setPwmFrequency(PHASE_W_HIGH, 8);
  
  // Initialize all outputs to LOW
  allOutputsOff();
  
  Serial.println("3-Phase SPWM Motor Driver Initialized");
  Serial.println("Commands: + (increase speed), - (decrease speed), s (stop)");
  Serial.println("==============================================");
}

void loop() {
  unsigned long currentTime = micros();
  
  // Update speed from potentiometer every 100ms
  if (currentTime - lastSpeedUpdate > 100000) {
    updateSpeedControl();
    lastSpeedUpdate = currentTime;
  }
  
  // Update SPWM every 100us for smooth operation
  if (currentTime - lastSPWMUpdate > 100) {
    updateSPWM();
    lastSPWMUpdate = currentTime;
  }
  
  // Read current sensors
  readCurrentSensors();
  
  // Update serial display every 500ms
  if (currentTime - lastSerialUpdate > 500000) {
    updateSerialDisplay();
    lastSerialUpdate = currentTime;
  }
  
  // Check for serial commands
  checkSerialCommands();
}

void generateSPWMTable() {
  // Generate sine wave lookup table
  for (int i = 0; i < 360; i++) {
    spwmTable[i] = (sin(i * PI / 180.0) + 1.0) * (SPWM_RESOLUTION - 1) / 2.0;
  }
}

void hallInterrupt() {
  // Read Hall sensor states
  int hallU = digitalRead(HALL_U);
  int hallV = digitalRead(HALL_V);
  int hallW = digitalRead(HALL_W);
  
  // Determine sector (0-5) from Hall sensors
  hallState = (hallU << 2) | (hallV << 1) | hallW;
  
  // Calculate motor speed from Hall sensor frequency
  unsigned long currentTime = micros();
  hallPeriod = currentTime - lastHallTime;
  lastHallTime = currentTime;
  
  if (hallPeriod > 0) {
    motorSpeed = 1000000 / (hallPeriod * 6);  // 6 transitions per electrical revolution
  }
  
  // Update electrical angle based on Hall sensors
  updateElectricalAngle();
}

void updateElectricalAngle() {
  // Map Hall state to electrical angle (60 degree sectors)
  switch (hallState) {
    case 0b101: currentSector = 0; electricalAngle = 0; break;
    case 0b100: currentSector = 1; electricalAngle = 60; break;
    case 0b110: currentSector = 2; electricalAngle = 120; break;
    case 0b010: currentSector = 3; electricalAngle = 180; break;
    case 0b011: currentSector = 4; electricalAngle = 240; break;
    case 0b001: currentSector = 5; electricalAngle = 300; break;
    default: break;
  }
}

void updateSPWM() {
  if (targetSpeed == 0) {
    allOutputsOff();
    return;
  }
  
  // Calculate electrical angle based on target speed and time
  static int angle = 0;
  unsigned long currentTime = micros();
  static unsigned long lastAngleUpdate = 0;
  
  if (currentTime - lastAngleUpdate > (1000000 / (SPWM_FREQUENCY * 360 * (targetSpeed / 100.0)))) {
    angle = (angle + 1) % 360;
    lastAngleUpdate = currentTime;
  }
  
  // Get SPWM values for each phase (120 degrees apart)
  int phaseU = spwmTable[angle];
  int phaseV = spwmTable[(angle + 120) % 360];
  int phaseW = spwmTable[(angle + 240) % 360];
  
  // Apply PWM to motor phases with dead time
  setPhasePWM(PHASE_U_HIGH, PHASE_U_LOW, phaseU);
  setPhasePWM(PHASE_V_HIGH, PHASE_V_LOW, phaseV);
  setPhasePWM(PHASE_W_HIGH, PHASE_W_LOW, phaseW);
}

void setPhasePWM(int highPin, int lowPin, int pwmValue) {
  // Implement dead time and complementary PWM
  if (pwmValue > SPWM_RESOLUTION / 2) {
    analogWrite(highPin, pwmValue);
    digitalWrite(lowPin, LOW);
  } else {
    digitalWrite(highPin, LOW);
    analogWrite(lowPin, SPWM_RESOLUTION - pwmValue);
  }
}

void allOutputsOff() {
  digitalWrite(PHASE_U_HIGH, LOW);
  digitalWrite(PHASE_U_LOW, LOW);
  digitalWrite(PHASE_V_HIGH, LOW);
  digitalWrite(PHASE_V_LOW, LOW);
  digitalWrite(PHASE_W_HIGH, LOW);
  digitalWrite(PHASE_W_LOW, LOW);
}

void updateSpeedControl() {
  // Read potentiometer for speed control
  int potValue = analogRead(SPEED_POT);
  targetSpeed = map(potValue, 0, 1023, 0, 100);
  
  // Smooth speed changes
  if (abs(targetSpeed - currentSpeed) > 2) {
    if (targetSpeed > currentSpeed) currentSpeed++;
    else currentSpeed--;
  }
}

void readCurrentSensors() {
  // Read current sensors (ACS712: 2.5V = 0A, 66mV/A)
  int rawU = analogRead(CURRENT_U);
  int rawV = analogRead(CURRENT_V);
  
  // Convert to voltage (0-5V)
  float voltageU = (rawU / 1023.0) * 5.0;
  float voltageV = (rawV / 1023.0) * 5.0;
  
  // Convert to current (ACS712 5A version: 185mV/A)
  phaseUCurrent = (voltageU - 2.5) / 0.185;
  phaseVCurrent = (voltageV - 2.5) / 0.185;
  
  // Apply filtering
  phaseUCurrent = 0.7 * phaseUCurrent + 0.3 * ((voltageU - 2.5) / 0.185);
  phaseVCurrent = 0.7 * phaseVCurrent + 0.3 * ((voltageV - 2.5) / 0.185);
}

void updateSerialDisplay() {
  Serial.println("=== Motor Status ===");
  Serial.print("Target Speed: "); Serial.print(targetSpeed); Serial.println("%");
  Serial.print("Actual Speed: "); Serial.print(motorSpeed); Serial.println(" RPM");
  Serial.print("Hall State: "); Serial.println(hallState, BIN);
  Serial.print("Electrical Angle: "); Serial.print(electricalAngle); Serial.println("°");
  Serial.print("Phase U Current: "); Serial.print(phaseUCurrent, 2); Serial.println(" A");
  Serial.print("Phase V Current: "); Serial.print(phaseVCurrent, 2); Serial.println(" A");
  Serial.print("Phase W Current: "); Serial.print(-(phaseUCurrent + phaseVCurrent), 2); Serial.println(" A");
  Serial.print("DC Bus Voltage: "); Serial.print(busVoltage, 1); Serial.println(" V");
  Serial.println("----------------------");
}

void checkSerialCommands() {
  if (Serial.available()) {
    char command = Serial.read();
    
    switch (command) {
      case '+':
        targetSpeed = min(targetSpeed + 10, 100);
        Serial.println("Speed increased");
        break;
        
      case '-':
        targetSpeed = max(targetSpeed - 10, 0);
        Serial.println("Speed decreased");
        break;
        
      case 's':
      case 'S':
        targetSpeed = 0;
        Serial.println("Motor stopped");
        break;
        
      case 'r':
      case 'R':
        targetSpeed = 50;
        Serial.println("Speed set to 50%");
        break;
    }
  }
}

// Function to set PWM frequency for better motor control
void setPwmFrequency(int pin, int divisor) {
  byte mode;
  if (pin == 5 || pin == 6 || pin == 9 || pin == 10) {
    switch (divisor) {
      case 1: mode = 0x01; break;
      case 8: mode = 0x02; break;
      case 64: mode = 0x03; break;
      case 256: mode = 0x04; break;
      case 1024: mode = 0x05; break;
      default: return;
    }
    if (pin == 5 || pin == 6) {
      TCCR0B = TCCR0B & 0b11111000 | mode;
    } else {
      TCCR1B = TCCR1B & 0b11111000 | mode;
    }
  } else if (pin == 3 || pin == 11) {
    switch (divisor) {
      case 1: mode = 0x01; break;
      case 8: mode = 0x02; break;
      case 32: mode = 0x03; break;
      case 64: mode = 0x04; break;
      case 128: mode = 0x05; break;
      case 256: mode = 0x06; break;
      case 1024: mode = 0x07; break;
      default: return;
    }
    TCCR2B = TCCR2B & 0b11111000 | mode;
  }
}