#include <LiquidCrystal.h>
#include <SPI.h>
#include <LedControl.h>
#include <Wire.h> 
#include <MCP23017.h>

const byte ROWS = 8;
const byte COLS = 8;

byte switchRows[ROWS];
byte RledRows[ROWS];
byte GledRows[ROWS];
byte BledRows[ROWS];

const int TIME_LIMIT = 60;

int score = 0;
int timer = 0;

#define MAX_DEVICES 1

// Define the data, clock, and CS pins (adapt to your wiring)
#define DATA_PIN 20
#define CLK_PIN 21
#define CS_PIN 2

LedControl lc = LedControl(DATA_PIN, CLK_PIN, CS_PIN, MAX_DEVICES);

// Define the LCD pin connections (adjust as needed)
#define LCD_RS 4
#define LCD_EN 3
#define LCD_D4 5
#define LCD_D5 51
#define LCD_D6 50
#define LCD_D7 52

LiquidCrystal lcd(LCD_RS, LCD_EN, LCD_D4, LCD_D5, LCD_D6, LCD_D7);

MCP23017 mcp = MCP23017(0x20);

void setup() {
  Serial.begin(115200);
  // Set up the LCD's number of columns and rows:
  lcd.begin(16, 2);
  lcd.setCursor(0, 0);
  //lcd.print("SuperDuper Deker");
  lcd.setCursor(0, 1);
  lcd.print("SELF TEST");
  delay(2000);
  lcd.clear();
  randomSeed(analogRead(A0));
  
  //Init LED array
  for (int i = 0; i < MAX_DEVICES; i++) {
    lc.shutdown(i,false);
    lc.setIntensity(i,15);
    lc.clearDisplay(i);
  }
  //Init MCP and switch state registers
  for (int i = 0; i < ROWS; i++) {
    switchRows[i] = 0x00;
    mcp.pinMode(i, INPUT);
    mcp.pinMode(i + 8, INPUT_PULLUP);
  }
}

void loop() { 
  lc.setLed(0,0,0,true);
  delay(5000);
  selftest_leds();
  selftest_switches();
}

bool scan() {
  for (int i = 0; i < ROWS; i++) {
    switchRows[i] = 0;
    mcp.pinMode(i, OUTPUT);
    mcp.digitalWrite(i, LOW);
    for (int j = 0; j < COLS; j++) {
      if (mcp.digitalRead(j + 8) == LOW) {
        switchRows[i] |= 1 << j;
      } else {
        switchRows[i] &= ~(1 << j);
      }
    }
    mcp.pinMode(i, INPUT);
  }
  return true;
}
bool read(int i, int j) {
  return (switchRows[i] >> j | 1) == 1;
}

bool show() {
  for (int i = 0; i < ROWS; i++) {
    for (int j = 0; j < COLS; j++) {
      lc.setLed(0, i, j, (RledRows[i] << j) | 1 == 1);
      lc.setLed(1, i, j, (GledRows[i] << j) | 1 == 1);
      lc.setLed(2, i, j, (BledRows[i] << j) | 1 == 1);
    }
  }
}

bool selftest_leds() {
  //init
  lc.clearDisplay(0);
  lcd.clear();
  lcd.print("LED Testing");

  //Cycle all the leds for all the colors.
  for (int i = 0; i < MAX_DEVICES; i++) {
    for (int j = 0; j < ROWS; j++) {
      for (int k = 0; k < COLS; k++) {
        lc.setLed(i, j, k, true);
        delay(100);
        lc.setLed(i, j, k, false);
      }
    }
  }
  return true;
}

bool selftest_switches() {
  //init
  lc.clearDisplay(0);
  lcd.clear();
  lcd.print("Switch Testing");

  //Light up first color when switch is on.
  while (true) {
    if (scan()) {
      memcpy(RledRows, switchRows, ROWS);
      show();
    }
  }
  return true;
}