#include <MatrixHardware_Teensy4_ShieldV5.h> 
#include <SmartMatrix.h>
#include <Entropy.h>
#include <EEPROM.h>
#include <Bounce2.h>

#define randomSeed(s) srandom(s)

#define COLOR_DEPTH 24                  // Choose the color depth used for storing pixels in the layers: 24 or 48 (24 is good for most sketches - If the sketch uses type `rgb24` directly, COLOR_DEPTH must be 24)
const uint16_t kMatrixWidth = 32;       // Set to the width of your display, must be a multiple of 8
const uint16_t kMatrixHeight = 32;      // Set to the height of your display
const uint8_t kRefreshDepth = 36;       // Tradeoff of color quality vs refresh rate, max brightness, and RAM usage.  36 is typically good, drop down to 24 if you need to.  On Teensy, multiples of 3, up to 48: 3, 6, 9, 12, 15, 18, 21, 24, 27, 30, 33, 36, 39, 42, 45, 48.  On ESP32: 24, 36, 48
const uint8_t kDmaBufferRows = 4;       // known working: 2-4, use 2 to save RAM, more to keep from dropping frames and automatically lowering refresh rate.  (This isn't used on ESP32, leave as default)
const uint8_t kPanelType = SM_PANELTYPE_HUB75_32ROW_MOD16SCAN;   // Choose the configuration that matches your panels.  See more details in MatrixCommonHub75.h and the docs: https://github.com/pixelmatix/SmartMatrix/wiki
const uint32_t kMatrixOptions = (SM_HUB75_OPTIONS_NONE);        // see docs for options: https://github.com/pixelmatix/SmartMatrix/wiki

const uint8_t kBackgroundLayerOptions = (SM_BACKGROUND_OPTIONS_NONE);
const uint8_t kScrollingLayerOptions = (SM_SCROLLING_OPTIONS_NONE);
const uint8_t kIndexedLayerOptions = (SM_INDEXED_OPTIONS_NONE);

const int BOARDHEIGHT = 16;
const int BOARDWIDTH = 16;
const int ADVANCE_INTERVAL = 500;
const int START_DELAY = 1000;

SMARTMATRIX_ALLOCATE_BUFFERS(matrix, kMatrixWidth, kMatrixHeight, kRefreshDepth, kDmaBufferRows, kPanelType, kMatrixOptions);
SMARTMATRIX_ALLOCATE_BACKGROUND_LAYER(backgroundLayer, kMatrixWidth, kMatrixHeight, COLOR_DEPTH, kBackgroundLayerOptions);

//WIRING
int RST_RECORD_BTN = 17;
int BTN = 18;
int JSTICK_UP = 21;
int JSTICK_DOWN = 20;
int JSTICK_RIGHT = 22;
int JSTICK_LEFT = 19;
int BRIGHTNESS_KNOB = A9;

//TETRIS VARIABLES
unsigned long timerA = 0;
unsigned long startupTimer = 0;

bool playingGame = false;
bool endScreenEnabled = false;
bool startScreenEnabled = false;
bool pollForResponse = false;

const byte colors[7][3] = {
    {0, 0, 0},
    {120, 37, 179},
    {100, 179, 179},
    {180, 134, 22},
    {80, 134, 22},
    {180, 34, 22},
    {180, 34, 122},
};

const int figures[7][4][4] = {
        {{1, 5, 9, 13}, {4, 5, 6, 7}},
        {{4, 5, 9, 10}, {2, 6, 5, 9}},
        {{6, 7, 9, 10}, {1, 5, 6, 10}},
        {{1, 2, 5, 9}, {0, 4, 5, 6}, {1, 5, 9, 8}, {4, 5, 6, 10}},
        {{1, 2, 6, 10}, {5, 6, 7, 9}, {2, 6, 10, 11}, {3, 5, 6, 7}},
        {{1, 4, 5, 6}, {1, 4, 5, 9}, {4, 5, 6, 9}, {1, 5, 6, 9}},
        {{1, 2, 5, 6}},
};

const int num_orientations[7] = {1,1,1,3,3,3,0};

struct color {
  byte r;
  byte g;
  byte b;
};

struct piece {
  color piece_color;
  int type;
  int orientation;
  int x;
  int y;
};


color game_board[ BOARDHEIGHT ][ BOARDWIDTH ]; 

