#include <BluetoothSerial.h>

BluetoothSerial SerialBT;

// Motor Driver Pins
#define IN1 12
#define IN2 14
#define IN3 27
#define IN4 26
#define ENA 25
#define ENB 33

// Lights and Horn Pins
#define FRONT_LIGHT 13
#define BACK_LIGHT 32
#define HORN 15

int speedPWM = 0; // PWM value for speed control (0 to 255)

void moveWheelchair(char command) {
  Serial.print("Executing Command in moveWheelchair: ");
  Serial.println(command);

  switch (command) {
    case 'F': // Forward
      Serial.println("Forward");
      analogWrite(ENA, speedPWM);
      analogWrite(ENB, speedPWM);
      digitalWrite(IN1, HIGH);
      digitalWrite(IN2, LOW);
      digitalWrite(IN3, HIGH);
      digitalWrite(IN4, LOW);
      break;

    case 'B': // Backward
      Serial.println("Backward");
      analogWrite(ENA, speedPWM);
      analogWrite(ENB, speedPWM);
      digitalWrite(IN1, LOW);
      digitalWrite(IN2, HIGH);
      digitalWrite(IN3, LOW);
      digitalWrite(IN4, HIGH);
      break;

    case 'L': // Left
      Serial.println("Left");
      analogWrite(ENA, speedPWM);
      analogWrite(ENB, speedPWM);
      digitalWrite(IN1, LOW);
      digitalWrite(IN2, HIGH);
      digitalWrite(IN3, HIGH);
      digitalWrite(IN4, LOW);
      break;

    case 'R': // Right
      Serial.println("Right");
      analogWrite(ENA, speedPWM);
      analogWrite(ENB, speedPWM);
      digitalWrite(IN1, HIGH);
      digitalWrite(IN2, LOW);
      digitalWrite(IN3, LOW);
      digitalWrite(IN4, HIGH);
      break;

    case 'S': // Stop
      Serial.println("Stop");
      analogWrite(ENA, 0);
      analogWrite(ENB, 0);
      digitalWrite(IN1, LOW);
      digitalWrite(IN2, LOW);
      digitalWrite(IN3, LOW);
      digitalWrite(IN4, LOW);
      break;

    case 'D': // Stop All
      Serial.println("Stop All");
      speedPWM = 0;
      moveWheelchair('S');
      break;

    default:
      Serial.print("Unknown moveWheelchair Command: ");
      Serial.println(command);
      break;
  }
}

void controlExtras(char command) {
  Serial.print("Executing Command in controlExtras: ");
  Serial.println(command);

  switch (command) {
    case 'W':
      Serial.println("Front Lights ON");
      digitalWrite(FRONT_LIGHT, HIGH);
      break;
    case 'w':
      Serial.println("Front Lights OFF");
      digitalWrite(FRONT_LIGHT, LOW);
      break;
    case 'U':
      Serial.println("Back Lights ON");
      digitalWrite(BACK_LIGHT, HIGH);
      break;
    case 'u':
      Serial.println("Back Lights OFF");
      digitalWrite(BACK_LIGHT, LOW);
      break;
    case 'V':
      Serial.println("Horn ON");
      digitalWrite(HORN, HIGH);
      break;
    case 'v':
      Serial.println("Horn OFF");
      digitalWrite(HORN, LOW);
      break;

    default:
      Serial.print("Unknown controlExtras Command: ");
      Serial.println(command);
      break;
  }
}

void updateSpeed(char command) {
  Serial.print("Executing Command in updateSpeed: ");
  Serial.println(command);

  switch (command) {
    case '0': speedPWM = 0; break;
    case '1': speedPWM = 25; break;
    case '2': speedPWM = 51; break;
    case '3': speedPWM = 76; break;
    case '4': speedPWM = 102; break;
    case '5': speedPWM = 127; break;
    case '6': speedPWM = 153; break;
    case '7': speedPWM = 178; break;
    case '8': speedPWM = 204; break;
    case '9': speedPWM = 229; break;
    case 'q': speedPWM = 255; break;

    default:
      Serial.print("Unknown speed command: ");
      Serial.println(command);
      return;
  }
  Serial.print("Speed set to PWM: ");
  Serial.println(speedPWM);
}

void setup() {
  // Initialize Serial (Hardware Serial)
  Serial.begin(115200);
  Serial.println("Wheelchair Controller Initialized");

  // Initialize Bluetooth Serial
 // SerialBT.begin("WheelchairController");
  Serial.println("Bluetooth is ready. Waiting for commands...");

  // Set motor pins as output
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);

  // Set lights and horn pins as output
  pinMode(FRONT_LIGHT, OUTPUT);
  pinMode(BACK_LIGHT, OUTPUT);
  pinMode(HORN, OUTPUT);

  // Initialize motors and extras in OFF state
  moveWheelchair('S');
  digitalWrite(FRONT_LIGHT, LOW);
  digitalWrite(BACK_LIGHT, LOW);
  digitalWrite(HORN, LOW);
}

void loop() {
  char command = 0;

  // Read from Bluetooth Serial
  // if (SerialBT.available()) {
  //   command = SerialBT.read();
  //   if (command == '\n' || command == '\r') {
  //     return; // Ignore newline or carriage return
  //   }
  //   Serial.print("Received via Bluetooth: ");
  //   Serial.print(command);
  //   Serial.print(" | ASCII: ");
  //   Serial.println((int)command);
  // }

  // Read from Hardware Serial (e.g., PC Serial Monitor)
  if (Serial.available()) {
    command = Serial.read();
    if (command == '\n' || command == '\r') {
      return; // Ignore newline or carriage return
    }
    Serial.print("Received via Hardware Serial: ");
    Serial.print(command);
    Serial.print(" | ASCII: ");
    Serial.println((int)command);
  }
 if (command == 'F' || command == 'B' || command == 'L' || command == 'R' || command == 'S' || command == 'D') {
    moveWheelchair(command); // Control movement
} else if (command == 'W' || command == 'U' || command == 'V') {
    controlExtras(command); // Control lights and horn
} else if (command == 'w' || command == 'u' || command == 'v') {
    controlExtras(command); // Control lights and horn (lowercase)
} else if (command >= '0' && command <= 'q') {
    updateSpeed(command); // Adjust speed
} else if (command != 0) {
    Serial.print("Ignoring invalid command: ");
    Serial.println(command);
}

}