#include <TFT_eSPI.h>
#include <SPI.h>

// Define screen dimensions
#define SCREEN_WIDTH 320
#define SCREEN_HEIGHT 240

// Define button pins
#define LEFT_BUTTON_PIN 10
#define PLAY_BUTTON_PIN 11
#define FORWARD_BUTTON_PIN 5
#define RIGHT_BUTTON_PIN 4

// Motor control pins
#define ENA 17
#define IN1 18
#define IN2 19
#define ENB 28
#define IN3 26
#define IN4 27

TFT_eSPI tft = TFT_eSPI();

String inputString = ""; // String to store button presses
enum State {INITIAL, CODE, DANCE};
State currentState = INITIAL;

unsigned long lastDebounceTime[4] = {0, 0, 0, 0};
unsigned long debounceDelay = 50;

bool lastButtonState[4] = {HIGH, HIGH, HIGH, HIGH};
bool buttonState[4] = {HIGH, HIGH, HIGH, HIGH};

// Forward declarations
void moveForward();
void moveReverse();
void turnLeft();
void turnRight();
void showInitialScreen();
void drawMatrixInterface();
void executeMovements();
void checkButtons();
void handleButtonPress(int button);
void resetToInitialState();
void showApplause();
void dance();

// Color toggles
bool leftActive = false;
bool forwardActive = false;
bool rightActive = false;

