#include <Arduino.h>
#include <FastLED.h>
#include "Lighting.h"
#include "Audio.h"
#include "LCD_Graphics.h"
#include "Adafruit_LEDBackpack.h"
#include <Adafruit_HX8357.h>
#include "Motors.h"

//ir beams, pull low, detected when high
//targets are high when hit

// Define the ball state variables (previously only declared as `extern`)
int ballX = 100;
int ballY = 100;
int ballDX = 2;
int ballDY = 2;
int ballRadius = 6;
unsigned long lastBallUpdate = 0;
uint8_t ballHue = 0;


 Adafruit_7segment matrix = Adafruit_7segment();
 
 // Use hardware SPI (on Uno, #13, #12, #11) and the above for CS/DC
 Adafruit_HX8357 tft = Adafruit_HX8357(TFT_CS, TFT_DC, TFT_RST);

// Game states
enum GameState { IDLE, GAMEPLAY, EDITING };
GameState currentState = IDLE;

// FreeRTOS handles
TaskHandle_t lightingTaskHandle;
TaskHandle_t gameTaskHandle;
TaskHandle_t lcdTaskHandle;
TaskHandle_t motorTaskHandle;

// motors
ESP_FlexyStepper spinner;
ESP_FlexyStepper hoop;

bool gameStarted = false;

// cursors to write highscores
int score1_cursorY;
int score2_cursorY;
int score3_cursorY;
int letterUnderlineX;
int letterUnderlineY;


int currentScore = 0;
int score1 = 500, score2 = 300, score3 = 100;
char initials1[4] = "AAA";
char initials2[4] = "BBB";
char initials3[4] = "CCC";
int editingScoreSlot = 0;

// Previous button states
bool lastBeam1 = HIGH;
bool lastBeam2 = HIGH;
bool lastTarget1 = HIGH;
bool lastTarget2 = HIGH;

bool ir1;
bool ir2;
bool target1;
bool target2;
bool b1;  // Button 1 - now active HIGH
bool b2;  // Button 2 - now active HIGH

//Initial variables
char letters[28] = {'A','B','C','D','E','F','G','H','I','J','K','L','M','N','O','P','Q','R','S','T','U','V','W','X','Y','Z','-','\0'};
int letterIndex = 0;
int charPos = 0;

// === Forward declarations (add these near the top of the file) ===
void handleButton1Press(int slot);
void handleButton2Press(int slot);



void highScore(){
  unsigned long start = micros();
  tft.setCursor(0, 0);
  tft.setTextSize(7);
  tft.setTextColor(HX8357_GREEN);
  tft.println("Welcome to Pinball!");
  tft.setTextSize(3);
  tft.println();
  tft.setTextColor(HX8357_RED);
  tft.setTextSize(5);
  tft.println("High Scores:");
  tft.setTextColor(HX8357_WHITE);
  tft.setTextSize(3);
  tft.println();

  score1_cursorY = tft.getCursorY();
  // tft.fillRect(0, score1_cursorY, 200, 30, HX8357_BLACK); // Adjust width/height as needed
  tft.setCursor(0, score1_cursorY); // Move cursor back 
  tft.print("1: ");
  tft.print(initials1);
  tft.print(" - ");
  tft.println(score1);

  tft.setTextSize(2);
  tft.println();
  tft.setTextSize(3);
  score2_cursorY = tft.getCursorY();
  // tft.fillRect(0, score2_cursorY, 200, 30, HX8357_BLACK); // Adjust width/height as needed
  tft.setCursor(0, score2_cursorY); // Move cursor back 
  tft.print("2: ");
  tft.print(initials2);
  tft.print(" - ");
  tft.println(score2);

  tft.setTextSize(2);
  tft.println();
  tft.setTextSize(3);
  score3_cursorY = tft.getCursorY();
  // tft.fillRect(0, score3_cursorY, 200, 30, HX8357_BLACK); // Adjust width/height as needed
  tft.setCursor(0, score3_cursorY); // Move cursor back 
  tft.print("3: ");
  tft.print(initials3);
  tft.print(" - ");
  tft.println(score3);
}

