#include <ArrayList.h>
////////////////////////////////
//
//
//
// Issues getting this to display with the 1351 drivers
// monochrome works but no luck with RGB because od distortion
//  Buy new OLED and try again with non-1351 drivers
// will also need SD card to hold files
//
//
////////////////////////////////////////////

#include <MD_MAX72xx.h>
#include <SPI.h>

#include <Adafruit_GFX.h>
#include <Adafruit_GrayOLED.h>
#include <Adafruit_SPITFT.h>
#include <Adafruit_SPITFT_Macros.h>
#include <gfxfont.h>

// Screen dimensions and OLED definitions
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 128  // Change this to 96 for 1.27" OLED.


//SWAPPING THE CD AND DIN INPUTS WILL MAKE THE LED MATRIX WORK
//I DON'T KNOW WHY
#define SCLK_PIN 13
#define MOSI_PIN 11
#define DC_PIN 7
#define CS_PIN 10
#define RST_PIN 8


//LED Matrix and Thumbstick Definitions
#define CLK_PIN 13
#define DATA_PIN 2

#define MATRIX_CS_PIN 10
#define MATRIX_DIGIN_PIN 11

#define VERT_PIN A4
#define HORZ_PIN A5
#define SEL_PIN 4

#define MAX_DEVICES 4

//Y = vertical and X = horizontal relative to all devices
const int maxX = 8;
const int maxY = MAX_DEVICES * 8;
const int minY = 0;
const int minX = 0;

ArrayList<int> wallCoordX(ArrayList<int>::DYNAMIC2, 100);
ArrayList<int> wallCoordY(ArrayList<int>::DYNAMIC2, 100);

MD_MAX72XX mx = MD_MAX72XX(MD_MAX72XX::GENERIC_HW, MATRIX_CS_PIN, MAX_DEVICES);

int x = 0;
int y = 0;

int startDestinationY = 5;
int startDestinationX = 5;

bool isInitialized = false;
bool hasMoved;

Adafruit_SSD1351 OLED = Adafruit_SSD1351(SCREEN_WIDTH, SCREEN_HEIGHT, &SPI, CS_PIN, DC_PIN, RST_PIN);

void setup() {
  //Adafruit_SSD1351(10, 7, 11, 13, 8);

  //mx.transform(MD_MAX72XX::TFUD);

  Serial.begin(9600);
  Serial.flush();

  mx.begin();
  mx.control(MD_MAX72XX::INTENSITY, 1);
  mx.clear();

  DrawRoom();

  pinMode(8, INPUT_PULLUP);
  //pinMode(13, OUTPUT);

  pinMode(VERT_PIN, INPUT);
  pinMode(HORZ_PIN, INPUT);
  pinMode(SEL_PIN, INPUT_PULLUP);

  y = startDestinationY;
  x = startDestinationX;

  mx.setPoint(x, y, true);
  mx.setPoint(12, 12, true);

  ControlWall(3, 3, true);
  //delay(1000);
}

void loop() {

  int horz = analogRead(HORZ_PIN);
  int vert = analogRead(VERT_PIN);

  DrawRoom();

  //ControlWall(3, 3, true);


  //inputs

  //THESE ARE INVERTED BECAUSE MOVEMENT IS RELATIVE TO LCD MATRIX
  // NOT THE CONTROLS ITSELF
  if (vert < 300) {
    //Serial.println("DOWN");
    MovePlayer(x - 1, y);
    hasMoved = true;
  }
  if (vert > 700) {
    //Serial.println("UP");
    MovePlayer(x + 1, y);
    hasMoved = true;
  }
  if (horz > 700) {
    //Serial.println("LEFT");
    MovePlayer(x, y - 1);
    hasMoved = true;
  }
  if (horz < 300) {
    //Serial.println("RIGHT");
    MovePlayer(x, y + 1);
    hasMoved = true;
  }


  if (digitalRead(SEL_PIN) == LOW) {
    PrintDebug();
  }

  mx.update();

  delay(100);
}


void MovePlayer(int targetX, int targetY) {
  if (!CheckPointForWall(targetX, targetY)) {
    // Serial.print("Target Pos X:");
    // Serial.print(targetX);
    // Serial.print(", Target Pos Y:");
    // Serial.print(targetY);
    // Serial.println();
    mx.setPoint(x, y, false);
    x = targetX;
    y = targetY;
    mx.setPoint(x, y, true);
  } else {
    //move failed
    Serial.println("WALL HIT");
  }
}



void DrawRoom() {
  for (int i = 0; i < maxX; i++) {
    for (int j = 0; j < maxY; j++) {
      if (i == 0 || i == maxX - 1 || j == 0 || j == maxY - 1)
        ControlWall(i, j, true);
    }
  }
}


void ControlWall(int xCoord, int yCoord, bool state) {
  if (state) {
    SetWallCoord(xCoord, yCoord);
  } else {
    //mx.setPoint(xCoord, yCoord, false);
  }
}

void SetWallCoord(int xCoord, int yCoord) {

  //if the object is not already in both arrays, store them to both arrays
  if (!CheckPointForWall(xCoord, yCoord)) {
    wallCoordX.add(xCoord);
    wallCoordY.add(yCoord);
    mx.setPoint(xCoord, yCoord, true);
    // Serial.print("Wall saved at: X ");
    // Serial.print(xCoord);
    // Serial.print(", Y ");
    // Serial.print(yCoord);
    // Serial.println();
    return true;
  }
  //Serial.println("Coord already exists, not building wall");
  return false;
}

bool CheckPointForWall(int coordX, int coordY) {
  for (int i = 0; i < wallCoordX.size(); i++) {
    for (int j = 0; j < wallCoordY.size(); j++) {
      // Serial.print("Wall checked at: X ");
      // Serial.print(wallCoordX.get(i));
      // Serial.print(", Y ");
      // Serial.print(wallCoordY.get(j));
      // Serial.println();

      //check each object in index and return if they're both in the same index
      if (coordX == wallCoordX.get(i) && coordY == wallCoordY.get(i))
        return true;
    }
  }
  return false;
}

void PrintDebug() {
  //Serial.println(wallCoordX.size());
  for (int i = 0; i < wallCoordX.size(); i++) {
    Serial.print("INDEX");
    Serial.print(i);
    Serial.print("|");
    Serial.print("X:");
    Serial.print(wallCoordX.get(i));
    Serial.print(", Y:");
    Serial.print(wallCoordY.get(i));
    Serial.println();
  }
}