/*
 * Complete 18-Pole BLDC Motor Controller with SPWM
 * Sinusoidal PWM for Smooth Operation
 * With Dual Current Transformers and Reverse Mode
 * For 350V DC Inverter with 0-200Hz Frequency Range
 */

// ==================== CONFIGURATION ====================
// Motor Specifications
const int POLE_PAIRS = 9;           // 18-pole motor = 9 pole pairs
const float MAX_SPEED_RPM = 1333.3; // Theoretical max at 200Hz
const float BASE_SPEED_RPM = 333.3; // Base speed at 50Hz

// SPWM Parameters
const int SPWM_RESOLUTION = 256;    // 8-bit PWM resolution
const int SPWM_TABLE_SIZE = 256;    // Size of sine wave table
const float MAX_MODULATION_INDEX = 0.95; // Maximum modulation index

// Pin Definitions
const int HALL_U = 2;               // Hall sensor U (Interrupt 0)
const int HALL_V = 3;               // Hall sensor V (Interrupt 1)
const int HALL_W = 4;               // Hall sensor W

const int PWM_U_H = 5;              // Phase U high side PWM
const int PWM_U_L = 6;              // Phase U low side PWM
const int PWM_V_H = 7;              // Phase V high side PWM
const int PWM_V_L = 8;              // Phase V low side PWM
const int PWM_W_H = 9;              // Phase W high side PWM
const int PWM_W_L = 10;             // Phase W low side PWM

// Current Transformers (5A CTs with burden resistors)
const int CT1_PHASE_U = A0;         // Current Transformer 1 - Phase U
const int CT2_PHASE_V = A1;         // Current Transformer 2 - Phase V
const int VOLTAGE_SENSE = A2;       // DC bus voltage sensor
const int TEMP_SENSE = A3;          // Motor temperature sensor

const int ENABLE_PIN = 12;          // System enable pin
const int FAULT_LED = 13;           // Fault indicator LED
const int DIRECTION_LED = 11;       // Direction indicator LED

// Control Parameters
const int MAX_PWM = 255;            // Maximum PWM value (8-bit)
const int MIN_PWM = 30;             // Minimum PWM for motor start
const int DEAD_TIME = 5;            // Dead time in microseconds (reduced for SPWM)
const int PHASE_CURRENT_LIMIT = 4;  // Amps RMS per phase (80% of 5A CT)
const int OVERCURRENT_LIMIT = 8;    // Amps peak overcurrent
const int OVERTEMP_LIMIT = 85;      // Degrees Celsius
const int OVERVOLTAGE_LIMIT = 380;  // Volts DC
const int UNDERVOLTAGE_LIMIT = 300; // Volts DC

// Current Transformer Calibration
const float CT_RATIO = 100.0;       // 5A:50mA = 100:1 ratio
const float BURDEN_RESISTOR = 22.0; // Ohms - burden resistor value
const float CT_SENSITIVITY = (BURDEN_RESISTOR * CT_RATIO) / 1000.0; // V/A

// SPWM Variables
uint8_t sineTable[SPWM_TABLE_SIZE]; // Sine wave lookup table
float electricalAngle = 0.0;        // Electrical angle in radians
float electricalSpeed = 0.0;        // Electrical speed in rad/s
float targetElectricalSpeed = 0.0;  // Target electrical speed
float modulationIndex = 0.0;        // Modulation index (0.0 to 1.0)

// V/F Control Parameters
const float BASE_VOLTAGE = 247.0;   // Maximum RMS voltage at base speed
const float BASE_FREQUENCY = 50.0;  // Base frequency in Hz
const float VF_RATIO = BASE_VOLTAGE / BASE_FREQUENCY; // Volts/Hz ratio

// ==================== GLOBAL VARIABLES ====================
volatile int hallState = 0;
volatile unsigned long lastHallTime = 0;
volatile unsigned long hallPeriod = 0;
volatile bool positionUpdate = false;

float targetSpeedRPM = 0;           // Commanded mechanical speed
float currentSpeedRPM = 0;          // Measured mechanical speed
float filteredSpeedRPM = 0;         // Filtered speed for control

