/*
 * Bluetooth Controlled Robot Car - Arduino Code
 * Compatible with Robo Connect App by Creative Amir
 * 
 * Hardware Connections:
 * =====================
 * RIGHT MOTOR BTS7960 Driver:
 * - RPWM (Forward) -> Pin 5
 * - LPWM (Backward) -> Pin 6
 * 
 * LEFT MOTOR BTS7960 Driver:
 * - RPWM (Forward) -> Pin 9
 * - LPWM (Backward) -> Pin 10
 * 
 * LED & ACCESSORIES:
 * - Front LED -> Pin 2
 * - Back LED -> Pin 3
 * - Right Indicator LED -> Pin 4
 * - Left Indicator LED -> Pin 7
 * - Emergency LED (can share with indicators) -> Pin 8
 * - Horn/Buzzer -> Pin 11
 * 
 * BLUETOOTH MODULE (HC-05/HC-06):
 * - TX -> Arduino RX (Pin 0)
 * - RX -> Arduino TX (Pin 1) via voltage divider
 * - VCC -> 5V
 * - GND -> GND
 */

// ========== PIN DEFINITIONS ==========
// Motor Driver Pins - BTS7960
#define RIGHT_MOTOR_FORWARD   5    // Right motor RPWM (PWM capable)
#define RIGHT_MOTOR_BACKWARD  6    // Right motor LPWM (PWM capable)
#define LEFT_MOTOR_FORWARD    9    // Left motor RPWM (PWM capable)
#define LEFT_MOTOR_BACKWARD   10   // Left motor LPWM (PWM capable)

// LED Pins
#define FRONT_LED_PIN         2    // Front headlight
#define BACK_LED_PIN          3    // Back/brake light
#define RIGHT_INDICATOR_PIN   4    // Right turn indicator
#define LEFT_INDICATOR_PIN    7    // Left turn indicator
#define EMERGENCY_LED_PIN     8    // Emergency flasher

// Horn Pin
#define HORN_PIN              11   // Horn/Buzzer

// ========== GLOBAL VARIABLES ==========
String receivedCommand = "";       // Stores incoming Bluetooth command
int currentSpeed = 50;             // Speed percentage (0-100), default 50%
int motorSpeed = 127;              // Actual PWM value (0-255), default ~50%

// LED States
bool frontLEDState = false;
bool backLEDState = false;
bool rightIndicatorState = false;
bool leftIndicatorState = false;
bool emergencyState = false;
bool hornState = false;

// Indicator Blinking
unsigned long lastBlinkTime = 0;
int blinkInterval = 500;           // Blink every 500ms
bool indicatorBlinkState = false;

// Command Buffer Timeout
unsigned long lastCharTime = 0;
const unsigned long COMMAND_TIMEOUT = 50; // 50ms timeout for incomplete commands

// ========== SETUP ==========
void setup() {
  // Initialize Serial for Bluetooth communication
  Serial.begin(9600);
  
  // Set motor pins as OUTPUT
  pinMode(RIGHT_MOTOR_FORWARD, OUTPUT);
  pinMode(RIGHT_MOTOR_BACKWARD, OUTPUT);
  pinMode(LEFT_MOTOR_FORWARD, OUTPUT);
  pinMode(LEFT_MOTOR_BACKWARD, OUTPUT);
  
  // Set LED and Horn pins as OUTPUT
  pinMode(FRONT_LED_PIN, OUTPUT);
  pinMode(BACK_LED_PIN, OUTPUT);
  pinMode(RIGHT_INDICATOR_PIN, OUTPUT);
  pinMode(LEFT_INDICATOR_PIN, OUTPUT);
  pinMode(EMERGENCY_LED_PIN, OUTPUT);
  pinMode(HORN_PIN, OUTPUT);
  
  // Initialize all outputs to LOW (OFF)
  stopMotors();
  digitalWrite(FRONT_LED_PIN, LOW);
  digitalWrite(BACK_LED_PIN, LOW);
  digitalWrite(RIGHT_INDICATOR_PIN, LOW);
  digitalWrite(LEFT_INDICATOR_PIN, LOW);
  digitalWrite(EMERGENCY_LED_PIN, LOW);
  digitalWrite(HORN_PIN, LOW);
  
  // Welcome message
  Serial.println("Robot Ready!");
}

