#include <Arduino.h>
#include <MD_MAX72xx.h>

//GENERIC_HW
//ICSTATION_HW
// Define hardware type and connections for ESP32-C3
//#define HARDWARE_TYPE MD_MAX72XX::FC16_HW  // Original, working (but not transform())
#define HARDWARE_TYPE MD_MAX72XX::ICSTATION_HW
#define MAX_DEVICES 4  // Number of 8x8 matrices in your module
#define CLK_PIN     4   // Clock pin (SCK) - GPIO4 
#define DATA_PIN    6   // Data pin (MOSI) - GPIO6
#define CS_PIN      7   // Chip Select pin (SS) - GPIO7
#define PIN_B1      19
#define PIN_B2      18
#define PIN_UP      10
#define PIN_DOWN    9
#define PIN_LEFT    1
#define PIN_RIGHT   8

// Create MD_MAX72XX object
MD_MAX72XX mx = MD_MAX72XX(HARDWARE_TYPE, DATA_PIN, CLK_PIN, CS_PIN, MAX_DEVICES);

unsigned int count = 1;
bool LEDState = LOW;
static unsigned long lastUpdate = 0;

//bool b1_down = false;

const int debounceDelay = 50; // ms

struct Button {
  uint8_t pin;
  bool lastStableState;
  bool lastReading;
  unsigned long lastDebounceTime;
  bool pressedEvent;
};

// struct Block {
//   uint8_t size;
//   unsigned char Data[]
// }



// struct Block {
//     std::vector<char> Data;
//     Block(size_t n) : Data(n) {}
// };


struct Block {
    std::vector<char> Data;
    Block(std::initializer_list<char> list) : Data(list) {}
};



Button btn1;
Button btn2;
Button btn_u;
Button btn_d;
Button btn_l;
Button btn_r;

int global_x;
int global_y;


unsigned char board[128] {
  0b10000001,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,

  0b10000000,
  0b10000000,
  0b11100000,
  0b11100000,
  0b11100000,
  0b11100001,
  0b11100011,
  0b11100111,

  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,

  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b10000001
};


unsigned char block_tmp0[4] {
  0b0000,
  0b1111,
  0b1000,
  0b0000
};

unsigned char block_L0[4] {
  0b0000,
  0b1111,
  0b1000,
  0b0000
};

unsigned char block_tmp1[4] {
  0b1000,
  0b1100,
  0b1110,
  0b1111
};

unsigned char block_L1[4] {
  0b1000,
  0b1100,
  0b1110,
  0b1111
};

Block block_L{
  0b1000,
  0b1100,
  0b1110,
  0b1111
};

Block block_tmp {
  0b1000,
  0b1100,
  0b1110,
  0b1111
};

unsigned char block_ground[8] {
  0b10000000,
  0b10000000,
  0b11100000,
  0b11100000,
  0b11100000,
  0b11100001,
  0b11100011,
  0b11100111
};

unsigned char char_symbol[8] {
  0b10000001,
  0b01110000,
  0b01111100,
  0b01111111,
  0b00000001,
  0b11111111,
  0b00000000,
  0b10000001
};
unsigned char char_symbol2[8] {
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000
};

unsigned char char_A[8] {
0B00000000,
0B00001000,
0B00010100,
0B00100010,
0B01000001,
0B01111111,
0B01000001,
0B01000001
};

unsigned char char_B[8] {
0B00000000,
0B00001000,
0B00010100,
0B00100010,
0B01000001,
0B01111111,
0B01000001,
0B01000001
};

unsigned char char_F[8] = {
  0b01000000,
  0b01000000,
  0b01001000,
  0b01001000,
  0b01001000,
  0b01111111,
  0b00000000,
  0b00000000
};

unsigned char char_S[8] {
0B00000000,
0B00111110,
0B01000001,
0B01000000,
0B00111110,
0B00000001,
0B01000001,
0B00111110
};




bool CheckCollision(Block block, int gx, int gy) {
  bool hit = false;
  gx += global_x;
  gy += global_y;

  Serial.print("global_y: ");
  Serial.print(gy);
  Serial.print("    global_x: ");
  Serial.println(gx);
  
  // Check left to right
  for (int ry=3; ry>=0; ry--) {
    // Ignore empty parts
    if (block.Data[ry] > 0) {
      // Serial.print("block[");
      // Serial.print(ry);
      // Serial.print("]   ");
      // pb(block.Data[ry]);

      //Serial.print("board[global_y + 3] ");
      //pb(board[gy + 3]);  
      // testing, might be smart!
      Serial.print("board[");
      Serial.print(global_y + ry);
      Serial.print("]  ");
      pb(board[global_y + ry]);
      Serial.println();

      //uint8_t row = block.Data[ry] << gx;
      uint8_t row = block.Data[ry] << (global_x);

      Serial.print("block[");
      Serial.print(ry);
      Serial.print("]  ");            
      pb(row);
      Serial.print("  (");
      pb(block.Data[ry]);
      Serial.println(")");

      // row = reverseBits8(row);

      // Serial.print("reverse row[");
      // Serial.print(ry);
      // Serial.print("]   ");
      // pb(row);
      // Serial.println();


      if (board[global_y + ry] & row) {
        hit = true;
        //Serial.println("Hit!");        
      } else {
        //Serial.println("No hit");
      }
    }
  }

  return hit;
}


