#include <Adafruit_NeoPixel.h>
#include <EEPROM.h>
#include <Wire.h>
#include <MPU6050.h>
#include <DFRobotDFPlayerMini.h>

// ================================
// ENHANCED GHOSTBUSTERS PROTON PACK v4.0
// Features: EEPROM Storage, Accelerometer, Authentic MP3 Sounds
// ================================

// ESP32 PIN DEFINITIONS
#define ION_ARM_SWITCH  16  // Main pack power
#define WAND_ACTIVATE   17  // Wand activation trigger
#define FIRE_TRIGGER    15  // Proton stream trigger
#define MANUAL_VENT_BTN 2   // Emergency thermal vent
#define SAFETY_INTERLOCK 5  // Critical safety system
#define PACK_DIAGNOSTIC 14  // Diagnostic/status check
#define SMOKE_PIN       21  // Thermal vent smoke

// MP3 PLAYER PINS (DFPlayer Mini)
#define MP3_RX_PIN      4   // Connect to DFPlayer TX
#define MP3_TX_PIN      22  // Connect to DFPlayer RX
#define MP3_BUSY_PIN    23  // Connect to DFPlayer BUSY

// RUMBLE MOTOR CONTROL (ESP32 Core 3.x API)
#define RUMBLE_MOSFET_PIN 12  // PWM pin to control MOSFET gate
#define RUMBLE_PWM_FREQ 1000  // 1kHz PWM frequency
#define RUMBLE_PWM_RESOLUTION 8 // 8-bit resolution (0-255)

// ACCELEROMETER PINS (MPU6050) - Using available pins
#define SDA_PIN         32  // I2C Data (available pin)
#define SCL_PIN         13  // I2C Clock (available pin)

// LED Outputs
#define POWER_CELL_PIN  18  // 15-segment power cell
#define CYCLOTRON_PIN   19  // 40-segment cyclotron ring
#define WAND_TIP_PIN    25  // Wand focusing tip (moved from 22)
#define WAND_SIDE_PIN   26  // Thermal warning strip (moved from 23)
#define WAND_BARGRAPH   27  // Power level bargraph (moved from 25)
#define WAND_STATUS_PIN 33  // Status indicator (moved from 26)

// LED Counts
#define POWER_CELL_LEDS 15
#define CYCLOTRON_LEDS  40
#define WAND_TIP_LEDS   6
#define WAND_SIDE_LEDS  8
#define WAND_BARGRAPH_LEDS 10
#define WAND_STATUS_LEDS 1

// PHYSICS CONSTANTS
#define AMBIENT_TEMP 20.0           
#define MAX_SAFE_TEMP 85.0          
#define CRITICAL_TEMP 95.0          
#define EMERGENCY_TEMP 105.0        
#define THERMAL_MASS 0.85           
#define HEAT_DISSIPATION 0.15       
#define FIRING_HEAT_RATE 2.8        
#define VENT_COOLING_RATE 4.5       

// POWER SYSTEM
#define PACK_VOLTAGE 14.8           
#define MIN_VOLTAGE 12.0            
#define VOLTAGE_DROP_RATE 0.002     
#define CHARGING_RECOVERY 0.001     

// CYCLOTRON PHYSICS
#define CYCLOTRON_STARTUP_TIME 4000
#define CYCLOTRON_MIN_RPM 120       
#define CYCLOTRON_MAX_RPM 1800      
#define CYCLOTRON_ACCELERATION 0.25

// CYCLOTRON HEAT VISUALIZATION
#define CYCLOTRON_COOL_TEMP 25.0    
#define CYCLOTRON_WARM_TEMP 45.0    
#define CYCLOTRON_HOT_TEMP 65.0     
#define CYCLOTRON_BLAZING_TEMP 85.0 
#define CYCLOTRON_HEAT_RATE 0.8     
#define CYCLOTRON_COOL_RATE 0.3     
#define CYCLOTRON_RPM_HEAT_FACTOR 0.0008 

// PROTON STREAM PHYSICS
#define STREAM_CHARGE_TIME 1500     
#define STREAM_MAX_DURATION 12000   
#define STREAM_COOLDOWN_RATIO 2.5   
#define BEAM_INSTABILITY_CHANCE 15  

// CROSSING STREAMS DETECTION
#define CROSSING_ANGLE_THRESHOLD 30.0  // Degrees for crossing detection
#define CROSSING_DISTANCE_MAX 2.0      // Max distance in meters
#define CROSSING_TIME_MIN 1000         // Min time for crossing detection

// TESTING MODE
#define TESTING_MODE true

// EEPROM ADDRESSES
#define EEPROM_SIZE 512
#define ADDR_EQUIPMENT_HEALTH 0     // Float (4 bytes)
#define ADDR_OPERATING_HOURS 4      // Unsigned long (4 bytes)
#define ADDR_TOTAL_SHOTS 8          // Unsigned long (4 bytes)
#define ADDR_MAINTENANCE_FLAG 12    // Bool (1 byte)
#define ADDR_LAST_SAVE_TIME 13      // Unsigned long (4 bytes)
#define ADDR_CHECKSUM 17            // Byte (1 byte)

// MP3 TRACK NUMBERS (organize your SD card with these numbers)
#define TRACK_STARTUP 1             // Proton pack startup
#define TRACK_ION_ARM_CHARGE 2      // Ion arm charging
#define TRACK_WAND_SYNC 3           // Wand synchronization
#define TRACK_READY 4               // System ready
#define TRACK_STREAM_CHARGE 5       // Stream charging
#define TRACK_STREAM_FIRE 6         // Proton stream firing
#define TRACK_STREAM_LOOP 7         // Continuous firing loop
#define TRACK_VENT_MANUAL 8         // Manual thermal vent
#define TRACK_VENT_EMERGENCY 9      // Emergency vent
#define TRACK_CROSSING_WARNING 10   // "Don't cross the streams!"
#define TRACK_CROSSING_EXPLOSION 11 // Crossing streams explosion
#define TRACK_SHUTDOWN 12           // System shutdown
#define TRACK_MALFUNCTION 13        // Equipment malfunction
#define TRACK_LOW_BATTERY 14        // Low battery warning

// UPDATE INTERVALS (ms)
#define PHYSICS_UPDATE_INTERVAL 50      
#define VISUAL_UPDATE_INTERVAL 33       
#define AUDIO_UPDATE_INTERVAL 25        
#define INPUT_UPDATE_INTERVAL 10        
#define STATUS_UPDATE_INTERVAL 2000     
#define SAFETY_CHECK_INTERVAL 250       
#define EEPROM_SAVE_INTERVAL 30000      // Save every 30 seconds
#define ACCELEROMETER_UPDATE_INTERVAL 50 // Check accelerometer every 50ms
#define RUMBLE_UPDATE_INTERVAL 20       // Update rumble every 20ms for smooth haptics

// ================================
// HARDWARE INITIALIZATION
// ================================

// NeoPixel strips
Adafruit_NeoPixel powerCell(POWER_CELL_LEDS, POWER_CELL_PIN, NEO_GRB + NEO_KHZ800);
Adafruit_NeoPixel cyclotron(CYCLOTRON_LEDS, CYCLOTRON_PIN, NEO_GRB + NEO_KHZ800);
Adafruit_NeoPixel wandTip(WAND_TIP_LEDS, WAND_TIP_PIN, NEO_GRB + NEO_KHZ800);
Adafruit_NeoPixel wandSide(WAND_SIDE_LEDS, WAND_SIDE_PIN, NEO_GRB + NEO_KHZ800);
Adafruit_NeoPixel wandBargraph(WAND_BARGRAPH_LEDS, WAND_BARGRAPH, NEO_GRB + NEO_KHZ800);
Adafruit_NeoPixel wandStatus(WAND_STATUS_LEDS, WAND_STATUS_PIN, NEO_GRB + NEO_KHZ800);

// MP3 Player - use hardware serial Serial2 for ESP32-S3
#define MP3_SERIAL_PORT Serial2
DFRobotDFPlayerMini mp3Player;

// Accelerometer
MPU6050 accelerometer;

// ================================
// SYSTEM STATES
// ================================

enum SystemState {
  POWERED_OFF,        
  SELF_DIAGNOSTIC,    
  ION_ARM_CHARGING,   
  WAND_SYNC,          
  IDLE_OPERATIONAL,   
  STREAM_CHARGING,    
  STREAM_ACTIVE,      
  THERMAL_WARNING,    
  THERMAL_CRITICAL,   
  EMERGENCY_VENT,     
  SAFETY_LOCKOUT,     
  MALFUNCTION,
  CROSSING_STREAMS_WARNING,
  CROSSING_STREAMS_CRITICAL
};

