#include "SPI.h"
#include "Adafruit_GFX.h"
#include "Adafruit_ILI9341.h"

#define TFT_DC 7
#define TFT_CS 6
#define TFT_MOSI 11
#define TFT_CLK 13
#define TFT_RST 10
#define TFT_MISO 12
Adafruit_ILI9341 screen = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_MOSI, TFT_CLK, TFT_RST, TFT_MISO);

int boxSize = 50; // Size of each box
int currentPosition1 = 5; // Initial position for first box (center)
int currentPosition2 = 6; // Initial position for second box (center-right)

// Define colors in RGB565 format
#define BLUE_CYAN 0x53DF
#define Background 0x0000

// Positions for the boxes on a 3x4 grid (3 rows, 4 columns)
int positions[12][2] = {
  {0, 0}, {60, 0}, {120, 0}, {180, 0}, // First row
  {0, 108}, {60, 108}, {120, 108}, {180, 108}, // Second row
  {0, 216}, {60, 216}, {120, 216}, {180, 216}  // Third row
};

// Mic mappings to positions
int micPositionMap[5] = {1, 4, 5, 6, 9}; // Mapped positions (index of positions array)

// Analog pins for controlling the movement
int micPins[5] = {A1, A2, A3, A4, A5}; // 5 microphones

// Function prototypes
void initializeScreen();
void drawRoundedBox(int x, int y);
void clearBox(int x, int y);
void moveBoxesSmooth(int fromX1, int fromY1, int toX1, int toY1, int fromX2, int fromY2, int toX2, int toY2);
int getHighestMicIndex();
void moveBoxes(int &currentPosition1, int &currentPosition2);

void setup() {
  Serial.begin(9600); // Initialize Serial communication
  initializeScreen();
  
  // Draw initial positions for both boxes
  drawRoundedBox(positions[currentPosition1][0], positions[currentPosition1][1]);
  drawRoundedBox(positions[currentPosition2][0], positions[currentPosition2][1]);
}

void loop() {
  // Move both boxes together based on the mic with the highest value
  moveBoxes(currentPosition1, currentPosition2);

  delay(100); // Small delay to prevent excessive reading
}

// Function to initialize the screen
void initializeScreen() {
  screen.begin();
  screen.fillScreen(Background); // Set background to #202531
  screen.setTextColor(ILI9341_WHITE);  
  screen.setTextSize(2);
  screen.setCursor(0, 0);
}

// Function to draw a single rounded box at any position
void drawRoundedBox(int x, int y) {
  screen.fillRoundRect(x, y, boxSize, boxSize, 10, BLUE_CYAN); // Rounded rectangle with radius 10
}

// Function to clear the box at any position
void clearBox(int x, int y) {
  screen.fillRect(x, y, boxSize, boxSize, Background); // Use the background color to clear
}

// Function to move smoothly from one position to another
void moveBoxesSmooth(int fromX1, int fromY1, int toX1, int toY1, int fromX2, int fromY2, int toX2, int toY2) {
  int steps = 12; // Number of steps for smooth movement
  for (int i = 0; i <= steps; i+=4) {
    int currentX1 = fromX1 + (toX1 - fromX1) * i / steps;
    int currentY1 = fromY1 + (toY1 - fromY1) * i / steps;
    int currentX2 = fromX2 + (toX2 - fromX2) * i / steps;
    int currentY2 = fromY2 + (toY2 - fromY2) * i / steps;

    // Draw the boxes at the new locations
    drawRoundedBox(currentX1, currentY1);
    drawRoundedBox(currentX2, currentY2);
    delay(30); // Delay for smoother animation

    // Only clear the previous boxes if not the final step
    if (i < steps) {
      clearBox(currentX1, currentY1);
      clearBox(currentX2, currentY2);
    }
  }

  // Draw the final positions without clearing
  drawRoundedBox(toX1, toY1);
  drawRoundedBox(toX2, toY2);
}

// Function to find the mic with the highest value
int getHighestMicIndex() {
  int highestValue = 0;
  int highestIndex = 0;
  
  // Loop through all microphones to find the highest value
  Serial.print("Mic values: ");
  for (int i = 0; i < 5; i++) {
    int micValue = analogRead(micPins[i]);
    Serial.print(micValue);
    Serial.print(" ");
    if (micValue > highestValue) {
      highestValue = micValue;
      highestIndex = i;
    }
  }
  Serial.println();
  Serial.print("Highest mic index: ");
  Serial.println(highestIndex);
  Serial.print("Highest value: ");
  Serial.println(highestValue);
  
  return highestIndex; // Return the index of the mic with the highest value
}

// Function to move the boxes based on the highest mic input
void moveBoxes(int &currentPosition1, int &currentPosition2) {
  int highestMicIndex = getHighestMicIndex(); // Get the index of the mic with the highest value
  int targetPosition1 = micPositionMap[highestMicIndex]; // Map the highest mic to its target position

  // Ensure the second box always stays adjacent to the first box
  int targetPosition2 = targetPosition1 + 1; // Move to the adjacent position

  // Print movement direction
  Serial.print("Direction: ");
  switch (highestMicIndex) {
    case 0:
      Serial.println("Top");
      break;
    case 1:
      Serial.println("Left");
      break;
    case 2:
      Serial.println("Center");
      break;
    case 3:
      Serial.println("Right");
      break;
    case 4:
      Serial.println("Bottom");
      break;
    default:
      Serial.println("Unknown");
      break;
  }

  // If the boxes are already in the target position, do nothing
  if (currentPosition1 == targetPosition1 && currentPosition2 == targetPosition2) {
    return; // No need to move, they are already in position
  }

  // Move both boxes together, keeping them adjacent
  moveBoxesSmooth(positions[currentPosition1][0], positions[currentPosition1][1], 
                  positions[targetPosition1][0], positions[targetPosition1][1], 
                  positions[currentPosition2][0], positions[currentPosition2][1], 
                  positions[targetPosition2][0], positions[targetPosition2][1]);
  
  // Update current positions
  currentPosition1 = targetPosition1;
  currentPosition2 = targetPosition2;
}