#include <Arduino.h>
#include <Wire.h>
#include <SoftwareSerial.h>
#include "MeMCore.h"

double _wheelDiameter = 6.0;
double _distanceBetweenWheels = 11.5;
int _basicMotorSpeed = 100;

// Target position and angle
double _targetX = 20.0;
double _targetY = 15.0;
double _targetAngle = 0.0;

// Current position and angle
double _currentX = 0.0;
double _currentY = 0.0;
double _thetaCurrent = 0.0;
double _thetaBaseline = 0.0;
double _errorAngle = 0.0;

MeDCMotor _leftMotor(9);
MeDCMotor _rightMotor(10);

MeGyro _gyro;

void setup() {
  Serial.begin();

  // Set no speed for motors initially
  _leftMotor.run((9) == M1 ? -(0) : (0));
  _rightMotor.run((10) == M1 ? -(0) : (0));

  // Initialize gyro
  _gyro.begin();

  // Wait for 2 seconds
  delay(2000);

  // Retrieve initial baseline for theta angle
  _thetaBaseline = _gyro.getAngleZ();
}

void loop() {
  // Calculate target angle based on target position
  _targetAngle = calculateTargetAngle(_targetX, _targetY);

  // Rotate to target angle
  rotateToTargetAngle();

  // Move forward for corresponding distance
  moveForwardToTarget();

  // Stop the robot after reaching the target
  stopMotors();
}

void serialEvent() {
  // No serial input used in this version
}

// Function to calculate target angle based on target position
double calculateTargetAngle(double targetX, double targetY) {

  double deltaX = targetX - _currentX;
  double deltaY = targetY - _currentY;

  return atan2(deltaY, deltaX) * 180 / M_PI;
}

void move(int leftMotorSpeed, int rightMotorSpeed)
{
  _leftMotor.run((9)==M1?-(leftMotorSpeed):(leftMotorSpeed));
  _rightMotor.run((10)==M1?-(rightMotorSpeed):(rightMotorSpeed));
}

// Function to rotate the robot to the target angle
void rotateToTargetAngle() {
  _errorAngle = _targetAngle - _thetaCurrent;

  while (abs(_errorAngle) > 5) {
    double w = _Kp_angle * _errorAngle;
    double leftWheelSpeed  = -_distanceBetweenWheels * w / _wheelDiameter;
    double rightWheelSpeed  = _distanceBetweenWheels * w / _wheelDiameter;

    // crop wheel speed
    if (leftWheelSpeed > 250)
      leftWheelSpeed = 250;
    if (rightWheelSpeed > 250)
      rightWheelSpeed = 250;

    // send the commands
    move(rightWheelSpeed, leftWheelSpeed);

    // retrieve new values from gyro
    _gyro.update();

    // get the theta angle (Rz)
    _thetaCurrent = _gyro.getAngleZ() - _thetaBaseline;
    _errorAngle = _targetAngle - _thetaCurrent;
  }
}
  }
}

// Function to move forward for the corresponding distance
void moveForwardToTarget() {
  // Calculate distance to target using Pythagorean theorem
  double distanceToTarget = sqrt(pow(_targetX - _currentX, 2) + pow(_targetY - _currentY, 2));

  // Calculate time to reach target based on speed = distance / time
  double timeToTarget = distanceToTarget / _basicMotorSpeed; // time in seconds

  // Move the robot forward
  move(rightWheelSpeed, leftWheelSpeed);

  // Wait for the calculated time
  delay(timeToTarget * 1000); // delay function takes time in milliseconds

  // Stop the motors
  stopMotors();
}

// Function to stop the motors
void stopMotors() {
  _leftMotor.stop();
  _rightMotor.stop();
}