color save_board[ BOARDHEIGHT ][ BOARDWIDTH ]; 

//DEBOUNCE VARS
int DEBOUNCE_INTERVAL_TIME = 5;
Bounce select_button = Bounce();
Bounce reset_button = Bounce();
Bounce up_switch = Bounce();
Bounce down_switch = Bounce();
Bounce right_switch = Bounce();
Bounce left_switch = Bounce();

bool select_button_state = false;
bool reset_button_state = false;
bool up_switch_state = false;
bool down_switch_state = false;
bool left_switch_state = false;
bool right_switch_state = false;
bool game_paused = false;

piece active_piece;

int score = 0;

bool inCurFigure(int boxNum, int figNum, int orientationNum){
  int i;
  
  for (i = 0; i < 4; i++)
    if (figures[figNum][orientationNum][i] == boxNum)
      return true;
  return false;
  
}

void setup() {
  // put your setup code here, to run once:
  pinMode(RST_RECORD_BTN, INPUT_PULLUP);
  pinMode(BTN, INPUT_PULLUP);
  pinMode(JSTICK_UP, INPUT_PULLUP);
  pinMode(JSTICK_DOWN, INPUT_PULLUP);
  pinMode(JSTICK_RIGHT, INPUT_PULLUP);
  pinMode(JSTICK_LEFT, INPUT_PULLUP);

  select_button.attach(BTN);
  select_button.interval(DEBOUNCE_INTERVAL_TIME);

  reset_button.attach(RST_RECORD_BTN);
  reset_button.interval(DEBOUNCE_INTERVAL_TIME);

  up_switch.attach(JSTICK_UP);
  up_switch.interval(DEBOUNCE_INTERVAL_TIME);

  down_switch.attach(JSTICK_DOWN);
  down_switch.interval(DEBOUNCE_INTERVAL_TIME);

  right_switch.attach(JSTICK_RIGHT);
  right_switch.interval(DEBOUNCE_INTERVAL_TIME);

  left_switch.attach(JSTICK_LEFT);
  left_switch.interval(DEBOUNCE_INTERVAL_TIME);


  uint32_t seed_value;
  Entropy.Initialize();
  seed_value = Entropy.random();
  randomSeed(seed_value);

  matrix.addLayer(&backgroundLayer);
  matrix.begin();
  
  backgroundLayer.enableColorCorrection(true);
  backgroundLayer.fillScreen({0, 0, 0});

  matrix.setBrightness(100); // 255 max
  backgroundLayer.swapBuffers();

  Serial.begin(9600);
  Serial.println("Begin"); 


  int rnd_color = (rand() % 6)+1; //Can't pick 0 since it's black
  int rnd_piece = 0;

  rnd_color = (rand() % 6)+1; //Can't pick 0 since it's black
  rnd_piece = rand() % 7;

  active_piece.x = 6;
  active_piece.y = 0;
  active_piece.piece_color.r = colors[rnd_color][0];
  active_piece.piece_color.g = colors[rnd_color][1];
  active_piece.piece_color.b = colors[rnd_color][2];
  active_piece.orientation = 0;
  active_piece.type = rnd_piece;

  update_buttons();
  
  playingGame = false;
  endScreenEnabled = false;
  startScreenEnabled = true;
  pollForResponse = true;

  resetGame();
  startupTimer = millis();
}

void loop() {
  int brightness_raw;
  double brightness_percent;
  int brightness_byte;

  update_buttons();
  
  brightness_raw = analogRead(BRIGHTNESS_KNOB);  // read the input pin
  brightness_percent = brightness_raw/(double)1023;
  brightness_byte = 255- (brightness_percent*255); //invert logic to make pot direction make sense.
  //Serial.println(brightness_byte); 
  matrix.setBrightness(brightness_byte); // 255 max

  //*************************************************************************

  //BUTTON CONTROL
  if (pollForResponse) {
      
      if (select_button.rose() and startScreenEnabled and ( ( millis() - startupTimer ) > START_DELAY)){
        update_buttons();
        resetGame();
        endScreenEnabled = false;
        startScreenEnabled = false;
        pollForResponse = false;
        playingGame = true;
      }
      if (select_button.rose() and endScreenEnabled){
        update_buttons();
        endScreenEnabled = false;
        startScreenEnabled = true;
        pollForResponse = true;
        playingGame = false;
      }
      if (reset_button.rose()){
        reset_record();
      }
      
  }


  if(startScreenEnabled){
    drawWelcomeScreen();
  }else if(playingGame){
    playGame();
  }else if(endScreenEnabled){
    drawEndScreen();
  }else{
    resetGame();
    startScreenEnabled =  true;
  }

  
}