SystemState currentState = POWERED_OFF;
SystemState previousState = POWERED_OFF;

// ================================
// ENHANCED PHYSICS ENGINE
// ================================

struct PhysicsEngine {
  float coreTemperature = AMBIENT_TEMP;
  float cyclotronTemp = AMBIENT_TEMP;
  float wandTemp = AMBIENT_TEMP;
  float ambientTemp = AMBIENT_TEMP;
  
  float packVoltage = PACK_VOLTAGE;
  float currentDraw = 0.0;
  bool powerStable = true;
  
  float currentRPM = 0.0;
  float targetRPM = CYCLOTRON_MIN_RPM;
  unsigned long cyclotronStartTime = 0;
  bool cyclotronStable = false;
  float cyclotronHeat = CYCLOTRON_COOL_TEMP;
  
  float streamPower = 0.0;
  float streamStability = 100.0;
  unsigned long streamStartTime = 0;
  unsigned long totalStreamTime = 0;
  unsigned long lastStreamEnd = 0;
  bool streamOverloaded = false;
  
  float equipmentHealth = 100.0;
  unsigned long operatingHours = 0;
  unsigned long totalShots = 0;
  bool requiresMaintenance = false;
  
  float humidity = 45.0;
  float atmosphericPressure = 1013.25;
  bool environmentalWarning = false;
} physics;

// ================================
// RUMBLE HAPTIC FEEDBACK SYSTEM
// ================================

struct RumbleSystem {
  bool enabled = true;
  uint8_t currentIntensity = 0;
  uint8_t targetIntensity = 0;
  uint8_t baseIntensity = 0;
  
  unsigned long patternStartTime = 0;
  unsigned long lastPatternUpdate = 0;
  bool patternActive = false;
  uint8_t patternType = 0;
  
  float pulsePhase = 0.0;
  uint8_t rampDirection = 1;
  unsigned long chaosTimer = 0;
  
  bool overrideActive = false;
  unsigned long overrideStartTime = 0;
  unsigned long overrideDuration = 200;
  
  uint8_t maxIntensity = 255;
  float smoothingFactor = 0.7;
} rumble;

// ================================
// CROSSING STREAMS DETECTION
// ================================

struct CrossingStreams {
  bool detectionEnabled = true;
  bool streamsDetected = false;
  unsigned long crossingStartTime = 0;
  float crossingAngle = 0.0;
  float crossingDistance = 0.0;
  bool warningIssued = false;
  bool criticalState = false;
  
  float wandAngleX = 0.0;
  float wandAngleY = 0.0;
  float wandAngleZ = 0.0;
  float lastAngleX = 0.0;
  float lastAngleY = 0.0;
  float lastAngleZ = 0.0;
  
  bool wandMoving = false;
  float movementThreshold = 5.0;
} crossing;

// ================================
// SAFETY SYSTEMS
// ================================

struct SafetySystem {
  bool interlockEngaged = false;
  bool thermalProtection = true;
  bool powerProtection = true;
  bool radiationShielding = true;
  bool emergencyStop = false;
  bool diagnosticMode = false;
  uint8_t safetyViolations = 0;
  bool maintenanceRequired = false;
} safety;

// ================================
// TIMING VARIABLES
// ================================

unsigned long lastPhysicsUpdate = 0;
unsigned long lastVisualUpdate = 0;
unsigned long lastAudioUpdate = 0;
unsigned long lastInputUpdate = 0;
unsigned long lastStatusUpdate = 0;
unsigned long lastSafetyCheck = 0;
unsigned long lastEEPROMSave = 0;
unsigned long lastAccelerometerUpdate = 0;
unsigned long lastRumbleUpdate = 0;

unsigned long systemStartTime = 0;
unsigned long stateChangeTime = 0;

bool ionArmEngaged = false;
bool wandConnected = false;
bool safetyEngaged = false;
bool ventRequested = false;
bool diagnosticRequested = false;

unsigned long lastInputChange[6] = {0};
bool lastInputState[6] = {false};
const unsigned long DEBOUNCE_DELAY = 20;

unsigned long lastDebugOutput = 0;

uint8_t cyclotronPosition = 0;
int powerCellSequence = 0;
float bargraphLevel = 0.0;
unsigned long lastCyclotronAnimation = 0;
unsigned long lastPowerCellAnimation = 0;

bool audioEnabled = true;
bool mp3Ready = false;
int currentTrack = 0;
bool trackPlaying = false;
unsigned long trackStartTime = 0;

// ================================
// SETUP
// ================================

void setup() {
  Serial.begin(115200);
  delay(1000);
  
  Serial.println("=========================================");
  Serial.println("  ENHANCED GHOSTBUSTERS EQUIPMENT v4.0");
  Serial.println("  💾 EEPROM Storage");
  Serial.println("  🎯 Crossing Streams Detection");
  Serial.println("  🎵 Authentic Movie Sounds");
  if (TESTING_MODE) {
    Serial.println("  *** TESTING MODE ENABLED ***");
  }
  Serial.println("=========================================");
  
  EEPROM.begin(EEPROM_SIZE);
  loadDataFromEEPROM();
  Serial.println("💾 EEPROM initialized - persistent data loaded");
  
  pinMode(ION_ARM_SWITCH, INPUT_PULLUP);
  pinMode(WAND_ACTIVATE, INPUT_PULLUP);
  pinMode(FIRE_TRIGGER, INPUT_PULLUP);
  pinMode(MANUAL_VENT_BTN, INPUT_PULLUP);
  pinMode(SAFETY_INTERLOCK, INPUT_PULLUP);
  pinMode(PACK_DIAGNOSTIC, INPUT_PULLUP);
  pinMode(SMOKE_PIN, OUTPUT);
  pinMode(MP3_BUSY_PIN, INPUT);
  pinMode(RUMBLE_MOSFET_PIN, OUTPUT);
  
  ledcAttach(RUMBLE_MOSFET_PIN, RUMBLE_PWM_FREQ, RUMBLE_PWM_RESOLUTION);
  ledcWrite(RUMBLE_MOSFET_PIN, 0);
  Serial.println("🎮 Rumble motor MOSFET initialized (Pin " + String(RUMBLE_MOSFET_PIN) + ")");
  
  Wire.begin(SDA_PIN, SCL_PIN);
  
  accelerometer.initialize();
  if (accelerometer.testConnection()) {
    Serial.println("🎯 MPU6050 accelerometer initialized");
    accelerometer.setFullScaleAccelRange(MPU6050_ACCEL_FS_4);
    accelerometer.setDLPFMode(MPU6050_DLPF_BW_20);
    crossing.detectionEnabled = true;
  } else {
    Serial.println("⚠️ MPU6050 not found - crossing streams detection disabled");
    crossing.detectionEnabled = false;
  }
  
  MP3_SERIAL_PORT.begin(9600, SERIAL_8N1, MP3_RX_PIN, MP3_TX_PIN);
  delay(500);
  
  if (mp3Player.begin(MP3_SERIAL_PORT)) {
    Serial.println("🎵 DFPlayer Mini initialized");
    mp3Player.volume(20);
    mp3Player.EQ(DFPLAYER_EQ_NORMAL);
    mp3Ready = true;
    delay(500);
  } else {
    Serial.println("⚠️ DFPlayer Mini not found - using fallback audio");
    mp3Ready = false;
  }
  
  powerCell.begin();
  cyclotron.begin();
  wandTip.begin();
  wandSide.begin();
  wandBargraph.begin();
  wandStatus.begin();
  
  powerCell.setBrightness(200);
  cyclotron.setBrightness(180);
  wandTip.setBrightness(255);
  wandSide.setBrightness(160);
  wandBargraph.setBrightness(200);
  wandStatus.setBrightness(180);
  
  clearAllSystems();
  initializePhysicsEngine();
  
  for (int i = 0; i < 6; i++) {
    bool currentPinState = false;
    switch (i) {
      case 0: currentPinState = !digitalRead(ION_ARM_SWITCH); break;
      case 1: currentPinState = !digitalRead(WAND_ACTIVATE); break;
      case 2: currentPinState = !digitalRead(SAFETY_INTERLOCK); break;
      case 3: currentPinState = !digitalRead(FIRE_TRIGGER); break;
      case 4: currentPinState = !digitalRead(MANUAL_VENT_BTN); break;
      case 5: currentPinState = !digitalRead(PACK_DIAGNOSTIC); break;
    }
    lastInputState[i] = currentPinState;
    lastInputChange[i] = 0;
    switch (i) {
      case 0: ionArmEngaged = currentPinState; break;
      case 1: wandConnected = currentPinState; break;
      case 2: safetyEngaged = currentPinState; safety.interlockEngaged = !currentPinState; break;
      case 3: break;
      case 4: ventRequested = currentPinState; break;
      case 5: diagnosticRequested = currentPinState; break;
    }
  }
  
  if (ionArmEngaged && safetyEngaged && currentState == POWERED_OFF) {
    Serial.println("🔥 INPUTS ALREADY ACTIVE - STARTING DIAGNOSTIC SEQUENCE!");
    setState(SELF_DIAGNOSTIC);
  }
  
  Serial.println();
  Serial.println("🔧 INPUT STATES INITIALIZED:");
  Serial.print("Ion Arm: "); Serial.println(ionArmEngaged ? "ENGAGED" : "disengaged");
  Serial.print("Safety: "); Serial.println(safetyEngaged ? "READY" : "LOCKED");
  Serial.print("Wand: "); Serial.println(wandConnected ? "CONNECTED" : "disconnected");
  
  Serial.println();
  Serial.println("🔧 ENHANCED SYSTEM INITIALIZED");
  Serial.print("💾 Equipment Health: "); Serial.print(physics.equipmentHealth, 1); Serial.println("%");
  Serial.print("⏰ Operating Hours: "); Serial.print(physics.operatingHours / 3600); Serial.println(" hours");
  Serial.print("🔫 Total Shots Fired: "); Serial.println(physics.totalShots);
  Serial.print("🎮 Rumble Motor: "); Serial.println(rumble.enabled ? "ENABLED" : "DISABLED");
  
  Serial.println("=== INITIAL PIN STATES ===");
  Serial.print("Ion Arm (Pin 16): ");
  Serial.print(digitalRead(ION_ARM_SWITCH));
  Serial.print(" -> ");
  Serial.println(!digitalRead(ION_ARM_SWITCH) ? "ACTIVE" : "inactive");
  
  Serial.print("Safety (Pin 5): ");
  Serial.print(digitalRead(SAFETY_INTERLOCK));
  Serial.print(" -> ");
  Serial.println(!digitalRead(SAFETY_INTERLOCK) ? "DISENGAGED" : "engaged");
  
  Serial.println("Ready for ion arm activation...");
  Serial.println();
  Serial.println("💡 CROSSING STREAMS TESTING:");
  if (crossing.detectionEnabled) {
    Serial.println("   🎯 Accelerometer active - automatic detection enabled");
    Serial.println("   📐 Tilt wand while firing to trigger crossing detection");
  } else {
    Serial.println("   🧪 Manual trigger: Hold Fire + Diagnostic together while firing");
  }
  Serial.println();
  Serial.println("🎮 RUMBLE MOTOR WIRING (ESP32 Core 3.x):");
  Serial.println("   Gate → Pin 12 (PWM Direct)");
  Serial.println("   Drain → Motor (-) & Ground");
  Serial.println("   Source → Motor (+) & VCC");
  Serial.println("   💡 Use N-Channel MOSFET (e.g., IRLB8721)");
  Serial.println("=========================================");
}

