#include <Wire.h>
#include <AccelStepper.h>
#include <MultiStepper.h>

// I2C Slave Address
#define I2C_SLAVE_ADDR 0x08

// Motor 1 (Door)
#define STEP1 3
#define DIR1 4
#define EN1 2

// Motor 2 (Left Drive)
#define STEP2 11
#define DIR2 12
#define EN2 10

// Motor 3 (Right Drive)
#define STEP3 15
#define DIR3 16
#define EN3 13

// Stepper motor instances
AccelStepper doorMotor(AccelStepper::DRIVER, STEP1, DIR1);
AccelStepper leftMotor(AccelStepper::DRIVER, STEP2, DIR2);
AccelStepper rightMotor(AccelStepper::DRIVER, STEP3, DIR3);

// MultiStepper for synchronized drive motion
MultiStepper driveMotors;

// Motion parameters
int speedForward = 1000;
int speedBackward = 1000;
int speedTurn = 1500;
int doorSpeed = 1000;
int doorAccel = 800;

long stepsToMove = 7500;
long steps_Door = 7500;

bool doorOpened = false;

// I2C command buffers
volatile char receivedCommand = 0;
volatile int receivedValue = 0;
volatile bool newCommandReceived = false;

void receiveEvent(int numBytes) {
  if (numBytes < 5) return;

  byte buffer[5];
  for (int i = 0; i < 5 && Wire.available(); i++) {
    buffer[i] = Wire.read();
  }

  receivedCommand = (char)buffer[0];
  memcpy((void *)&receivedValue, &buffer[1], sizeof(int));
  newCommandReceived = true;
}

void setup() {
  Serial.begin(9600);
  while (!Serial);  // Wait for Serial monitor

  // Setup I2C
  Wire.begin(I2C_SLAVE_ADDR);
  Wire.onReceive(receiveEvent);

  // Enable all motors
  pinMode(EN1, OUTPUT);
  pinMode(EN2, OUTPUT);
  pinMode(EN3, OUTPUT);
  digitalWrite(EN1, LOW);
  digitalWrite(EN2, LOW);
  digitalWrite(EN3, LOW);

  // Setup stepper parameters
  doorMotor.setMaxSpeed(doorSpeed);
  doorMotor.setAcceleration(doorAccel);

  leftMotor.setMaxSpeed(speedForward);
  leftMotor.setAcceleration(500);
  rightMotor.setMaxSpeed(speedForward);
  rightMotor.setAcceleration(500);

  driveMotors.addStepper(leftMotor);
  driveMotors.addStepper(rightMotor);

  Serial.println("Motor Driver Board Ready");
}

void loop() {
  if (newCommandReceived) {
    newCommandReceived = false;

    // Display received data
    Serial.print("Received Command: ");
    Serial.print(receivedCommand);
    Serial.print(" | Value: ");
    Serial.println(receivedValue);

    // Update stepsToMove for motion
    if (receivedCommand == 'F' || receivedCommand == 'B' ||
        receivedCommand == 'L' || receivedCommand == 'R') {
      stepsToMove = receivedValue;
    }

    // Handle commands
    switch (receivedCommand) {
      case 'F': goForward(); break;
      case 'B': goBackward(); break;
      case 'L': turnLeft(); break;
      case 'R': turnRight(); break;
      case 'O': openDoor(); break;
      case 'C': closeDoor(); break;
      default:
        Serial.println("Unknown command");
    }
  }
}

// === Drive Functions ===

void goForward() {
  long positions[2];
  positions[0] = leftMotor.currentPosition() + stepsToMove;
  positions[1] = rightMotor.currentPosition() + stepsToMove;
  driveMotors.moveTo(positions);
  driveMotors.runSpeedToPosition();
}

void goBackward() {
  long positions[2];
  positions[0] = leftMotor.currentPosition() - stepsToMove;
  positions[1] = rightMotor.currentPosition() - stepsToMove;
  driveMotors.moveTo(positions);
  driveMotors.runSpeedToPosition();
}

void turnLeft() {
  long positions[2];
  positions[0] = leftMotor.currentPosition() - stepsToMove;
  positions[1] = rightMotor.currentPosition() + stepsToMove;
  driveMotors.moveTo(positions);
  driveMotors.runSpeedToPosition();
}

void turnRight() {
  long positions[2];
  positions[0] = leftMotor.currentPosition() + stepsToMove;
  positions[1] = rightMotor.currentPosition() - stepsToMove;
  driveMotors.moveTo(positions);
  driveMotors.runSpeedToPosition();
}

// === Door Functions ===

void openDoor() {
  if (!doorOpened) {
    doorMotor.moveTo(doorMotor.currentPosition() - steps_Door);
    doorMotor.runToPosition();
    doorOpened = true;
    Serial.println("Door opened");
  }
}

void closeDoor() {
  if (doorOpened) {
    doorMotor.moveTo(doorMotor.currentPosition() + steps_Door);
    doorMotor.runToPosition();
    doorOpened = false;
    Serial.println("Door closed");
  }
}