void resetGame(){
  
   //initialize board
   int i, j;
   for(i=0; i<=(BOARDHEIGHT-1); i++) {
      for(j=0;j<=(BOARDWIDTH-1);j++) {
        game_board[i][j].r = 0;
        game_board[i][j].g = 0;
        game_board[i][j].b = 0;

        save_board[i][j].r = 0;
        save_board[i][j].g = 0;
        save_board[i][j].b = 0;
      }
   }

   game_paused = false;
   score = 0;
   timerA = millis();
  
  
}

void save_game_board(){
  int i, j;
  
   for(i=0; i<=(BOARDHEIGHT-1); i++) {
      for(j=0;j<=(BOARDWIDTH-1);j++) {
        save_board[i][j].r = game_board[i][j].r;
        save_board[i][j].g = game_board[i][j].g;
        save_board[i][j].b = game_board[i][j].b;
      }
   }
   
}

void restore_game_board(){
  int i, j;
  
   for(i=0; i<=(BOARDHEIGHT-1); i++) {
      for(j=0;j<=(BOARDWIDTH-1);j++) {
        game_board[i][j].r = save_board[i][j].r;
        game_board[i][j].g = save_board[i][j].g;
        game_board[i][j].b = save_board[i][j].b;
      }
   }
   
}

void playGame(){

    if(select_button.rose()){

      if(not game_paused){
        pause();
      }
      else{
        unpause();
      }
    
    }

    if(not game_paused){
      gameLogic();
    }

  
}

void gameLogic(){
  

    if(up_switch.rose()){
      if(not intersects(active_piece.x,active_piece.y,get_rotate_orientation())){
        erase_active_piece();
        active_piece.orientation = get_rotate_orientation();
        draw_active_piece();
      }
    }

    if(right_switch.rose()){
      if(not intersects(active_piece.x+1,active_piece.y,active_piece.orientation)){
        erase_active_piece();
        active_piece.x = active_piece.x + 1;
        draw_active_piece();
      }
    }

    if(left_switch.rose()){
      if(not intersects(active_piece.x-1,active_piece.y,active_piece.orientation)){
        erase_active_piece();
        active_piece.x = active_piece.x - 1;
        draw_active_piece();
      }
    }

    if(down_switch.rose()){
      if(not intersects(active_piece.x,active_piece.y+1,active_piece.orientation)){
        erase_active_piece();
        active_piece.y = active_piece.y + 1;
        draw_active_piece();
      }
    }

  if( ( millis() - timerA ) > ADVANCE_INTERVAL){
    if(not intersects(active_piece.x,active_piece.y+1,active_piece.orientation)){
        erase_active_piece();
        active_piece.y = active_piece.y + 1;
        draw_active_piece();
     }else{
        freeze_current_piece();
     }


    timerA = millis();
    
  }

  draw_active_piece();
  draw_board();
  
}

void freeze_current_piece(){
  int rnd_color = 0;
  int rnd_piece = 0;

  rnd_color = (rand() % 6)+1; //Can't pick 0 since it's black
  rnd_piece = rand() % 7;
  
  //break lines function
  breakLines();
  
  //Create new piece
  active_piece.x = 6;
  active_piece.y = 0;
  active_piece.piece_color.r = colors[rnd_color][0];
  active_piece.piece_color.g = colors[rnd_color][1];
  active_piece.piece_color.b = colors[rnd_color][2];
  active_piece.orientation = 0;
  active_piece.type = rnd_piece;

  //If intersects, game over
   //if( intersects(active_piece.x,active_piece.y,active_piece.orientation)){
   if(isStartPositionPopulated()){    
        gameLose();
   }

  
  
}

