#include <LiquidCrystal_I2C.h>

// LCD Configuration (2004 LCD with I2C)
LiquidCrystal_I2C lcd(0x27, 20, 4); // Adjust I2C address if needed

// Motor Control Pins
// High-side MOSFETs
const int U_HIGH = 2;
const int V_HIGH = 3;
const int W_HIGH = 4;

// Low-side MOSFETs
const int U_LOW = 5;
const int V_LOW = 6;
const int W_LOW = 7;

// Hall Sensor Pins
const int HALL_U = 8;
const int HALL_V = 9;
const int HALL_W = 10;

// Current Sensor Pins
const int CURRENT_U = A0;
const int CURRENT_V = A1;

// Speed Control
const int POT_PIN = A2;

// SPWM Parameters
const int PWM_FREQ = 20000;    // 20kHz PWM frequency
const int PWM_RESOLUTION = 8;  // 8-bit resolution
const int SPWM_STEPS = 256;    // Number of steps in SPWM table

// SPWM Lookup Table
int spwmTable[SPWM_STEPS];
float frequency = 50.0;  // Default frequency in Hz
float amplitude = 0.8;   // Modulation index (0-1)

// Motor Control Variables
volatile int hallState = 0;
volatile unsigned long lastHallTime = 0;
volatile unsigned long hallPeriod = 0;
float rpm = 0;
float targetRPM = 0;

// Current Measurement
float currentU = 0;
float currentV = 0;
float currentW = 0;

// Timing Variables
unsigned long lastDisplayUpdate = 0;
unsigned long lastSerialUpdate = 0;
const int DISPLAY_UPDATE_INTERVAL = 500;
const int SERIAL_UPDATE_INTERVAL = 100;

// Hall Sensor ISR Variables
volatile bool hallUState = false;
volatile bool hallVState = false;
volatile bool hallWState = false;

void setup() {
  // Initialize Serial
  Serial.begin(115200);
  
  // Initialize LCD
  lcd.init();
  lcd.backlight();
  lcd.clear();
  
  // Display startup message
  lcd.setCursor(0, 0);
  lcd.print("3-Phase Motor Driver");
  lcd.setCursor(0, 1);
  lcd.print("Initializing...");
  
  // Initialize motor control pins
  pinMode(U_HIGH, OUTPUT);
  pinMode(V_HIGH, OUTPUT);
  pinMode(W_HIGH, OUTPUT);
  pinMode(U_LOW, OUTPUT);
  pinMode(V_LOW, OUTPUT);
  pinMode(W_LOW, OUTPUT);
  
  // Initialize hall sensor pins with pullups
  pinMode(HALL_U, INPUT_PULLUP);
  pinMode(HALL_V, INPUT_PULLUP);
  pinMode(HALL_W, INPUT_PULLUP);
  
  // Attach interrupts for hall sensors
  attachInterrupt(digitalPinToInterrupt(HALL_U), hallU_ISR, CHANGE);
  attachInterrupt(digitalPinToInterrupt(HALL_V), hallV_ISR, CHANGE);
  attachInterrupt(digitalPinToInterrupt(HALL_W), hallW_ISR, CHANGE);
  
  // Initialize PWM frequency (Arduino UNO/Nano specific)
  // Note: For Raspberry Pi, you might need different PWM setup
  setupPWM();
  
  // Generate SPWM table
  generateSPWMTable();
  
  // Set initial motor state
  stopMotor();
  
  delay(2000);
  lcd.clear();
  
  Serial.println("3-Phase Motor Driver Started");
  Serial.println("RPM\tFreq\tCurU\tCurV\tHall");
  Serial.println("--------------------------------");
}

void loop() {
  // Read speed control potentiometer
  readSpeedControl();
  
  // Update motor control
  updateMotorControl();
  
  // Read current sensors
  readCurrentSensors();
  
  // Calculate RPM from hall sensors
  calculateRPM();
  
  // Update displays
  updateLCD();
  updateSerial();
  
  // Small delay to prevent overwhelming the system
  delay(10);
}