bool systemEnabled = false;
bool faultCondition = false;
bool reverseDirection = false;      // Reverse mode flag
String faultMessage = "";

// Current measurement variables
float currentU = 0;                 // Phase U current (Amps RMS)
float currentV = 0;                 // Phase V current (Amps RMS)
float currentPeak = 0;              // Peak current for protection
float currentFilteredU = 0;         // Filtered current U
float currentFilteredV = 0;         // Filtered current V

// Timing variables
unsigned long lastSPWMTime = 0;
unsigned long lastControlTime = 0;
unsigned long lastCurrentTime = 0;
unsigned long lastDisplayTime = 0;

const unsigned long SPWM_INTERVAL = 100;     // 100us = 10kHz SPWM update
const unsigned long CONTROL_INTERVAL = 1000; // 1ms control loop
const unsigned long CURRENT_INTERVAL = 100;  // 100us current sampling
const unsigned long DISPLAY_INTERVAL = 500000; // 500ms display update

// ==================== SETUP ====================
void setup() {
  // Initialize pins
  initializePins();
  
  // Set high-frequency PWM (31.25 kHz)
  setupPWMFrequency();
  
  // Generate sine wave table
  generateSineTable();
  
  // Initialize serial communication
  Serial.begin(115200);
  Serial.println("=== 18-Pole BLDC Motor Controller with SPWM ===");
  Serial.println("Sinusoidal PWM for Smooth Operation");
  Serial.println("Commands: +RPM, -RPM, START, STOP, REVERSE, STATUS");
  Serial.println("Example: +500 (for 500 RPM), -300 (reverse 300 RPM)");
  
  // Attach Hall sensor interrupts
  attachInterrupt(digitalPinToInterrupt(HALL_U), hallISR, CHANGE);
  attachInterrupt(digitalPinToInterrupt(HALL_V), hallISR, CHANGE);
  
  // Calibrate current sensors
  calibrateCurrentSensors();
  
  // Initial motor stop
  stopMotor();
  
  Serial.println("System initialized. Send 'START' to begin.");
}

// ==================== MAIN LOOP ====================
void loop() {
  // Read and process serial commands
  processSerialCommands();
  
  unsigned long currentTime = micros();
  
  // High-speed SPWM update (every 100us = 10kHz)
  if (currentTime - lastSPWMTime >= SPWM_INTERVAL) {
    lastSPWMTime = currentTime;
    if (systemEnabled && !faultCondition) {
      updateSPWM();
    }
  }
  
  // High-speed current sampling (every 100us)
  if (currentTime - lastCurrentTime >= CURRENT_INTERVAL) {
    lastCurrentTime = currentTime;
    measureCurrents();
  }
  
  // Control loop (every 1ms)
  if (currentTime - lastControlTime >= CONTROL_INTERVAL) {
    lastControlTime = currentTime;
    
    // Update sensor readings and check for faults
    monitorSystem();
    
    // Only run control if system is enabled and no faults
    if (systemEnabled && !faultCondition) {
      calculateSpeed();
      speedController();
      updateVFControl();
    }
  }
  
  // Display update (every 500ms)
  if (currentTime - lastDisplayTime >= DISPLAY_INTERVAL) {
    lastDisplayTime = currentTime;
    updateDisplay();
  }
}

// ==================== SPWM FUNCTIONS ====================
void generateSineTable() {
  // Generate sine wave lookup table with offset for 3-phase
  for (int i = 0; i < SPWM_TABLE_SIZE; i++) {
    float angle = 2.0 * PI * i / SPWM_TABLE_SIZE;
    // Scale to 0-255 range with 127.5 offset (for bipolar to unipolar conversion)
    sineTable[i] = (uint8_t)(127.5 + 127.5 * sin(angle));
  }
  Serial.println("Sine wave table generated");
}