void breakLines(){
  int i,j,k = 0;
  int clearedLines = 0;

   for(i=(BOARDHEIGHT-1);i>0;i--){
    while (isLineComplete(i)){
      clearedLines = clearedLines+1;
      for(j=i;j>1;j--){
        for(k=0;k<BOARDWIDTH;k++){
          game_board[k][j].r = game_board[k][j-1].r;
          game_board[k][j].g = game_board[k][j-1].g;
          game_board[k][j].b = game_board[k][j-1].b;
        }
      }
    }
  }

  if(clearedLines == 1){
    score = score+4;
  }else if (clearedLines == 2){
    score = score+10;
  }else if (clearedLines == 3){
    score = score+30;
  }else if (clearedLines >= 4){
    score = score+120 + ((clearedLines-4)*10);
  }
  
}

bool isLineComplete(int line){
  int i = 0;
  bool complete = true;

  for(i=0;i<BOARDWIDTH;i++){
    if(isPositionEmpty(i,line)){
      complete = false;
    }
  }

  return complete;
}

void gameLose(){

  int record_int = 0;
  record_int  = readIntFromEEPROM(1);

  if(record_int<score){
    writeIntIntoEEPROM(1,score);
  }

  playingGame = false;
  pollForResponse = true;
  endScreenEnabled = true;
}


void drawWelcomeScreen(){

  backgroundLayer.setFont(font5x7);
  backgroundLayer.fillScreen({0,0,0});
  backgroundLayer.drawString(1, 14, {255, 255, 255}, "TETRIS"); //{255, 255, 255} White

  /*
  backgroundLayer.setFont(font5x7);
  backgroundLayer.drawString(4, 18, {0, 255, 0}, "Play?"); //{255, 255, 255} White
  */
  backgroundLayer.swapBuffers();
  
}

void pause(){
  game_paused = true;
  save_game_board();

   int i, j;
   for(i=6; i<=(6+3); i++) {
      for(j=0;j<=(BOARDWIDTH-1);j++) {
          setBoardPosition(j,i,0,0,0);
      }
   }

  draw_board();
  
  backgroundLayer.setFont(font5x7);
  backgroundLayer.drawString(4, 13, {255, 255, 255}, "PAUSE"); //{255, 255, 255} White
  backgroundLayer.swapBuffers();
  
}

void unpause(){
  game_paused = false;
  restore_game_board();

  draw_board();

  timerA = millis();
}

void drawEndScreen(){
  String tmp_str = "";
  int digits = 0;
  int Xoffset = 0;
  int record_int = 0;

  //EEPROM.write(0, 4);
  record_int  = readIntFromEEPROM(1);
  
  backgroundLayer.setFont(font5x7);
  backgroundLayer.drawString(4, 1, {255, 255, 255}, "SCORE"); //{255, 255, 255} White
  backgroundLayer.swapBuffers();
    
  tmp_str = String(score);
  digits = tmp_str.length();
  Xoffset = (32-(4*digits))/2;
  backgroundLayer.fillScreen({0,0,0});
  backgroundLayer.drawString(Xoffset, 9, {255, 0, 255}, tmp_str.c_str()); //{255, 255, 255} White
  backgroundLayer.swapBuffers();

  backgroundLayer.drawString(1, 17, {255, 255, 255}, "RECORD"); //{255, 255, 255} White
  backgroundLayer.swapBuffers();

  tmp_str = String(record_int);
  digits = tmp_str.length();
  Xoffset = (32-(4*digits))/2;
  backgroundLayer.drawString(Xoffset, 25, {255, 0, 255}, tmp_str.c_str()); //{255, 255, 255} White
  backgroundLayer.swapBuffers();
}

void reset_record(){
  int record_int = 0;
  record_int  = readIntFromEEPROM(1);

  if(record_int != 0){
    writeIntIntoEEPROM(1,0);
  }

  int snake_record_int = 0;
  snake_record_int  = EEPROM.read(0);

  if(snake_record_int != 2){
    EEPROM.write(0, 2);
  }
  
}

void writeIntIntoEEPROM(int address, int number)
{ 
  EEPROM.write(address, number >> 8);
  EEPROM.write(address + 1, number & 0xFF);
}

int readIntFromEEPROM(int address)
{
  byte byte1 = EEPROM.read(address);
  byte byte2 = EEPROM.read(address + 1);
  return (byte1 << 8) + byte2;
}

int get_rotate_orientation(){
  int possible_orientations = 0;
  int retVal = 0;
  possible_orientations = num_orientations[active_piece.type];

  if (possible_orientations == active_piece.orientation){
    retVal = 0;
  }else{
    retVal = active_piece.orientation + 1;
  }

  return retVal;
  
}