uint8_t reverseBits8(uint8_t b) {
    uint8_t r = 0;
    for (uint8_t i = 0; i < 8; i++) {
        r = (r << 1) | (b & 1);
        b >>= 1; // logical shift because b is unsigned
    }
    return r;
}


unsigned char reverse(unsigned char b) {
   b = (b & 0xF0) >> 4 | (b & 0x0F) << 4;
   b = (b & 0xCC) >> 2 | (b & 0x33) << 2;
   b = (b & 0xAA) >> 1 | (b & 0x55) << 1;
   return b;
}

byte reverseBits(byte b) {
    byte r = 0;
    for (int i = 0; i < 8; i++) {
        r <<= 1;          // make room for next bit
        r |= (b & 1);     // copy lowest bit
        b >>= 1;          // move next bit into position
    }
    return r;
}


void DrawBoard() {
  unsigned char p1;
  for (char yy=0; yy<32; yy++) {
    p1 = board[yy];
    for (char xx=0; xx<8; xx++) {
      if (BitIsSet(p1, xx))
        mx.setPoint(xx, yy, true);
      else
        mx.setPoint(xx, yy, false);
    }
  }  
}

void EraseBlock(Block block, char x, char y) {
  for (char j=0; j<block.Data.size(); j++) {
    for (char i=0; i<block.Data.size(); i++) {
      mx.setPoint(x+i, y+j, false);
    }
  }  
}

void DrawBlock(Block block, char x, char y) {
  unsigned char p1;
  // bool isSet = (value & (1 << n)) != 0

  for (char j=0; j<block.Data.size(); j++) {
    p1 = block.Data[j];
    for (char i=0; i<block.Data.size(); i++) {
      if (BitIsSet(p1, i))
        mx.setPoint(x+i, y+j, true);
      else
        mx.setPoint(x+i, y+j, false);
    }
  }  
}

void DrawBigBlock(unsigned char block[8], char x, char y) {
  unsigned char p1;
  // bool isSet = (value & (1 << n)) != 0

  for (char j=0; j<8; j++) {
    p1 = block[j];
    for (char i=0; i<8; i++) {
      if (BitIsSet(p1, i))
        mx.setPoint(x+i, y+j, true);
      else
        mx.setPoint(x+i, y+j, false);
    }
  }  
}


void Rotate90CCW(Block src, Block& dst) {
  Serial.println("Rotating CCW");
  for (int r = 0; r < 4; r++) {
    dst.Data[r] = 0;  // clear row
    for (int c = 0; c < 4; c++) {
        // Extract bit r from row (3 - c)
        unsigned char bit = (src.Data[3 - c] >> r) & 1;

        // Write it into bit c of row r
        dst.Data[r] |= (bit << c);
    }
  }
}

void rotate90CW(Block src, Block& dst) {
  Serial.println("Rotating CW");
  for (int r = 0; r < 4; r++) {
    dst.Data[r] = 0;
    for (int c = 0; c < 4; c++) {
        // Extract bit from src[c], bit index (3 - r)
        unsigned char bit = (src.Data[c] >> (3 - r)) & 1;

        // Place it into dst[r], bit index c
        dst.Data[r] |= (bit << c);
    }
  }
}

void updateButton(Button &btn) {
  bool reading = digitalRead(btn.pin) == LOW; // active-low
  if (reading != btn.lastReading) {
    btn.lastDebounceTime = millis();
  }
  if ((millis() - btn.lastDebounceTime) > debounceDelay) {
    if (reading != btn.lastStableState) {
      btn.lastStableState = reading;
      if (btn.lastStableState) {
        btn.pressedEvent = true; // Just pressed
      }
    }
  }
  btn.lastReading = reading;
}

/*
unsigned char rotated[4];
rotate90(block_t, rotated);
*/

bool BitIsSet(char value, char n) {
  return (value & (1 << n)) != 0;
}


