// VERSION 1.0
#define SW_VERS "1.0"

// TARGET
// LOLIN(WEMOS) D1 R2 & mini

// DEBUG Const
#define DEBUG true  //set to true for debug output, false for no debug output
#define DEBUG_SERIAL \
  if (DEBUG) Serial

// Add HW target (eventually boards installed)

// Add External Library
// Display SSD1306
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
// FastLED
#include <FastLED.h>

// Add Internal Library
#include "BTN.h"
#include "Splash.h"

// Pin assignment
#define PIN_WS2812B 13
#define PIN_BTN1 15
#define PIN_BTN2 2
#define PIN_BTN3 4
#define PIN_BTN4 16
#define PIN_BTN5 17
#define PIN_SCL1 26
#define PIN_SDA1 25
#define PIN_SCL2 18
#define PIN_SDA2 5

#define OLED_RESET -1 // Reset pin # (or -1 if sharing Arduino reset pin)

// HW Constants
#define NUM_COLS 5
#define NUM_ROWS 5
#define NUM_PIXELS NUM_COLS*NUM_ROWS     // Please EVEN number the number of LEDs (pixels) on WS2812B LED strip
#define BRIGHTNESS 255    // 255 max
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 32 // OLED display height, in pixels

// SW Constants
#define NUM_LAND_COLORS 5
#define M_YELLOW  0xFFFF00
#define M_ORANGE  0xFF7F00
#define M_RED     0xFF0000
#define M_BLUE    0x0000FF
#define M_GREEN   0x00FF00
uint32_t const landColors[NUM_LAND_COLORS] = {M_YELLOW, M_ORANGE, M_RED, M_BLUE, M_GREEN};

#define M_WHITE   0xFFFFFF
#define M_MAGENTA 0xFF00FF
#define M_CYAN    0x00FFFF

// HW Global variables
CRGB ws2812b[NUM_PIXELS];
MyBtn btn[NUM_LAND_COLORS];
int pinBtn[NUM_LAND_COLORS] = {PIN_BTN1, PIN_BTN2, PIN_BTN3, PIN_BTN4, PIN_BTN5};

TwoWire I2Cone = TwoWire(0);
TwoWire I2Ctwo = TwoWire(1);
Adafruit_SSD1306 display1(SCREEN_WIDTH, SCREEN_HEIGHT, &I2Cone, OLED_RESET);
Adafruit_SSD1306 display2(SCREEN_WIDTH, SCREEN_HEIGHT, &I2Ctwo, OLED_RESET);
//Adafruit_SSD1306 display1(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
//Adafruit_SSD1306 display2(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// SW Global variables
enum gameStateEnum {INIT_LEDCONQUER, START_LEDCONQUER, GAME_LEDCONQUER, WINNER_LEDCONQUER, FINISH_LEDCONQUER, INIT_LED, START_LED, GAME_LED, WINNER_LED, FINISH_LED};
enum gameStateEnum gameState = INIT_LEDCONQUER;
enum gameStateEnum gameStateOld = START_LEDCONQUER;
bool first = false;

uint32_t rgbPlay1 = M_CYAN;    // Color of player1
uint32_t rgbPlay2 = M_MAGENTA;    // Color of player2
bool readyPlay1 = false;  // Player1 ready to play
bool readyPlay2 = false;  // Player2 ready to play

bool turnPlay1 = true;
bool winPlay1, winPlay2 = false;      // true when the relative player won
bool lockPlay1, lockPlay2 = false;

uint8_t scorePlay1, scorePlay2 = 0;