// ================================
// MAIN LOOP
// ================================

void loop() {
  unsigned long currentTime = millis();
  
  if (currentTime - lastInputUpdate >= INPUT_UPDATE_INTERVAL) {
    handleOptimizedInputs();
    lastInputUpdate = currentTime;
  }
  
  if (crossing.detectionEnabled && currentTime - lastAccelerometerUpdate >= ACCELEROMETER_UPDATE_INTERVAL) {
    updateAccelerometerData();
    checkCrossingStreams();
    lastAccelerometerUpdate = currentTime;
  }
  
  if (currentTime - lastPhysicsUpdate >= PHYSICS_UPDATE_INTERVAL) {
    updateOptimizedPhysics();
    lastPhysicsUpdate = currentTime;
  }
  
  if (currentTime - lastVisualUpdate >= VISUAL_UPDATE_INTERVAL) {
    updateOptimizedVisuals();
    lastVisualUpdate = currentTime;
  }
  
  if (currentTime - lastRumbleUpdate >= RUMBLE_UPDATE_INTERVAL) {
    updateRumbleSystem();
    lastRumbleUpdate = currentTime;
  }
  
  if (currentTime - lastAudioUpdate >= AUDIO_UPDATE_INTERVAL) {
    updateMP3Audio();
    lastAudioUpdate = currentTime;
  }
  
  if (currentTime - lastSafetyCheck >= SAFETY_CHECK_INTERVAL) {
    updateOptimizedSafety();
    lastSafetyCheck = currentTime;
  }
  
  if (currentTime - lastEEPROMSave >= EEPROM_SAVE_INTERVAL) {
    saveDataToEEPROM();
    lastEEPROMSave = currentTime;
  }
  
  if (currentTime - lastStatusUpdate >= STATUS_UPDATE_INTERVAL) {
    reportOptimizedStatus();
    lastStatusUpdate = currentTime;
  }
  
  updateOptimizedStateMachine();
}

// ================================
// EEPROM PERSISTENCE SYSTEM
// ================================

void loadDataFromEEPROM() {
  byte checksum = EEPROM.read(ADDR_CHECKSUM);
  byte calculatedChecksum = calculateEEPROMChecksum();
  
  if (checksum == calculatedChecksum && checksum != 0xFF) {
    EEPROM.get(ADDR_EQUIPMENT_HEALTH, physics.equipmentHealth);
    EEPROM.get(ADDR_OPERATING_HOURS, physics.operatingHours);
    EEPROM.get(ADDR_TOTAL_SHOTS, physics.totalShots);
    physics.requiresMaintenance = EEPROM.read(ADDR_MAINTENANCE_FLAG);
    
    Serial.println("💾 Restored equipment data from EEPROM");
  } else {
    physics.equipmentHealth = 100.0;
    physics.operatingHours = 0;
    physics.totalShots = 0;
    physics.requiresMaintenance = false;
    
    Serial.println("💾 EEPROM empty - initialized with defaults");
    saveDataToEEPROM();
  }
}

void saveDataToEEPROM() {
  EEPROM.put(ADDR_EQUIPMENT_HEALTH, physics.equipmentHealth);
  EEPROM.put(ADDR_OPERATING_HOURS, physics.operatingHours);
  EEPROM.put(ADDR_TOTAL_SHOTS, physics.totalShots);
  EEPROM.write(ADDR_MAINTENANCE_FLAG, physics.requiresMaintenance);
  
  unsigned long currentTime = millis();
  EEPROM.put(ADDR_LAST_SAVE_TIME, currentTime);
  
  byte checksum = calculateEEPROMChecksum();
  EEPROM.write(ADDR_CHECKSUM, checksum);
  
  EEPROM.commit();
  
  static unsigned long lastSaveMessage = 0;
  if (millis() - lastSaveMessage > 60000) {
    Serial.println("💾 Equipment data saved to EEPROM");
    lastSaveMessage = millis();
  }
}

byte calculateEEPROMChecksum() {
  byte checksum = 0;
  for (int i = 0; i < ADDR_CHECKSUM; i++) {
    checksum ^= EEPROM.read(i);
  }
  return checksum;
}

// ================================
// ACCELEROMETER & CROSSING STREAMS
// ================================

void updateAccelerometerData() {
  if (!crossing.detectionEnabled) return;
  
  int16_t ax, ay, az;
  int16_t gx, gy, gz;
  
  accelerometer.getAcceleration(&ax, &ay, &az);
  accelerometer.getRotation(&gx, &gy, &gz);
  
  if (ax == 0 && ay == 0 && az == 0) {
    static unsigned long lastErrorReport = 0;
    if (millis() - lastErrorReport > 10000) {
      Serial.println("⚠️ Accelerometer read error - crossing detection may be unreliable");
      lastErrorReport = millis();
    }
    return;
  }
  
  float accelX = ax / 8192.0;
  float accelY = ay / 8192.0; 
  float accelZ = az / 8192.0;
  
  crossing.lastAngleX = crossing.wandAngleX;
  crossing.lastAngleY = crossing.wandAngleY;
  crossing.lastAngleZ = crossing.wandAngleZ;
  
  crossing.wandAngleX = atan2(accelY, accelZ) * 180.0 / PI;
  crossing.wandAngleY = atan2(accelX, accelZ) * 180.0 / PI;
  crossing.wandAngleZ = atan2(accelX, accelY) * 180.0 / PI;
  
  float angleChangeX = abs(crossing.wandAngleX - crossing.lastAngleX);
  float angleChangeY = abs(crossing.wandAngleY - crossing.lastAngleY);
  float angleChangeZ = abs(crossing.wandAngleZ - crossing.lastAngleZ);
  
  float totalMovement = angleChangeX + angleChangeY + angleChangeZ;
  crossing.wandMoving = (totalMovement > crossing.movementThreshold);
}

