/*
 * BURN-E Robot - Arduino Control System
 * Basic Utility Repair Nano Engineer
 * 
 * Features:
 * - 4 Servo Motors for movement
 * - Ultrasonic sensor for obstacle detection
 * - LED strip for welding effects
 * - Buzzer for sound effects
 * - Autonomous and manual control modes
 */

#include <Servo.h>
#include <Adafruit_NeoPixel.h>

// Pin definitions
#define SERVO_TORCH_PIN 3      // Welding torch arm
#define SERVO_ARM_PIN 5        // Right arm
#define SERVO_HEAD_PIN 6       // Head pan
#define SERVO_DRIVE_PIN 9      // Drive system
#define LED_STRIP_PIN 10       // WS2812B LED strip
#define BUZZER_PIN 11          // Buzzer
#define ULTRASONIC_TRIG 12     // Ultrasonic trigger
#define ULTRASONIC_ECHO 13     // Ultrasonic echo

// Constants
#define LED_COUNT 8            // Number of LEDs in strip
#define OBSTACLE_DISTANCE 20   // Obstacle detection distance (cm)
#define SERVO_DELAY 15         // Delay between servo steps (ms)

// Create servo objects
Servo torchServo;
Servo armServo;
Servo headServo;
Servo driveServo;

// Create LED strip object
Adafruit_NeoPixel strip(LED_COUNT, LED_STRIP_PIN, NEO_GRB + NEO_KHZ800);

// Robot state variables
enum RobotMode {
  IDLE,
  WELDING,
  MOVING,
  SCANNING,
  EMERGENCY_STOP
};

RobotMode currentMode = IDLE;
unsigned long lastModeChange = 0;
unsigned long lastWeldingEffect = 0;
bool isWelding = false;
int headPosition = 90;  // Center position
int torchPosition = 90;
int armPosition = 90;

// Movement patterns
int weldingSequence[] = {70, 110, 90, 120, 80, 100, 90};
int scanningSequence[] = {45, 135, 90, 60, 120, 90};
int currentSequenceStep = 0;

void setup() {
  Serial.begin(9600);
  Serial.println("BURN-E Initializing...");
  
  // Initialize servos
  torchServo.attach(SERVO_TORCH_PIN);
  armServo.attach(SERVO_ARM_PIN);
  headServo.attach(SERVO_HEAD_PIN);
  driveServo.attach(SERVO_DRIVE_PIN);
  
  // Initialize LED strip
  strip.begin();
  strip.show(); // Initialize all pixels to 'off'
  
  // Initialize pins
  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(ULTRASONIC_TRIG, OUTPUT);
  pinMode(ULTRASONIC_ECHO, INPUT);
  
  // Move to initial positions
  moveToInitialPosition();
  
  // Startup sequence
  playStartupSequence();
  
  Serial.println("BURN-E Ready!");
  currentMode = IDLE;
}

void loop() {
  // Check for obstacles
  float distance = getDistance();
  
  // Handle emergency stop
  if (distance < 10 && currentMode == MOVING) {
    currentMode = EMERGENCY_STOP;
    emergencyStop();
  }
  
  // Main state machine
  switch (currentMode) {
    case IDLE:
      handleIdleMode();
      break;
      
    case WELDING:
      handleWeldingMode();
      break;
      
    case MOVING:
      handleMovingMode();
      break;
      
    case SCANNING:
      handleScanningMode();
      break;
      
    case EMERGENCY_STOP:
      handleEmergencyStop();
      break;
  }
  
  // Update LED effects
  updateLEDEffects();
  
  // Check for mode changes
  checkModeChange();
  
  delay(50); // Main loop delay
}

void moveToInitialPosition() {
  Serial.println("Moving to initial position...");
  
  // Move all servos to center position slowly
  moveServoSmoothly(torchServo, 90);
  delay(500);
  moveServoSmoothly(armServo, 90);
  delay(500);
  moveServoSmoothly(headServo, 90);
  delay(500);
  moveServoSmoothly(driveServo, 90);
  
  // Update position variables
  torchPosition = 90;
  armPosition = 90;
  headPosition = 90;
}