void displayText(uint8_t player, char const * text, int n = 0) {
  char line[20];
  int16_t  x1, y1;
  uint16_t w, h;

  snprintf(line, 20, text, n);

  if (player == 1) {
    //Wire.begin(PIN_SDA1, PIN_SCL1); 
    display1.clearDisplay();
    display1.setTextSize(2);
    display1.setTextColor(WHITE); // Draw white text
    display1.getTextBounds(line, 0, 0, &x1, &y1, &w, &h); //calc width of new string
    display1.setCursor((SCREEN_WIDTH - w) / 2, (SCREEN_HEIGHT - h) / 2);
    display1.print(line);
    display1.display();
  }
  else {
    //Wire.begin(PIN_SDA2, PIN_SCL2);
    display2.clearDisplay();
    display2.setTextSize(2);
    display2.setTextColor(WHITE); // Draw white text
    display2.getTextBounds(line, 0, 0, &x1, &y1, &w, &h); //calc width of new string
    display2.setCursor((SCREEN_WIDTH - w) / 2, (SCREEN_HEIGHT - h) / 2);
    display2.print(line);
    display2.display();  
  }
}

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

  DEBUG_SERIAL.println("SETUP"); 

  FastLED.addLeds<NEOPIXEL, PIN_WS2812B>(ws2812b, NUM_PIXELS);

  for (int i=0; i<NUM_LAND_COLORS; i++) {
    pinMode(pinBtn[i], INPUT_PULLUP);
    btn[i].configPin(pinBtn[i]);
  }

  I2Cone.begin(PIN_SDA1, PIN_SCL1, 100000); 
  I2Ctwo.begin(PIN_SDA2, PIN_SCL2, 100000);  

  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  //Wire.begin(PIN_SDA1, PIN_SCL1);
  if(!display1.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { 
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }

  //Wire.begin(PIN_SDA2, PIN_SCL2);
  if(!display2.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { 
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }

  //Wire.begin(PIN_SDA1, PIN_SCL1);
  display1.clearDisplay();
  //Wire.begin(PIN_SDA2, PIN_SCL2);
  display2.clearDisplay(); 

  randomSeed(analogRead(0));

  // Print SW Version
  displayText(1, SW_VERS);
  displayText(2, SW_VERS);

  delay(2000);
}

void loop() {
  readBtns();
    
  // First evaluation for first time in new state
  if (gameStateOld != gameState) {
    first = true;
    gameStateOld = gameState;
  }

  switch (gameState) {
    case INIT_LEDCONQUER:
      if (first) {
        first = false;
        clearLeds();
        clearBtnsAllEvents();                      
        
        readyPlay1 = readyPlay2 = false;

        //Wire.begin(PIN_SDA1, PIN_SCL1);
        display1.clearDisplay();
        display1.drawBitmap(0, 0, epd_bitmap_Splash_LedConquer, 128, 32, 1);
        display1.display();
        //Wire.begin(PIN_SDA2, PIN_SCL2);
        display2.clearDisplay();
        display2.drawBitmap(0, 0, epd_bitmap_Splash_LedConquer, 128, 32, 1);
        display2.display();

        DEBUG_SERIAL.println("INIT_LEDCONQUER");
      }
      
      if (btn[0].wasReleased()) {
        btn[0].clearReleased();
        readyPlay1 = true;
        displayText(1, "Ready");
      }
      
      if (btn[NUM_LAND_COLORS-1].wasReleased()) {
        btn[NUM_LAND_COLORS-1].clearReleased();
        readyPlay2 = true;
        displayText(2, "Ready");
      }

      animInitLedConquer();

      if (readyPlay1 && readyPlay2) {
        gameState = START_LEDCONQUER;
      }      

      // TODO bonus game?
      if (btn[0].wasLongPressed() && btn[NUM_LAND_COLORS-1].wasLongPressed()) {
        btn[0].clearLongPressed();
        btn[NUM_LAND_COLORS-1].clearLongPressed();
        gameState = INIT_LED;
      }
      
      break;     
    case START_LEDCONQUER:
      if (first) {
        first = false;
        //clearLeds();
        clearBtnsAllEvents();

        displayText(1, "Starting...");
        displayText(2, "Starting...");              

        DEBUG_SERIAL.println("START_LEDCONQUER");
      }

      if (animStartLedConquer()) {
        gameState = GAME_LEDCONQUER;
      }

      break;
    case GAME_LEDCONQUER:
      if (first) {
        first = false;
        //clearLeds();
        clearBtnsAllEvents();        

        scorePlay1 = scorePlay2 = 1;
        winPlay1 = winPlay2 = false;
        lockPlay1 = lockPlay2 = false;
        turnPlay1 = true;

        displayText(1, "Your Turn\n%d", (int)scorePlay1);
        displayText(2, "Wait\n%d", (int)scorePlay2); 

        DEBUG_SERIAL.println("GAME_LEDCONQUER");
      }

      for (int i=0; i<NUM_LAND_COLORS; i++) {
        if (btn[i].wasPressed()) {
          //btn[i].clearPressed();
          clearBtnsAllEvents();

          if (turnPlay1) {
            scorePlay1 += updateLeds(rgbPlay1, landColors[i]);
          }
          else {
            scorePlay2 += updateLeds(rgbPlay2, landColors[i]);
          }
          FastLED.show();

          turnPlay1 = !turnPlay1;
          if (turnPlay1) {
            displayText(1, "Your Turn\n%d", (int)scorePlay1);
            displayText(2, "Wait\n%d", (int)scorePlay2);
            if (checkPlayLock(rgbPlay1)) {
              lockPlay1 = true;
              scorePlay2 = NUM_PIXELS - scorePlay1;
              winPlay1 = scorePlay1 >= scorePlay2;
              winPlay2 = scorePlay2 >= scorePlay1;
              gameState = WINNER_LEDCONQUER;
            }
          }
          else {
            displayText(1, "Wait\n%d", (int)scorePlay1);
            displayText(2, "Your Turn\n%d", (int)scorePlay2); 
            if (checkPlayLock(rgbPlay2)) {
              lockPlay2 = true;
              scorePlay1 = NUM_PIXELS - scorePlay2;
              winPlay2 = scorePlay2 >= scorePlay1;
              winPlay1 = scorePlay1 >= scorePlay2;
              gameState = WINNER_LEDCONQUER;
            }
          }
          break;
        }
      }
      break;
    case WINNER_LEDCONQUER:
      if (first) {
        first = false;
        clearBtnsAllEvents();        

        if (winPlay1) {
          if (winPlay2) {
            displayText(1, "DRAW: %d", scorePlay1);
          }
          else {
            displayText(1, "WIN: %d", scorePlay1);
          }
        }
        else {
          displayText(1, "LOSE: %d", scorePlay1);
        }

        if (winPlay2) {
          if (winPlay1) {
            displayText(1, "DRAW: %d", scorePlay1);
          }
          else {          
            displayText(2, "WIN: %d", scorePlay2);
          }
        }
        else {
          displayText(2, "LOSE: %d", scorePlay2);
        }       

        DEBUG_SERIAL.println("WINNER_LEDCONQUER");
      }

      if (animWinnerLedConquer()) {
        gameState = FINISH_LEDCONQUER;
      }

      break;
    case FINISH_LEDCONQUER:
      if (first) {
        first = false;
        clearBtnsAllEvents();       

        DEBUG_SERIAL.println("FINISH_LEDCONQUER");
      }

      for (int i=0; i<NUM_LAND_COLORS; i++) {
        if (btn[i].wasReleased()) {
          clearBtnsAllEvents(); 
          gameState = INIT_LEDCONQUER;
        }
      }

      break;
    case INIT_LED:
      if (first) {
        first = false;
        clearLeds();
        clearBtnsAllEvents(); 
        
        readyPlay1 = readyPlay2 = false;
          
        //Wire.begin(PIN_SDA1, PIN_SCL1);
        display1.clearDisplay();
        display1.drawBitmap(0, 0, epd_bitmap_Splash_LedConquer, 128, 32, 1);
        display1.display();
        //Wire.begin(PIN_SDA2, PIN_SCL2);
        display2.clearDisplay();
        display2.drawBitmap(0, 0, epd_bitmap_Splash_LedConquer, 128, 32, 1);
        display2.display();          

        DEBUG_SERIAL.println("INIT_LED");
      }
      
      if (btn[0].wasReleased()) {
        btn[0].clearReleased();
        readyPlay1 = true;
        displayText(1, "Ready");
      }

      if (btn[NUM_LAND_COLORS-1].wasReleased()) {
        btn[NUM_LAND_COLORS-1].clearReleased();
        readyPlay2 = true;
        displayText(2, "Ready");
      }

      if (readyPlay1 && readyPlay2) {
        gameState = START_LED;
      }

      animInitLed();

      if (btn[0].wasLongPressed() && btn[NUM_LAND_COLORS].wasLongPressed()) {
        btn[0].clearLongPressed();
        btn[NUM_LAND_COLORS].clearLongPressed();
        gameState = INIT_LED;
      }    
      break;
    case START_LED:
      if (first) {
        first = false;
        clearLeds();
        clearBtnsAllEvents();

        DEBUG_SERIAL.println("START_LED");
      }

      if (animStartLed()) {
        gameState = GAME_LED;
      }

      break;    
    case GAME_LED:
      if (first) {
        first = false;
        clearLeds();
        clearBtnsAllEvents();       
        
        scorePlay1 = scorePlay2 = 5;
        winPlay1 = winPlay2 = false;

        displayText(1, "GO !");
        displayText(2, "GO !");        

        DEBUG_SERIAL.println("GAME_LED");
      }

      if (btn[0].wasPressed()) {
        btn[0].clearPressed();

        displayText(1, "%d", (int)(scorePlay1));            
        displayText(2, "%d", (int)(scorePlay2));                 
      }

      if (btn[NUM_LAND_COLORS].wasPressed()) {
        btn[NUM_LAND_COLORS].clearPressed();

        displayText(2, "%d", (int)(scorePlay2)); 
        displayText(1, "%d", (int)(scorePlay1));                
      }

      if ((animGameLed()) || (scorePlay1 == 0) || (scorePlay2 == 0)) {
        winPlay1 = scorePlay1 <= scorePlay2;
        winPlay2 = scorePlay2 <= scorePlay1;
        gameState = WINNER_LED;
      }

      break;
    case WINNER_LED:
      if (first) {
        first = false;
        clearLeds();
        clearBtnsAllEvents();

        if (winPlay1) {
          displayText(1, "WIN: %d", scorePlay1);
        }
        else {
          displayText(1, "LOSE: %d", scorePlay1);
        }

        if (winPlay2) {
          displayText(2, "WIN: %d", scorePlay2);
        }
        else {
          displayText(2, "LOSE: %d", scorePlay2);
        }   

        DEBUG_SERIAL.println("WINNER_LED");
      }

      if (animWinnerLed()) {
        gameState = FINISH_LED;
      }

      break;     
    case FINISH_LED:
      if (first) {
        first = false;
        clearBtnsAllEvents();

        DEBUG_SERIAL.println("FINISH_LED");
      }

      for (int i=0; i<NUM_LAND_COLORS; i++) {
        if (btn[i].wasReleased()) {
          clearBtnsAllEvents();
          gameState = INIT_LED;
        }
      }

      break;
    default:
      gameState = INIT_LEDCONQUER;
      break;      
  }

}

bool animInitLed() {
  static long timerInitFade = 0;
  const uint8_t TIME_INIT_FADE_ANIM = 10;

  if ((millis() - timerInitFade) > TIME_INIT_FADE_ANIM) { 

    FastLED.show();
    timerInitFade = millis();
  }

  return true;
}

bool animGameLed() {
  const uint16_t TIME_GAME_ANIM = 60*1000;
  static long timerGame = 0;

  if (timerGame == 0) timerGame = millis();
  if ((millis() - timerGame) > TIME_GAME_ANIM) {
    timerGame = 0;
    return true;
  }

  return false;
}

bool animWinnerLed() {
  return animWinnerLedConquer();
}

bool animStartLed() {
  return animStartLedConquer();
}

bool animInitLedConquer() {
  static long timerInit = 0;
  const uint16_t TIME_INIT_ANIM = 500;
  //static uint8_t row = 0;
  //static uint8_t col = 0;
  static uint8_t row_play1 = 0;
  static uint8_t col_play1 = 0;
  static uint8_t row_play2 = NUM_ROWS-1;
  static uint8_t col_play2 = NUM_COLS-1;  
  // 0 - increase row
  // 1 - increase col
  // 2 - decrease row
  // 3 - decrease col
  static uint8_t pos_state_play1 = 0;
  static uint8_t pos_state_play2 = 2;

  uint8_t randomNum;

  if ((millis() - timerInit) > TIME_INIT_ANIM) {
    // random color in all the grid
/*  
    randomNum = random(0, NUM_LAND_COLOR);
    switch (randomNum) {
      case 0:
        ws2812b.setPixelColor(getLedIndex(row, col), YELLOW);
        break;
      case 1:
        ws2812b.setPixelColor(getLedIndex(row, col), ORANGE);
        break;
      case 2:
        ws2812b.setPixelColor(getLedIndex(row, col), RED);
        break;
      case 3:
        ws2812b.setPixelColor(getLedIndex(row, col), BLUE);
        break;
      case 4:
        ws2812b.setPixelColor(getLedIndex(row, col), GREEN);
        break;
      default:
        break;        
    }
    if (row < NUM_ROWS-1) {
      row++;
    }
    else {
      row = 0;
      if (col < NUM_COLS-1) {
        col++;
      }
      else {
        col = 0;
      }
    }
*/
    // random position of players in perimeter
    if (!readyPlay1) {
      ws2812b[getLedIndex(row_play1, col_play1)] = CRGB(0);
      switch (pos_state_play1) {
        case 0:
          row_play1++;
          if (row_play1 == (NUM_ROWS-1)) {
            if (readyPlay2) {
              pos_state_play1 = 2;
            }
            else {
              pos_state_play1 = 1;
            }
          }
          break;
        case 1:
          col_play1++;
          if (col_play1 == (NUM_COLS-1)) {
            if (readyPlay2) {
              pos_state_play1 = 3;
            }
            else {
              pos_state_play1 = 2;
            }
          }
          break;           
        case 2:
          row_play1--;
          if (row_play1 == 0) {
            if (readyPlay2) {
              pos_state_play1 = 0;
            }
            else {
              pos_state_play1 = 3;
            }
          }
          break;
        case 3:
          col_play1--;
          if (col_play1 == 0) {
            if (readyPlay2) {
              pos_state_play1 = 1;
            }
            else {
              pos_state_play1 = 0;
            }
          }
          break;
        default:
          row_play1 = 0;
          col_play1 = 0;
          pos_state_play1 = 0;
          row_play2 = NUM_ROWS-1;
          col_play2 = NUM_COLS-1;
          pos_state_play2 = 2;
          break;
      }
      ws2812b[getLedIndex(row_play1, col_play1)] = CRGB(rgbPlay1);
    }

    if (!readyPlay2) {
      ws2812b[getLedIndex(row_play2, col_play2)] = CRGB(0);
      switch (pos_state_play2) {
        case 0:
          row_play2++;
          if (row_play2 == (NUM_ROWS-1)) {
            if (readyPlay1) {
              pos_state_play2 = 2;
            }
            else {
              pos_state_play2 = 1;
            }
          }
          break;
        case 1:
          col_play2++;
          if (col_play2 == (NUM_COLS-1)) {
            if (readyPlay1) {
              pos_state_play2 = 3;
            }
            else {
              pos_state_play2 = 2;
            }
          }
          break;           
        case 2:
          row_play2--;
          if (row_play2 == 0) {
            if (readyPlay1) {
              pos_state_play2 = 0;
            }
            else {
              pos_state_play2 = 3;
            }
          }
          break;
        case 3:
          col_play2--;
          if (col_play2 == 0) {
            if (readyPlay1) {
              pos_state_play2 = 1;
            }
            else {
              pos_state_play2 = 0;
            }
          }
          break;
        default:
          row_play1 = 0;
          col_play1 = 0;
          pos_state_play1 = 0;
          row_play2 = NUM_ROWS-1;
          col_play2 = NUM_COLS-1;
          pos_state_play2 = 2;
          break;
      }
      ws2812b[getLedIndex(row_play2, col_play2)] = CRGB(rgbPlay2);
    }

    FastLED.show();

    timerInit = millis();  
  }

  if (readyPlay1) {
    row_play1 = 0;
    col_play1 = 0;
    pos_state_play1 = 0;      
  }

  if (readyPlay2) {
    row_play2 = NUM_ROWS-1;
    col_play2 = NUM_COLS-1;
    pos_state_play2 = 2;     
  } 

  return true;
}

bool animStartLedConquer() {
  static long timerStart = 0;
  const uint16_t TIME_START_ANIM = 200; // TODO 20
  static uint8_t step = 1;
  static uint8_t row = 0;
  static uint8_t col = 0;

  uint8_t randomNum;

  if ((millis() - timerStart) > TIME_START_ANIM) {
    if ((ws2812b[getLedIndex(row, col)] != CRGB(rgbPlay1)) && ((ws2812b[getLedIndex(row, col)]) != CRGB(rgbPlay2))) {
      randomNum = random(0, NUM_LAND_COLORS);
      ws2812b[getLedIndex(row, col)] = CRGB(landColors[randomNum]);
    }

    if (row < NUM_ROWS-1) {
      row++;
    }
    else {
      row = 0;
      if (col < NUM_COLS-1) {
        col++;
      }
      else {
        col = 0;
      }
    }    
    FastLED.show();

    if ((row == 0) && (col == 0)) {
      return true;
    }
 
    timerStart = millis();
  }

  return false;
}

bool animWinnerLedConquer() {
  static long timerWinner = 0;
  const uint16_t TIME_WINNER_ANIM = 200; 
  uint8_t row = 0;
  uint8_t col = 0;
  
  if ((millis() - timerWinner) > TIME_WINNER_ANIM) {
    while ((ws2812b[getLedIndex(row, col)] == CRGB(rgbPlay1)) || ((ws2812b[getLedIndex(row, col)]) == CRGB(rgbPlay2))) {
      if (row < NUM_ROWS-1) {
        row++;
      }
      else {
        row = 0;
        if (col < NUM_COLS-1) {
          col++;
        }
        else {
          col = 0;
        }
      }

      if ((row == 0) && (col == 0)) {
        return true;
      }
    }    

      
    if (lockPlay1) {
      ws2812b[getLedIndex(row, col)] = CRGB(rgbPlay2);
    }
    else {
      ws2812b[getLedIndex(row, col)] = CRGB(rgbPlay1);
    }

    FastLED.show();

    timerWinner = millis();
  }

  return false;
}

void clearLeds() {
  FastLED.clear();
  FastLED.show();
}

void clearBtnsAllEvents() {
  for (int i=0; i<NUM_LAND_COLORS; i++) {
    btn[i].clearAllEvents();
  }
}

void readBtns() {
  for (int i=0; i<NUM_LAND_COLORS; i++) {
    btn[i].readBtn();
  }  
}

uint8_t updateLeds(uint32_t colorPlay, uint32_t color) {
  uint8_t row, col;
  uint8_t numLedFound = 0;

  for (row = 0; row < NUM_ROWS; row++) {
    for (col = 0; col < NUM_COLS; col++) {
      if (ws2812b[getLedIndex(row, col)] == CRGB(colorPlay)) {
        numLedFound += changeAdjLeds(color, row, col, colorPlay);
      }  
    }
  }

  return numLedFound;
}

uint8_t changeAdjLeds(uint32_t colorSearched, uint8_t row, uint8_t col, uint32_t colorTarget) {
  uint8_t numLedFound = 0;
  
  //DEBUG_SERIAL.println(colorSearched, HEX);
  if ((row-1) >= 0) {
    //DEBUG_SERIAL.println("CHECK ROW-1");
    if (ws2812b[getLedIndex(row-1, col)] == CRGB(colorSearched)) {
      ws2812b[getLedIndex(row-1, col)] = CRGB(colorTarget);
      numLedFound += 1;
      numLedFound += changeAdjLeds(colorSearched, row-1, col, colorTarget);
      //DEBUG_SERIAL.println("ROW-1");
    }
  }
  if ((col-1) >= 0) {
    //DEBUG_SERIAL.println("CHECK COL-1");
    if (ws2812b[getLedIndex(row, col-1)] == CRGB(colorSearched)) {
      ws2812b[getLedIndex(row, col-1)] = CRGB(colorTarget);
      numLedFound += 1;
      numLedFound += changeAdjLeds(colorSearched, row, col-1, colorTarget);
      //DEBUG_SERIAL.println("COL-1");
    }
  }
  if ((row+1) < NUM_ROWS) {
    //DEBUG_SERIAL.println("CHECK ROW+1");
    if (ws2812b[getLedIndex(row+1, col)] == CRGB(colorSearched)) {
      ws2812b[getLedIndex(row+1, col)] = CRGB(colorTarget);
      numLedFound += 1;
      numLedFound += changeAdjLeds(colorSearched, row+1, col, colorTarget);
      //DEBUG_SERIAL.println("ROW+1");
    }
  }
  if ((col+1) < NUM_COLS) {
    //DEBUG_SERIAL.println("CHECK COL+1");
    if (ws2812b[getLedIndex(row, col+1)] == CRGB(colorSearched)) {
      ws2812b[getLedIndex(row, col+1)] = CRGB(colorTarget);
      numLedFound += 1;
      numLedFound += changeAdjLeds(colorSearched, row, col+1, colorTarget);
      //DEBUG_SERIAL.println("COL+1");
    }
  }  

  return numLedFound;
}

uint8_t checkPlayLock(uint32_t colorPlay) {
  uint8_t row, col;
  uint8_t numLedFound = 0;

  for (row = 0; row < NUM_ROWS; row++) {
    for (col = 0; col < NUM_COLS; col++) {
      if (ws2812b[getLedIndex(row, col)] == CRGB(colorPlay)) {
        for (int i=0; i<NUM_LAND_COLORS; i++) {
          if (checkAdjLeds(landColors[i], row, col)) {
            return false;
          }
        }
      }
    }
  }

  return true;
}

bool checkAdjLeds(uint32_t colorSearched, uint8_t row, uint8_t col) {
  //DEBUG_SERIAL.println(colorSearched, HEX);
  if ((row-1) >= 0) {
    if (ws2812b[getLedIndex(row-1, col)] == CRGB(colorSearched)) {
      return true;
    }
  }
  if ((col-1) >= 0) {
    if (ws2812b[getLedIndex(row, col-1)] == CRGB(colorSearched)) {
      return true;
    }
  }
  if ((row+1) < NUM_ROWS) {
    if (ws2812b[getLedIndex(row+1, col)] == CRGB(colorSearched)) {
      return true;
    }
  }
  if ((col+1) < NUM_COLS) {
    if (ws2812b[getLedIndex(row, col+1)] == CRGB(colorSearched)) {
      return true;
    }
  }  

  return false;
}

uint8_t getLedIndex(uint8_t row, uint8_t col) {
  uint8_t static const ledIndex[NUM_ROWS][NUM_COLS] = {
    {0, 9, 10, 19, 20} ,
    {1, 8, 11, 18, 21} ,
    {2, 7, 12, 17, 22} ,
    {3, 6, 13, 16, 23} ,
    {4, 5, 14, 15, 24}
  };

  return ledIndex[row][col];
}