void checkCrossingStreams() {
  if (!crossing.detectionEnabled || currentState != STREAM_ACTIVE) {
    crossing.streamsDetected = false;
    crossing.warningIssued = false;
    crossing.criticalState = false;
    return;
  }
  
  bool potentialCrossing = false;
  
  if (crossing.detectionEnabled) {
    if (abs(crossing.wandAngleX) > 30.0 && abs(crossing.wandAngleX) < 150.0) {
      if (abs(crossing.wandAngleY) > 30.0 && abs(crossing.wandAngleY) < 150.0) {
        potentialCrossing = true;
      }
    }
    
    if (crossing.wandMoving && currentState == STREAM_ACTIVE) {
      potentialCrossing = true;
    }
  }
  
  if (!digitalRead(FIRE_TRIGGER) && !digitalRead(PACK_DIAGNOSTIC)) {
    potentialCrossing = true;
    Serial.println("🧪 MANUAL CROSSING STREAMS TRIGGER ACTIVATED");
  }
  
  if (potentialCrossing && !crossing.streamsDetected) {
    crossing.streamsDetected = true;
    crossing.crossingStartTime = millis();
    crossing.crossingAngle = abs(crossing.wandAngleX) + abs(crossing.wandAngleY);
    
    Serial.println("⚠️ POTENTIAL STREAM CROSSING DETECTED!");
    if (crossing.detectionEnabled) {
      Serial.print("Wand angles: X=");
      Serial.print(crossing.wandAngleX, 1);
      Serial.print("°, Y=");
      Serial.print(crossing.wandAngleY, 1);
      Serial.println("°");
    }
    
  } else if (crossing.streamsDetected && potentialCrossing) {
    unsigned long crossingDuration = millis() - crossing.crossingStartTime;
    
    if (crossingDuration > 2000 && !crossing.warningIssued) {
      crossing.warningIssued = true;
      setState(CROSSING_STREAMS_WARNING);
      Serial.println("🚨 DON'T CROSS THE STREAMS!");
      
    } else if (crossingDuration > 5000 && !crossing.criticalState) {
      crossing.criticalState = true;
      setState(CROSSING_STREAMS_CRITICAL);
      Serial.println("🚨💥 CRITICAL CROSSING STREAMS EVENT!");
    }
    
  } else if (crossing.streamsDetected && !potentialCrossing) {
    crossing.streamsDetected = false;
    crossing.warningIssued = false;
    crossing.criticalState = false;
    
    if (currentState == CROSSING_STREAMS_WARNING || currentState == CROSSING_STREAMS_CRITICAL) {
      setState(STREAM_ACTIVE);
    }
    
    Serial.println("✅ Stream crossing resolved");
  }
}

// ================================
// ENHANCED MP3 AUDIO SYSTEM
// ================================

void updateMP3Audio() {
  if (mp3Ready && trackPlaying) {
    bool busy = digitalRead(MP3_BUSY_PIN);
    if (!busy && millis() - trackStartTime > 1000) {
      trackPlaying = false;
      currentTrack = 0;
    }
  }
}

void playMP3Track(int trackNumber, bool loop = false) {
  if (!mp3Ready) return;
  
  if (currentTrack != trackNumber || !trackPlaying) {
    currentTrack = trackNumber;
    trackStartTime = millis();
    trackPlaying = true;
    
    if (loop) {
      mp3Player.loop(trackNumber);
    } else {
      mp3Player.play(trackNumber);
    }
    
    Serial.print("🎵 Playing track ");
    Serial.print(trackNumber);
    Serial.print(": ");
    Serial.println(getTrackName(trackNumber));
  }
}

void stopMP3() {
  if (mp3Ready && trackPlaying) {
    mp3Player.stop();
    trackPlaying = false;
    currentTrack = 0;
  }
}

String getTrackName(int track) {
  switch (track) {
    case TRACK_STARTUP: return "Startup";
    case TRACK_ION_ARM_CHARGE: return "Ion Arm Charging";
    case TRACK_WAND_SYNC: return "Wand Sync";
    case TRACK_READY: return "System Ready";
    case TRACK_STREAM_CHARGE: return "Stream Charging";
    case TRACK_STREAM_FIRE: return "Proton Stream";
    case TRACK_STREAM_LOOP: return "Firing Loop";
    case TRACK_VENT_MANUAL: return "Manual Vent";
    case TRACK_VENT_EMERGENCY: return "Emergency Vent";
    case TRACK_CROSSING_WARNING: return "Crossing Warning";
    case TRACK_CROSSING_EXPLOSION: return "Crossing Explosion";
    case TRACK_SHUTDOWN: return "Shutdown";
    case TRACK_MALFUNCTION: return "Malfunction";
    case TRACK_LOW_BATTERY: return "Low Battery";
    default: return "Unknown";
  }
}

// ================================
// PWM HELPER FUNCTION (ESP32 Core 3.x)
// ================================

void writeRumblePWM(uint8_t value) {
  ledcWrite(RUMBLE_MOSFET_PIN, value);
}

// ================================
// ENHANCED RUMBLE HAPTIC SYSTEM (ESP32 Core 3.x)
// ================================

void updateRumbleSystem() {
  if (!rumble.enabled) {
    writeRumblePWM(0);
    return;
  }
  
  // Handle override patterns (for instant feedback)
  if (rumble.overrideActive) {
    if (millis() - rumble.overrideStartTime > rumble.overrideDuration) {
      rumble.overrideActive = false;
      rumble.targetIntensity = rumble.baseIntensity;
    }
  }
  
  // Update based on current state
  switch (currentState) {
    case POWERED_OFF:
      rumble.targetIntensity = 0;
      break;
      
    case SELF_DIAGNOSTIC:
      // Diagnostic pulse pattern
      if (millis() % 1000 < 100) {
        rumble.targetIntensity = 60;
      } else {
        rumble.targetIntensity = 0;
      }
      break;
      
    case ION_ARM_CHARGING: {
      // Gentle ramping vibration
      float voltageRange = (physics.packVoltage * 10 - 120) / (148 - 120); // Normalize 0-1
      rumble.targetIntensity = (uint8_t)(20 + (voltageRange * 60)); // Map to 20-80
      break;
    }
      
    case STREAM_CHARGING: {
      // Intensifying buzz
      unsigned long chargeTime = millis() - stateChangeTime;
      float chargeProgress = (float)chargeTime / STREAM_CHARGE_TIME;
      rumble.targetIntensity = (uint8_t)(80 + (chargeProgress * 80)); // Map to 80-160
      rumble.targetIntensity = constrain(rumble.targetIntensity, 80, 160);
      break;
    }
      
    case STREAM_ACTIVE:
      // Intense firing vibration with physics feedback
      rumble.targetIntensity = (uint8_t)(180 + (physics.streamPower * 0.3));
      // Add heat-based intensity
      if (physics.coreTemperature > 70.0) {
        rumble.targetIntensity += 20;
      }
      // Add instability chaos
      if (physics.streamStability < 80.0) {
        if (millis() % 200 < 50) {
          rumble.targetIntensity += random(0, 40);
        }
      }
      rumble.targetIntensity = constrain(rumble.targetIntensity, 0, 255);
      break;
      
    case THERMAL_WARNING:
      // Warning pulse pattern
      if (millis() % 500 < 250) {
        rumble.targetIntensity = 100;
      } else {
        rumble.targetIntensity = 30;
      }
      break;
      
    case THERMAL_CRITICAL:
    case EMERGENCY_VENT:
      // Rapid danger pulses
      if (millis() % 200 < 100) {
        rumble.targetIntensity = 200;
      } else {
        rumble.targetIntensity = 0;
      }
      break;
      
    case CROSSING_STREAMS_WARNING:
      // Escalating warning vibration
      rumble.targetIntensity = (uint8_t)(150 + sin(millis() * 0.01) * 50);
      break;
      
    case CROSSING_STREAMS_CRITICAL:
      // Maximum chaos
      rumble.targetIntensity = 255;
      if (millis() % 100 < 50) {
        rumble.targetIntensity = (uint8_t)random(200, 255);
      }
      break;
      
    default:
      rumble.targetIntensity = rumble.baseIntensity;
      break;
  }
  
  // Smooth intensity transitions
  if (rumble.currentIntensity < rumble.targetIntensity) {
    rumble.currentIntensity += 3;
    if (rumble.currentIntensity > rumble.targetIntensity) {
      rumble.currentIntensity = rumble.targetIntensity;
    }
  } else if (rumble.currentIntensity > rumble.targetIntensity) {
    rumble.currentIntensity -= 2;
    if (rumble.currentIntensity < rumble.targetIntensity) {
      rumble.currentIntensity = rumble.targetIntensity;
    }
  }
  
  // Apply intensity limits
  rumble.currentIntensity = constrain(rumble.currentIntensity, 0, rumble.maxIntensity);
  
  // Write to motor
  writeRumblePWM(rumble.currentIntensity);
}