void playStartupSequence() {
  Serial.println("Playing startup sequence...");
  
  // LED startup effect
  for (int i = 0; i < LED_COUNT; i++) {
    strip.setPixelColor(i, strip.Color(0, 255, 0)); // Green
    strip.show();
    delay(100);
  }
  
  // Sound effect
  for (int i = 0; i < 3; i++) {
    tone(BUZZER_PIN, 1000, 200);
    delay(300);
  }
  
  // Clear LEDs
  strip.clear();
  strip.show();
}

void handleIdleMode() {
  // Subtle breathing LED effect
  static int brightness = 0;
  static int direction = 1;
  
  brightness += direction * 2;
  if (brightness >= 50) direction = -1;
  if (brightness <= 0) direction = 1;
  
  for (int i = 0; i < LED_COUNT; i++) {
    strip.setPixelColor(i, strip.Color(0, brightness, brightness/2));
  }
  strip.show();
  
  // Random head movements
  if (millis() - lastModeChange > 3000) {
    if (random(100) < 10) { // 10% chance
      int newHeadPos = random(60, 120);
      moveServoSmoothly(headServo, newHeadPos);
      headPosition = newHeadPos;
    }
  }
}

void handleWeldingMode() {
  Serial.println("Welding mode active");
  
  // Execute welding sequence
  if (currentSequenceStep < sizeof(weldingSequence)/sizeof(int)) {
    int targetPos = weldingSequence[currentSequenceStep];
    moveServoSmoothly(torchServo, targetPos);
    torchPosition = targetPos;
    
    // Welding effect
    createWeldingEffect();
    
    currentSequenceStep++;
    delay(1000);
  } else {
    // Sequence complete
    currentSequenceStep = 0;
    currentMode = IDLE;
    lastModeChange = millis();
    Serial.println("Welding sequence complete");
  }
}

void handleMovingMode() {
  Serial.println("Moving mode active");
  
  // Simple forward movement
  driveServo.write(120); // Move forward
  
  // Scan ahead while moving
  int scanPos = 90 + 30 * sin(millis() / 1000.0);
  moveServoSmoothly(headServo, scanPos);
  headPosition = scanPos;
  
  // Check if we should stop moving
  if (millis() - lastModeChange > 3000) {
    driveServo.write(90); // Stop
    currentMode = IDLE;
    lastModeChange = millis();
  }
}

void handleScanningMode() {
  Serial.println("Scanning mode active");
  
  // Execute scanning sequence
  if (currentSequenceStep < sizeof(scanningSequence)/sizeof(int)) {
    int targetPos = scanningSequence[currentSequenceStep];
    moveServoSmoothly(headServo, targetPos);
    headPosition = targetPos;
    
    // Scanning LED effect
    createScanningEffect();
    
    currentSequenceStep++;
    delay(800);
  } else {
    // Sequence complete
    currentSequenceStep = 0;
    currentMode = IDLE;
    lastModeChange = millis();
    Serial.println("Scanning complete");
  }
}

void handleEmergencyStop() {
  // Stop all movement
  driveServo.write(90);
  
  // Flash red LEDs
  for (int i = 0; i < LED_COUNT; i++) {
    strip.setPixelColor(i, strip.Color(255, 0, 0)); // Red
  }
  strip.show();
  
  // Emergency sound
  tone(BUZZER_PIN, 2000, 100);
  delay(200);
  
  // Clear LEDs
  strip.clear();
  strip.show();
  delay(200);
  
  // Check if obstacle is clear
  if (getDistance() > OBSTACLE_DISTANCE) {
    currentMode = IDLE;
    lastModeChange = millis();
    Serial.println("Emergency cleared");
  }
}

