#include <AccelStepper.h>
#include <ButtonDebounce.h>
#include <MD_MAX72xx.h>
#include <LedControl.h>

#define STEPSIZE 52
#define MAX_MOVES 256
#define UP 1
#define LEFT 2
#define DOWN 3
#define RIGHT 4
#define CLK_PIN 12
#define DATA_PIN 10
#define CS_PIN 11
#define NUM_SEGMENTS 4

AccelStepper downRight (1, 3, 2);
AccelStepper downLeft (1, 5, 4);
AccelStepper upLeft (1, 7, 6);
AccelStepper upRight (1, 9, 8);

ButtonDebounce up(22, 100);
ButtonDebounce left(23, 100);
ButtonDebounce down(24, 100);
ButtonDebounce right(25, 100);
ButtonDebounce enter(53, 100);

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

int currentRow = 7;
int currentCol = 0;
int currentModule = 3;
int moves[MAX_MOVES];
int count = 0;
int upCounter = 1;
int downCounter = 1;
int speed = 1500;
int stepSize = 100;
int start = 0;
int first = 1;
int prevDownState = 1;


void upChanged(const int state) 
{
  byte block = 0;
  if(String(state) == "0")
  {
    if(currentRow <= 7 && currentRow > 0 &&currentModule <= 1 )
    {
      currentRow--;
    }
    else if(currentRow < 7 && currentModule > 1 )
    {
      currentRow++;
    }
    else if(currentModule >= 3)
    {
      currentModule = currentModule -3;
      currentCol = 7-currentCol;
      currentRow = 7;
    }
    else if(currentModule == 2 && currentRow >=7)
    {
      currentModule = currentModule -1;
      currentCol = 7-currentCol;
      currentRow = 7;
    }
    else
    {
      block = 1;
    }
    Serial.print(currentModule);
    Serial.print(currentRow);
    Serial.println(currentCol);
    if(!block)
    {
      moves[count] = UP;
      count++;
    }
    
  }
}
void leftChanged(const int state) 
{
  byte block = 0;
  if(String(state) == "0")
  {
    if(currentCol > 0 && currentModule > 1)
    {
      currentCol--;
    }
    else if(currentModule == 2 && currentCol > 0 || currentModule == 3)
    {
      currentModule--;
      currentCol = 7;
    }

    else if(currentCol < 7 && currentModule <= 1)
    {
      currentCol++;
    }
    else if(currentModule == 0 && currentCol >= 7)
    {
      currentModule++;
      currentCol = 0;
    }
    Serial.print(currentModule);
    Serial.print(currentRow);
    Serial.println(currentCol);

    if(!block)
    {
      moves[count] = LEFT;
      count++;
    }
  }
}
void downChanged(const int state) 
{
  byte block = 0;
  if(String(state) == "0")
  {
    if(currentRow < 7 &&currentModule <= 1 )
    {
      currentRow++;
    }
    else if(currentRow > 0 && currentModule > 1 )
    {
      currentRow--;
    }
    else if(currentModule == 0)
    {
      currentModule = currentModule +3;
      currentCol = 7-currentCol;
      currentRow = 7;
    }
    else if(currentModule == 1)
    {
      currentModule++;
      currentCol = 7-currentCol;
      currentRow = 7;
    }
    Serial.print(currentModule);
    Serial.print(currentRow);
    Serial.println(currentCol);

    if(!block)
    {
      moves[count] = DOWN;
      count++;
    }
  }
}
void rightChanged(const int state) 
{
  byte block = 0;
  if(String(state) == "0")
  {
    if(currentCol < 7 && currentModule > 1)
    {
      currentCol++;
    }
    else if(currentModule == 2 && currentCol >= 7)
    {
      currentModule++;
      currentCol = 0;
    }
    else if(currentCol > 0 && currentModule <= 1)
    {
      currentCol--;
    }
    else if(currentModule == 1 && currentCol <= 0)
    {
      currentModule--;
      currentCol = 7;
    }
    Serial.print(currentModule);
    Serial.print(currentRow);
    Serial.println(currentCol);
    if(!block)
    {
      moves[count] = RIGHT;
      count++;
    }
  }
}

