// UART, I2C LCD, NeoPixle and strange serial behaviour
// https://forum.arduino.cc/t/uart-i2c-lcd-neopixle-and-strange-serial-behaviour/1419313

/* Game controller for Arduino Uno
   - 3x3 micro switch matrix (3 outputs pulsed, 3 inputs read)
   - WS2812 (Adafruit_NeoPixel) LEDs (one per item position)
   - Adafruit Sound Board connected over SoftwareSerial
   - Implements setupGame() and startGame() per user spec
*/

#include <Adafruit_NeoPixel.h>
#include <PostNeoSWSerial.h>
#include <Servo.h>
#include <LiquidCrystal_I2C.h>
#include <Wire.h>


LiquidCrystal_I2C lcd(0x27, 16, 2) ;
//
// === CONFIGURATION ===
//

// Matrix pins (change to match wiring)
const uint8_t ROW_PINS[3] = {5, 6, 7};     // outputs (pulsed)
const uint8_t COL_PINS[3] = {A0, A1, A2};     // inputs (read)

// NeoPixel
const uint8_t LED_PIN = 4;
const uint16_t NUM_LEDS = 9;
Adafruit_NeoPixel strip(NUM_LEDS, LED_PIN, NEO_GRB + NEO_KHZ800);

// Sound board (SoftwareSerial)
const uint8_t SOUND_RX_PIN = 8;  // UNO pin reading from sound board TX (unused here)
const uint8_t SOUND_TX_PIN = 9;  // UNO pin sending to sound board RX

PostNeoSWSerial soundSerial(SOUND_RX_PIN, SOUND_TX_PIN);
// other pins
const uint8_t ESC_PIN = 10;          // local
const uint8_t DISPENCER_PIN = 11;     // local
const uint8_t POWER_PIN = 12;         // local
const uint8_t LID_BTN_PIN = A3;
const uint8_t RESET_BTN_PIN = 3;
const uint8_t ENABLE_PIN = 2;

Servo ESC;
static uint16_t delays[300];
// Timing & animation parameters
const uint16_t SPIN_TOTAL_MS = 5000;     // spin slow-down period (5 seconds)
const uint16_t SPIN_MIN_DELAY = 12;      // minimum per-step delay (fast)
const uint16_t SPIN_MAX_DELAY = 500;     // maximum per-step delay (end)
const uint16_t POST_ALL_IN_PLACE_DELAY = 2000; // 2s after all items placed
const uint16_t SHORT_DELAY_AFTER_REMOVAL = 2000; // short pause before spin starts
const uint8_t MAX_SAFE_REMOVALS = 6; // when 6th removal cause failure behavior

// Colors
uint32_t COLOR_OFF;
uint32_t COLOR_ORANGE;
uint32_t COLOR_GREEN;
uint32_t COLOR_RED;
uint32_t COLOR_BLUE;

// =========================================================================
// State
bool lastPresence[9];         // presence at last scan
bool removedPermanent[9];     // items already removed and set green
uint8_t removedCount = 0;     // how many unique items removed (and turned green)
volatile bool isGameEnabled;
bool gameReady;
bool escPowered;
//
// =========================================================================

void disableSystem(){
  isGameEnabled=false;
}

// Helper: map row, col to index 0..8
inline uint8_t idx(uint8_t row, uint8_t col){
  return row*3 + col;
}

// Scan matrix and return presence[] (true if switch detects item present)
// We pulse each row HIGH briefly and read columns. If column reads HIGH when row is high,
// that switch is closed (item present).
void scanMatrix(bool presence[9]){
  //Serial.println(F("Scaning matrix"));
  // start with everything false
  for (uint8_t i=0;i<9;i++) presence[i] = false;

  for (uint8_t r=0; r<3; r++){
    // pulse row HIGH
    digitalWrite(ROW_PINS[r], HIGH);
    delayMicroseconds(120); // small settle time

    for (uint8_t c=0; c<3; c++){
      //Serial.print(F("Row: ")); Serial.print(r); Serial.print(F(", Col: ")); Serial.println(c);
      int val = digitalRead(COL_PINS[c]);
      // We assume wiring provides HIGH to the column input when switch closed and row HIGH.
      // If your wiring uses pull-ups/active-low, invert the test here.
      if (val){
        presence[idx(r,c)] = true;
      }
      //Serial.print(F("Val: ")); Serial.println(val); Serial.println();
      delayMicroseconds(10);
    }

    // set row back LOW
    digitalWrite(ROW_PINS[r], LOW);
    delayMicroseconds(30);
  }
  //Serial.print(F("Got: "));
  //for (uint8_t i=0;i<9;i++) {Serial.print(presence[i]); }
  //Serial.println();
}