void gameTask(void *pvParameters) {
  static int lastDisplayedScore = -1;
  bool lastIR1 = HIGH;
  bool lastIR2 = HIGH;

  lastTarget1 = LOW, lastTarget2 = LOW;
  bool lastButton1 = LOW, lastButton2 = LOW;  // buttons now default to LOW (unpressed)

  while (true) {
    // ======= INITIALS ENTRY MODE =======
    if (currentState == EDITING) {
      if (lastButton1 == LOW && b1 == HIGH) handleButton1Press(editingScoreSlot);
      if (lastButton2 == LOW && b2 == HIGH) handleButton2Press(editingScoreSlot);
      lastButton1 = b1;
      lastButton2 = b2;
      vTaskDelay(pdMS_TO_TICKS(50));
      continue;
    }

    // ======= GAMEPLAY EVENTS =======
    if (lastIR1 == HIGH && ir1 == LOW) {
      Serial.println("[IR1] Beam broken");

      if (currentState == IDLE) {
        currentState = GAMEPLAY;
        currentScore = 0;
        currentLightMode = LIGHT_CRAZY;
        Serial.println("Game started!");
      } else if (currentState == GAMEPLAY) {
        play_MONO_wav_file("/YouLose.wav");
        currentLightMode = LIGHT_RED_BLINK;

        // High Score Handling
        if (currentScore > score1) {
          score3 = score2; strcpy(initials3, initials2);
          score2 = score1; strcpy(initials2, initials1);
          score1 = currentScore; strcpy(initials1, "AAA");
          editingScoreSlot = 1;
        } else if (currentScore > score2) {
          score3 = score2; strcpy(initials3, initials2);
          score2 = currentScore; strcpy(initials2, "AAA");
          editingScoreSlot = 2;
        } else if (currentScore > score3) {
          score3 = currentScore; strcpy(initials3, "AAA");
          editingScoreSlot = 3;
        } else {
          currentState = IDLE;
          currentScore = 0;
          continue;
        }

        // Start Editing
        charPos = 0;
        letterIndex = 0;
        currentState = EDITING;
        int underlineY = (editingScoreSlot == 1 ? score1_cursorY :
                          editingScoreSlot == 2 ? score2_cursorY : score3_cursorY) + 25;
        tft.fillRect(50, underlineY, 20, 3, HX8357_WHITE);
      }

      currentScore = 0;
    }

    if (lastIR2 == HIGH && ir2 == LOW && currentState == GAMEPLAY) {
      currentScore += 250;
      play_MONO_wav_file("/Point.wav");
      currentLightMode = LIGHT_CRAZY;
    }

    if (lastTarget1 == LOW && target1 == HIGH && currentState == GAMEPLAY) {
      currentScore += 100;
      play_MONO_wav_file("/Point.wav");
      currentLightMode = LIGHT_GREEN_BLINK;
    }

    if (lastTarget2 == LOW && target2 == HIGH && currentState == GAMEPLAY) {
      currentScore += 150;
      play_MONO_wav_file("/Point.wav");
      currentLightMode = LIGHT_GREEN_BLINK;
    }

    lastIR1 = ir1;
    lastIR2 = ir2;
    lastTarget1 = target1;
    lastTarget2 = target2;
    lastButton1 = b1;
    lastButton2 = b2;

    matrix.println(currentScore);
    matrix.writeDisplay();    
    Serial.printf("Current Score: %d\n",currentScore);

    vTaskDelay(pdMS_TO_TICKS(10));
  }
}


void lcdTask(void *pvParameters) {
  GameState lastState = IDLE;

  while (true) {
    if (currentState != lastState) {
      tft.fillScreen(HX8357_BLACK);  // Clear only on state change
      lastState = currentState;

      if (currentState == GAMEPLAY) {
        initPinballScene();  // Static scene at start
      } else {
        highScore();         // Show high scores in IDLE or EDITING
      }
    }

    if (currentState == GAMEPLAY) {
      updateBallPosition();  // Animate pinball splash
    }

    vTaskDelay(pdMS_TO_TICKS(33));  // ~30 FPS
  }
}

void motorTask(void *pvParameters) {
  int stepCount = 0;
  int direction = 1;  // 1 = forward, -1 = backward

  while (true) {
    //digitalWrite(enableHOOP, LOW);
    //digitalWrite(enableSPINNER, LOW);

    // Move motors in current direction
    HOOP.startJogging(direction);
    SPINNER.startJogging(direction);

    digitalWrite(enableHOOP, HIGH);
    digitalWrite(enableSPINNER, HIGH);

    stepCount++;

    if (stepCount >= 4) {
      direction *= -1;  // reverse direction
      stepCount = 0;
    }

    vTaskDelay(pdMS_TO_TICKS(200));  // short delay between steps
  }
}

void handleButton1Press(int slot) {
  Serial.println("Button 1 Pressed");
  
  letterIndex = (letterIndex + 1) % 27;
  char selected = letters[letterIndex];

  // Update initials
  if (slot == 1) initials1[charPos] = selected;
  else if (slot == 2) initials2[charPos] = selected;
  else if (slot == 3) initials3[charPos] = selected;

  // Determine cursor Y based on slot
  int y = (slot == 1 ? score1_cursorY :
           slot == 2 ? score2_cursorY :
                       score3_cursorY);

  // Clear and redraw only the active initials line
  tft.fillRect(0, y, 240, 30, HX8357_BLACK);  // adjust width as needed
  tft.setCursor(0, y);
  tft.setTextSize(3);
  tft.setTextColor(HX8357_WHITE);
  tft.print(slot);
  tft.print(": ");
  if (slot == 1) tft.print(initials1);
  else if (slot == 2) tft.print(initials2);
  else if (slot == 3) tft.print(initials3);

  // Redraw underline under selected character
  letterUnderlineX = 50 + charPos * 20;
  int underlineY = y + 25;
  tft.fillRect(0, underlineY, 240, 5, HX8357_BLACK);  // Clear all underline
  tft.fillRect(letterUnderlineX, underlineY, 20, 3, HX8357_WHITE);
}