void updateSPWM() {
  // Calculate electrical angle based on speed and time
  unsigned long currentTime = micros();
  float deltaTime = (currentTime - lastSPWMTime) / 1000000.0; // Convert to seconds
  
  // Update electrical angle (integrate speed)
  electricalAngle += electricalSpeed * deltaTime;
  
  // Normalize angle to 0-2PI range
  while (electricalAngle >= 2.0 * PI) electricalAngle -= 2.0 * PI;
  while (electricalAngle < 0) electricalAngle += 2.0 * PI;
  
  // Apply direction
  if (reverseDirection) {
    electricalAngle = -electricalAngle;
    if (electricalAngle < 0) electricalAngle += 2.0 * PI;
  }
  
  // Calculate phase angles (120 degrees apart)
  float angleU = electricalAngle;
  float angleV = electricalAngle + (2.0 * PI / 3.0);
  float angleW = electricalAngle + (4.0 * PI / 3.0);
  
  // Normalize phase angles
  while (angleV >= 2.0 * PI) angleV -= 2.0 * PI;
  while (angleW >= 2.0 * PI) angleW -= 2.0 * PI;
  
  // Get PWM values from sine table with modulation index
  int pwmU = getSPWMValue(angleU, modulationIndex);
  int pwmV = getSPWMValue(angleV, modulationIndex);
  int pwmW = getSPWMValue(angleW, modulationIndex);
  
  // Apply dead time and update PWM outputs
  applySPWM(pwmU, pwmV, pwmW);
}

int getSPWMValue(float angle, float modulation) {
  // Convert angle to table index
  int index = (int)((angle / (2.0 * PI)) * SPWM_TABLE_SIZE) % SPWM_TABLE_SIZE;
  
  // Get sine value from table (0-255 range)
  int sineValue = sineTable[index];
  
  // Apply modulation index (voltage control)
  int modulatedValue = 127 + (int)((sineValue - 127) * modulation);
  
  // Ensure within bounds
  return constrain(modulatedValue, 0, 255);
}

void applySPWM(int pwmU, int pwmV, int pwmW) {
  // For SPWM, we use complementary PWM with dead time
  // High and low sides are complementary
  
  // Phase U
  analogWrite(PWM_U_H, pwmU);
  analogWrite(PWM_U_L, 255 - pwmU); // Complementary
  
  // Phase V
  analogWrite(PWM_V_H, pwmV);
  analogWrite(PWM_V_L, 255 - pwmV); // Complementary
  
  // Phase W
  analogWrite(PWM_W_H, pwmW);
  analogWrite(PWM_W_L, 255 - pwmW); // Complementary
}

void allPWMOff() {
  // Turn off all PWM outputs (50% duty = zero voltage)
  analogWrite(PWM_U_H, 127);
  analogWrite(PWM_U_L, 127);
  analogWrite(PWM_V_H, 127);
  analogWrite(PWM_V_L, 127);
  analogWrite(PWM_W_H, 127);
  analogWrite(PWM_W_L, 127);
}

// ==================== V/F CONTROL ====================
void updateVFControl() {
  // Calculate electrical frequency from mechanical speed
  float electricalRPM = currentSpeedRPM * POLE_PAIRS;
  float electricalFrequency = electricalRPM / 60.0; // Convert to Hz
  
  // V/F control: Voltage proportional to frequency up to base speed
  float targetVoltage = 0.0;
  
  if (electricalFrequency <= BASE_FREQUENCY) {
    // Constant V/F ratio below base speed
    targetVoltage = electricalFrequency * VF_RATIO;
  } else {
    // Constant voltage above base speed (field weakening)
    targetVoltage = BASE_VOLTAGE;
  }
  
  // Calculate modulation index (0-1.0)
  // For 350V DC bus, maximum peak phase voltage = 350V/sqrt(3) ≈ 202V
  // Maximum RMS voltage = 202V/sqrt(2) ≈ 143V
  // But we're targeting 247V line-line RMS = 142.6V phase RMS
  
  float maxPhaseVoltage = 350.0 / sqrt(3); // Maximum possible phase voltage
  modulationIndex = (targetVoltage / maxPhaseVoltage) * sqrt(2); // Convert RMS to peak
  
  // Limit modulation index
  modulationIndex = constrain(modulationIndex, 0.0, MAX_MODULATION_INDEX);
  
  // Update electrical speed for SPWM (radians per second)
  electricalSpeed = 2.0 * PI * electricalFrequency;
  
  // Synchronize electrical angle with Hall sensor position
  if (positionUpdate) {
    synchronizeAngleWithHall();
    positionUpdate = false;
  }
}

