#include <WiFi.h>

// Configuration
const char* ssid = "Current_Simulator";
WiFiServer server(5000);

// Pin definitions
#define PULSE_PIN 27      // MOSFET switch for pulses
#define PWM_PIN 26        // MOSFET for PWM voltage control
#define LED_PIN 2
#define ACS712_PIN 34     // ACS712T current sensor

// ACS712T configuration (30A model)
#define ACS712_SENSITIVITY 0.066  // 66mV/A for 30A model
#define ACS712_VCC 5.0            // Sensor supply voltage
#define ACS712_QUIESCENT_VOUT (ACS712_VCC / 2)  // 2.5V at 0A

// PWM configuration
#define PWM_FREQ 25000    // 25kHz for MOSFET
#define PWM_RESOLUTION 8  // 8-bit = 0-255
#define MAX_VOLTAGE 12.0  // Maximum output voltage
const int MAX_DUTY = 255;

// Pulse control
bool pulseActive = false;
unsigned long pulseStartTime = 0;
unsigned long pulseDuration = 0;
int pulseRepeatCount = 0;
int pulseRepeatCounter = 0;
unsigned long repeatInterval = 0;
unsigned long lastRepeatTime = 0;

// Voltage control
float targetVoltage = 0;
float currentDuty = 0;

// Current sensor calibration
float currentOffset = 0;
bool currentCalibrated = false;

// Buffer for incoming data
String inputBuffer = "";

void setup() {
  Serial.begin(115200);
  delay(2000);
  
  Serial.println("\n\n=================================");
  Serial.println("ADVANCED CURRENT SIMULATOR");
  Serial.println("ACS712T + Dual MOSFET Control");
  Serial.println("=================================");
  
  // Initialize pins
  pinMode(PULSE_PIN, OUTPUT);
  pinMode(PWM_PIN, OUTPUT);
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(PULSE_PIN, LOW);
  digitalWrite(LED_PIN, LOW);
  
  // Setup PWM
  ledcSetup(0, PWM_FREQ, PWM_RESOLUTION);
  ledcAttachPin(PWM_PIN, 0);
  ledcWrite(0, 0);  // Start with 0% duty
  
  // Blink LED startup sequence
  for(int i = 0; i < 3; i++) {
    digitalWrite(LED_PIN, HIGH);
    delay(200);
    digitalWrite(LED_PIN, LOW);
    delay(200);
  }
  
  // Start WiFi AP
  Serial.print("Starting WiFi AP '");
  Serial.print(ssid);
  Serial.println("'...");
  
  if (WiFi.softAP(ssid, NULL, 1, 0, 1)) {
    Serial.println("✓ WiFi AP started!");
    Serial.print("IP Address: ");
    Serial.println(WiFi.softAPIP());
  } else {
    Serial.println("✗ WiFi AP failed!");
  }
  
  server.begin();
  server.setNoDelay(true);
  Serial.println("✓ TCP server started on port 5000");
  
  // Calibrate current sensor on startup
  calibrateCurrentSensor();
  
  Serial.println("\nREADY FOR COMMANDS:");
  Serial.println("PING - Test connection");
  Serial.println("READ - Get measurements");
  Serial.println("VOLTAGE <0-12> - Set output voltage");
  Serial.println("PWM <0-100> - Set PWM duty cycle");
  Serial.println("PULSE <1-500> [repeat] [interval] - MOSFET pulse");
  Serial.println("CALIBRATE_CURRENT - Calibrate ACS712T zero point");
  Serial.println("=================================\n");
}

void calibrateCurrentSensor() {
  Serial.println("Calibrating ACS712T...");
  
  // Take multiple samples for offset
  long sum = 0;
  const int samples = 100;
  
  for(int i = 0; i < samples; i++) {
    sum += analogRead(ACS712_PIN);
    delay(1);
  }
  
  int avgAdc = sum / samples;
  currentOffset = (avgAdc / 4095.0) * 3.3;
  currentCalibrated = true;
  
  Serial.print("Calibrated offset: ");
  Serial.print(currentOffset, 4);
  Serial.println("V");
  Serial.println("✓ Current sensor calibrated");
}

float readCurrent() {
  if (!currentCalibrated) {
    calibrateCurrentSensor();
  }
  
  // Read ACS712T
  int adcValue = analogRead(ACS712_PIN);
  float voltage = (adcValue / 4095.0) * 3.3;
  
  // Remove offset
  float sensorVoltage = voltage - currentOffset;
  
  // Convert to current (A = (V - 2.5) / sensitivity)
  // For ACS712T 30A: sensitivity = 66mV/A = 0.066V/A
  float current = sensorVoltage / ACS712_SENSITIVITY;
  
  return current;
}

float readOutputVoltage() {
  // Estimate voltage based on PWM duty cycle
  // Assuming linear relationship: Vout = (duty/255) * 12V
  float estimatedVoltage = (currentDuty / MAX_DUTY) * MAX_VOLTAGE;
  return estimatedVoltage;
}