void setup() {
  Serial.begin(115200); // for debugging
  soundSerial.begin(9600); // default; adapt to your soundboard's baud if needed

  Serial.println(F("---------------------------------"));
  Serial.println(F("  Starburst Projectile System"));
  Serial.println(F("---------------------------------"));
  delay(100);
  Serial.println(F("Begin init"));

  ESC.attach(ESC_PIN);
  ESC.write(0);

  isGameEnabled=false;
  gameReady=false;
  escPowered=false;

  // NeoPixel init and colors
  strip.begin();
  strip.show(); // all off
  COLOR_OFF = strip.Color(0,0,0);
  COLOR_ORANGE = strip.Color(90, 200, 0); // tweak if needed
  COLOR_GREEN = strip.Color(255, 0, 0);
  COLOR_RED = strip.Color(0, 255, 0);
  COLOR_BLUE = strip.Color(0, 0, 255);

  Wire.begin();
  lcd.init( ) ; // Starts LCD
  lcd.clear(); //C1ears any characters on the LCD display
  lcd.backlight( ) ; // Make sure backlight is on

  lcd.setCursor (4, 0) ;
  lcd.print(F("Starburst"));
  lcd.setCursor (0, 1) ;
  lcd.print(F("Projectile System"));

  // Matrix pins
  for (uint8_t i=0;i<3;i++){
    pinMode(ROW_PINS[i], OUTPUT);
    digitalWrite(ROW_PINS[i], LOW); // idle LOW
    pinMode(COL_PINS[i], INPUT);    // we'll read these (pulled by rows when pulsed)
  }

  // initialize states
  for (uint8_t i=0;i<9;i++){
    lastPresence[i] = false;
    removedPermanent[i] = false;
  }

  pinMode(POWER_PIN, OUTPUT);
  pinMode(DISPENCER_PIN, OUTPUT);
  pinMode(RESET_BTN_PIN, INPUT_PULLUP);
  pinMode(LID_BTN_PIN, INPUT_PULLUP);
  pinMode(ENABLE_PIN, INPUT_PULLUP);

  digitalWrite(POWER_PIN, HIGH);
  digitalWrite(DISPENCER_PIN, HIGH);

  attachInterrupt(digitalPinToInterrupt(ENABLE_PIN), disableSystem, RISING);

  Serial.println(F("Performing initial scan"));
  bool presence[9];
  uint8_t countPresent = 0;
  scanMatrix(presence);
  for (uint8_t i=0;i<9;i++){
    if (presence[i]){
      countPresent++;
    } 
  }
  if (countPresent==NUM_LEDS){
    Serial.println(F("All items detectes at boot, game in ready state"));
    gameReady=true;
  }

  // Let the hardware settle
  delay(200);
}

// NeoPixel helpers
void setLedColor(uint8_t i, uint32_t color){
  if (i >= NUM_LEDS) return;
  strip.setPixelColor(i, color);
}

void showAllOff(){ for (uint8_t i=0;i<NUM_LEDS;i++) strip.setPixelColor(i, COLOR_OFF); strip.show(); }
void showAllOrange(){ for (uint8_t i=0;i<NUM_LEDS;i++) strip.setPixelColor(i, COLOR_ORANGE); strip.show(); }
void showAllGreen(){ for (uint8_t i=0;i<NUM_LEDS;i++) strip.setPixelColor(i, COLOR_GREEN); strip.show(); }
void showAllRed(){ for (uint8_t i=0;i<NUM_LEDS;i++) strip.setPixelColor(i, COLOR_RED); strip.show(); }

// Sound senders — adapt to your board's protocol. For many boards you either:
// - send a small text command over serial, or
// - toggle a dedicated trigger pin, or
// - send numeric bytes. Replace bodies below as appropriate for your hardware.
void playPop(){
  // Example: send "POP\n" — replace with the correct command for your board.
  soundSerial.println(F("#01"));
  // If your board requires a single byte ID, use soundSerial.write(byteVal);
}

void playSuccess(){
  soundSerial.println(F("#02"));
}

void playFailure(){
  soundSerial.println(F("#03"));
}

void playTrackNumber(uint8_t track){
  // Generic helper for a numbered track if that is how your board works:
  // Example: sending "T3\n" to play track 3. Replace accordingly.
  soundSerial.print('T');
  soundSerial.println(track);
}