void triggerRumbleOverride(uint8_t intensity, unsigned long duration) {
  rumble.overrideActive = true;
  rumble.overrideStartTime = millis();
  rumble.overrideDuration = duration;
  rumble.targetIntensity = intensity;
}

// ================================
// PHYSICS ENGINE - ENHANCED
// ================================

void initializePhysicsEngine() {
  physics.coreTemperature = AMBIENT_TEMP;
  physics.cyclotronTemp = AMBIENT_TEMP;
  physics.wandTemp = AMBIENT_TEMP;
  physics.packVoltage = PACK_VOLTAGE;
  physics.currentRPM = 0.0;
  physics.streamPower = 0.0;
  physics.streamStability = 100.0;
  
  // Initialize with some realistic wear if this is an older pack
  if (physics.operatingHours > 1000) {
    physics.equipmentHealth = max(60.0f, 100.0f - (physics.operatingHours / 100.0f));
  }
}

void updateOptimizedPhysics() {
  unsigned long deltaTime = millis() - lastPhysicsUpdate;
  float deltaSeconds = deltaTime / 1000.0;
  
  // Update operating hours
  if (currentState != POWERED_OFF) {
    physics.operatingHours += deltaTime;
  }
  
  // Core temperature simulation
  float targetTemp = AMBIENT_TEMP;
  float heatGeneration = 0.0;
  
  // Heat sources
  if (currentState == STREAM_ACTIVE) {
    heatGeneration += FIRING_HEAT_RATE * physics.streamPower / 100.0;
  }
  if (physics.currentRPM > CYCLOTRON_MIN_RPM) {
    heatGeneration += (physics.currentRPM / CYCLOTRON_MAX_RPM) * 1.2;
  }
  
  targetTemp = AMBIENT_TEMP + heatGeneration * 10.0;
  
  // Heat dissipation
  float coolingRate = HEAT_DISSIPATION;
  if (currentState == EMERGENCY_VENT) {
    coolingRate *= 3.0; // Venting cools faster
  }
  
  physics.coreTemperature += (targetTemp - physics.coreTemperature) * coolingRate * deltaSeconds;
  physics.coreTemperature = max((float)AMBIENT_TEMP, physics.coreTemperature);
  
  // Cyclotron RPM physics
  if (currentState >= ION_ARM_CHARGING && currentState <= STREAM_ACTIVE) {
    if (currentState == STREAM_ACTIVE) {
      physics.targetRPM = CYCLOTRON_MAX_RPM;
    } else {
      physics.targetRPM = CYCLOTRON_MIN_RPM + (physics.packVoltage - MIN_VOLTAGE) * 50;
    }
    
    // Accelerate cyclotron
    if (physics.currentRPM < physics.targetRPM) {
      physics.currentRPM += CYCLOTRON_ACCELERATION * deltaTime;
    } else if (physics.currentRPM > physics.targetRPM) {
      physics.currentRPM -= CYCLOTRON_ACCELERATION * deltaTime * 0.5;
    }
  } else {
    // Decelerate when not operational
    physics.currentRPM -= CYCLOTRON_ACCELERATION * deltaTime * 2.0;
  }
  
  physics.currentRPM = constrain(physics.currentRPM, 0, CYCLOTRON_MAX_RPM);
  physics.cyclotronStable = (abs(physics.currentRPM - physics.targetRPM) < 50.0);
  
  // Power system simulation
  if (currentState == STREAM_ACTIVE) {
    physics.packVoltage -= VOLTAGE_DROP_RATE * physics.streamPower * deltaSeconds;
  } else if (currentState != POWERED_OFF) {
    physics.packVoltage -= VOLTAGE_DROP_RATE * 0.1 * deltaSeconds; // Idle consumption
  }
  
  // Voltage recovery when not firing
  if (currentState != STREAM_ACTIVE && physics.packVoltage < PACK_VOLTAGE) {
    physics.packVoltage += CHARGING_RECOVERY * deltaSeconds;
  }
  
  physics.packVoltage = constrain(physics.packVoltage, MIN_VOLTAGE - 2.0, PACK_VOLTAGE);
  physics.powerStable = (physics.packVoltage > MIN_VOLTAGE);
  
  // Stream power physics
  if (currentState == STREAM_CHARGING) {
    unsigned long chargeTime = millis() - stateChangeTime;
    physics.streamPower = (float)(chargeTime * 100) / STREAM_CHARGE_TIME;
    physics.streamPower = constrain(physics.streamPower, 0.0, 100.0);
  } else if (currentState == STREAM_ACTIVE) {
    // Maintain power but add instability over time
    unsigned long fireTime = millis() - stateChangeTime;
    physics.streamStability = max(20.0f, 100.0f - (fireTime / 120.0f));
    
    // Heat affects stability
    if (physics.coreTemperature > 70.0) {
      physics.streamStability -= (physics.coreTemperature - 70.0) * 0.5;
    }
    
    // Power fluctuation based on stability
    if (physics.streamStability < 50.0) {
      physics.streamPower = 100.0 + (float)random(-20, 10);
    }
  } else {
    physics.streamPower = 0.0;
    physics.streamStability = 100.0;
  }
  
  // Equipment degradation
  if (currentState == STREAM_ACTIVE) {
    physics.equipmentHealth -= 0.001 * deltaSeconds;
    if (physics.coreTemperature > MAX_SAFE_TEMP) {
      physics.equipmentHealth -= 0.005 * deltaSeconds;
    }
  }
  
  physics.equipmentHealth = constrain(physics.equipmentHealth, 0.0, 100.0);
  physics.requiresMaintenance = (physics.equipmentHealth < 25.0);
}

// ================================
// INPUT HANDLING - ENHANCED
// ================================

void handleOptimizedInputs() {
  // Read all inputs with debouncing
  bool ionArmPressed = readDebounced(ION_ARM_SWITCH, 0);
  bool wandPressed = readDebounced(WAND_ACTIVATE, 1);
  bool safetyPressed = readDebounced(SAFETY_INTERLOCK, 2);
  bool firePressed = readDebounced(FIRE_TRIGGER, 3);
  bool ventPressed = readDebounced(MANUAL_VENT_BTN, 4);
  bool diagPressed = readDebounced(PACK_DIAGNOSTIC, 5);
  
  // Update system states
  ionArmEngaged = ionArmPressed;
  wandConnected = wandPressed;
  safetyEngaged = safetyPressed;
  safety.interlockEngaged = !safetyPressed;
  ventRequested = ventPressed;
  diagnosticRequested = diagPressed;
  
  // Handle state transitions based on inputs
  handleStateTriggers(firePressed, ventPressed, diagPressed);
}

bool readDebounced(int pin, int index) {
  bool currentState = !digitalRead(pin); // Active low
  unsigned long currentTime = millis();
  
  if (currentState != lastInputState[index]) {
    lastInputChange[index] = currentTime;
  }
  
  if ((currentTime - lastInputChange[index]) > DEBOUNCE_DELAY) {
    if (currentState != lastInputState[index]) {
      lastInputState[index] = currentState;
      return currentState;
    }
  }
  
  return lastInputState[index];
}

void handleStateTriggers(bool firePressed, bool ventPressed, bool diagPressed) {
  // Emergency vent always takes priority
  if (ventPressed && currentState != EMERGENCY_VENT) {
    setState(EMERGENCY_VENT);
    triggerRumbleOverride(200, 500);
    return;
  }
  
  // Fire trigger handling
  static bool lastFireState = false;
  if (firePressed && !lastFireState) {
    // Fire button pressed
    if (currentState == IDLE_OPERATIONAL) {
      setState(STREAM_CHARGING);
      physics.totalShots++;
      triggerRumbleOverride(120, 200);
    }
  } else if (!firePressed && lastFireState) {
    // Fire button released
    if (currentState == STREAM_CHARGING || currentState == STREAM_ACTIVE) {
      setState(IDLE_OPERATIONAL);
    }
  }
  lastFireState = firePressed;
  
  // Diagnostic mode
  if (diagPressed && currentState == IDLE_OPERATIONAL) {
    safety.diagnosticMode = true;
  } else {
    safety.diagnosticMode = false;
  }
}

