// Define motor driver pins
const int motor1In1 = 15;  // IN1 for Motor 1
const int motor1In2 = 2;  // IN2 for Motor 1
const int motor2In1 = 0;  // IN3 for Motor 2
const int motor2In2 = 4;  // IN4 for Motor 2
const int motor1PWM = 16;  // PWM for Motor 1 speed control
const int motor2PWM = 17;  // PWM for Motor 2 speed control

// setting PWM properties
const int freq = 1000;
const int resolution = 8;

// Encoder pins and variables
const int encoder1Pin = 33; // Encoder output for Motor 1
const int encoder2Pin = 32; // Encoder output for Motor 2
volatile int encoder1Count = 0;
volatile int encoder2Count = 0;
const int slots = 20;       // Number of slots in encoder disk

// Variables for speed, position, and heading calculations
float wheelDiameter = 0.065;  // Wheel diameter in meters (adjust if needed)
float wheelBase = 0.15;       // Distance between wheels in meters (adjust if needed)
float motorSpeed1 = 0;        // Speed of Motor 1 (in m/s)
float motorSpeed2 = 0;        // Speed of Motor 2 (in m/s)
float posX = 0, posY = 0;     // Position coordinates of the robot
float heading = 0;            // Heading angle of the robot (in radians)
unsigned long prevTime = 0;   // To calculate elapsed time

// Interrupt service routines for encoder counting
void IRAM_ATTR encoder1ISR() {
  encoder1Count++;
}

void IRAM_ATTR encoder2ISR() {
  encoder2Count++;
}

// Function to set motor direction and speed
void setMotorSpeed(int motor, int direction, int speed) {
  if (motor == 1) { // Motor 1
    digitalWrite(motor1In1, direction == 1 ? HIGH : LOW);
    digitalWrite(motor1In2, direction == 1 ? LOW : HIGH);
    ledcWrite(motor1PWM, speed);  // Channel 0 for motor1 PWM
  } 
  else if (motor == 2) { // Motor 2
    digitalWrite(motor2In1, direction == 1 ? HIGH : LOW);
    digitalWrite(motor2In2, direction == 1 ? LOW : HIGH);
    ledcWrite(motor2PWM, speed);  // Channel 1 for motor2 PWM
  }
}

// Function to calculate speed, position, and heading
void calculateOdometry() {
  // Calculate time elapsed
  unsigned long currentTime = millis();
  float deltaTime = (currentTime - prevTime) / 1000.0; // seconds
  prevTime = currentTime;

  // Calculate wheel speeds (m/s)
  float distPerCount = (PI * wheelDiameter) / slots;
  motorSpeed1 = (encoder1Count * distPerCount) / deltaTime;
  motorSpeed2 = (encoder2Count * distPerCount) / deltaTime;

  // Reset encoder counts
  encoder1Count = 0;
  encoder2Count = 0;

  // Calculate linear and angular velocity
  float linearVelocity = (motorSpeed1 + motorSpeed2) / 2;
  float angularVelocity = (motorSpeed1 - motorSpeed2) / wheelBase;
  Serial.print("angularVelocity =");
  Serial.println(angularVelocity);

  // Update robot's position and heading
  heading += angularVelocity * deltaTime;
  posX += linearVelocity * cos(heading) * deltaTime;
  posY += linearVelocity * sin(heading) * deltaTime;
}

void setup() {
  // Motor pins setup
  pinMode(motor1In1, OUTPUT);
  pinMode(motor1In2, OUTPUT);
  pinMode(motor2In1, OUTPUT);
  pinMode(motor2In2, OUTPUT);

  // configure PWM for motor speed control
  ledcAttach(motor1PWM, freq, resolution);
  ledcAttach(motor2PWM, freq, resolution);

  // Setup PWM channels for motor speed control
  //ledcAttachPin(motor1PWM, 0); // Attach Motor1 PWM to Channel 0
  //ledcAttachPin(motor2PWM, 1); // Attach Motor2 PWM to Channel 1
  //ledcSetup(0, 1000, 8);       // 1 kHz frequency, 8-bit resolution for Motor 1
  //ledcSetup(1, 1000, 8);       // 1 kHz frequency, 8-bit resolution for Motor 2

  // Encoder pins setup
  pinMode(encoder1Pin, INPUT_PULLUP);
  pinMode(encoder2Pin, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(encoder1Pin), encoder1ISR, RISING);
  attachInterrupt(digitalPinToInterrupt(encoder2Pin), encoder2ISR, RISING);

  Serial.begin(115200); // For debugging output
  prevTime = millis();  // Initialize time
}

void loop() {
  // Set motors to forward at 50% speed (adjust as needed)
  setMotorSpeed(1, 1, 128); // Motor 1 forward, 50% speed
  setMotorSpeed(2, 1, 128); // Motor 2 forward, 50% speed

  // Calculate odometry for position and heading
  calculateOdometry();

  // Print speed, position, and heading for debugging
  Serial.print("Speed (m/s): Motor1 = ");
  Serial.print(motorSpeed1);
  Serial.print(", Motor2 = ");
  Serial.println(motorSpeed2);

  Serial.print("Position (m): X = ");
  Serial.print(posX);
  Serial.print(", Y = ");
  Serial.println(posY);

  Serial.print("Heading (rad): ");
  Serial.println(heading);

  delay(100); // Adjust for desired loop rate
}