#include <Stepper.h>
#include <LiquidCrystal_I2C.h>
#include <SD.h>

#define CS_PIN 53



const int stepsPerRevolution = 200;  

Stepper myStepperH(stepsPerRevolution, 8, 9, 10, 11);
Stepper myStepperV(stepsPerRevolution, 4, 5, 6, 7);
LiquidCrystal_I2C lcd(0x27,  20, 4);

int testWidth = 7;
int testHeight = 37;

int a[7][37];


int cursorIndex = 0;
int menuIndex = 0;

bool upArrow = LOW;
bool downArrow = LOW;
bool leftArrow = LOW;
bool rightArrow = LOW;

bool enter = LOW;


bool upInputFreeze = false;
bool downInputFreeze = false;
bool leftInputFreeze = false;
bool rightInputFreeze = false;

bool enterFreeze = false;


bool startTest = 0;
bool mainMenu = 1;
bool liveSensor = 0;

bool verticalLimit = 1;
bool horizontalLimit = 1;

bool topLimit = 1;



int upPin = 2;
int downPin = 3;
int leftPin = 25;
int rightPin = 24;

int enterPin = 1;


int vLimitPin = 13;
int hLimitPin = 12;
int topLimitPin = 23;


int refIndex = 0;
int menuRefIndex = 0;

int stepsToInchH = 5;
int stepsToInchV = 5;

int horizontalStepperPos = 0;
int verticalStepperPos = 0;

int minimumDesired = 40;
int sensitivity = 0.5;

int sensorData = 40;

int hStepperDelay = 1;
int vStepperDelay = 1;

int avg[] = {0, 0, 0};


void hStep (int hSteps)
{
  for (int i = 0; i < abs(hSteps); i++)
  {
   myStepperH.step(1*(hSteps/abs(hSteps)));
   delay(hStepperDelay);
  }
  horizontalStepperPos = horizontalStepperPos+(hSteps/stepsToInchH);

}

void vStep (int vSteps)
{
  for (int i = 0; i < abs(vSteps); i++)
  {
   myStepperV.step(1*(vSteps/abs(vSteps)));
   delay(vStepperDelay);
  }
  verticalStepperPos = verticalStepperPos+(vSteps/stepsToInchV);

}

void center ()
{
  hStep(((testWidth/2)-horizontalStepperPos)*stepsToInchH);
}

void home () 
{
  clearAll ();
  lcd.setCursor(0,0);
  lcd.print("Homing...");
  while (verticalLimit == 1)
  {
    verticalLimit = digitalRead(vLimitPin);
    vStep(-1);
  }
  vStep(+1);
  verticalStepperPos = 0;
  while (horizontalLimit == 1)
  {
    horizontalLimit = digitalRead(hLimitPin);
    hStep(-1);
  }
  hStep(1);
  horizontalStepperPos = 0;

  delay(500);
}

void printMainMenu ()
{
  lcd.setCursor(0, 0);
  lcd.print("Begin Test");  
  lcd.setCursor(0, 1);
  lcd.print("Manual Mode"); 
  lcd.setCursor(0, 2);
  lcd.print("Test Settings"); 
  lcd.setCursor(0, 3);
  lcd.print("Home Sensor"); 
}

void controlLiveSensor ()
{
  myStepperH.setSpeed(20);
  myStepperV.setSpeed(20);
  int ceilingHeight = searchCeiling ();

  int hold = 0;

  while (liveSensor == 1)
  {
    if (hold == 0)
    {
      clearAll ();
      lcd.setCursor(8,2);
      lcd.print("Ft/s");
      lcd.setCursor(9,1);
      lcd.print(sensorData);
      lcd.setCursor(1,0);
      lcd.print("X Pos:");
      lcd.setCursor(7,0);
      lcd.print(horizontalStepperPos);
      lcd.setCursor(1,3);
      lcd.print("Y Pos:");
      lcd.setCursor(7,3);
      lcd.print(verticalStepperPos);
      lcd.setCursor(10,3);
      lcd.print("From C:");
      lcd.setCursor(17,3);
      lcd.print(ceilingHeight - verticalStepperPos);
      hold = 1;
    }
    else 
    {
      hold = hold + 1;
    }
    if (hold > 10)
    {
      hold = 0;
    }

    upArrow = digitalRead(upPin);
    downArrow = digitalRead(downPin);
    leftArrow = digitalRead(leftPin);
    rightArrow = digitalRead(rightPin);

    enter = digitalRead(enterPin);

    if (enter == 0)
    {
      liveSensor = 0;
    }

    if (upArrow == 0 && verticalStepperPos < ceilingHeight)
    {
      vStep(stepsToInchV);
    }
    if (downArrow == 0 && verticalStepperPos > 0)
    {
      vStep(-stepsToInchV);
    }
    if (leftArrow == 0 && horizontalStepperPos > 0)
    {
      hStep(-stepsToInchH);
    }
    if (rightArrow == 0 && horizontalStepperPos < 7)
    {
      hStep(stepsToInchH);
    }
    //Serial.println(verticalStepperPos);


  }
    

  myStepperH.setSpeed(60);
  myStepperV.setSpeed(60);
}




int searchCeiling ()
{
  while ((verticalStepperPos <37) && (topLimit == 1))
    {
      vStep(stepsToInchV);
      topLimit = digitalRead(topLimitPin);
      delay (500);
    }
  return verticalStepperPos;
}

