// Motor and direction pins
#define MOTOR_PWM_PIN 5       // PWM pin for motor speed (LED simulation)
#define DIR_PIN_LEFT 18       // Direction control pin for left motor
#define DIR_PIN_RIGHT 19      // Direction control pin for right motor

// Encoder pins (simulated)
#define ENCODER_PIN_A_LEFT 15
#define ENCODER_PIN_B_LEFT 2
#define ENCODER_PIN_A_RIGHT 16
#define ENCODER_PIN_B_RIGHT 17

// Robot parameters
const float WHEEL_DIAMETER = 0.065; // Wheel diameter in meters
const float WHEEL_BASE = 0.15;      // Distance between wheels in meters
const int COUNTS_PER_REV = 1440;    // Encoder counts per revolution

// Position and heading variables
float x = 0.0, y = 0.0, theta = 0.0; // Position (x, y) and heading (theta in radians)
volatile long leftEncoderCount = 0, rightEncoderCount = 0;

// Previous encoder counts for odometry calculation
long prevLeftCount = 0, prevRightCount = 0;

// Interrupt Service Routines (ISR) to simulate encoder counting
void IRAM_ATTR leftEncoderISR() {
  int stateA = digitalRead(ENCODER_PIN_A_LEFT);
  int stateB = digitalRead(ENCODER_PIN_B_LEFT);
  leftEncoderCount += (stateA == stateB) ? 1 : -1; // Increment or decrement count
}

void IRAM_ATTR rightEncoderISR() {
  int stateA = digitalRead(ENCODER_PIN_A_RIGHT);
  int stateB = digitalRead(ENCODER_PIN_B_RIGHT);
  rightEncoderCount += (stateA == stateB) ? 1 : -1; // Increment or decrement count
}

void setup() {
  Serial.begin(115200);

  // Motor and direction pins setup
  pinMode(MOTOR_PWM_PIN, OUTPUT);
  pinMode(DIR_PIN_LEFT, OUTPUT);
  pinMode(DIR_PIN_RIGHT, OUTPUT);

  // Encoder pins setup with interrupt
  pinMode(ENCODER_PIN_A_LEFT, INPUT_PULLUP);
  pinMode(ENCODER_PIN_B_LEFT, INPUT_PULLUP);
  pinMode(ENCODER_PIN_A_RIGHT, INPUT_PULLUP);
  pinMode(ENCODER_PIN_B_RIGHT, INPUT_PULLUP);

  // Attach interrupts for encoders
  attachInterrupt(digitalPinToInterrupt(ENCODER_PIN_A_LEFT), leftEncoderISR, CHANGE);
  attachInterrupt(digitalPinToInterrupt(ENCODER_PIN_A_RIGHT), rightEncoderISR, CHANGE);
}

void setMotorSpeed(int speed) {
  // Clamp speed between 0 and 255
  speed = constrain(speed, 0, 255);
  analogWrite(MOTOR_PWM_PIN, speed);  // LED brightness as speed indicator
}

void setMotorDirection(bool forward) {
  if (forward) {
    digitalWrite(DIR_PIN_LEFT, HIGH);
    digitalWrite(DIR_PIN_RIGHT, LOW);
  } else {
    digitalWrite(DIR_PIN_LEFT, LOW);
    digitalWrite(DIR_PIN_RIGHT, HIGH);
  }
}

void calculateOdometry() {
  // Capture the latest encoder counts
  long leftCounts = leftEncoderCount;
  long rightCounts = rightEncoderCount;

  // Calculate distances for each wheel
  float deltaLeft = ((leftCounts - prevLeftCount) / (float)COUNTS_PER_REV) * PI * WHEEL_DIAMETER;
  float deltaRight = ((rightCounts - prevRightCount) / (float)COUNTS_PER_REV) * PI * WHEEL_DIAMETER;

  // Update previous counts for next calculation
  prevLeftCount = leftCounts;
  prevRightCount = rightCounts;

  // Calculate odometry
  float deltaS = (deltaLeft + deltaRight) / 2.0;         // Average distance traveled
  float deltaTheta = (deltaRight - deltaLeft) / WHEEL_BASE; // Change in heading angle

  // Update robot position and heading angle
  theta += deltaTheta;
  x += deltaS * cos(theta);
  y += deltaS * sin(theta);

  // Output odometry data
  Serial.print("Position (x, y): ");
  Serial.print(x, 2);
  Serial.print(", ");
  Serial.print(y, 2);
  Serial.print(" | Heading (theta): ");
  Serial.println(theta * (180 / PI)); // Convert theta to degrees
}

void loop() {
  int targetSpeed = 128;   // Set target speed (0-255 range for PWM)
  bool forward = true;     // Set direction

  setMotorSpeed(targetSpeed);
  setMotorDirection(forward);
  
  // Simulate motion and update position and heading
  calculateOdometry();
  
  delay(100); // Short delay for simulation speed
}