// startMotor stub — user requested that on failure we call start motor after 3s.
// Implement actual motor control in this function.
void startMotor(){
  Serial.println(F("startMotor(): called (implement motor start here)"));
  // Example: digitalWrite(MOTOR_PIN, HIGH);
}

// Helper: flash all LEDs color n times
void flashAll(uint32_t color, uint8_t times=3, uint16_t onMs=160, uint16_t offMs=120){
  for (uint8_t t=0;t<times;t++){
    for (uint8_t i=0;i<NUM_LEDS;i++) setLedColor(i, color);
    strip.show();
    delay(onMs);
    showAllOff();
    delay(offMs);
  }
}

// ========================================================================
// Function 1: setupGame()
// - scan matrix, set LEDs orange for positions that have item
// - when all 9 items are in place, wait 2s, play sound, and turn all LEDs off
void setupGame(){
  Serial.println(F("setupGame: scanning items..."));

  // initialize states
  for (uint8_t i=0;i<9;i++){
    lastPresence[i] = false;
    removedPermanent[i] = false;
  }
  removedCount=0;

  bool presence[9];

  uint8_t countPresent = 0;

  while (!(countPresent>=9)){
    scanMatrix(presence);
    countPresent = 0;
    for (uint8_t i=0;i<9;i++){
      lastPresence[i] = presence[i]; // store baseline
      if (presence[i]){
        setLedColor(i, COLOR_ORANGE);
        countPresent++;
      } else {
        setLedColor(i, COLOR_OFF);
      }
    }
    strip.show();

    Serial.print(F("Items present: "));
    Serial.println(countPresent);
    delay(200);
  }

  
  Serial.println(F("All 9 in place."));
  flashAll(COLOR_GREEN, 2, 200, 200);
  playSuccess(); // Use success sound (adapt as needed)
  delay(POST_ALL_IN_PLACE_DELAY);
  showAllOff();
  gameReady=true;

}


// Spin animation that starts fast and slows over SPIN_TOTAL_MS,
// playing pop sound on each step, and ending exactly on targetIndex.
// If failure==true, flash red & play failure; else flash green & play success.
// Returns when finished and updates removedPermanent/removedCount if success.
// Spin animation without storing a 300-element delay table.
// Computes each delay step on demand, saving ~600 bytes of SRAM.
void runSpinAndResolve(uint8_t startIndex, uint8_t targetIndex, bool failure){
    const uint16_t baseCycles = 6;
    uint16_t totalSteps = baseCycles * NUM_LEDS;

    uint8_t offset = (targetIndex + NUM_LEDS - (startIndex % NUM_LEDS)) % NUM_LEDS;
    totalSteps += offset;

    if (totalSteps > 300) totalSteps = 300;

    uint8_t cur = startIndex;

    // Track total time for dynamic scaling
    const float totalMs = SPIN_TOTAL_MS;
    float elapsed = 0;

    for (uint16_t step = 0; step < totalSteps; step++){
        float t = (float)step / (float)totalSteps;       // 0..1
        float ease = 1.0f - (1.0f - t)*(1.0f - t);        // quadratic ease-out
        float d = SPIN_MIN_DELAY + (SPIN_MAX_DELAY - SPIN_MIN_DELAY) * ease;

        // scale delay so entire spin lasts exactly SPIN_TOTAL_MS
        float remainingSteps = (float)(totalSteps - step);
        float remainingTime = totalMs - elapsed;
        float scaledDelay = remainingTime / remainingSteps;
        if (scaledDelay < 1) scaledDelay = 1;

        elapsed += scaledDelay;

        // Step spinner
        cur = (cur + 1) % NUM_LEDS;

        // Draw LEDs (persistent green + spinner orange)
        for (uint8_t i=0; i<NUM_LEDS; i++){
            if (removedPermanent[i]) setLedColor(i, COLOR_GREEN);
            else setLedColor(i, COLOR_OFF);
        }
        setLedColor(cur, COLOR_ORANGE);
        strip.show();

        playPop();
        delay((uint16_t)scaledDelay);
        if (!isGameEnabled) {
          break;
        }
    }

    // END OF SPIN
    if (failure){
        playFailure();
        flashAll(COLOR_RED, 3, 200, 180);
        delay(3000);
        startMotor();
    } else {
        playSuccess();
        flashAll(COLOR_GREEN, 3, 160, 120);
        removedPermanent[targetIndex] = true;
        removedCount++;
    }

    // Redraw final permanent LEDs
    for (uint8_t i=0;i<NUM_LEDS;i++){
        if (removedPermanent[i]) setLedColor(i, COLOR_GREEN);
        else setLedColor(i, COLOR_OFF);
    }
    strip.show();
}