// ================================
// STATE MACHINE - ENHANCED
// ================================

void updateOptimizedStateMachine() {
  unsigned long stateTime = millis() - stateChangeTime;
  
  switch (currentState) {
    case POWERED_OFF:
      if (ionArmEngaged && safetyEngaged) {
        setState(SELF_DIAGNOSTIC);
      }
      break;
      
    case SELF_DIAGNOSTIC:
      if (stateTime > 3000) {
        setState(ION_ARM_CHARGING);
      }
      break;
      
    case ION_ARM_CHARGING:
      if (stateTime > 4000 && physics.cyclotronStable) {
        setState(WAND_SYNC);
      }
      break;
      
    case WAND_SYNC:
      if (stateTime > 2000) {
        setState(IDLE_OPERATIONAL);
      }
      break;
      
    case STREAM_CHARGING:
      if (stateTime > STREAM_CHARGE_TIME) {
        setState(STREAM_ACTIVE);
      }
      break;
      
    case STREAM_ACTIVE:
      // Check for overheating
      if (physics.coreTemperature > CRITICAL_TEMP) {
        setState(THERMAL_CRITICAL);
      } else if (physics.coreTemperature > MAX_SAFE_TEMP) {
        setState(THERMAL_WARNING);
      }
      
      // Check for power issues
      if (!physics.powerStable) {
        setState(MALFUNCTION);
      }
      break;
      
    case EMERGENCY_VENT:
      if (stateTime > 5000) {
        setState(IDLE_OPERATIONAL);
      }
      break;
      
    case CROSSING_STREAMS_WARNING:
      // Handled by crossing streams detection
      break;
      
    case CROSSING_STREAMS_CRITICAL:
      // Simulate explosion effect for 3 seconds
      if (stateTime > 3000) {
        setState(IDLE_OPERATIONAL);
      }
      break;
  }
  
  // Global safety checks
  if (!ionArmEngaged || !safetyEngaged) {
    if (currentState != POWERED_OFF) {
      setState(POWERED_OFF);
    }
  }
}

void setState(SystemState newState) {
  if (newState == currentState) return;
  
  previousState = currentState;
  currentState = newState;
  stateChangeTime = millis();
  
  // Audio triggers for state changes
  switch (newState) {
    case SELF_DIAGNOSTIC:
      playMP3Track(TRACK_STARTUP);
      break;
    case ION_ARM_CHARGING:
      playMP3Track(TRACK_ION_ARM_CHARGE);
      break;
    case WAND_SYNC:
      playMP3Track(TRACK_WAND_SYNC);
      break;
    case IDLE_OPERATIONAL:
      playMP3Track(TRACK_READY);
      break;
    case STREAM_CHARGING:
      playMP3Track(TRACK_STREAM_CHARGE);
      break;
    case STREAM_ACTIVE:
      playMP3Track(TRACK_STREAM_FIRE, true);
      break;
    case EMERGENCY_VENT:
      stopMP3();
      playMP3Track(TRACK_VENT_EMERGENCY);
      break;
    case CROSSING_STREAMS_WARNING:
      playMP3Track(TRACK_CROSSING_WARNING);
      break;
    case CROSSING_STREAMS_CRITICAL:
      playMP3Track(TRACK_CROSSING_EXPLOSION);
      break;
    case POWERED_OFF:
      stopMP3();
      break;
  }
  
  Serial.print("🔄 STATE: ");
  Serial.print(getStateName(previousState));
  Serial.print(" → ");
  Serial.println(getStateName(currentState));
}

// ================================
// VISUAL EFFECTS - ENHANCED
// ================================

void updateOptimizedVisuals() {
  clearAllSystems();
  
  switch (currentState) {
    case POWERED_OFF:
      // All systems dark
      break;
      
    case SELF_DIAGNOSTIC:
      animateDiagnosticSequence();
      break;
      
    case ION_ARM_CHARGING:
      animatePowerCell();
      animateChargingCyclotron();
      break;
      
    case WAND_SYNC:
      animatePowerCell();
      animateCyclotron();
      animateWandSync();
      break;
      
    case IDLE_OPERATIONAL:
      animatePowerCell();
      animateCyclotron();
      animateWandIdle();
      break;
      
    case STREAM_CHARGING:
      animatePowerCell();
      animateCyclotron();
      animateWandCharging();
      break;
      
    case STREAM_ACTIVE:
      animatePowerCell();
      animateCyclotron();
      animateWandFiring();
      break;
      
    case THERMAL_WARNING:
      animateOverheatWarning();
      break;
      
    case THERMAL_CRITICAL:
      animateOverheatCritical();
      break;
      
    case EMERGENCY_VENT:
      animateEmergencyVent();
      break;
      
    case CROSSING_STREAMS_WARNING:
      animateCrossingWarning();
      break;
      
    case CROSSING_STREAMS_CRITICAL:
      animateCrossingCritical();
      break;
      
    case MALFUNCTION:
      animateMalfunction();
      break;
  }
  
  // Update all strips
  powerCell.show();
  cyclotron.show();
  wandTip.show();
  wandSide.show();
  wandBargraph.show();
  wandStatus.show();
}

void animateDiagnosticSequence() {
  unsigned long stateTime = millis() - stateChangeTime;
  uint8_t brightness = 50 + ((stateTime % 1000) * 205 / 999); // Map 0-999 to 50-255
  
  // Sweep through power cell
  int activeCell = (stateTime / 200) % POWER_CELL_LEDS;
  for (int i = 0; i < POWER_CELL_LEDS; i++) {
    if (i == activeCell) {
      powerCell.setPixelColor(i, powerCell.Color(brightness, brightness, 0));
    }
  }
  
  // Simple cyclotron rotation
  int activeSegment = (stateTime / 100) % CYCLOTRON_LEDS;
  cyclotron.setPixelColor(activeSegment, cyclotron.Color(brightness, 0, 0));
}

void animatePowerCell() {
  // Classic 15-segment power cell animation
  unsigned long animTime = millis();
  int activeCells = (int)((physics.packVoltage * 10 - 120) / (148 - 120) * POWER_CELL_LEDS);
  activeCells = constrain(activeCells, 1, POWER_CELL_LEDS);
  
  for (int i = 0; i < POWER_CELL_LEDS; i++) {
    if (i < activeCells) {
      uint8_t brightness = 200;
      // Add slight flicker for realism
      if (random(100) < 5) brightness -= (uint8_t)random(0, 50);
      
      if (i < activeCells * 0.3) {
        powerCell.setPixelColor(i, powerCell.Color(0, brightness, brightness)); // Cyan
      } else if (i < activeCells * 0.7) {
        powerCell.setPixelColor(i, powerCell.Color(0, brightness, 0)); // Green
      } else {
        powerCell.setPixelColor(i, powerCell.Color(brightness, brightness, 0)); // Yellow
      }
    }
  }
}

void animateChargingCyclotron() {
  // Slow charge-up animation
  unsigned long stateTime = millis() - stateChangeTime;
  float progress = min(1.0f, (float)stateTime / 4000.0f);
  
  int activeSegments = (int)(progress * CYCLOTRON_LEDS);
  uint8_t brightness = (uint8_t)(100 + (progress * 155));
  
  for (int i = 0; i < activeSegments; i++) {
    cyclotron.setPixelColor(i, cyclotron.Color(brightness, 0, 0));
  }
}

void animateCyclotron() {
  // Physics-based cyclotron rotation
  float rpm = physics.currentRPM;
  float rotationSpeed = (rpm / CYCLOTRON_MAX_RPM) * 50.0; // Speed factor (0-50)
  
  unsigned long animTime = millis();
  cyclotronPosition = ((unsigned long)(animTime / (100.0 - rotationSpeed))) % CYCLOTRON_LEDS;
  
  // Heat-based color
  uint8_t red = 150;
  uint8_t green = 0;
  uint8_t blue = 0;
  
  if (physics.cyclotronTemp > CYCLOTRON_WARM_TEMP) {
    red = 255;
    float tempRange = (physics.cyclotronTemp - CYCLOTRON_WARM_TEMP) / (CYCLOTRON_HOT_TEMP - CYCLOTRON_WARM_TEMP);
    green = (uint8_t)(tempRange * 100);
    green = constrain(green, 0, 100);
  }
  
  // Draw cyclotron segments
  for (int i = 0; i < 4; i++) {
    int segmentStart = (cyclotronPosition + i * (CYCLOTRON_LEDS / 4)) % CYCLOTRON_LEDS;
    int segmentSize = CYCLOTRON_LEDS / 8;
    
    for (int j = 0; j < segmentSize; j++) {
      int pixelIndex = (segmentStart + j) % CYCLOTRON_LEDS;
      uint8_t brightness = (uint8_t)(map(j, 0, segmentSize - 1, 255, 50));
      cyclotron.setPixelColor(pixelIndex, cyclotron.Color(
        (red * brightness) / 255,
        (green * brightness) / 255,
        (blue * brightness) / 255
      ));
    }
  }
}