void synchronizeAngleWithHall() {
  // Map Hall sensor state to electrical angle (60 degree sectors)
  // Each Hall state corresponds to 60 electrical degrees
  float angleOffset = 0.0;
  
  switch(hallState) {
    case 0b101: angleOffset = 0 * PI / 3.0; break;    // 0°
    case 0b100: angleOffset = 1 * PI / 3.0; break;    // 60°
    case 0b110: angleOffset = 2 * PI / 3.0; break;    // 120°
    case 0b010: angleOffset = 3 * PI / 3.0; break;    // 180°
    case 0b011: angleOffset = 4 * PI / 3.0; break;    // 240°
    case 0b001: angleOffset = 5 * PI / 3.0; break;    // 300°
    default: return; // Invalid state
  }
  
  // Add small phase advance for better performance
  angleOffset += 0.1; // ~5.7 degrees advance
  
  // Set electrical angle to synchronized position
  electricalAngle = angleOffset;
}

// ==================== INITIALIZATION FUNCTIONS ====================
void initializePins() {
  // Hall sensor inputs
  pinMode(HALL_U, INPUT_PULLUP);
  pinMode(HALL_V, INPUT_PULLUP);
  pinMode(HALL_W, INPUT_PULLUP);
  
  // PWM outputs
  pinMode(PWM_U_H, OUTPUT);
  pinMode(PWM_U_L, OUTPUT);
  pinMode(PWM_V_H, OUTPUT);
  pinMode(PWM_V_L, OUTPUT);
  pinMode(PWM_W_H, OUTPUT);
  pinMode(PWM_W_L, OUTPUT);
  
  // Control pins
  pinMode(ENABLE_PIN, OUTPUT);
  pinMode(FAULT_LED, OUTPUT);
  pinMode(DIRECTION_LED, OUTPUT);
  
  // Analog inputs
  pinMode(CT1_PHASE_U, INPUT);
  pinMode(CT2_PHASE_V, INPUT);
  pinMode(VOLTAGE_SENSE, INPUT);
  pinMode(TEMP_SENSE, INPUT);
  
  digitalWrite(ENABLE_PIN, LOW);
  digitalWrite(FAULT_LED, LOW);
  digitalWrite(DIRECTION_LED, LOW);
}

void setupPWMFrequency() {
  // Set PWM frequency to 31.25 kHz for better motor control
  TCCR0B = TCCR0B & 0b11111000 | 0x01;  // Timer0 (pins 5,6) - 31.25 kHz
  TCCR1B = TCCR1B & 0b11111000 | 0x01;  // Timer1 (pins 9,10) - 31.25 kHz  
  TCCR2B = TCCR2B & 0b11111000 | 0x01;  // Timer2 (pins 3,11) - 31.25 kHz
}

void calibrateCurrentSensors() {
  Serial.println("Calibrating current sensors...");
  delay(1000);
  
  // Simple offset calibration - measure zero current level
  float sum1 = 0, sum2 = 0;
  for (int i = 0; i < 100; i++) {
    sum1 += analogRead(CT1_PHASE_U);
    sum2 += analogRead(CT2_PHASE_V);
    delay(10);
  }
  
  Serial.println("Current sensor calibration complete");
}