// ========================================================================
// Function 2: startGame()
// - set all LEDs to orange
// - watch for an item removal (presence true->false)
// - when removal observed: all LEDs except that location switch off
// - short delay, then spin animation that slows over 5s and stops on removed location
// - each step plays pop sound
// - after stop flash green, play success sound, mark LED green permanently
// - repeat until 5 items removed. On removal of 6th: behave same but final flash red, play failure and call startMotor() after 3s
// - previous removed LEDs remain green
void startGame(){
  if (gameReady && isGameEnabled){
    Serial.println(F("startGame: commencing..."));
  }
  else{
    Serial.println(F("The game has not been initialised please set up the game"));
    for (uint8_t i=0;i<NUM_LEDS;i++){
      setLedColor(i, COLOR_RED);
    }
    strip.show();
    exit(0);
  }
  gameReady=false;

  // initial capture of presence state
  bool presence[9];
  scanMatrix(presence);
  for (uint8_t i=0;i<9;i++){
    lastPresence[i] = presence[i];
  }

  // set all LEDs to orange initially, even if no item - as per spec
  for (uint8_t i=0;i<NUM_LEDS;i++){
    if (!removedPermanent[i]) setLedColor(i, COLOR_ORANGE);
    else setLedColor(i, COLOR_GREEN); // already removed items stay green
  }
  strip.show();

  // Main loop: keep running until we reach failure or user stops program.
  // We'll poll matrix in a loop and act when a present->absent change occurs
  // NOTE: This is blocking sequence per removal (which is required by spec).
  while (isGameEnabled){
    // scan
    scanMatrix(presence);

    // detect any new removal: previously present and now absent, and not already marked removedPermanent
    int removedIndex = -1;
    for (uint8_t i=0;i<NUM_LEDS;i++){
      if (lastPresence[i] == true && presence[i] == false && !removedPermanent[i]){
        removedIndex = i;
        break;
      }
    }

    // update baseline presence for next detection
    for (uint8_t i=0;i<NUM_LEDS;i++) lastPresence[i] = presence[i];

    if (removedIndex >= 0){
      Serial.print(F("Detected removal at index "));
      Serial.println(removedIndex);

      // On removal: all LEDs except that location should switch off.
      for (uint8_t i=0;i<NUM_LEDS;i++){
        if (i == removedIndex){
          // keep it orange as the spec says "all LEDs except the one corresponding to that location should switch off"
          setLedColor(i, COLOR_ORANGE);
        } else {
          if (removedPermanent[i]) setLedColor(i, COLOR_GREEN);
          else setLedColor(i, COLOR_OFF);
        }
      }
      strip.show();

      // short delay
      delay(SHORT_DELAY_AFTER_REMOVAL);

      // choose a start index for spin. We'll start from currently lit orange LED (the removedIndex),
      // but spec says the loop cycles to the next LED each time, so we'll start from removedIndex.
      uint8_t startIndex = removedIndex;

      // Determine if this removal is the special (6th) removal
      uint8_t futureRemovalCount = removedCount + 1; // if we accept this removal as new
      bool isFailure = (futureRemovalCount >= MAX_SAFE_REMOVALS); // when equals 6 => failure

      // Run spin, which will end on removedIndex
      runSpinAndResolve(startIndex, removedIndex, isFailure);

      // If it was failure, we should stop the game loop or continue? Spec: on removal of 6th item the animation should play as normal however when it gets to the end all Leds should flash red ... and startMotor() called after 3s.
      // After failure, we break out.
      if (isFailure){
        Serial.println(F("Failure condition reached (6th removal). Exiting startGame loop."));
        break;
      }

      // If not failure, continue until 5 items removed (spec said repeat until 5 items removed; on removal of 6th do failure).
      if (removedCount >= 5){
        Serial.println(F("Reached 5 removals. Game cycle continues awaiting 6th removal (which will be failure)."));
        // continue loop — the spec says cycle repeats until 5 items have been removed; we'll continue monitoring.
      }
    }

    // tiny delay to avoid hammering CPU
    delay(40);
  } // end while
  if(!isGameEnabled)Serial.println(F("Game has been disabled, halting midway through!"));

  Serial.println(F("startGame(): finished (exited loop)."));
}