int searchMinimum (int cielingH) 
{
  center ();
  vStep(-verticalStepperPos*stepsToInchV);
  int value = 0;
  while ((value < minimumDesired) && (verticalStepperPos <cielingH) && (topLimit == 1))
    {
      vStep(stepsToInchV);
      topLimit = digitalRead(topLimitPin);
      value = sensorData;
    }
  return verticalStepperPos;
}


void test () { 

  clearAll ();
  home ();
  int ceilingHeight = searchCeiling ();
  int minHeight = searchMinimum (ceilingHeight);
  hStep (-horizontalStepperPos*stepsToInchH);
  vStep (minHeight - verticalStepperPos);




  if (SD.exists("test.txt")){
    SD.remove("test.txt");
  }

  File myFile = SD.open("test.txt", FILE_WRITE);

  

  Serial.write(myFile.read());



  lcd.setCursor(0,0);
  lcd.print("Test in progress...");

  for(int i = minHeight; i<ceilingHeight; i++)
	{
    hStep (-horizontalStepperPos*stepsToInchH);
    for(int j = 0; j<testWidth; j++)
    {
      for(int k = 0; k<3; k++){
        avg[k] = sensorData;
        delay(100);
      }
      myFile.write((avg[1]+avg[2]+avg[3])/3);
      myFile.write(",");
      //a[i][j] = 1;
      hStep(stepsToInchH);
    }
    vStep(stepsToInchV);
    myFile.write("\n");
	}

  myFile.close();


  startTest = 0;
  clearAll ();
  lcd.setCursor(0,0);
  lcd.print("All done :)");
  delay(1500);
  clearAll ();
}



void clearRow(byte rowToClear)
{
  lcd.setCursor(0, rowToClear);
  lcd.print("                    ");  
}

void clearArrow(byte rowToClear)
{
  lcd.setCursor(14, rowToClear);
  lcd.print("      ");  
}

void clearAll()
{
  for(int i = 0; i<4; i++)
  {
    lcd.setCursor(0, i);
    lcd.print("                    ");  
   }
}



void setup() {
  // set the speed at 60 rpm:
  myStepperH.setSpeed(60);
  myStepperV.setSpeed(60);
  // initialize the serial port:
  Serial.begin(115200);


  if (!SD.begin(CS_PIN)) {
    Serial.println("Card initialization failed!");
    while (true);
  }


  lcd.init();
  lcd.backlight();


  pinMode(upPin, INPUT_PULLUP);
  pinMode(downPin, INPUT_PULLUP);
  pinMode(leftPin, INPUT_PULLUP);
  pinMode(rightPin, INPUT_PULLUP);


  pinMode(enterPin, INPUT_PULLUP);

  pinMode(vLimitPin, INPUT_PULLUP);
  pinMode(hLimitPin, INPUT_PULLUP);

  pinMode(topLimitPin, INPUT_PULLUP);



  lcd.setCursor(2,0);
  lcd.print("Price Industries");
  lcd.setCursor(5,1);
  lcd.print("Air Throw");
  lcd.setCursor(1,2);
  lcd.print("Velocity Test Cart");
  lcd.setCursor(9,3);
  lcd.print(":)");
  delay(2000);
  clearAll ();
  delay(1000);

  //home ();
}

void loop() {

  refIndex = cursorIndex;
  menuRefIndex = menuIndex;
  

  upArrow = digitalRead(upPin);
  downArrow = digitalRead(downPin);
  leftArrow = digitalRead(leftPin);
  rightArrow = digitalRead(rightPin);

  enter = digitalRead(enterPin);

  verticalLimit = digitalRead(vLimitPin);
  horizontalLimit = digitalRead(hLimitPin);
  
  topLimit = digitalRead(topLimitPin);


  if (mainMenu == 1)
  {
    printMainMenu ();
  }



  if (upArrow == 0) {
      if (upInputFreeze == false && cursorIndex > 0) {
        cursorIndex = cursorIndex-1;
      }
      upInputFreeze = true;
  } else {
    upInputFreeze = false;
  }

   
  if (downArrow == 0) {
      if (downInputFreeze == false && cursorIndex < 3) {
        
        cursorIndex = 1+cursorIndex;
      }
      downInputFreeze = true;
  } else {
    downInputFreeze = false;
  }

  if (leftArrow == 0) {
      if (leftInputFreeze == false && menuIndex > 0) {
        
        menuIndex = menuIndex-1;
      }
      leftInputFreeze = true;
  } else {
    leftInputFreeze = false;
  }


  if (rightArrow == 0) {
      if (rightInputFreeze == false && menuIndex < 4) {
        
        menuIndex = 1+menuIndex;
      }
      rightInputFreeze = true;
  } else {
    rightInputFreeze = false;
  }



  if (enter == 0) {
      if (enterFreeze == false && cursorIndex == 0 && startTest == 0) 
      {
        startTest = 1;
      }
      if (enterFreeze == false && cursorIndex == 1 && startTest == 0 && enter == 0) 
      {
        liveSensor = 1;
      }
      if (enterFreeze == false && cursorIndex == 3 && startTest == 0) 
      {
        home ();
      }
      enterFreeze = true;
  } else {
    enterFreeze = false;
  }


  if (abs(cursorIndex-refIndex)>0) clearArrow(refIndex);
  //if (abs(menuIndex-menuRefIndex)>0) clearRow(refIndex);
  
  lcd.setCursor(15,cursorIndex);
  lcd.print("<---");


  if (liveSensor == 1)
  {
    controlLiveSensor ();
  }

  if (startTest == 1)
  {
    
    test ();

  delay(1000);
  }
 








}