#include "gButton.h"
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <AccelStepper.h>

gButton button(12);
gButton button1(11);
gButton button2(10);
LiquidCrystal_I2C lcd(0x27, 16, 4);

float drillDepthmm = 0.00;
float drillDepth = -5;  // eqautes to .125mm .125 div by 0.025
int Safe = -6400 ; /////////////////////////////

int RoughCut = 10;
int FineCut = 5;
int Cutdepthstep = 0;
float stepMM = 0.0125;

const int stepPin = 41;
const int directionPin = 40;
int button1State = 0;
int button2State = 0;
int button3State = 0;
int State = 0;
float num = 0.0;
int Drill = 0;
float GlassThick = 10.00;
int GlassThickMenu = 0;
int arm = 0;
AccelStepper stepper(AccelStepper::DRIVER, stepPin, directionPin);

void ButtonCheck() {

}

void setup() {
  button.debounceTime(55);
  button1.debounceTime(55);
  button2.debounceTime(55);
  Serial.begin(115200); //Open the serial monitor to see the output
  button.begin(); //Initialize the button
  button1.begin(); //Initialize the button
  button2.begin(); //Initialize the button
  lcd.init(); // initialize the lcd
  lcd.backlight();
  lcd.setCursor(4, 0);
  lcd.print("Drill Ready");
  stepper.setMaxSpeed(1000);
  stepper.setSpeed(50);
}