void handleButton2Press(int slot) {
  charPos++;
  Serial.println("Button 2 Pressed");

  if (charPos > 2) {
    currentState = IDLE;
    charPos = 0;
    currentScore = 0;  
    Serial.println("Initials entry complete.");
    return;
  }

  letterIndex = 0;

  // Determine Y position for the underline
  int y = (slot == 1 ? score1_cursorY :
           slot == 2 ? score2_cursorY :
                       score3_cursorY);

  // Clear and redraw the initials row
  tft.fillRect(0, y, 240, 30, HX8357_BLACK);  // adjust width if needed
  tft.setCursor(0, y);
  tft.setTextSize(3);
  tft.setTextColor(HX8357_WHITE);
  tft.print(slot);
  tft.print(": ");
  if (slot == 1) tft.print(initials1);
  else if (slot == 2) tft.print(initials2);
  else if (slot == 3) tft.print(initials3);

  // Draw new underline under the next character
  letterUnderlineX = 50 + charPos * 20;
  int underlineY = y + 25;
  tft.fillRect(0, underlineY, 240, 5, HX8357_BLACK);  // clear any existing underline
  tft.fillRect(letterUnderlineX, underlineY, 20, 3, HX8357_WHITE);
}

void setup() {
  Serial.begin(115200);

  delay(1000);
  SetupLights();

  Serial.println("Motors Setup Begin");
  MotorsSetup();
  spinner.connectToPins(SPINNER_STEP_PIN, SPINNER_DIRECTION_PIN);
  hoop.connectToPins(HOOP_STEP_PIN, HOOP_DIRECTION_PIN);
  Serial.println("Motors Setup End");
  
  // begin LCD display
  tft.begin(8000000);
  tft.fillScreen(HX8357_BLACK);
  tft.setRotation(3);
  highScore();

  /*
  Serial.println("AudioSetup Begin");
  i2s_new_channel(&chan_cfg, &tx_handle, NULL);
  i2s_channel_init_std_mode(tx_handle, &std_cfg);
  Serial.println("AudioSetup End");

  if (!SPIFFS.begin(true)) {
  Serial.println("SPIFFS Mount Failed");
  return;
  } else {
    Serial.println("SPIFFS mounted successfully");
  }
    */

  //initialize 7 segment
  matrix.begin(0x70);  // Default I2C address for Adafruit 7-segment
  matrix.clear();
  matrix.print(currentScore);
  matrix.writeDisplay();


  pinMode (LED_BUILTIN, OUTPUT);   
  pinMode (buttonPin1, INPUT_PULLUP);   
  pinMode (buttonPin2, INPUT_PULLUP);  
  pinMode (IRPin1, INPUT);   

  pinMode (targetPin1, INPUT_PULLUP);   
  pinMode (targetPin2, INPUT_PULLUP);   
  pinMode (IRPin2, INPUT);  

  bool ir1 = digitalRead(IRPin1);
  bool ir2 = digitalRead(IRPin2);
  bool target1 = digitalRead(targetPin1);
  bool target2 = digitalRead(targetPin2);
  bool b1 = digitalRead(buttonPin1);  // Button 1 - now active HIGH
  bool b2 = digitalRead(buttonPin2);  // Button 2 - now active HIGH

  /*
  xTaskCreatePinnedToCore(motorTask, "MotorTask", 4096, NULL, 1, &motorTaskHandle, 1);
  xTaskCreatePinnedToCore(gameTask, "GameTask", 4096, NULL, 1, &gameTaskHandle, 1);
  xTaskCreatePinnedToCore(lightTask, "LightTask", 2048, NULL, 1, &lightingTaskHandle, 1);
  xTaskCreatePinnedToCore(lcdTask, "LCDTask", 4096, NULL, 1, &lcdTaskHandle, 1);
  */

  Serial.println("Setup End");
}

void loop(){
  highScore();

  Serial.println("In loop");
  
  switchLights();

  if(currentScore%2==0){
    turnMotorsBackward();
  }
  else{  
    turnMotorsForward();
  }

  if(digitalRead(buttonPin1)==LOW){
    digitalWrite(LED_BUILTIN,HIGH);
  }
  else if(digitalRead(buttonPin2)==LOW){
    digitalWrite(LED_BUILTIN,HIGH);
  }
  else if(digitalRead(targetPin1)==LOW){
    digitalWrite(LED_BUILTIN,HIGH);
  }
  else if(digitalRead(targetPin2)==LOW){
    digitalWrite(LED_BUILTIN,HIGH);
  }
  else{
    digitalWrite(LED_BUILTIN,LOW);
  }
  
  currentScore+=1;
  matrix.println(currentScore);
  matrix.writeDisplay();  
}