#include <math.h>

// Define the coordinates for the points (A, B, C, D)
float points[4][2] = {
  {0.0, 0.0},           // A (0,0)
  {44.55, -22.7},       // B (44.55, -22.7)
  {-22.7, -44.55},      // C (-22.7, -44.55)
  {21.85, -67.25}       // D (21.85, -67.25)
};

// Define constants
const float radius_of_wheel = 0.35; // meters
const float speed = 7.5;            // m/s
const float kcal_per_meter = 0.4;
const float perimeter_of_wheel = 2 * PI * radius_of_wheel;

// Function to calculate Euclidean distance between two points
float calculate_distance(float p1[2], float p2[2]) {
  return sqrt(pow(p2[0] - p1[0], 2) + pow(p2[1] - p1[1], 2));
}

// Function to calculate time (distance / speed)
float calculate_time(float distance) {
  return distance / speed;
}

// Function to calculate calories (distance * kcal per meter)
float calculate_calories(float distance) {
  return distance * kcal_per_meter;
}

// Function to calculate tracker angle
float calculate_tracker_angle(float distance) {
  return fmod(distance / perimeter_of_wheel, 360);
}

// Function to generate all possible routes
void generate_routes(int start, int end, int routes[3][3]) {
  routes[0][0] = start;
  routes[0][1] = end;

  // Adding some possible routes with intermediates
  routes[1][0] = start;
  routes[1][1] = 2; // Assuming point 2 (C) as intermediate
  routes[1][2] = end;

  routes[2][0] = start;
  routes[2][1] = 1; // Assuming point 1 (B) as intermediate
  routes[2][2] = end;
}

// Main function
void setup() {
  // Initialize serial communication
  Serial.begin(9600);

  // Display start message
  Serial.println("GTU ELECTRONICS");

  // Randomly assign start and end points (for simplicity, we use point 0 as start and point 3 as end here)
  int start = random(0, 4); // Randomly pick a start point
  int end = random(0, 4);   // Randomly pick an end point

  // Ensure start and end are not the same
  while (start == end) {
    end = random(0, 4);
  }

  // Display assigned points
  Serial.print("Start: ");
  Serial.print(start);
  Serial.print(" End: ");
  Serial.println(end);

  // Generate possible routes
  int routes[3][3]; // 3 routes with maximum 3 points
  generate_routes(start, end, routes);

  // Choose random route (here, we just choose one of the possible routes)
  int random_route_index = random(0, 3);

  // Calculate distances and other parameters for both routes
  float distance_random = 0.0;
  for (int i = 0; i < 2; i++) {
    distance_random += calculate_distance(points[routes[random_route_index][i]], points[routes[random_route_index][i+1]]);
  }

  float time_random = calculate_time(distance_random);
  float calories_random = calculate_calories(distance_random);
  float tracker_angle_random = calculate_tracker_angle(distance_random);

  // Calculate for the shortest route (for simplicity, assuming route 0 is the shortest here)
  float distance_shortest = 0.0;
  for (int i = 0; i < 2; i++) {
    distance_shortest += calculate_distance(points[routes[0][i]], points[routes[0][i+1]]);
  }

  float time_shortest = calculate_time(distance_shortest);
  float calories_shortest = calculate_calories(distance_shortest);
  float tracker_angle_shortest = calculate_tracker_angle(distance_shortest);

  // Display results on Serial Monitor
  Serial.println("Route 1 (Random):");
  Serial.print("Distance: ");
  Serial.print(distance_random);
  Serial.print(" m, Time: ");
  Serial.print(time_random);
  Serial.print(" s, Calories: ");
  Serial.print(calories_random);
  Serial.print(" kcal, Tracker Angle: ");
  Serial.println(tracker_angle_random);

  Serial.println("Route 2 (Shortest):");
  Serial.print("Distance: ");
  Serial.print(distance_shortest);
  Serial.print(" m, Time: ");
  Serial.print(time_shortest);
  Serial.print(" s, Calories: ");
  Serial.print(calories_shortest);
  Serial.print(" kcal, Tracker Angle: ");
  Serial.println(tracker_angle_shortest);

  // Print Differences
  Serial.println("Differences:");
  Serial.print("Difference in Distance: ");
  Serial.println(fabs(distance_random - distance_shortest));
  Serial.print("Difference in Time: ");
  Serial.println(fabs(time_random - time_shortest));
  Serial.print("Difference in Calories: ");
  Serial.println(fabs(calories_random - calories_shortest));
  Serial.print("Difference in Tracker Angle: ");
  Serial.println(fabs(tracker_angle_random - tracker_angle_shortest));
}

void loop() {
  // Nothing to do in loop
}