String readMeasurements() {
  float voltage = readOutputVoltage();
  float current = readCurrent();
  float dutyPercent = (currentDuty / MAX_DUTY) * 100.0;
  
  char buffer[50];
  sprintf(buffer, "%.2f,%.3f,%.1f", voltage, current, dutyPercent);
  return String(buffer);
}

void setOutputVoltage(float voltage) {
  if (voltage < 0) voltage = 0;
  if (voltage > MAX_VOLTAGE) voltage = MAX_VOLTAGE;
  
  targetVoltage = voltage;
  
  // Convert voltage to duty cycle
  float dutyRatio = voltage / MAX_VOLTAGE;
  int newDuty = (int)(dutyRatio * MAX_DUTY);
  
  // Limit duty cycle
  if (newDuty < 0) newDuty = 0;
  if (newDuty > MAX_DUTY) newDuty = MAX_DUTY;
  
  currentDuty = newDuty;
  ledcWrite(0, newDuty);
  
  Serial.print("Voltage set to ");
  Serial.print(voltage);
  Serial.print("V (Duty: ");
  Serial.print((newDuty * 100.0) / MAX_DUTY);
  Serial.println("%)");
}

void setPWM(int dutyPercent) {
  if (dutyPercent < 0) dutyPercent = 0;
  if (dutyPercent > 100) dutyPercent = 100;
  
  int dutyValue = (dutyPercent * MAX_DUTY) / 100;
  currentDuty = dutyValue;
  ledcWrite(0, dutyValue);
  
  // Update target voltage estimate
  targetVoltage = (dutyPercent / 100.0) * MAX_VOLTAGE;
  
  Serial.print("PWM set to ");
  Serial.print(dutyPercent);
  Serial.print("% (Duty value: ");
  Serial.print(dutyValue);
  Serial.println(")");
}

void startPulse(unsigned long duration_ms, int repeat = 1, unsigned long interval_ms = 0) {
  // Stop any ongoing pulse
  if (pulseActive) {
    digitalWrite(PULSE_PIN, LOW);
    digitalWrite(LED_PIN, LOW);
    pulseActive = false;
  }
  
  // Send acknowledgment
  Serial.print("PULSE_ACK: ");
  Serial.print(duration_ms);
  Serial.print("ms x");
  Serial.print(repeat);
  if (interval_ms > 0) {
    Serial.print(" every ");
    Serial.print(interval_ms);
    Serial.print("ms");
  }
  Serial.println();
  
  // Store pulse parameters
  pulseActive = true;
  pulseDuration = duration_ms;
  pulseRepeatCount = repeat;
  pulseRepeatCounter = 1;
  repeatInterval = interval_ms;
  pulseStartTime = millis();
  lastRepeatTime = pulseStartTime;
  
  // Start pulse
  digitalWrite(PULSE_PIN, HIGH);
  digitalWrite(LED_PIN, HIGH);
}

void checkPulse() {
  if (!pulseActive) return;
  
  unsigned long currentTime = millis();
  unsigned long elapsedTime = currentTime - pulseStartTime;
  
  // Check if current pulse duration is complete
  if (elapsedTime >= pulseDuration) {
    // Turn off pulse
    digitalWrite(PULSE_PIN, LOW);
    digitalWrite(LED_PIN, LOW);
    
    // Check if we need to repeat
    if (pulseRepeatCounter < pulseRepeatCount) {
      // Check interval
      if (interval_ms > 0) {
        unsigned long timeSinceLast = currentTime - lastRepeatTime;
        if (timeSinceLast < (pulseDuration + interval_ms)) {
          return; // Wait for interval
        }
      }
      
      pulseRepeatCounter++;
      pulseStartTime = currentTime;
      lastRepeatTime = currentTime;
      
      // Start next pulse
      digitalWrite(PULSE_PIN, HIGH);
      digitalWrite(LED_PIN, HIGH);
    } else {
      // All pulses completed
      pulseActive = false;
      Serial.println("PULSE_DONE");
    }
  }
}