// ========== MAIN LOOP ==========
void loop() {
  // Read Bluetooth commands
  while (Serial.available() > 0) {
    char c = Serial.read();
    lastCharTime = millis();
    
    if (c == '\n' || c == '\r') {
      // End of command received
      if (receivedCommand.length() > 0) {
        receivedCommand.trim(); // Remove any extra whitespace
        processCommand(receivedCommand);
        receivedCommand = ""; // Clear for next command
      }
    } else if (c >= 32 && c <= 126) {
      // Only accept printable ASCII characters
      receivedCommand += c;
      
      // Prevent buffer overflow
      if (receivedCommand.length() > 50) {
        Serial.println("Warning: Command buffer overflow! Clearing...");
        receivedCommand = "";
      }
    }
  }
  
  // Check for incomplete command timeout
  if (receivedCommand.length() > 0 && (millis() - lastCharTime) > COMMAND_TIMEOUT) {
    Serial.print("Warning: Incomplete command timeout: '");
    Serial.print(receivedCommand);
    Serial.println("' - Clearing buffer");
    receivedCommand = ""; // Clear incomplete command
  }
  
  // Handle indicator blinking
  handleIndicatorBlink();
}

// ========== COMMAND PROCESSING ==========
void processCommand(String cmd) {
  // Print received command to Serial Monitor for debugging
  Serial.print("Received Command: ");
  Serial.println(cmd);
  
  // ===== MOVEMENT COMMANDS =====
  if (cmd == "F") {
    Serial.println("Action: Moving Forward");
    moveForward();
  }
  else if (cmd == "B") {
    Serial.println("Action: Moving Backward");
    moveBackward();
  }
  else if (cmd == "L") {
    Serial.println("Action: Turning Left");
    turnLeft();
  }
  else if (cmd == "R") {
    Serial.println("Action: Turning Right");
    turnRight();
  }


  else if (cmd == "FL" || cmd == "LF") {
  Serial.println("Action: Moving Forward-Left");
  moveForwardLeft();
}
else if (cmd == "FR" || cmd == "RF") {
  Serial.println("Action: Moving Forward-Right");
  moveForwardRight();
}
else if (cmd == "BL" || cmd == "LB") {
  Serial.println("Action: Moving Backward-Left");
  moveBackwardLeft();
}
else if (cmd == "BR" || cmd == "RB") {
  Serial.println("Action: Moving Backward-Right");
  moveBackwardRight();
}
  else if (cmd == "S") {
    Serial.println("Action: Stopped");
    stopMotors();
  }
  
  // ===== FRONT LED COMMANDS =====
  else if (cmd == "LED_FRONT_ON") {
    Serial.println("Action: Front LED ON");
    frontLEDState = true;
    digitalWrite(FRONT_LED_PIN, HIGH);
  }
  else if (cmd == "LED_FRONT_OFF") {
    Serial.println("Action: Front LED OFF");
    frontLEDState = false;
    digitalWrite(FRONT_LED_PIN, LOW);
  }
  
  // ===== BACK LED COMMANDS =====
  else if (cmd == "LED_BACK_ON") {
    Serial.println("Action: Back LED ON");
    backLEDState = true;
    digitalWrite(BACK_LED_PIN, HIGH);
  }
  else if (cmd == "LED_BACK_OFF") {
    Serial.println("Action: Back LED OFF");
    backLEDState = false;
    digitalWrite(BACK_LED_PIN, LOW);
  }
  
  // ===== RIGHT INDICATOR COMMANDS =====
  else if (cmd == "RIGHT_INDICATOR_ON") {
    Serial.println("Action: Right Indicator ON");
    rightIndicatorState = true;
    leftIndicatorState = false;
    emergencyState = false;
    digitalWrite(LEFT_INDICATOR_PIN, LOW);
    digitalWrite(EMERGENCY_LED_PIN, LOW);
  }
  else if (cmd == "RIGHT_INDICATOR_OFF") {
    Serial.println("Action: Right Indicator OFF");
    rightIndicatorState = false;
    digitalWrite(RIGHT_INDICATOR_PIN, LOW);
  }
  
  // ===== LEFT INDICATOR COMMANDS =====
  else if (cmd == "LEFT_INDICATOR_ON") {
    Serial.println("Action: Left Indicator ON");
    leftIndicatorState = true;
    rightIndicatorState = false;
    emergencyState = false;
    digitalWrite(RIGHT_INDICATOR_PIN, LOW);
    digitalWrite(EMERGENCY_LED_PIN, LOW);
  }
  else if (cmd == "LEFT_INDICATOR_OFF") {
    Serial.println("Action: Left Indicator OFF");
    leftIndicatorState = false;
    digitalWrite(LEFT_INDICATOR_PIN, LOW);
  }
  
  // ===== EMERGENCY LED COMMANDS =====
  else if (cmd == "EMERGENCY_ON") {
    Serial.println("Action: Emergency LED ON");
    emergencyState = true;
    rightIndicatorState = false;
    leftIndicatorState = false;
    digitalWrite(RIGHT_INDICATOR_PIN, LOW);
    digitalWrite(LEFT_INDICATOR_PIN, LOW);
  }
  else if (cmd == "EMERGENCY_OFF") {
    Serial.println("Action: Emergency LED OFF");
    emergencyState = false;
    digitalWrite(RIGHT_INDICATOR_PIN, LOW);
    digitalWrite(LEFT_INDICATOR_PIN, LOW);
    digitalWrite(EMERGENCY_LED_PIN, LOW);
  }
  
  // ===== HORN COMMANDS =====
  else if (cmd == "HORN_ON") {
    Serial.println("Action: Horn ON");
    hornState = true;
    digitalWrite(HORN_PIN, HIGH);
  }
  else if (cmd == "HORN_OFF") {
    Serial.println("Action: Horn OFF");
    hornState = false;
    digitalWrite(HORN_PIN, LOW);
  }
  
  // ===== SPEED COMMANDS =====
  else if (cmd.startsWith("SPEED_")) {
    String speedStr = cmd.substring(6); // Extract number after "SPEED_"
    speedStr.trim(); // Remove any whitespace
    
    // Check if there's actually a number after SPEED_
    if (speedStr.length() == 0) {
      Serial.println("Action: Incomplete SPEED command - ignored");
      return;
    }
    
    int speedValue = speedStr.toInt();
    
    if (speedValue >= 0 && speedValue <= 100) {
      currentSpeed = speedValue;
      motorSpeed = map(speedValue, 0, 100, 0, 255); // Convert to PWM (0-255)
      Serial.print("Action: Speed changed to ");
      Serial.print(currentSpeed);
      Serial.print("% (PWM: ");
      Serial.print(motorSpeed);
      Serial.println(")");
    } else {
      Serial.print("Action: Invalid speed value: ");
      Serial.println(speedValue);
    }
  }
  
  // ===== CONNECTION CONFIRMATION =====
  else if (cmd == "CONNECTED") {
    Serial.println("Action: Connection confirmed!");
  }
  
  // ===== UNKNOWN COMMAND =====
  else {
    Serial.println("Action: Unknown command!");
  }
}