void setup() {
  Serial.begin(115200);
  Serial.println("Starting...");

  // Initialize the TFT screen
  tft.init();
  tft.setRotation(1);

  // Initialize the buttons
  pinMode(LEFT_BUTTON_PIN, INPUT_PULLDOWN);
  pinMode(PLAY_BUTTON_PIN, INPUT_PULLDOWN);
  pinMode(FORWARD_BUTTON_PIN, INPUT_PULLDOWN);
  pinMode(RIGHT_BUTTON_PIN, INPUT_PULLDOWN);

  // Initialize the motors
  pinMode(ENA, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(ENB, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  // Set up initial interface
  showInitialScreen();
}

void loop() {
  checkButtons();
}

void checkButtons() {
  int buttonPins[4] = {LEFT_BUTTON_PIN, PLAY_BUTTON_PIN, FORWARD_BUTTON_PIN, RIGHT_BUTTON_PIN};
      leftActive = false;
      forwardActive = false;
      rightActive = false;
  for (int i = 0; i < 4; i++) {
    bool reading = digitalRead(buttonPins[i]);

    if (reading != lastButtonState[i]) {
      lastDebounceTime[i] = millis();
    }

    if ((millis() - lastDebounceTime[i]) > debounceDelay) {
      if (reading != buttonState[i]) {
        buttonState[i] = reading;

        if (buttonState[i] == HIGH) {
          handleButtonPress(i);
        }
      }
    }

    lastButtonState[i] = reading;
  }
}

void handleButtonPress(int button) {
  
  if (currentState == INITIAL) {
    if (button == 0) { // Left button for Dance
      currentState = DANCE;
      Serial.println("Dance mode selected");
      dance();
      resetToInitialState();
    } else if (button == 1) { // Play button for Code
      currentState = CODE;
      Serial.println("Code mode selected");
      drawMatrixInterface();
    }
    return;
  }

  if (currentState == CODE) {
    drawMatrixInterface();
    if (button == 1) { // Play button to execute commands
      executeMovements();
      resetToInitialState();
    } else {
      String command;
      if (button == 0) {
        command = "turn left";
        leftActive = !leftActive; // Toggle color
      } else if (button == 2) {
        command = "forward";
        forwardActive = !forwardActive; // Toggle color
      } else if (button == 3) {
        command = "turn right";
        rightActive = !rightActive; // Toggle color
      }
      inputString += command + ".";
      drawMatrixInterface();
      Serial.println(String("Button pressed: ") + command);
    }
  }
}

void executeMovements() {
  tft.fillScreen(TFT_BLACK);
  tft.setCursor(20, 100);
  tft.setTextColor(TFT_WHITE, TFT_BLACK);
  tft.setTextSize(4);
  tft.print("Let's go!");
  delay(500);

  Serial.println("Executing movements...");
  int startIndex = 0;
  while (startIndex < inputString.length()) {
    int endIndex = inputString.indexOf('.', startIndex);
    if (endIndex == -1) {
      endIndex = inputString.length();
    }
    String direction = inputString.substring(startIndex, endIndex);
    tft.fillScreen(random(0xFFFF));
    tft.setCursor(60, 100);
    tft.setTextColor(TFT_WHITE, TFT_BLACK);
    tft.setTextSize(4);
    tft.print(direction);
    
    Serial.println(String("Moving: ") + direction);

    if (direction == "forward") {
      moveForward();
    } else if (direction == "turn left") {
      turnLeft();
    } else if (direction == "turn right") {
      turnRight();
    }
    delay(800); 
    stopMotors();// Execute each movement for 0.5 second

    startIndex = endIndex + 1;
  }
  inputString = ""; // Clear the string after executing the movements
  showApplause();
  resetToInitialState();
}

void resetToInitialState() {
  delay(2000); // Wait for 2 seconds before showing the initial screen
  currentState = INITIAL;
  showInitialScreen();
}

void stopMotors() {
  analogWrite(ENB, 0); // Stop
  analogWrite(ENA, 0); // Stop
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
  Serial.println("Motor STOP");
}

void moveForward() {
  analogWrite(ENA, 200); // Full speed
  analogWrite(ENB, 200); // Full speed
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  Serial.println("Motor Forward");
}

void turnLeft() {
  analogWrite(ENA, 0); // Full speed
  analogWrite(ENB, 125); // Full speed
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  Serial.println("Motor Left");
}

void turnRight() {
  analogWrite(ENA, 125);
  analogWrite(ENB, 0);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW); // Full speed
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  Serial.println("Motor Right");
}

void showInitialScreen() {
  tft.fillScreen(TFT_BLACK);
  tft.setCursor(60, 10);
  tft.setTextSize(3);
  tft.setTextColor(TFT_BLUE, TFT_BLACK);
  tft.print("BLUEBO ROBOT");

  // Draw "Press P to code" box
  tft.fillRect(30, 90, 140, 60, TFT_MAGENTA );
  tft.setCursor(40, 100);
  tft.setTextColor(TFT_WHITE, TFT_MAGENTA);
  tft.setTextSize(2);
  tft.print("Press left");
  tft.setCursor(40, 130);
  tft.print("to dance");

  // Draw "Press left to dance" box
  tft.fillRect(170, 90, 120, 60, TFT_BLUE);
  tft.setCursor(180, 100);
  tft.setTextColor(TFT_BLACK, TFT_BLUE);
  tft.setTextSize(2);
  tft.print("Press P");
  tft.setCursor(180, 130);
  tft.print("to code");
  
}

void drawMatrixInterface() {
  tft.fillScreen(TFT_BLACK);

  // Draw buttons
  tft.fillRect(20, 60, 80, 60, leftActive ? TFT_ORANGE : TFT_RED);
  tft.fillRect(110, 60, 90, 60, forwardActive ? TFT_LIGHTGREY : TFT_YELLOW);
  tft.fillRect(220, 60, 80, 60, rightActive ? TFT_CYAN : TFT_GREEN);

  // Draw labels
  tft.setCursor(25, 80);
  tft.setTextColor(TFT_WHITE, leftActive ? TFT_ORANGE : TFT_RED);
  tft.setTextSize(2);
  tft.print("Left");

  tft.setCursor(115, 80);
  tft.setTextColor(TFT_BLACK, forwardActive ? TFT_LIGHTGREY : TFT_YELLOW);
  tft.setTextSize(2);
  tft.print("Forward");

  tft.setCursor(225, 80);
  tft.setTextColor(TFT_WHITE, rightActive ? TFT_CYAN : TFT_GREEN);
  tft.setTextSize(2);
  tft.print("Right");

  // Draw "Press P to play" box
  tft.fillRect(100, 160, 150, 60, TFT_BLUE);
  tft.setCursor(110, 165);
  tft.setTextColor(TFT_WHITE, TFT_BLUE);
  tft.setTextSize(2);
  tft.print("Press P to");
  tft.setCursor(140, 195);
  tft.print("play");
}

void showApplause() {
  tft.fillScreen(TFT_BLACK);
  tft.setCursor(50, 100);
  tft.setTextColor(TFT_WHITE, TFT_BLACK);
  tft.setTextSize(4);
  tft.print("Applause!");

  // Simulate applause animation
  for (int i = 0; i < 10; i++) {
    tft.fillScreen(random(0xFFFF));
    tft.print("clap!");
    delay(100);
  }
}

void dance() {
  tft.fillScreen(TFT_BLACK);
  tft.setCursor(50, 100);
  tft.setTextColor(TFT_WHITE, TFT_BLACK);
  tft.setTextSize(4);
  tft.print("Dancing!");

  // Simulate dance movements
  for (int i = 0; i < 10; i++) {
    int move = random(3);
    switch (move) {
      case 0:
        moveForward();
        tft.fillScreen(TFT_BLUE);
        tft.setCursor(50, 100);
        tft.setTextColor(TFT_WHITE, TFT_BLUE);
        tft.setTextSize(4);
        tft.print("Forward");
        break;
      case 1:
        turnLeft();
        tft.fillScreen(TFT_RED);
        tft.setCursor(50, 100);
        tft.setTextColor(TFT_WHITE, TFT_RED);
        tft.setTextSize(4);
        tft.print("Left");
        break;
      case 2:
        turnRight();
        tft.fillScreen(TFT_GREEN);
        tft.setCursor(50, 100);
        tft.setTextColor(TFT_WHITE, TFT_GREEN);
        tft.setTextSize(4);
        tft.print("Right");
        break;
    }
    delay(100); // Execute each movement for 0.5 second
  }
  stopMotors();
  showApplause();
}