// File: main.ino
#include <Arduino.h>
#include "maze.h"
#include "MazeMap.h"

// Global variables
int prev_cell = 0; // Previous cell, starting at 0
int curr_cell = 1; // Current cell, starting at 1
bool visited[625] = {false}; // Visited cells tracking

// Predefined route as movements
const char plannedRoute[] = {
  'f', 'f', 'l', 'r', 'r', 'l', 'f', 'l', 'l', 'r', 'r', 'l', 'r', 'f', 'l', 'r',
  'f', 'l', 'l', 'f', 'f', 'f', 'r', 'r', 'f', 'l', 'r', 'r', 'l', 'l', 'r', 'r',
  'l', 'r', 'r', 'l', 'l', 'f', 'l', 'f', 'f', 'r', 'f', 'f', 'l', 'r', 'r', 'l',
  'l', 'f', 'r', 'l', 'r', 'f', 'l', 'f', 'f', 'r', 'l', 'l', 'f', 'f', 'r', 'f',
  'f', 'r', 'l', 'r', 'l', 'f', 'l', 'l', 'r', 'f', 'l', 'r', 'r', 'f', 'r', 'f',
  'l', 'l', 'r', 'l', 'l', 'r', 'r', 'f', 'f', 'r', 'f', 'r', 'l', 'r', 'l', 'f',
  'l', 'l', 'r', 'r', 'f', 'r', 'f', 'f', 'l', 'f', 'l', 'f', 'l', 'r', 'r', 'f'
};
int routeLength = sizeof(plannedRoute) / sizeof(plannedRoute[0]);
int routeIndex = 0;

// RGB LED pins
const int redPin = 9;
const int greenPin = 10;
const int bluePin = 11;

// 7-Segment Display pins (example mapping)
const int segPins[7] = {2, 3, 4, 5, 6, 7, 8};

// Directions
enum Direction {FORWARD, LEFT, RIGHT, BACKWARD};
const char directionSymbols[4] = {'f', 'l', 'r', 'b'};

// Helper functions
void setRGB(int r, int g, int b);
void displayDirection(char direction);
void moveRover(char direction);
bool followPlannedRoute(int maze_map[625][3]);
void transmitSteeringAngles(int curr_cell, int prev_cell);

void setup() {
  // Initialize pins
  pinMode(redPin, OUTPUT);
  pinMode(greenPin, OUTPUT);
  pinMode(bluePin, OUTPUT);
  for (int i = 0; i < 7; i++) {
    pinMode(segPins[i], OUTPUT);
  }

  Serial.begin(9600); // Start serial communication

  // Start the maze-solving process
  if (!followPlannedRoute(maze_map)) {
    Serial.println("Rover could not complete the route. Check maze or route.");
  }
}

void loop() {
  // Do nothing, task handled in setup
}

void setRGB(int r, int g, int b) {
  digitalWrite(redPin, r);
  digitalWrite(greenPin, g);
  digitalWrite(bluePin, b);
}

void displayDirection(char direction) {
  // Update the 7-segment display for the direction
  switch (direction) {
    case 'f': // Forward
      digitalWrite(segPins[0], HIGH);
      digitalWrite(segPins[1], LOW);
      digitalWrite(segPins[2], HIGH);
      digitalWrite(segPins[3], HIGH);
      digitalWrite(segPins[4], HIGH);
      digitalWrite(segPins[5], HIGH);
      digitalWrite(segPins[6], LOW); // Example for "f"
      break;
    case 'l': // Left
      // Update pins for "l"
      break;
    case 'r': // Right
      // Update pins for "r"
      break;
    case 'b': // Backward
      // Update pins for "b"
      break;
  }
}

void moveRover(char direction) {
  // Set RGB and display for the direction
  switch (direction) {
    case 'f':
      setRGB(0, 1, 0); // Green
      displayDirection('f');
      delay(1000); // Forward: 1 second
      break;
    case 'l':
      setRGB(0, 0, 1); // Blue
      displayDirection('l');
      delay(2000); // Left turn: 2 seconds
      break;
    case 'r':
      setRGB(1, 0, 0); // Red
      displayDirection('r');
      delay(2000); // Right turn: 2 seconds
      break;
    case 'b':
      setRGB(1, 0, 1); // Magenta
      displayDirection('b');
      delay(3000); // Backward: 3 seconds
      break;
  }
}

bool followPlannedRoute(int maze_map[625][3]) {
  Serial.println("Starting route navigation...");

  // Iterate through the planned route
  while (routeIndex < routeLength) {
    char next_move = plannedRoute[routeIndex];
    routeIndex++;

    // Initialize variables for movement validation
    int next_prev_cell = prev_cell;
    int next_curr_cell = curr_cell;
    bool flag_move;

    // Find the next move direction dynamically
    maze_sensor(maze_map, prev_cell, curr_cell, next_move, next_prev_cell, next_curr_cell, flag_move);

    if (flag_move) {
      // Valid move
      Serial.print("Moving: ");
      Serial.println(next_move);

      // Mark the current cell as visited
      visited[curr_cell - 1] = true;

      // Transmit steering angles
      transmitSteeringAngles(curr_cell, prev_cell);

      // Command rover to move
      moveRover(next_move);

      // Update previous and current cell
      prev_cell = curr_cell;
      curr_cell = next_curr_cell;

    } else {
      // Failed to move in the required direction
      Serial.print("Failed to move: ");
      Serial.println(next_move);
      return false;
    }
  }

  // Check if the rover reached the final cell
  if (curr_cell == 625) {
    Serial.println("Route completed successfully!");
    return true;
  } else {
    Serial.println("Route incomplete. Did not reach target.");
    return false;
  }
}

void transmitSteeringAngles(int curr_cell, int prev_cell) {
  // Calculate the Turn-in-Place double Ackermann steering angles
  float M = 1.3; // Wheel base
  float L = 0.7; // Track width

  float angle1, angle2, angle3, angle4, angle5, angle6;

  // Example calculation (these calculations should be based on the actual Turn-in-Place logic)
  float delta_x = (curr_cell - prev_cell) % 25; // Horizontal distance
  float delta_y = (curr_cell - prev_cell) / 25; // Vertical distance

  // Compute steering angles for all 6 wheels (simple approximation for demonstration)
  angle1 = atan2(delta_y, M + L);
  angle2 = atan2(delta_y, M - L);
  angle3 = atan2(delta_y, -M + L);
  angle4 = atan2(delta_y, -M - L);
  angle5 = atan2(delta_y, M);
  angle6 = atan2(delta_y, -M);

  // Transmit angles via Serial
  Serial.print("Steering Angles: ");
  Serial.print("n1=");
  Serial.print(angle1);
  Serial.print(", n2=");
  Serial.print(angle2);
  Serial.print(", n3=");
  Serial.print(angle3);
  Serial.print(", n4=");
  Serial.print(angle4);
  Serial.print(", n5=");
  Serial.print(angle5);
  Serial.print(", n6=");
  Serial.println(angle6);
}