// ========== MOTOR MOVEMENT FUNCTIONS ==========
void moveForward() {
  analogWrite(RIGHT_MOTOR_FORWARD, motorSpeed);
  analogWrite(RIGHT_MOTOR_BACKWARD, 0);
  analogWrite(LEFT_MOTOR_FORWARD, motorSpeed);
  analogWrite(LEFT_MOTOR_BACKWARD, 0);
}

void moveBackward() {
  analogWrite(RIGHT_MOTOR_FORWARD, 0);
  analogWrite(RIGHT_MOTOR_BACKWARD, motorSpeed);
  analogWrite(LEFT_MOTOR_FORWARD, 0);
  analogWrite(LEFT_MOTOR_BACKWARD, motorSpeed);
}

void turnLeft() {
  // Left motor backward, right motor forward (pivot turn)
  analogWrite(RIGHT_MOTOR_FORWARD, motorSpeed);
  analogWrite(RIGHT_MOTOR_BACKWARD, 0);
  analogWrite(LEFT_MOTOR_FORWARD, 0);
  analogWrite(LEFT_MOTOR_BACKWARD, motorSpeed);
}

void turnRight() {
  // Right motor backward, left motor forward (pivot turn)
  analogWrite(RIGHT_MOTOR_FORWARD, 0);
  analogWrite(RIGHT_MOTOR_BACKWARD, motorSpeed);
  analogWrite(LEFT_MOTOR_FORWARD, motorSpeed);
  analogWrite(LEFT_MOTOR_BACKWARD, 0);
}