void processCommand(String cmd) {
  cmd.trim();
  
  if (cmd.length() == 0) {
    return;
  }
  
  Serial.print("CMD: ");
  Serial.println(cmd);
  
  if (cmd == "PING") {
    Serial.println("PONG");
  } 
  else if (cmd == "READ") {
    Serial.println(readMeasurements());
  }
  else if (cmd == "CALIBRATE_CURRENT") {
    calibrateCurrentSensor();
    Serial.println("CALIBRATED");
  }
  else if (cmd.startsWith("VOLTAGE")) {
    cmd = cmd.substring(7);
    cmd.trim();
    
    float voltage = cmd.toFloat();
    if (voltage >= 0 && voltage <= 12.0) {
      setOutputVoltage(voltage);
      Serial.println("VOLTAGE_SET");
    } else {
      Serial.println("ERROR: Voltage must be 0-12V");
    }
  }
  else if (cmd.startsWith("PWM")) {
    cmd = cmd.substring(3);
    cmd.trim();
    
    int duty = cmd.toInt();
    if (duty >= 0 && duty <= 100) {
      setPWM(duty);
      Serial.println("PWM_SET");
    } else {
      Serial.println("ERROR: PWM must be 0-100%");
    }
  }
  else if (cmd.startsWith("PULSE")) {
    cmd = cmd.substring(5);
    cmd.trim();
    
    float duration = 0;
    int repeat = 1;
    int interval = 0;
    
    int space1 = cmd.indexOf(' ');
    int space2 = cmd.indexOf(' ', space1 + 1);
    
    if (space1 > 0) {
      duration = cmd.substring(0, space1).toFloat();
      
      if (duration < 1 || duration > 500) {
        Serial.println("ERROR: Pulse duration must be 1-500ms");
        return;
      }
      
      if (space2 > 0) {
        repeat = cmd.substring(space1 + 1, space2).toInt();
        interval = cmd.substring(space2 + 1).toInt();
      } else if (space1 > 0 && cmd.indexOf(' ', space1 + 1) == -1) {
        repeat = cmd.substring(space1 + 1).toInt();
      }
    } else {
      duration = cmd.toFloat();
    }
    
    if (duration > 0 && duration <= 500) {
      startPulse((unsigned long)duration, repeat, interval);
    } else {
      Serial.println("ERROR: Invalid pulse duration (1-500ms)");
    }
  }
  else {
    Serial.print("ERROR: Unknown command: ");
    Serial.println(cmd);
  }
}

void processSerialInput() {
  while (Serial.available()) {
    char c = Serial.read();
    
    if (c == '\n' || c == '\r') {
      if (inputBuffer.length() > 0) {
        processCommand(inputBuffer);
        inputBuffer = "";
      }
    } else {
      inputBuffer += c;
      if (inputBuffer.length() > 100) {
        inputBuffer = "";
      }
    }
  }
}

void handleWiFiClient() {
  WiFiClient client = server.available();
  
  if (client) {
    Serial.println("New WiFi client connected");
    client.setTimeout(1000);
    client.setNoDelay(true);
    
    while (client.connected()) {
      checkPulse();
      processSerialInput();
      
      if (client.available()) {
        String cmd = client.readStringUntil('\n');
        cmd.trim();
        
        if (cmd.length() > 0) {
          Serial.print("WiFi CMD: ");
          Serial.println(cmd);
          
          if (cmd == "PING") {
            client.println("PONG");
          }
          else if (cmd == "READ") {
            client.println(readMeasurements());
          }
          else if (cmd == "CALIBRATE_CURRENT") {
            calibrateCurrentSensor();
            client.println("CALIBRATED");
          }
          else if (cmd.startsWith("VOLTAGE")) {
            cmd = cmd.substring(7);
            cmd.trim();
            
            float voltage = cmd.toFloat();
            if (voltage >= 0 && voltage <= 12.0) {
              setOutputVoltage(voltage);
              client.println("VOLTAGE_SET");
            } else {
              client.println("ERROR: Voltage must be 0-12V");
            }
          }
          else if (cmd.startsWith("PWM")) {
            cmd = cmd.substring(3);
            cmd.trim();
            
            int duty = cmd.toInt();
            if (duty >= 0 && duty <= 100) {
              setPWM(duty);
              client.println("PWM_SET");
            } else {
              client.println("ERROR: PWM must be 0-100%");
            }
          }
          else if (cmd.startsWith("PULSE")) {
            cmd = cmd.substring(5);
            cmd.trim();
            
            float duration = 0;
            int repeat = 1;
            int interval = 0;
            
            int space1 = cmd.indexOf(' ');
            int space2 = cmd.indexOf(' ', space1 + 1);
            
            if (space1 > 0) {
              duration = cmd.substring(0, space1).toFloat();
              
              if (duration < 1 || duration > 500) {
                client.println("ERROR: Pulse duration must be 1-500ms");
                continue;
              }
              
              if (space2 > 0) {
                repeat = cmd.substring(space1 + 1, space2).toInt();
                interval = cmd.substring(space2 + 1).toInt();
              } else if (space1 > 0 && cmd.indexOf(' ', space1 + 1) == -1) {
                repeat = cmd.substring(space1 + 1).toInt();
              }
            } else {
              duration = cmd.toFloat();
            }
            
            if (duration > 0 && duration <= 500) {
              client.println("PULSE_ACK");
              startPulse((unsigned long)duration, repeat, interval);
            } else {
              client.println("ERROR: Invalid pulse duration");
            }
          } else {
            client.println("ERROR: Unknown command");
          }
        }
      }
      
      delay(1);
    }
    
    client.stop();
    Serial.println("WiFi client disconnected");
  }
}

void loop() {
  // Check pulse timing
  checkPulse();
  
  // Process serial input
  processSerialInput();
  
  // Handle WiFi clients
  handleWiFiClient();
  
  // Small delay for stability
  delay(1);
}