// ==================== CURRENT MEASUREMENT FUNCTIONS ====================
void measureCurrents() {
  static int sampleCount = 0;
  static float sumU = 0, sumV = 0;
  static float maxU = 0, maxV = 0;
  
  // Read raw ADC values
  int rawU = analogRead(CT1_PHASE_U);
  int rawV = analogRead(CT2_PHASE_V);
  
  // Convert to voltage (0-5V range)
  float voltageU = (rawU * 5.0 / 1024.0) - 2.5; // Remove 2.5V offset
  float voltageV = (rawV * 5.0 / 1024.0) - 2.5;
  
  // Convert to current using CT sensitivity
  float instantU = voltageU / CT_SENSITIVITY;
  float instantV = voltageV / CT_SENSITIVITY;
  
  // Track peak current for protection
  if (abs(instantU) > maxU) maxU = abs(instantU);
  if (abs(instantV) > maxV) maxV = abs(instantV);
  
  // For RMS calculation (simplified)
  sumU += instantU * instantU;
  sumV += instantV * instantV;
  
  sampleCount++;
  
  // Calculate RMS every 100 samples (~10ms)
  if (sampleCount >= 100) {
    currentU = sqrt(sumU / sampleCount);
    currentV = sqrt(sumV / sampleCount);
    currentPeak = max(maxU, maxV);
    
    // Low-pass filter for display
    currentFilteredU = 0.9 * currentFilteredU + 0.1 * currentU;
    currentFilteredV = 0.9 * currentFilteredV + 0.1 * currentV;
    
    // Reset for next cycle
    sumU = 0;
    sumV = 0;
    maxU = 0;
    maxV = 0;
    sampleCount = 0;
  }
}

// ==================== HALL SENSOR INTERRUPT ====================
void hallISR() {
  // Read current Hall state
  int u = digitalRead(HALL_U);
  int v = digitalRead(HALL_V);
  int w = digitalRead(HALL_W);
  hallState = (u << 2) | (v << 1) | w;
  
  // Calculate timing for speed measurement
  unsigned long currentTime = micros();
  hallPeriod = currentTime - lastHallTime;
  lastHallTime = currentTime;
  
  // Flag for position synchronization
  positionUpdate = true;
}

// ==================== SPEED CONTROL ====================
void calculateSpeed() {
  if (hallPeriod > 0 && hallPeriod < 1000000) { // Valid period (1Hz min)
    // Calculate electrical RPM: 60,000,000 us/min / (us per Hall change * 6 changes/elec rev)
    float electricalRPM = 60000000.0 / (hallPeriod * 6.0);
    
    // Convert to mechanical RPM: electrical RPM / pole pairs
    currentSpeedRPM = electricalRPM / POLE_PAIRS;
    
    // Apply direction sign
    if (reverseDirection) {
      currentSpeedRPM = -currentSpeedRPM;
    }
    
    // Low-pass filter for smoother display
    filteredSpeedRPM = 0.9 * filteredSpeedRPM + 0.1 * currentSpeedRPM;
  } else {
    // Motor likely stopped or very slow
    currentSpeedRPM = 0;
    filteredSpeedRPM = 0;
  }
}

void speedController() {
  // Simple PI controller for speed
  static float integral = 0;
  const float Kp = 2.0;   // Proportional gain (increased for SPWM)
  const float Ki = 0.05;  // Integral gain
  const float maxIntegral = 1000;
  
  float error = targetSpeedRPM - filteredSpeedRPM;
  
  // Anti-windup
  if (abs(error) < 100) {
    integral += error;
    integral = constrain(integral, -maxIntegral, maxIntegral);
  } else {
    integral = 0;
  }
  
  // Output is target electrical speed (rad/s)
  float newElectricalSpeed = (error * Kp) + (integral * Ki);
  
  // Convert to electrical rad/s (mechanical RPM * pole_pairs * 2π / 60)
  targetElectricalSpeed = newElectricalSpeed * POLE_PAIRS * 2.0 * PI / 60.0;
  
  // Limit electrical speed (200Hz = 1256 rad/s)
  targetElectricalSpeed = constrain(targetElectricalSpeed, -1256.0, 1256.0);
  
  // Smooth acceleration/deceleration
  float accelerationLimit = 100.0; // rad/s²
  float speedError = targetElectricalSpeed - electricalSpeed;
  if (abs(speedError) > accelerationLimit * 0.001) { // 1ms time base
    speedError = constrain(speedError, -accelerationLimit * 0.001, accelerationLimit * 0.001);
  }
  electricalSpeed += speedError;
}