void pb(unsigned char value) {
    for (int i = 7; i >= 0; i--) {
        Serial.print((value >> i) & 1);
    }
}


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

  pinMode(PIN_B1, INPUT_PULLUP);
  pinMode(PIN_B2, INPUT_PULLUP);

  pinMode(PIN_UP, INPUT_PULLUP);
  pinMode(PIN_DOWN, INPUT_PULLUP);
  pinMode(PIN_LEFT, INPUT_PULLUP);
  pinMode(PIN_RIGHT, INPUT_PULLUP);

  btn1 = {PIN_B1, false, false, 0, false};
  btn2 = {PIN_B2, false, false, 0, false};

  btn_u = {PIN_UP, false, false, 0, false};
  btn_d = {PIN_DOWN, false, false, 0, false};
  btn_l = {PIN_LEFT, false, false, 0, false};
  btn_r = {PIN_RIGHT, false, false, 0, false};

  global_x = 2;
  global_y = 2;

  // Initialize the display
  mx.begin();
  mx.clear();
  mx.control(MD_MAX72XX::INTENSITY, 2);  // Set brightness (0-15)

  // testing this for rotation
  //  for (uint8_t i = 0; i < 4; i++) {
  //    mx.transform(i, MD_MAX72XX::TSR); // TSU = Turn 180°
  //  }
  /*
  TSL: Transform Shift Left (rotate display 90° left / counterclockwise)
  TSR: Transform Shift Right (rotate display 90° right / clockwise)
  TSU: Transform Shift Up (rotate display 180°)
  TSD: Transform Shift Down (no rotation, default orientation)
  TFLR: Transform Flip Left-Right (mirror horizontally)
  TFUD: Transform Flip Up-Down (mirror vertically)
  TRC: Transform Rotate Clockwise (rotate 90° right)
  TINV: Transform Invert (invert all pixels: on ↔ off)
  */
  // END testing

 
  //randomSeed(analogRead(0));  // Seed random number generator

  // DrawBlock(block_L, 2,2);
  //DrawBigBlock(block_ground, 0, 12);
  DrawBoard();

  Serial.println("Start");
}


void loop() {

  //delay(500);
  //return;

  // for (char y=0; y<32; y++) {
  //   mx.clear();
  //   DrawBigBlock(char_F, 0, y);
  //   delay(200);    
  // }
  // return;

  updateButton(btn1);
  updateButton(btn2);

  updateButton(btn_u);
  updateButton(btn_d);
  updateButton(btn_l);
  updateButton(btn_r);

  if (btn1.pressedEvent) {
    Serial.print("Btn 1 pressed");
    Rotate90CCW(block_L, block_tmp);
    DrawBlock(block_tmp, global_x, global_y);
    for (int i = 0; i < 4; i++) {
      block_L.Data[i] = block_tmp.Data[i];
    }
    btn1.pressedEvent = false;
  }

  if (btn2.pressedEvent) {
    Serial.print("Btn 2 pressed");
    rotate90CW(block_L, block_tmp);
    DrawBlock(block_tmp, 2, 2);
    for (int i = 0; i < 4; i++) {
      block_L.Data[i] = block_tmp.Data[i];
    }
    btn2.pressedEvent = false;
  }

  // UP BUTTON ---------------------------
  if (btn_u.pressedEvent) {
    //Serial.println("BU press");
    if (block_L.Data.size() + global_x > 7) {
      Serial.println("OOB! U");
    } 
    else if (CheckCollision(block_L, -1, 0)) {
      Serial.println("Collision! U");
    }
    else  {
      EraseBlock(block_L, global_x, global_y);
      global_x++;
      DrawBlock(block_L, global_x, global_y);
    }
    btn_u.pressedEvent = false;
    //Serial.print("global_x: ");
    //Serial.println(global_x);
  }
  // DOWN BUTTON ---------------------------
  if (btn_d.pressedEvent) {
    //Serial.println("BD press");
    if (global_x < 1) {
      Serial.println("OOB! D");
    } else {
      EraseBlock(block_tmp, global_x, global_y);
      global_x--;
      DrawBlock(block_tmp, global_x, global_y);
    }
    btn_d.pressedEvent = false;
    //Serial.print("global_x: ");
    //Serial.println(global_x);
  }
  // LEFT BUTTON ---------------------------
  if (btn_l.pressedEvent) {
    //Serial.println("BL press");
    if (CheckCollision(block_L, 0, 1)){
      Serial.println("Collision happened");      
    } else {
      EraseBlock(block_L, global_x, global_y);
      global_y++;    
      DrawBlock(block_L, global_x, global_y);
    }
    btn_l.pressedEvent = false;
  }
  // RIGHT BUTTON ---------------------------
  if (btn_r.pressedEvent) {
    Serial.println("BR press");
    EraseBlock(block_tmp, global_x, global_y);
    global_y--;
    DrawBlock(block_tmp, global_x, global_y);
    btn_r.pressedEvent = false;
  }


  // Run this every 500 ms
  if (millis() - lastUpdate >= 500) {
    
    // Update the display from buffer
    //mx.update();
    
    lastUpdate = millis();

    //if (++count % 100 == 0)
    //Serial.println(digitalRead(PIN_B1));

    if (count > 65000)
     count = 0;
    
    // Toggle onboard LED to show activity
    //LEDState = !LEDState;
    //digitalWrite(LED_PIN, LEDState);
  }

}