void moveUp(int steps)
{
  upLeft.move(-stepSize);
  upRight.move(stepSize);
  downLeft.move(-stepSize);
  downRight.move(stepSize);

    do
    {
      upLeft.run();
      upRight.run();
      downLeft.run();
      downRight.run();
    }
    while(upLeft.distanceToGo() != 0);
}

void moveDown(int steps)
{
  upLeft.move(stepSize);
  upRight.move(-stepSize);
  downLeft.move(stepSize);
  downRight.move(-stepSize);

    do
    {
      upLeft.run();
      upRight.run();
      downLeft.run();
      downRight.run();
    }
    while(upLeft.distanceToGo() != 0);
}

void moveLeft(int steps)
{
  upLeft.move(-stepSize);
  upRight.move(-stepSize);
  downLeft.move(-stepSize);
  downRight.move(-stepSize);

    do
    {
      upLeft.run();
      upRight.run();
      downLeft.run();
      downRight.run();
    }
    while(upLeft.distanceToGo() != 0);
}

void moveRight(int steps)
{
  upLeft.move(stepSize);
  upRight.move(stepSize);
  downLeft.move(stepSize);
  downRight.move(stepSize);

    do
    {
      upLeft.run();
      upRight.run();
      downLeft.run();
      downRight.run();
    }
    while(upLeft.distanceToGo() != 0);
}

void enterChanged(const int state) 
{
  downRight.setCurrentPosition(0);
  downLeft.setCurrentPosition(0);
  upRight.setCurrentPosition(0);
  upLeft.setCurrentPosition(0);
  upCounter = 1;
  downCounter = 1;

  if(String(state) == "0")
  {
    for(int i = 0; i < MAX_MOVES; i++)
    {
      Serial.print("Move ");
      Serial.print(i);
      Serial.print(":");
      Serial.println(moves[i]);
      if(moves[i] == UP)
      {
        moveUp(stepSize);
      }
      else if(moves[i] == LEFT)
      {
        moveLeft(stepSize);
      }
      else if(moves[i] == DOWN)
      {
        moveDown(stepSize);
      }
      else if(moves[i] == RIGHT)
      {
        moveRight(stepSize);
      }
      else if(moves[i] == 0)
      {
        
      }
    }
  }
}


void setup() 
{
  up.setCallback(upChanged);
  left.setCallback(leftChanged);
  down.setCallback(downChanged);
  right.setCallback(rightChanged);
  enter.setCallback(enterChanged);

  for (int i = 0; i < NUM_SEGMENTS; i++)
  {
    lc.shutdown(i, false);
    lc.setIntensity(i, 8);
    lc.clearDisplay(i);
  }

  lc.setLed(currentModule, currentRow, currentCol, true);

  downRight.setMaxSpeed(300);
  downLeft.setMaxSpeed(300);
  upRight.setMaxSpeed(300);
  upLeft.setMaxSpeed(300);
  upLeft.setAcceleration(2000);
  upRight.setAcceleration(2000);
  downLeft.setAcceleration(2000);
  downRight.setAcceleration(2000);

  pinMode(STEPSIZE, INPUT_PULLUP);

  Serial.begin(9600);

}



void loop() 
{
  up.update();
  left.update();
  down.update();
  right.update();
  enter.update();

  lc.setLed(currentModule, currentRow, currentCol, true);

  if(digitalRead(STEPSIZE))
  {
    stepSize = 100;
  }
  else
  {
    stepSize = 1000;
  }

  if(count > MAX_MOVES)
  {
    count = 0;
  }

}

void moveStepper(int steps, int speed)
{
  upLeft.setSpeed(speed);
  int newPosition = upLeft.currentPosition() + steps;
  upLeft.moveTo(newPosition);
}