void setupPWM() {
  // Configure Timer1 for 20kHz PWM frequency (Arduino UNO/Nano)
  // For Raspberry Pi, this would be different
  TCCR1A = 0;
  TCCR1B = 0;
  TCNT1 = 0;
  
  // Set PWM frequency to ~20kHz
  TCCR1A |= (1 << WGM11);
  TCCR1B |= (1 << WGM12) | (1 << WGM13);
  TCCR1B |= (1 << CS10); // No prescaler
  ICR1 = 799; // 20kHz PWM frequency for 16MHz clock
  
  TCCR1A |= (1 << COM1A1) | (1 << COM1B1);
}

void generateSPWMTable() {
  // Generate sine wave lookup table
  for (int i = 0; i < SPWM_STEPS; i++) {
    float angle = 2.0 * PI * i / SPWM_STEPS;
    spwmTable[i] = (sin(angle) + 1.0) * 127.5 * amplitude;
  }
}

void readSpeedControl() {
  int potValue = analogRead(POT_PIN);
  targetRPM = map(potValue, 0, 1023, 0, 16000); // 0-3000 RPM range
  frequency = targetRPM / 60.0 * 2.0; // Convert RPM to electrical frequency (2 pole pairs assumed)
}

void updateMotorControl() {
  static unsigned long lastSPWMTime = 0;
  static int spwmIndex = 0;
  
  unsigned long currentTime = micros();
  unsigned long spwmPeriod = 1000000L / (frequency * SPWM_STEPS); // Period per SPWM step in microseconds
  
  if (currentTime - lastSPWMTime >= spwmPeriod) {
    lastSPWMTime = currentTime;
    
    // Get SPWM values for each phase with 120-degree phase shift
    int uValue = spwmTable[spwmIndex];
    int vValue = spwmTable[(spwmIndex + SPWM_STEPS/3) % SPWM_STEPS];
    int wValue = spwmTable[(spwmIndex + 2*SPWM_STEPS/3) % SPWM_STEPS];
    
    // Apply hall sensor based commutation
    applyCommutation(uValue, vValue, wValue);
    
    spwmIndex = (spwmIndex + 1) % SPWM_STEPS;
  }
}

void applyCommutation(int u, int v, int w) {
  // Read current hall state (atomic read)
  noInterrupts();
  bool uHall = hallUState;
  bool vHall = hallVState;
  bool wHall = hallWState;
  interrupts();
  
  // Determine which phases to energize based on hall sensors
  // This is a simplified 6-step commutation
  if (uHall && !vHall && wHall) {
    // Position 1: Energize U->V
    analogWrite(U_HIGH, u);
    analogWrite(V_LOW, v);
    digitalWrite(W_HIGH, LOW);
    digitalWrite(W_LOW, HIGH);
  }
  else if (uHall && !vHall && !wHall) {
    // Position 2: Energize U->W
    analogWrite(U_HIGH, u);
    digitalWrite(V_HIGH, LOW);
    digitalWrite(V_LOW, HIGH);
    analogWrite(W_LOW, w);
  }
  else if (uHall && vHall && !wHall) {
    // Position 3: Energize V->W
    digitalWrite(U_HIGH, LOW);
    digitalWrite(U_LOW, HIGH);
    analogWrite(V_HIGH, v);
    analogWrite(W_LOW, w);
  }
  else if (!uHall && vHall && !wHall) {
    // Position 4: Energize V->U
    analogWrite(U_LOW, u);
    analogWrite(V_HIGH, v);
    digitalWrite(W_HIGH, LOW);
    digitalWrite(W_LOW, HIGH);
  }
  else if (!uHall && vHall && wHall) {
    // Position 5: Energize W->U
    analogWrite(U_LOW, u);
    digitalWrite(V_HIGH, LOW);
    digitalWrite(V_LOW, HIGH);
    analogWrite(W_HIGH, w);
  }
  else if (!uHall && !vHall && wHall) {
    // Position 6: Energize W->V
    digitalWrite(U_HIGH, LOW);
    digitalWrite(U_LOW, HIGH);
    analogWrite(V_LOW, v);
    analogWrite(W_HIGH, w);
  }
  else {
    // Invalid state - stop motor for safety
    stopMotor();
  }
}

