#include <WiFi.h>

// Mock dyno simulation parameters
const int SERIAL_BAUD_RATE = 9600;
const int DATA_SEND_INTERVAL_MS = 100; // Send data every 100ms
const int SIMULATION_STEP_MS = 50;     // Update simulation every 50ms

// Simulation state
enum SimulationState {
  IDLE,
  ACCELERATING,
  HOLDING,
  DECELERATING
};

SimulationState currentState = IDLE;
unsigned long lastDataSend = 0;
unsigned long lastSimulationUpdate = 0;
unsigned long stateStartTime = 0;

// RPM simulation variables
float currentRPM = 0.0;
float targetRPM = 0.0;
float rpmAcceleration = 0.0;
float maxRPM = 11000.0;
float minRPM = 800.0;    // Idle RPM
float idleRPM = 800.0;

// Dyno run parameters
const float ACCELERATION_RATE = 25.0;  // RPM per 100ms during acceleration
const float DECELERATION_RATE = 30.0;  // RPM per 100ms during deceleration
const int HOLD_DURATION_MS = 2000;     // Hold at max RPM for 2 seconds
const int IDLE_DURATION_MS = 3000;     // Stay at idle for 3 seconds

// Add some realistic noise and variations
float rpmNoise = 0.0;
int noiseCounter = 0;

void setup() {
  Serial.begin(SERIAL_BAUD_RATE);

  // Initialize random seed
  randomSeed(analogRead(0));

  // Wait for serial to initialize
  delay(2000);

  Serial.println("ESP32 Mock Dyno Data Generator");
  Serial.println("Sending RPM data for dyno simulation...");
  Serial.println("==================================");

  // Start at idle
  currentRPM = idleRPM;
  targetRPM = idleRPM;
  currentState = IDLE;
  stateStartTime = millis();

  delay(1000);
}

void loop() {
  unsigned long currentTime = millis();

  // Update simulation state
  if (currentTime - lastSimulationUpdate >= SIMULATION_STEP_MS) {
    updateSimulation();
    lastSimulationUpdate = currentTime;
  }

  // Send data to Java application
  if (currentTime - lastDataSend >= DATA_SEND_INTERVAL_MS) {
    sendRPMData();
    lastDataSend = currentTime;
  }

  delay(10); // Small delay to prevent excessive CPU usage
}

void updateSimulation() {
  unsigned long currentTime = millis();
  unsigned long stateElapsed = currentTime - stateStartTime;

  switch (currentState) {
    case IDLE:
      // Stay at idle RPM for a while, then start accelerating
      targetRPM = idleRPM + random(-50, 51); // Add some idle variation
      if (stateElapsed > IDLE_DURATION_MS) {
        currentState = ACCELERATING;
        targetRPM = maxRPM;
        stateStartTime = currentTime;
        Serial.println("Starting dyno run - ACCELERATING");
      }
      break;

    case ACCELERATING:
      // Accelerate towards max RPM
      if (currentRPM < maxRPM - 100) {
        currentRPM += ACCELERATION_RATE * (SIMULATION_STEP_MS / 100.0);
        // Add some realistic acceleration curve (not perfectly linear)
        float accelerationFactor = 1.0 - (currentRPM / maxRPM) * 0.3;
        currentRPM = currentRPM * accelerationFactor + currentRPM * (1.0 - accelerationFactor);
      } else {
        currentState = HOLDING;
        stateStartTime = currentTime;
        Serial.println("Reached max RPM - HOLDING");
      }
      break;

    case HOLDING:
      // Hold at max RPM with some variation
      targetRPM = maxRPM + random(-100, 101);
      if (stateElapsed > HOLD_DURATION_MS) {
        currentState = DECELERATING;
        targetRPM = idleRPM;
        stateStartTime = currentTime;
        Serial.println("Starting deceleration - DECELERATING");
      }
      break;

    case DECELERATING:
      // Decelerate back to idle
      if (currentRPM > idleRPM + 100) {
        currentRPM -= DECELERATION_RATE * (SIMULATION_STEP_MS / 100.0);
        // Add engine braking effect (faster deceleration at higher RPM)
        float brakingFactor = 1.0 + (currentRPM / maxRPM) * 0.5;
        currentRPM -= (DECELERATION_RATE * brakingFactor - DECELERATION_RATE) * (SIMULATION_STEP_MS / 100.0);
      } else {
        currentRPM = idleRPM;
        currentState = IDLE;
        stateStartTime = currentTime;
        Serial.println("Returned to idle - IDLE");
        Serial.println("==================================");
      }
      break;
  }

  // Add realistic noise and fluctuations
  addRealisticNoise();

  // Ensure RPM doesn't go below 0 or above reasonable limits
  currentRPM = constrain(currentRPM, 0, maxRPM + 500);
}

void addRealisticNoise() {
  // Add different types of noise based on RPM range
  float baseNoise = random(-10, 11); // Basic noise

  // Engine vibration increases with RPM
  float vibrationNoise = (currentRPM / 1000.0) * random(-5, 6);

  // Periodic fluctuations (simulating engine cycles)
  noiseCounter++;
  float periodicNoise = sin(noiseCounter * 0.1) * (currentRPM / 2000.0) * 3.0;

  // Combine all noise sources
  rpmNoise = baseNoise + vibrationNoise + periodicNoise;

  // Apply noise with some smoothing
  currentRPM += rpmNoise * 0.3; // Reduce noise impact
}

void sendRPMData() {
  // Send just the RPM value as your Java code expects
  Serial.println(currentRPM, 0); // Send as integer (no decimal places)

  // Optional: Send debug info to Serial Monitor (comment out for production)
  // Serial.print("DEBUG - State: ");
  // Serial.print(getStateString());
  // Serial.print(", RPM: ");
  // Serial.print(currentRPM, 1);
  // Serial.print(", Noise: ");
  // Serial.println(rpmNoise, 1);
}

String getStateString() {
  switch (currentState) {
    case IDLE: return "IDLE";
    case ACCELERATING: return "ACCELERATING";
    case HOLDING: return "HOLDING";
    case DECELERATING: return "DECELERATING";
    default: return "UNKNOWN";
  }
}

// Function to manually trigger a dyno run (call this if you want manual control)
void triggerDynoRun() {
  if (currentState == IDLE) {
    currentState = ACCELERATING;
    stateStartTime = millis();
    Serial.println("Manual dyno run triggered!");
  }
}

// Function to reset to idle (emergency stop)
void resetToIdle() {
  currentState = IDLE;
  currentRPM = idleRPM;
  targetRPM = idleRPM;
  stateStartTime = millis();
  Serial.println("Reset to idle!");
}