void setEscPower(){
  if (escPowered){
      Serial.println(F("Powering on motor"));
      ESC.write(0);
      delay(200);
      digitalWrite(POWER_PIN, LOW);
      delay(2500);
  }
  else{
    Serial.println(F("Powering off motor"));
    ESC.write(0);
    digitalWrite(POWER_PIN, HIGH);
  }

}

// ========================================================================
// Example loop() usage: you can call setupGame() and startGame() from loop or from Serial commands.
// For demo, we'll call nothing in loop — user should call these functions from their higher-level code.
// ========================================================================
void loop(){
  Serial.println(F("Initialization complete"));
  Serial.println(F("Wating for command"));
  while (!isGameEnabled){
    if (!digitalRead(RESET_BTN_PIN)) {
      Serial.println(F("Reset button detected"));
      setupGame();
    }

    if (!digitalRead(ENABLE_PIN) && !escPowered) {
      Serial.println(F("Game is now enabled"));
      escPowered=true;
      setEscPower();
    }
    if (digitalRead(ENABLE_PIN) && escPowered) {
      Serial.println(F("Game is now disabled"));
      escPowered=false;
      setEscPower();
    }
    if (gameReady&&escPowered){isGameEnabled=true; Serial.println(F("All startup conditions met"));}


    // For testing / manual triggers via serial console:
    if (Serial.available()){
      String cmd = Serial.readStringUntil('\n');
      cmd.trim();
      if (cmd.equalsIgnoreCase("init")){
        Serial.println(F("This is the problem locaton!"));
        Serial.print('>');
        Serial.print(cmd);
        Serial.println('<');
        setupGame();
      } else if (cmd.equalsIgnoreCase("start")){
        startGame();

      } else if (cmd.equalsIgnoreCase("scan")){
        bool p[9]; scanMatrix(p);
        Serial.print(F("Presence: "));
        for (uint8_t i=0;i<9;i++) Serial.print(p[i] ? '1' : '0');
        Serial.println();

      } else if (cmd.equalsIgnoreCase("light")){
        Serial.println(F("Starting rrggbb flash"));
        flashAll(COLOR_RED, 2, 200, 200);
        delay(500);
        flashAll(COLOR_GREEN, 2, 200, 200);
        delay(500);
        flashAll(COLOR_BLUE, 2, 200, 200);
      } 
      else if (cmd.equalsIgnoreCase("D_on")){
        digitalWrite(DISPENCER_PIN,LOW);
        Serial.println(F("dispenser is on"));
      } 
      else if (cmd.equalsIgnoreCase("D_off")){
        digitalWrite(DISPENCER_PIN,HIGH);
        Serial.println(F(" dispenser has stopped"));
      } 
      else if (cmd.equalsIgnoreCase("E_off")){
        digitalWrite(POWER_PIN,HIGH);
        Serial.println(F("ESC is now off"));
      } 
      else if (cmd.equalsIgnoreCase("E_on")){
        digitalWrite(POWER_PIN,LOW);
        Serial.println(F("ESC is now powered"));
      } 
      else if (cmd.equalsIgnoreCase("pop")){
        playPop();
      } 
      else if (cmd.equalsIgnoreCase("sucsess")){
        playSuccess();
      } 
      else if (cmd.equalsIgnoreCase("list")){
        soundSerial.println(F("L"));
      } 
      else if (cmd.equalsIgnoreCase("fail")){
        playFailure();
      } 
      else {
        Serial.println(F("Commands: init | start | scan | light | E_on | E_off | D_on | D_off | pop | sucsess | list"));
      }
      
    }
    // Forward soundboard serial output to the Arduino USB serial monitor
    while (soundSerial.available()) {
        char c = soundSerial.read();
        Serial.print(c);   // writes raw data exactly as received
    }

  }
  
  Serial.println(F("Setup complete, waiting for lid"));
  while(digitalRead(LID_BTN_PIN)){ delay(200);} // wate for close
  Serial.println(F("Lid closed"));
  delay(1000);//debounce

  Serial.println(F("Waiting for lid to be opened"));
  flashAll(COLOR_GREEN, 3, 400, 400);
  while(!digitalRead(LID_BTN_PIN)&&isGameEnabled){ 
    delay(200);
  } //wate for open
  Serial.println(F("GO! GO! GO!"));
  startGame();

  delay(1000);
  escPowered=false;
  setEscPower();

  Serial.println(F("End of game, waiting for reset"));
  while (isGameEnabled && digitalRead(RESET_BTN_PIN)) {delay(200);} // the game has finished wait for it to be reset or disabled

  isGameEnabled=false;
}