void readCurrentSensors() {
  // Read current sensors (ACS712 or similar)
  // Convert ADC reading to current (adjust scaling based on your sensor)
  int adcU = analogRead(CURRENT_U);
  int adcV = analogRead(CURRENT_V);
  
  // Convert to current (example for ACS712 5A module)
  // 2.5V = 0A, 185mV per Amp
  currentU = (adcU * (5.0 / 1023.0) - 2.5) / 0.185;
  currentV = (adcV * (5.0 / 1023.0) - 2.5) / 0.185;
  
  // Calculate W phase current (assuming balanced 3-phase)
  currentW = -currentU - currentV;
}

void calculateRPM() {
  if (hallPeriod > 0) {
    // Assuming 3 hall sensors per revolution (6 steps)
    rpm = 1000000.0 * 60.0 / (hallPeriod * 6.0);
  } else {
    rpm = 0;
  }
}

void updateLCD() {
  if (millis() - lastDisplayUpdate >= DISPLAY_UPDATE_INTERVAL) {
    lastDisplayUpdate = millis();
    
    // Line 1: RPM and Frequency
    lcd.setCursor(0, 0);
    lcd.print("RPM:");
    lcd.print(int(rpm));
    lcd.print("  Freq:");
    lcd.print(frequency, 1);
    lcd.print("Hz ");
    
    // Line 2: Current Measurements
    lcd.setCursor(0, 1);
    lcd.print("Iu:");
    lcd.print(abs(currentU), 2);
    lcd.print("A Iv:");
    lcd.print(abs(currentV), 2);
    lcd.print("A");
    
    // Line 3: Target RPM and Hall State
    lcd.setCursor(0, 2);
    lcd.print("Target:");
    lcd.print(int(targetRPM));
    lcd.print(" RPM  ");
    
    // Line 4: Hall Sensor State
    lcd.setCursor(0, 3);
    lcd.print("Hall:");
    lcd.print(hallUState);
    lcd.print(hallVState);
    lcd.print(hallWState);
    lcd.print("  Status:OK");
  }
}

void updateSerial() {
  if (millis() - lastSerialUpdate >= SERIAL_UPDATE_INTERVAL) {
    lastSerialUpdate = millis();
    
    Serial.print(rpm);
    Serial.print("\t");
    Serial.print(frequency, 1);
    Serial.print("\t");
    Serial.print(currentU, 2);
    Serial.print("\t");
    Serial.print(currentV, 2);
    Serial.print("\t");
    Serial.print(hallUState);
    Serial.print(hallVState);
    Serial.print(hallWState);
    Serial.println();
  }
}

void stopMotor() {
  digitalWrite(U_HIGH, LOW);
  digitalWrite(V_HIGH, LOW);
  digitalWrite(W_HIGH, LOW);
  digitalWrite(U_LOW, LOW);
  digitalWrite(V_LOW, LOW);
  digitalWrite(W_LOW, LOW);
}

// Hall Sensor Interrupt Service Routines
void hallU_ISR() {
  hallUState = digitalRead(HALL_U);
  updateHallTiming();
}

void hallV_ISR() {
  hallVState = digitalRead(HALL_V);
  updateHallTiming();
}

void hallW_ISR() {
  hallWState = digitalRead(HALL_W);
  updateHallTiming();
}

void updateHallTiming() {
  unsigned long currentTime = micros();
  hallPeriod = currentTime - lastHallTime;
  lastHallTime = currentTime;
}