void animateWandSync() {
  unsigned long stateTime = millis() - stateChangeTime;
  uint8_t brightness = (uint8_t)((sin(stateTime * 0.01) + 1) * 127);
  
  // All wand LEDs pulse in sync
  for (int i = 0; i < WAND_TIP_LEDS; i++) {
    wandTip.setPixelColor(i, wandTip.Color(0, 0, brightness));
  }
  
  wandStatus.setPixelColor(0, wandStatus.Color(brightness, brightness, 0));
}

void animateWandIdle() {
  // Gentle blue glow
  for (int i = 0; i < WAND_TIP_LEDS; i++) {
    wandTip.setPixelColor(i, wandTip.Color(0, 0, 80));
  }
  
  // Status indicator
  wandStatus.setPixelColor(0, wandStatus.Color(0, 100, 0));
  
  // Bargraph shows power level
  int bars = (int)((physics.packVoltage * 10 - 120) / (148 - 120) * WAND_BARGRAPH_LEDS);
  bars = constrain(bars, 0, WAND_BARGRAPH_LEDS);
  for (int i = 0; i < WAND_BARGRAPH_LEDS; i++) {
    if (i < bars) {
      if (i < 3) {
        wandBargraph.setPixelColor(i, wandBargraph.Color(0, 150, 0)); // Green
      } else if (i < 7) {
        wandBargraph.setPixelColor(i, wandBargraph.Color(150, 150, 0)); // Yellow
      } else {
        wandBargraph.setPixelColor(i, wandBargraph.Color(150, 0, 0)); // Red
      }
    }
  }
}

void animateWandCharging() {
  unsigned long stateTime = millis() - stateChangeTime;
  float progress = min(1.0f, (float)stateTime / (float)STREAM_CHARGE_TIME);
  
  // Wand tip builds up energy
  int activeTips = (int)(progress * WAND_TIP_LEDS);
  for (int i = 0; i < activeTips; i++) {
    uint8_t brightness = (uint8_t)(100 + (progress * 155));
    wandTip.setPixelColor(i, wandTip.Color(brightness, brightness, brightness));
  }
  
  // Status shows charging
  uint8_t pulseRate = 20 - (progress * 15); // Map progress 0-1 to rate 20-5
  pulseRate = constrain(pulseRate, 5, 20);
  uint8_t brightness = (millis() / pulseRate) % 2 ? 255 : 0;
  wandStatus.setPixelColor(0, wandStatus.Color(brightness, brightness, 0));
  
  // Bargraph fills up
  int bars = (int)(progress * WAND_BARGRAPH_LEDS);
  for (int i = 0; i < bars; i++) {
    wandBargraph.setPixelColor(i, wandBargraph.Color(255, 100, 0));
  }
}

void animateWandFiring() {
  // Intense white/blue stream
  for (int i = 0; i < WAND_TIP_LEDS; i++) {
    uint8_t flicker = (uint8_t)random(200, 255);
    wandTip.setPixelColor(i, wandTip.Color(flicker, flicker, 255));
  }
  
  // Side thermal warning
  uint8_t thermalLevel = (uint8_t)((physics.coreTemperature - AMBIENT_TEMP) / (MAX_SAFE_TEMP - AMBIENT_TEMP) * 255);
  thermalLevel = constrain(thermalLevel, 0, 255);
  for (int i = 0; i < WAND_SIDE_LEDS; i++) {
    wandSide.setPixelColor(i, wandSide.Color(thermalLevel, 0, 0));
  }
  
  // Status shows active firing
  wandStatus.setPixelColor(0, wandStatus.Color(255, 0, 0));
  
  // Bargraph shows power fluctuation
  for (int i = 0; i < WAND_BARGRAPH_LEDS; i++) {
    uint8_t flicker = (uint8_t)random(150, 255);
    wandBargraph.setPixelColor(i, wandBargraph.Color(flicker, 0, flicker));
  }
}

void animateOverheatWarning() {
  uint8_t brightness = (millis() / 250) % 2 ? 255 : 100;
  
  // Flash everything orange/red
  for (int i = 0; i < POWER_CELL_LEDS; i++) {
    powerCell.setPixelColor(i, powerCell.Color(brightness, brightness/2, 0));
  }
  
  for (int i = 0; i < CYCLOTRON_LEDS; i++) {
    cyclotron.setPixelColor(i, cyclotron.Color(brightness, brightness/4, 0));
  }
}

void animateOverheatCritical() {
  uint8_t brightness = (millis() / 100) % 2 ? 255 : 0;
  
  // Rapid red flashing
  for (int i = 0; i < POWER_CELL_LEDS; i++) {
    powerCell.setPixelColor(i, powerCell.Color(brightness, 0, 0));
  }
  
  for (int i = 0; i < CYCLOTRON_LEDS; i++) {
    cyclotron.setPixelColor(i, cyclotron.Color(brightness, 0, 0));
  }
}

void animateEmergencyVent() {
  // Blue cooling vents
  uint8_t brightness = 50 + ((millis() % 1000) * 205 / 999); // Map 0-999 to 50-255
  
  for (int i = 0; i < POWER_CELL_LEDS; i++) {
    powerCell.setPixelColor(i, powerCell.Color(0, 0, brightness));
  }
  
  // Activate smoke effect
  digitalWrite(SMOKE_PIN, HIGH);
}

void animateCrossingWarning() {
  // Alternating yellow/orange warning
  uint8_t phase = (millis() / 300) % 2;
  uint8_t brightness = 255;
  
  for (int i = 0; i < CYCLOTRON_LEDS; i++) {
    if ((i + phase) % 2) {
      cyclotron.setPixelColor(i, cyclotron.Color(brightness, brightness, 0));
    } else {
      cyclotron.setPixelColor(i, cyclotron.Color(brightness, brightness/2, 0));
    }
  }
}

void animateCrossingCritical() {
  // Chaotic multicolor explosion effect
  for (int i = 0; i < POWER_CELL_LEDS; i++) {
    powerCell.setPixelColor(i, powerCell.Color((uint8_t)random(0, 255), (uint8_t)random(0, 255), (uint8_t)random(0, 255)));
  }
  
  for (int i = 0; i < CYCLOTRON_LEDS; i++) {
    cyclotron.setPixelColor(i, cyclotron.Color((uint8_t)random(0, 255), (uint8_t)random(0, 255), (uint8_t)random(0, 255)));
  }
  
  for (int i = 0; i < WAND_TIP_LEDS; i++) {
    wandTip.setPixelColor(i, wandTip.Color((uint8_t)random(0, 255), (uint8_t)random(0, 255), (uint8_t)random(0, 255)));
  }
}

void animateMalfunction() {
  // Erratic red/yellow glitching
  unsigned long time = millis();
  bool glitch = (time / 50) % 3 == 0;
  
  if (glitch) {
    for (int i = 0; i < POWER_CELL_LEDS; i++) {
      if (random(2)) {
        powerCell.setPixelColor(i, powerCell.Color(255, (uint8_t)random(0, 100), 0));
      }
    }
  }
}

// ================================
// SAFETY SYSTEM - ENHANCED
// ================================

void updateOptimizedSafety() {
  // Reset safety violations counter periodically
  static unsigned long lastSafetyReset = 0;
  if (millis() - lastSafetyReset > 60000) {
    safety.safetyViolations = 0;
    lastSafetyReset = millis();
  }
  
  // Temperature safety checks
  if (physics.coreTemperature > EMERGENCY_TEMP) {
    triggerSafetyViolation("EMERGENCY TEMPERATURE EXCEEDED");
    if (currentState != EMERGENCY_VENT) {
      setState(EMERGENCY_VENT);
    }
  } else if (physics.coreTemperature > CRITICAL_TEMP) {
    if (currentState == STREAM_ACTIVE) {
      setState(THERMAL_CRITICAL);
    }
  }
  
  // Power safety checks
  if (physics.packVoltage < MIN_VOLTAGE && currentState != POWERED_OFF) {
    triggerSafetyViolation("POWER VOLTAGE CRITICALLY LOW");
    setState(MALFUNCTION);
  }
  
  // Equipment health checks
  if (physics.equipmentHealth < 10.0) {
    triggerSafetyViolation("EQUIPMENT HEALTH CRITICAL");
    safety.maintenanceRequired = true;
    setState(MALFUNCTION);
  } else if (physics.equipmentHealth < 25.0) {
    safety.maintenanceRequired = true;
  }
  
  // Stream duration safety
  if (currentState == STREAM_ACTIVE) {
    unsigned long streamDuration = millis() - stateChangeTime;
    if (streamDuration > STREAM_MAX_DURATION) {
      triggerSafetyViolation("MAXIMUM STREAM DURATION EXCEEDED");
      setState(THERMAL_CRITICAL);
    }
  }
  
  // Safety interlock checks
  if (safety.interlockEngaged && currentState != POWERED_OFF && currentState != SAFETY_LOCKOUT) {
    triggerSafetyViolation("SAFETY INTERLOCK ENGAGED DURING OPERATION");
    setState(SAFETY_LOCKOUT);
  }
  
  // Multiple safety violations trigger lockout
  if (safety.safetyViolations >= 3) {
    setState(SAFETY_LOCKOUT);
    safety.emergencyStop = true;
  }
  
  // Environmental checks
  checkEnvironmentalConditions();
}