void createWeldingEffect() {
  // Bright white/blue welding flash
  for (int i = 0; i < LED_COUNT; i++) {
    int intensity = random(200, 255);
    strip.setPixelColor(i, strip.Color(intensity, intensity, 255));
  }
  strip.show();
  
  // Welding sound
  tone(BUZZER_PIN, random(800, 1200), 100);
  
  delay(random(50, 200));
  
  // Fade effect
  for (int fade = 255; fade >= 0; fade -= 10) {
    for (int i = 0; i < LED_COUNT; i++) {
      strip.setPixelColor(i, strip.Color(fade/3, fade/3, fade));
    }
    strip.show();
    delay(20);
  }
}

void createScanningEffect() {
  // Moving blue light
  static int scanPos = 0;
  
  strip.clear();
  strip.setPixelColor(scanPos, strip.Color(0, 100, 255));
  strip.setPixelColor((scanPos + 1) % LED_COUNT, strip.Color(0, 50, 128));
  strip.show();
  
  scanPos = (scanPos + 1) % LED_COUNT;
  
  // Scanning beep
  tone(BUZZER_PIN, 1500, 50);
}

void updateLEDEffects() {
  // This function is called every loop to update LED effects
  // Current effects are handled in mode-specific functions
}

void moveServoSmoothly(Servo &servo, int targetPosition) {
  int currentPosition = servo.read();
  int step = (targetPosition > currentPosition) ? 1 : -1;
  
  for (int pos = currentPosition; pos != targetPosition; pos += step) {
    servo.write(pos);
    delay(SERVO_DELAY);
  }
  servo.write(targetPosition); // Ensure exact position
}

float getDistance() {
  // Send ultrasonic pulse
  digitalWrite(ULTRASONIC_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(ULTRASONIC_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(ULTRASONIC_TRIG, LOW);
  
  // Read echo
  long duration = pulseIn(ULTRASONIC_ECHO, HIGH);
  float distance = duration * 0.034 / 2; // Convert to cm
  
  return distance;
}

void checkModeChange() {
  // Auto mode changes based on time and conditions
  if (millis() - lastModeChange > 5000) { // 5 seconds in current mode
    
    if (currentMode == IDLE) {
      // Randomly choose next activity
      int choice = random(3);
      switch (choice) {
        case 0:
          currentMode = WELDING;
          Serial.println("Starting welding sequence");
          break;
        case 1:
          currentMode = MOVING;
          Serial.println("Starting movement");
          break;
        case 2:
          currentMode = SCANNING;
          Serial.println("Starting scan sequence");
          break;
      }
      lastModeChange = millis();
      currentSequenceStep = 0;
    }
  }
}

void emergencyStop() {
  Serial.println("EMERGENCY STOP ACTIVATED!");
  
  // Stop all servos
  driveServo.write(90);
  
  // Flash warning
  for (int i = 0; i < 5; i++) {
    for (int j = 0; j < LED_COUNT; j++) {
      strip.setPixelColor(j, strip.Color(255, 0, 0));
    }
    strip.show();
    tone(BUZZER_PIN, 2000, 200);
    delay(300);
    
    strip.clear();
    strip.show();
    delay(200);
  }
}

// เพิ่มโมดูล Bluetooth HC-05 สำหรับควบคุมผ่านมือถือ
#include <SoftwareSerial.h>

SoftwareSerial bluetooth(2, 4); // RX, TX

void setup() {
  bluetooth.begin(9600);
  // ... existing setup code
}

void loop() {
  // Check for Bluetooth commands
  if (bluetooth.available()) {
    String command = bluetooth.readString();
    command.trim();
    
    if (command == "WELD") {
      currentMode = WELDING;
    } else if (command == "MOVE") {
      currentMode = MOVING;
    } else if (command == "SCAN") {
      currentMode = SCANNING;
    } else if (command == "STOP") {
      currentMode = EMERGENCY_STOP;
    }
  }
  
  // ... existing loop code
}