#include "Arduino.h"
#include "Wire.h"
#include "MeMCore.h"

MeGyro gyro(PORT_1); // Initialize the gyro sensor on port 1
MeDCMotor _leftMotor(9);
MeDCMotor _rightMotor(10);
// Define constants
const int targetSpeed = 50; // Speed for translation movements
const int wheelDistanceBetweenWheels = 11; // Distance between the wheels in centimeters
const int wheelRadius = 3; // Radius of the wheels in centimeters

// Function to calculate wheel speeds based on translational and rotational speeds
void calculateWheelSpeeds(float v, float theta_dot, float &vL, float &vR) {
  vL = v/wheelRadius - (wheelDistanceBetweenWheels*theta_dot)/wheelRadius*2;
  vR = v/wheelRadius + (wheelDistanceBetweenWheels*theta_dot)/wheelRadius*2;
}

// Function to perform rotation movement
void rotate(float targetAngle, float v) {
  const float KP = 0.5; // Proportional control gain
  const float MIN_SPEED = 10; // Minimum rotation speed

  float currentAngle;
  float error;
  float rotationSpeed;

  do {
    // Get current angle
    gyro.update();
    currentAngle = gyro.getAngleZ();
    Serial.print("Current angle :");
    Serial.print(currentAngle);
    Serial.print(" Objective :");
    Serial.println(targetAngle);

    // Calculate error
    error = targetAngle - currentAngle;
    while (error > 180) error -= 360;
    while (error < -180) error += 360;

    // Calculate rotation speed using proportional control
    rotationSpeed = KP * error;

    // Constrain rotation speed to ensure it doesn't exceed the minimum speed
    rotationSpeed = constrain(rotationSpeed, -MIN_SPEED, MIN_SPEED);

    // Calculate wheel speeds based on rotation speed
    float vL, vR;
    calculateWheelSpeeds(v, rotationSpeed, vL, vR);

    // Apply speeds to motors
    _leftMotor.run(-vL);
    _rightMotor.run(vR);
  } while (abs(error) > 5); // Continue rotation until error is within acceptable threshold
}


// Function to perform translation movement
void translate(float distance) {
  // Calculate time required to travel given distance at target speed
  float time = distance / targetSpeed;
  _leftMotor.run(-targetSpeed);
  _rightMotor.run(targetSpeed);
  Serial.print("Time calculated for next segment : ");
  Serial.println(time*1000);
  delay(time * 1000); // Convert time to milliseconds
}

void setup() {
  Serial.begin(9600); // Initialize serial communication
  gyro.begin(); // Initialize the gyro sensor
  _leftMotor.run(0);
  _rightMotor.run(0);
}

void loop() {
  // Array containing the 2D positions of the 4 corners of the square
  float targetPoints[4][2] = {{0, 0}, {10, 0}, {10, 10}, {0, 10}};

  for (int i = 0; i < 4; i++) {
    float targetX = targetPoints[i][0];
    float targetY = targetPoints[i][1];

    // Calculate rotation angle towards target point
    float currentX = 0; // Assume starting from origin
    float currentY = 0; // Assume starting from origin
    float targetAngle = atan2(targetY - currentY, targetX - currentX) * 180 / PI;

    // Perform rotation towards target point
    rotate(targetAngle, targetSpeed);

    // Calculate distance to target point
    float distance = sqrt(pow(targetX - currentX, 2) + pow(targetY - currentY, 2));

    // Perform translation towards target point
    translate(distance);
  }
}