void triggerSafetyViolation(const char* reason) {
  safety.safetyViolations++;
  
  Serial.print("🚨 SAFETY VIOLATION #");
  Serial.print(safety.safetyViolations);
  Serial.print(": ");
  Serial.println(reason);
  
  // Emergency rumble pattern
  triggerRumbleOverride(255, 1000);
  
  // Audio warning if available
  if (mp3Ready) {
    playMP3Track(TRACK_MALFUNCTION);
  }
}

void checkEnvironmentalConditions() {
  // Simulate environmental monitoring
  // In a real implementation, you might have actual sensors
  
  physics.humidity = 45.0 + sin(millis() * 0.00001) * 10.0; // Simulate humidity variation
  physics.atmosphericPressure = 1013.25 + sin(millis() * 0.000005) * 20.0; // Pressure variation
  
  // Check for adverse conditions
  if (physics.humidity > 80.0 || physics.humidity < 20.0) {
    physics.environmentalWarning = true;
  } else if (physics.atmosphericPressure < 980.0 || physics.atmosphericPressure > 1040.0) {
    physics.environmentalWarning = true;
  } else {
    physics.environmentalWarning = false;
  }
  
  // Extreme environmental conditions affect equipment
  if (physics.environmentalWarning) {
    physics.equipmentHealth -= 0.0001; // Slow degradation
  }
}

// ================================
// STATUS REPORTING - ENHANCED
// ================================

void reportOptimizedStatus() {
  if (!TESTING_MODE) return;
  
  Serial.println("\n════════════════════════════════════════");
  Serial.println("        🔧 PROTON PACK STATUS REPORT");
  Serial.println("════════════════════════════════════════");
  
  // System state
  Serial.print("🔋 STATE: ");
  Serial.print(getStateName(currentState));
  unsigned long uptime = millis() - systemStartTime;
  Serial.print(" (Uptime: ");
  Serial.print(uptime / 1000);
  Serial.println("s)");
  
  // Physics status
  Serial.println("\n📊 PHYSICS ENGINE:");
  Serial.print("   🌡️  Core Temp: ");
  Serial.print(physics.coreTemperature, 1);
  Serial.print("°C  (Max Safe: ");
  Serial.print(MAX_SAFE_TEMP, 0);
  Serial.println("°C)");
  
  Serial.print("   ⚡ Pack Voltage: ");
  Serial.print(physics.packVoltage, 2);
  Serial.print("V  (Min: ");
  Serial.print(MIN_VOLTAGE, 1);
  Serial.println("V)");
  
  Serial.print("   🔄 Cyclotron: ");
  Serial.print(physics.currentRPM, 0);
  Serial.print(" RPM  (Target: ");
  Serial.print(physics.targetRPM, 0);
  Serial.println(" RPM)");
  
  if (currentState == STREAM_ACTIVE || currentState == STREAM_CHARGING) {
    Serial.print("   💥 Stream Power: ");
    Serial.print(physics.streamPower, 1);
    Serial.print("%  (Stability: ");
    Serial.print(physics.streamStability, 1);
    Serial.println("%)");
  }
  
  // Equipment status
  Serial.println("\n🔧 EQUIPMENT STATUS:");
  Serial.print("   💚 Health: ");
  Serial.print(physics.equipmentHealth, 1);
  Serial.println("%");
  
  Serial.print("   ⏰ Operating Hours: ");
  Serial.print(physics.operatingHours / 3600000); // Convert ms to hours
  Serial.println(" hours");
  
  Serial.print("   🔫 Total Shots: ");
  Serial.println(physics.totalShots);
  
  if (physics.requiresMaintenance) {
    Serial.println("   ⚠️  MAINTENANCE REQUIRED");
  }
  
  // Input status
  Serial.println("\n🎮 INPUT STATUS:");
  Serial.print("   Ion Arm: ");
  Serial.print(ionArmEngaged ? "ENGAGED" : "disengaged");
  Serial.print("  |  Wand: ");
  Serial.print(wandConnected ? "CONNECTED" : "disconnected");
  Serial.print("  |  Safety: ");
  Serial.println(safetyEngaged ? "READY" : "LOCKED");
  
  // Safety status
  Serial.println("\n🛡️  SAFETY SYSTEMS:");
  Serial.print("   Violations: ");
  Serial.print(safety.safetyViolations);
  Serial.print("/3  |  Thermal Protection: ");
  Serial.print(safety.thermalProtection ? "ACTIVE" : "disabled");
  Serial.print("  |  Emergency Stop: ");
  Serial.println(safety.emergencyStop ? "TRIGGERED" : "ready");
  
  // Crossing streams status
  if (crossing.detectionEnabled) {
    Serial.println("\n🎯 CROSSING STREAMS:");
    Serial.print("   Detection: ");
    Serial.print(crossing.streamsDetected ? "ACTIVE" : "monitoring");
    if (crossing.streamsDetected) {
      Serial.print("  (");
      Serial.print((millis() - crossing.crossingStartTime) / 1000.0, 1);
      Serial.print("s)");
    }
    Serial.println();
    
    Serial.print("   Wand Angles: X=");
    Serial.print(crossing.wandAngleX, 1);
    Serial.print("°, Y=");
    Serial.print(crossing.wandAngleY, 1);
    Serial.print("°, Z=");
    Serial.print(crossing.wandAngleZ, 1);
    Serial.println("°");
  }
  
  // Audio system
  Serial.println("\n🎵 AUDIO SYSTEM:");
  Serial.print("   MP3 Player: ");
  Serial.print(mp3Ready ? "READY" : "offline");
  if (trackPlaying) {
    Serial.print("  |  Playing: Track ");
    Serial.print(currentTrack);
    Serial.print(" (");
    Serial.print(getTrackName(currentTrack));
    Serial.println(")");
  } else {
    Serial.println("  |  Status: idle");
  }
  
  // Rumble system
  Serial.println("\n🎮 RUMBLE SYSTEM:");
  Serial.print("   Status: ");
  Serial.print(rumble.enabled ? "ENABLED" : "disabled");
  Serial.print("  |  Intensity: ");
  Serial.print(rumble.currentIntensity);
  Serial.print("/255  |  Target: ");
  Serial.println(rumble.targetIntensity);
  
  Serial.println("════════════════════════════════════════\n");
}

// ================================
// UTILITY FUNCTIONS
// ================================

void clearAllSystems() {
  powerCell.clear(); powerCell.show();
  cyclotron.clear(); cyclotron.show();
  wandTip.clear(); wandTip.show();
  wandSide.clear(); wandSide.show();
  wandBargraph.clear(); wandBargraph.show();
  wandStatus.clear(); wandStatus.show();
}

String getStateName(SystemState state) {
  switch (state) {
    case POWERED_OFF: return "OFF";
    case SELF_DIAGNOSTIC: return "DIAG";
    case ION_ARM_CHARGING: return "CHARGING";
    case WAND_SYNC: return "SYNC";
    case IDLE_OPERATIONAL: return "READY";
    case STREAM_CHARGING: return "CHARGE";
    case STREAM_ACTIVE: return "FIRING";
    case THERMAL_WARNING: return "WARN";
    case THERMAL_CRITICAL: return "CRITICAL";
    case EMERGENCY_VENT: return "VENT";
    case SAFETY_LOCKOUT: return "LOCKOUT";
    case MALFUNCTION: return "FAULT";
    case CROSSING_STREAMS_WARNING: return "CROSS-WARN";
    case CROSSING_STREAMS_CRITICAL: return "CROSS-CRIT";
    default: return "UNKNOWN";
  }
}