void loop() {
  lcd.setCursor(0, 1);
  lcd.print("                    ");

  ButtonCheck();
  bool activate;
  bool activate1;
  bool activate2;
  activate = button.up();
  activate1 = button1.up();
  activate2 = button2.up();

  if (activate) {
    //Serial.println("Button activated");
    State = 1;
    lcd.setCursor(0, 1);
    lcd.print("** Rough Jog Mode **");
    lcd.setCursor(0, 2);
    lcd.print("Current Pos:");
    lcd.setCursor(0, 3);
    lcd.print("FineJ   JogU   JogD");
  }

  /////////////////////////////////////////// ROUGH  JOG MODE
  while (State == 1) {
    activate = button.up();
    activate1 = button1.sustained();
    activate2 = button2.sustained();
    if (activate) {
      lcd.setCursor(0, 1);
      lcd.print("                    ");
      lcd.setCursor(0, 1);
      lcd.print("** Fine Jog Mode **");
      lcd.setCursor(0, 3);
      lcd.print("GlassT   JogU   JogD");
      State = 2;
    }
    if (activate1) {
      Drill = 1;

    }
    while (Drill == 1) {
      stepper.setSpeed(-1500);
      stepper.runSpeed();
      activate = button1.change();
      if (activate) {
        Drill = 0;
        displayUpdate();
      }
    }
    if (activate2) {
      Drill = 2;
    }
    while (Drill == 2) {
      stepper.setSpeed(1500);
      stepper.runSpeed();
      activate = button2.change();
      if (activate) {
        Drill = 0;
        displayUpdate();
      }
    }
  }
  /////////////////////////////////////////////////////Fine Jog Mode
  while (State == 2) {
    activate = button.up();
    activate1 = button1.sustained();
    activate2 = button2.sustained();
    if (activate) {
      lcd.setCursor(0, 1);
      lcd.print("                    ");
      lcd.setCursor(0, 1);
      lcd.print("* Glass thickness *");
      lcd.setCursor(0, 3);
      lcd.print("                    ");
      lcd.setCursor(0, 3);
      lcd.print("ARM   Thick-  Thick+");
      lcd.setCursor(0, 2);
      lcd.print("                    ");
      lcd.setCursor(0, 2);
      lcd.print("Glass Thick:");
      lcd.setCursor(12, 2);
      lcd.print(GlassThick, 1);
      State = 3;
    }
    if (activate1) {
      Drill = 1;

    }
    while (Drill == 1) {
      stepper.setSpeed(-100);
      stepper.runSpeed();
      activate = button1.change();
      if (activate) {
        Drill = 0;
        displayUpdate();
      }
    }
    if (activate2) {
      Drill = 2;
    }
    while (Drill == 2) {
      stepper.setSpeed(100);
      stepper.runSpeed();
      activate = button2.change();
      if (activate) {
        Drill = 0;
        displayUpdate();
      }
    }
  }
  //////////////////////////////////////////////   Glass Thickness
  while (State == 3) {
    activate = button.up();
    activate1 = button1.sustained();
    activate2 = button2.sustained();
    if (activate) {
      lcd.setCursor(0, 1);
      lcd.print("                    ");
      lcd.setCursor(0, 1);
      lcd.print("***** ARM MODE *****");
      lcd.setCursor(0, 3);
      lcd.print("                    ");
      lcd.setCursor(0, 2);
      lcd.print("                    ");
      lcd.setCursor(0, 3);
      lcd.print("RESET    ARM   START");
      State = 4;
    }
    if (activate1) {
      String length1 = String (GlassThick);
      //Serial.println(length1.length());  // get 4 digit
      int length2 = length1.length();
      length2 = length2 + 12;
      lcd.setCursor(length2, 2);
      lcd.print("    ");
      lcd.setCursor(12, 2);
      lcd.print(GlassThick, 1);
      GlassThick = GlassThick - 0.1;
      delay(100);
    }
    if (activate2) {
      String length1 = String (GlassThick);
      int length2 = length1.length();
      length2 = length2 + 12;
      //Serial.println(length1.length());  // get 4 digit

      lcd.setCursor(length2, 2);
      lcd.print("   ");
      lcd.setCursor(12, 2);
      lcd.print(GlassThick, 1);
      GlassThick = GlassThick + 0.1;
      delay(100);
    }
  }
  //////////////////////////////////////////////////Arm Mode
  while (State == 4) {
    activate = button.up();
    activate1 = button1.up();
    activate2 = button2.up();
    if (activate) {

      State = 0;
    }
    if (activate1) {
      if (arm == 0 ) {
        lcd.setCursor(0, 3);
        //Serial.println(stepper.currentPosition());
        stepper.setCurrentPosition(0);
        //Serial.println(stepper.currentPosition());
        float pos = stepper.currentPosition();
        //Serial.print(pos);

        Safe = round(Safe);
        stepper.setSpeed(4000);
        stepper.setAcceleration(500.0);
        stepper.runToNewPosition(Safe);


      }

      arm = 1;
    }
    if (activate2) {
      lcd.setCursor(0, 3);
      if (arm == 1) {
        float CutCalc = RoughCut * stepMM;
        Cutdepthstep = GlassThick / CutCalc;
        Serial.println(RoughCut);
        Serial.println(stepMM);
        Serial.println(CutCalc);
        State = 5;
        lcd.setCursor(0, 0);
        lcd.print("                    ");
        lcd.setCursor(0, 0);
        lcd.print("----- Running -----");
        lcd.setCursor(0, 1);
        lcd.print("Drill depth:        ");
        lcd.setCursor(0, 2);
        lcd.print("Passes:        ");
        lcd.setCursor(0, 3);
        lcd.print("                    ");
        lcd.setCursor(0, 3);
        lcd.print("STOP    STOP    STOP");
        stepper.runToNewPosition(0);
      }
    }
  }
  ///////////////////////////////////////////////Run ode
  while (State == 5) {
    stepper.setSpeed(3000);
    stepper.setMaxSpeed(3000);
    stepper.setAcceleration(2000);
    Serial.println(Cutdepthstep);



    int Roughcutamount = Cutdepthstep * .8 ;
    int finecutamount = Cutdepthstep * .4 ;
    int runTime = 0;

    int RoughClear1 = Roughcutamount * .25;
    int RoughClear2 = Roughcutamount * .5;
    int RoughClear3 = Roughcutamount * .75 ;
    int RoughClear4 = Roughcutamount * 4;

    int FineClear1 = finecutamount * .25;
    int FineClear2 = finecutamount * .5;
    int FineClear3 = finecutamount * .75 ;
    int FineClear4 = finecutamount * 4;

    ///////////////////////////////////////////////////////////Rough
    for (int i = 0; i <= Roughcutamount; i++) {
      drillDepthmm = drillDepth * 0.0125;
      Serial.println("Rough");
      lcd.setCursor(12, 1);
      lcd.print("       ");
      lcd.setCursor(12, 1);
      lcd.print(drillDepthmm, 2);
      lcd.setCursor(12, 2);
      lcd.print(i);

      if ( i == RoughClear1) {
        int wait = 1;
        lcd.setCursor(12, 1);
        lcd.print("       ");
        lcd.setCursor(12, 1);
        lcd.print("Re-Ro1");
        stepper.setAcceleration(400.0);
        stepper.runToNewPosition(Safe);
        while (wait == 1 ) {
          activate = button.up();
          if (activate) {
            wait = 0;
          }
        }}

      if ( i == RoughClear2) {
        int wait = 1;
        lcd.setCursor(12, 1);
        lcd.print("       ");
        lcd.setCursor(12, 1);
        lcd.print("Re-Ro(2)");
        stepper.setAcceleration(400.0);
        stepper.runToNewPosition(Safe);
        while (wait == 1 ) {
          activate = button.up();
          if (activate) {
            wait = 0;
          }
        }}
      if ( i == RoughClear3) {
        int wait = 1;
        lcd.setCursor(12, 1);
        lcd.print("       ");
        lcd.setCursor(12, 1);
        lcd.print("Re-Ro(3)");
                stepper.setAcceleration(400.0);
        stepper.runToNewPosition(Safe);
        while (wait == 1 ) {
          activate = button.up();
          if (activate) {
            wait = 0;
          }
        }}
      if ( i == RoughClear4) {
        int wait = 1;
        lcd.setCursor(12, 1);
        lcd.print("       ");
        lcd.setCursor(12, 1);
        lcd.print("Re-Ro(4)");
                stepper.setAcceleration(400.0);
        stepper.runToNewPosition(Safe);
        while (wait == 1 ) {
          activate = button.up();
          if (activate) {
            wait = 0;
          }
        }}





        for (int i = 0; i <= 3; i++) {
          stop();
          runTime = 1;
          while (runTime == 1) {

            stepper.moveTo(drillDepth);
            stepper.run();
            //Serial.println()("Moving Down");
            if (stepper.currentPosition() == drillDepth) {
              runTime = 2;
              delay(2000);
            }
          }
          while (runTime == 2) {
            stop();
            stepper.moveTo(0);
            stepper.run();
            //Serial.println()("Moving Up");
            if (stepper.currentPosition() == 0) {
              runTime = 0;
            }
          }
        }
        drillDepth = drillDepth + RoughCut;
        activate = button.up();
        activate1 = button1.sustained();
        if (activate) {
          stepper.runToNewPosition(5000);
        }
        if (activate1) {
          int exitme = 1;
          while (exitme == 1) {
            stepper.runToNewPosition(Safe);
            State = 0;
            runTime = 0;
            arm = 0;
            i = 100;

          }
        }
      }
      ///////////////////////////////////////////////////////////Fine
      for (int i = 0; i <= finecutamount; i++) {
        drillDepthmm = drillDepth * 0.0125;
        Serial.println("Fine");
        lcd.setCursor(12, 1);
        lcd.print("       ");
        lcd.setCursor(12, 1);
        lcd.print(drillDepthmm, 2);
        lcd.setCursor(12, 2);
        lcd.print(i);

     if ( i == FineClear1) {
        int wait = 1;
        lcd.setCursor(12, 1);
        lcd.print("       ");
        lcd.setCursor(12, 1);
        lcd.print("Re-Fi(1)");
                stepper.setAcceleration(400.0);
        stepper.runToNewPosition(Safe);
        while (wait == 1 ) {
          activate = button.up();
          if (activate) {
            wait = 0;
          }
        }}
     if ( i == FineClear2) {
        int wait = 1;
        lcd.setCursor(12, 1);
        lcd.print("       ");
        lcd.setCursor(12, 1);
        lcd.print("Re-Fi(2)");
                stepper.setAcceleration(400.0);
        stepper.runToNewPosition(Safe);
        while (wait == 1 ) {
          activate = button.up();
          if (activate) {
            wait = 0;
          }
        }}
     if ( i == FineClear3) {
        int wait = 1;
        lcd.setCursor(12, 1);
        lcd.print("       ");
        lcd.setCursor(12, 1);
        lcd.print("Re-Fi(3)");
                stepper.setAcceleration(400.0);
        stepper.runToNewPosition(Safe);
        while (wait == 1 ) {
          activate = button.up();
          if (activate) {
            wait = 0;
          }
        }}
     if ( i == FineClear4) {
        int wait = 1;
        lcd.setCursor(12, 1);
        lcd.print("       ");
        lcd.setCursor(12, 1);
        lcd.print("Re-Fi(4)");
                stepper.setAcceleration(400.0);
        stepper.runToNewPosition(Safe);
        while (wait == 1 ) {
          activate = button.up();
          if (activate) {
            wait = 0;
          }
        }}


        for (int i = 0; i <= 3; i++) {
          runTime = 1;

          while (runTime == 1) {


            stepper.moveTo(drillDepth);
            stepper.run();
            //Serial.println()("Moving Down");
            if (stepper.currentPosition() == drillDepth) {
              runTime = 2;
              delay(2000);
            }
          }
          while (runTime == 2) {

            stepper.moveTo(0);
            stepper.run();
            //Serial.println()("Moving Up");
            if (stepper.currentPosition() == 0) {
              runTime = 0;
            }
          }
        }
        drillDepth = drillDepth + FineCut;
      }
      ///int RoughCut = 10;
      ///int FineCut = 5;



      State = 0;
      activate = button.up();
      if (activate) {
        stepper.runToNewPosition(5000);
      }
    }
    ///////////////////////////////////////////////////End
  }


  void displayUpdate() {
    float pos = stepper.currentPosition();
    Serial.println(pos);
    float pos1 = pos * 0.0125;
    lcd.setCursor(12, 2);
    lcd.print("        ");
    lcd.setCursor(12, 2);
    lcd.print(pos1);
  }

  void stop() {

  }