bool intersects(int x,int y,int orientation){
  bool intersection = false;
  int i,j = 0;

  for (i = 0; i < 4; i++){
    for (j = 0; j < 4; j++){
      if (inCurFigure((i*4+j),active_piece.type,orientation)){
        if ( ((i+y) > (BOARDHEIGHT-1))  ){
          intersection = true;
        }
        if ( ( (j+x) > (BOARDWIDTH-1)) or ( (j+x) < 0) ){
          intersection = true;
        }
        if (isPositionPopulated(j+x,i+y)){
          intersection = true;
        }
        
      }
    }
  }

  return intersection;
  
}

bool coordinatesEqual(int firstCoord[2],int secondCoord[2]){
  bool xEqual = false;
  bool yEqual = false;

  if (firstCoord[0] == secondCoord[0]){
    xEqual = true;
  }

  if (firstCoord[1] == secondCoord[1]){
    yEqual = true;
  }

  if(xEqual && yEqual){
    return true;
  }else{
    return false;
  }
}

void draw_active_piece(){

  int i,j=0;

  for (i = 0; i < 4; i++){
    for (j = 0; j < 4; j++){
      if (inCurFigure((i*4+j),active_piece.type,active_piece.orientation)){
        setBoardPosition(active_piece.x+j,active_piece.y+i,active_piece.piece_color.r,active_piece.piece_color.g,active_piece.piece_color.b);
      }
    }
    
  }

  
}

void erase_active_piece(){
  
  int i,j=0;

  for (i = 0; i < 4; i++){
    for (j = 0; j < 4; j++){
      if (inCurFigure((i*4+j),active_piece.type,active_piece.orientation)){
        setBoardPosition(active_piece.x+j,active_piece.y+i,0,0,0);
      }
    }
    
  }
  
}

void setBoardPosition(int x, int y, byte r, byte g, byte b){
  game_board[x][y].r = r;
  game_board[x][y].g = g;
  game_board[x][y].b = b;
}

//Ignores Active part
bool isPositionPopulated(int x, int y){
  bool populated = false;
  bool isCurrentActivePosition = false;
  int i,j = 0;

  for (i = 0; i < 4; i++){
    for (j = 0; j < 4; j++){
      if (inCurFigure((i*4+j),active_piece.type,active_piece.orientation)){
        if (((active_piece.x + j) == x) and ((active_piece.y + i) == y)){
          isCurrentActivePosition = true;
        }
      }
    }
  }

  if(not isCurrentActivePosition){
    if(game_board[x][y].r > 0 or game_board[x][y].g > 0 or game_board[x][y].b > 0){
      populated = true;
    }
  }
  

  return populated;

}

bool isStartPositionPopulated(){
  bool populated = false;
  int i,j = 0;

  for (i = 0; i < 4; i++){
    for (j = 0; j < 4; j++){
      if (inCurFigure((i*4+j),active_piece.type,active_piece.orientation)){
        if(not isPositionEmpty((active_piece.x + j),(active_piece.y + i))){
          populated = true;
        }
      }
    }
  }

  return populated;
}

//does not ignore
bool isPositionEmpty(int x, int y){
  bool posEmpty = true;
  
  if(game_board[x][y].r > 0 or game_board[x][y].g > 0 or game_board[x][y].b > 0){
      posEmpty = false;
    }

  return posEmpty;
}

void draw_board(){

  backgroundLayer.fillScreen({0, 0, 0});
  
  int i, j;
   for(i=0; i<=(BOARDHEIGHT-1); i++) {
      for(j=0;j<=(BOARDWIDTH-1);j++) {
          backgroundLayer.fillRectangle(i*2,j*2,i*2+1,j*2+1,{game_board[i][j].r, game_board[i][j].g, game_board[i][j].b});
      }
   }
  

  backgroundLayer.swapBuffers();
}



void update_buttons(){
  select_button.update();
  reset_button.update();
  up_switch.update();
  down_switch.update();
  right_switch.update();
  left_switch.update();


  select_button_state = not select_button.read();
  reset_button_state = not reset_button.read();
  up_switch_state = not up_switch.read();
  down_switch_state = not down_switch.read();
  right_switch_state = not right_switch.read();
  left_switch_state = not left_switch.read();

  
}