// ==================== REVERSE MODE FUNCTIONS ====================
void setReverseMode(bool reverse) {
  if (systemEnabled && abs(currentSpeedRPM) > 10) {
    Serial.println("Stop motor before changing direction!");
    return;
  }
  
  reverseDirection = reverse;
  digitalWrite(DIRECTION_LED, reverse ? HIGH : LOW);
  
  if (reverse) {
    Serial.println("Reverse mode ACTIVATED");
    if (targetSpeedRPM > 0) {
      targetSpeedRPM = -targetSpeedRPM;
    }
  } else {
    Serial.println("Forward mode ACTIVATED");
    if (targetSpeedRPM < 0) {
      targetSpeedRPM = -targetSpeedRPM;
    }
  }
}

void toggleDirection() {
  setReverseMode(!reverseDirection);
}

// ==================== SAFETY MONITORING ====================
void monitorSystem() {
  float voltage = readVoltage();
  float temperature = readTemperature();
  
  // Check for fault conditions
  if (currentPeak > OVERCURRENT_LIMIT) {
    triggerFault("OVER CURRENT PEAK: " + String(currentPeak, 1) + "A");
  }
  else if (currentFilteredU > PHASE_CURRENT_LIMIT || currentFilteredV > PHASE_CURRENT_LIMIT) {
    triggerFault("PHASE CURRENT LIMIT: U=" + String(currentFilteredU, 1) + 
                 "A V=" + String(currentFilteredV, 1) + "A");
  }
  else if (voltage > OVERVOLTAGE_LIMIT) {
    triggerFault("OVERVOLTAGE: " + String(voltage, 1) + "V");
  }
  else if (voltage < UNDERVOLTAGE_LIMIT) {
    triggerFault("UNDERVOLTAGE: " + String(voltage, 1) + "V");
  }
  else if (temperature > OVERTEMP_LIMIT) {
    triggerFault("OVERTEMP: " + String(temperature, 1) + "C");
  }
  else if (faultCondition) {
    // Clear fault if conditions return to normal
    clearFault();
  }
}

float readVoltage() {
  const float DIVIDER_RATIO = 11.0; // 10:1 divider
  int raw = analogRead(VOLTAGE_SENSE);
  float voltage = (raw * 5.0 / 1024.0) * DIVIDER_RATIO;
  return voltage;
}

float readTemperature() {
  int raw = analogRead(TEMP_SENSE);
  float temp = (raw * 5.0 / 1024.0) * 100.0;
  return temp;
}

void triggerFault(String message) {
  faultCondition = true;
  faultMessage = message;
  systemEnabled = false;
  
  stopMotor();
  digitalWrite(FAULT_LED, HIGH);
  
  Serial.println("!!! FAULT: " + message);
  Serial.println("System halted. Send 'RESET' to clear fault.");
}

void clearFault() {
  faultCondition = false;
  faultMessage = "";
  digitalWrite(FAULT_LED, LOW);
  Serial.println("Fault cleared.");
}

// ==================== SYSTEM CONTROL FUNCTIONS ====================
void stopMotor() {
  electricalSpeed = 0;
  modulationIndex = 0;
  allPWMOff();
  digitalWrite(ENABLE_PIN, LOW);
  digitalWrite(DIRECTION_LED, LOW);
}

void startSystem() {
  if (faultCondition) {
    Serial.println("Cannot start - fault condition present");
    return;
  }
  
  systemEnabled = true;
  digitalWrite(ENABLE_PIN, HIGH);
  
  // Soft start
  if (reverseDirection) {
    targetSpeedRPM = -50;
  } else {
    targetSpeedRPM = 50;
  }
  Serial.println("System STARTED - Motor enabled");
}