void moveForwardLeft() {
  // Reduce left motor speed for gradual left turn
  int reducedSpeed = motorSpeed * 0.5; // 50% speed on left
  analogWrite(RIGHT_MOTOR_FORWARD, motorSpeed);
  analogWrite(RIGHT_MOTOR_BACKWARD, 0);
  analogWrite(LEFT_MOTOR_FORWARD, reducedSpeed);
  analogWrite(LEFT_MOTOR_BACKWARD, 0);
}

void moveForwardRight() {
  // Reduce right motor speed for gradual right turn
  int reducedSpeed = motorSpeed * 0.5; // 50% speed on right
  analogWrite(RIGHT_MOTOR_FORWARD, reducedSpeed);
  analogWrite(RIGHT_MOTOR_BACKWARD, 0);
  analogWrite(LEFT_MOTOR_FORWARD, motorSpeed);
  analogWrite(LEFT_MOTOR_BACKWARD, 0);
}

void moveBackwardLeft() {
  // Reduce left motor speed while going backward
  int reducedSpeed = motorSpeed * 0.5;
  analogWrite(RIGHT_MOTOR_FORWARD, 0);
  analogWrite(RIGHT_MOTOR_BACKWARD, motorSpeed);
  analogWrite(LEFT_MOTOR_FORWARD, 0);
  analogWrite(LEFT_MOTOR_BACKWARD, reducedSpeed);
}

void moveBackwardRight() {
  // Reduce right motor speed while going backward
  int reducedSpeed = motorSpeed * 0.5;
  analogWrite(RIGHT_MOTOR_FORWARD, 0);
  analogWrite(RIGHT_MOTOR_BACKWARD, reducedSpeed);
  analogWrite(LEFT_MOTOR_FORWARD, 0);
  analogWrite(LEFT_MOTOR_BACKWARD, motorSpeed);
}

void stopMotors() {
  analogWrite(RIGHT_MOTOR_FORWARD, 0);
  analogWrite(RIGHT_MOTOR_BACKWARD, 0);
  analogWrite(LEFT_MOTOR_FORWARD, 0);
  analogWrite(LEFT_MOTOR_BACKWARD, 0);
}

// ========== INDICATOR BLINKING HANDLER ==========
void handleIndicatorBlink() {
  unsigned long currentTime = millis();
  
  if (currentTime - lastBlinkTime >= blinkInterval) {
    lastBlinkTime = currentTime;
    indicatorBlinkState = !indicatorBlinkState;
    
    // Right indicator blinking
    if (rightIndicatorState) {
      digitalWrite(RIGHT_INDICATOR_PIN, indicatorBlinkState ? HIGH : LOW);
    }
    
    // Left indicator blinking
    if (leftIndicatorState) {
      digitalWrite(LEFT_INDICATOR_PIN, indicatorBlinkState ? HIGH : LOW);
    }
    
    // Emergency blinking (both indicators + emergency LED)
    if (emergencyState) {
      digitalWrite(RIGHT_INDICATOR_PIN, indicatorBlinkState ? HIGH : LOW);
      digitalWrite(LEFT_INDICATOR_PIN, indicatorBlinkState ? HIGH : LOW);
      digitalWrite(EMERGENCY_LED_PIN, indicatorBlinkState ? HIGH : LOW);
    }
  }
}