// ==================== SERIAL COMMAND PROCESSING ====================
void processSerialCommands() {
  if (Serial.available()) {
    String command = Serial.readStringUntil('\n');
    command.trim();
    command.toUpperCase();
    
    if (command == "START") {
      startSystem();
    }
    else if (command == "STOP") {
      systemEnabled = false;
      targetSpeedRPM = 0;
      stopMotor();
      Serial.println("System STOPPED - Motor disabled");
    }
    else if (command == "REVERSE") {
      toggleDirection();
    }
    else if (command == "FORWARD") {
      setReverseMode(false);
    }
    else if (command == "RESET") {
      faultCondition = false;
      faultMessage = "";
      digitalWrite(FAULT_LED, LOW);
      Serial.println("System RESET");
    }
    else if (command == "STATUS") {
      printStatus();
    }
    else if (command == "SPWM") {
      printSPWMStatus();
    }
    else if (command.startsWith("+") || command.startsWith("-")) {
      setSpeed(command);
    }
    else {
      Serial.println("Unknown command. Valid: +RPM, -RPM, START, STOP, REVERSE, FORWARD, STATUS, SPWM, RESET");
    }
  }
}

void setSpeed(String command) {
  if (!systemEnabled) {
    Serial.println("System not enabled. Send 'START' first.");
    return;
  }
  
  float speed = command.substring(1).toFloat();
  
  if (command.startsWith("+")) {
    if (reverseDirection) {
      Serial.println("Warning: Positive speed in reverse mode. Use FORWARD command or negative speed.");
      setReverseMode(false);
    }
    targetSpeedRPM = speed;
  } else {
    if (!reverseDirection) {
      Serial.println("Warning: Negative speed in forward mode. Use REVERSE command or positive speed.");
      setReverseMode(true);
    }
    targetSpeedRPM = -speed;
  }
  
  targetSpeedRPM = constrain(targetSpeedRPM, -MAX_SPEED_RPM, MAX_SPEED_RPM);
  Serial.println("Speed set to: " + String(targetSpeedRPM) + " RPM");
}

// ==================== DISPLAY FUNCTIONS ====================
void updateDisplay() {
  static int displayCount = 0;
  displayCount++;
  
  if (displayCount >= 10) {
    Serial.print("Speed: ");
    Serial.print(filteredSpeedRPM);
    Serial.print(" RPM | Mod: ");
    Serial.print(modulationIndex, 2);
    Serial.print(" | Freq: ");
    Serial.print(electricalSpeed / (2 * PI), 1);
    Serial.print(" Hz | I_U: ");
    Serial.print(currentFilteredU, 1);
    Serial.println("A");
    displayCount = 0;
  }
}

void printStatus() {
  Serial.println("=== SPWM MOTOR STATUS ===");
  Serial.println("System: " + String(systemEnabled ? "ENABLED" : "DISABLED"));
  Serial.println("Direction: " + String(reverseDirection ? "REVERSE" : "FORWARD"));
  Serial.println("Fault: " + String(faultCondition ? "YES - " + faultMessage : "NO"));
  Serial.println("Target Speed: " + String(targetSpeedRPM) + " RPM");
  Serial.println("Actual Speed: " + String(filteredSpeedRPM) + " RPM");
  Serial.println("Electrical Speed: " + String(electricalSpeed / (2 * PI), 1) + " Hz");
  Serial.println("Modulation Index: " + String(modulationIndex, 3));
  Serial.println("Phase U Current: " + String(currentFilteredU, 2) + " A RMS");
  Serial.println("Phase V Current: " + String(currentFilteredV, 2) + " A RMS");
  Serial.println("DC Voltage: " + String(readVoltage(), 1) + " V");
  Serial.println("Hall State: " + String(hallState, BIN));
  Serial.println("====================");
}

void printSPWMStatus() {
  Serial.println("=== SPWM DETAILS ===");
  Serial.println("PWM Frequency: 31.25 kHz");
  Serial.println("SPWM Update: 10 kHz");
  Serial.println("Sine Table Size: " + String(SPWM_TABLE_SIZE));
  Serial.println("Max Modulation: " + String(MAX_MODULATION_INDEX, 2));
  Serial.println("Electrical Angle: " + String(electricalAngle, 3) + " rad");
  Serial.println("V/F Ratio: " + String(VF_RATIO, 1) + " V/Hz");
  Serial.println("====================");
}