/* Functionality Specification 
1. Press Up Button to lift the router
2. Press Down Button to drop the router.
3. Press Safety button and top button to take router to the top to top. Micro switch  or contact switch will stop the lift.
4. Press Safety button and bottom button to take router to the  botton. Micro switch  will stop the lift.
5. Encoder - Clockwise with move the lift up a number of steps at a time
6. Encoder - CounterClockwise with move the lift down a number of steps at a time
7. Encoder Switch - Will switch to lift to 'Slow' mode when the number of steps per trun are reduced.
8. Contact switch (two magents, on the router and flat surface above the router) is use to set the top of the router exactly level with the table 
9.  zero button with reset theh display to 0mm
10. Display will be updated with the movement in mm from last zero reset

logic
 if up pressed go up whilst pressed
 if up and safety pressed - go allthe way to the top even if they leave go until limit switch
 if down pressed go down whilst pressed
 if down and safety pressed go all the way down eve if thye elave go until limit switch

 if encoder is trune move a high set number of steps clockwise or ccw per click. 
 if encode it switched move a low set number of steps clockwise or ccw per click.
 whenever the encodeis clicked the mode display should be updates.

 At any point if the contact switch is closed ....STOP and tru red led on. 
 wehn disconnected turn the red led off.

 if th zero nbutton is pushed, set measure ment displat to zero

 whenever the lift is moving the measure display should be updated

*/

//LCD Diaply Library Setup //

#include <LiquidCrystal_I2C.h>

#define I2C_ADDR    0x27
#define LCD_COLUMNS 16
#define LCD_LINES   2

LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);

//Stepper Motor Library Setup //

// #include <Stepper.h>


//Set up pin values //

int contactled = 19;

int upbtn = 27;
int safetybtn = 14;
int downbtn = 12;

int zerobtn = 15;

int bottomlimit = 26;
int toplimit = 25;

int switchpin = 33;
int dtpinA = 32;
int clkpinB = 35;

const int step = 2;
const int dir = 4;
const int MS1 = 37;
const int MS2 = 35;
const int MS3 = 33;

int contactinterrupt = 18;

// Other variables

int switchstate = HIGH;
int currentstatedtpinA = LOW;
int laststatedtpinA = currentstatedtpinA;

String encoderspeed = "SLOW";

int encodermove = 1;
int gototopinitiated = 1;
int gotobottominitiated = 1;

int topbtnstate;
int toplimitstate;

String ledstate = "OFF";

const int stepsPerRevolutuion (400);


//tet variables to be deleted 

int upcount=1;
int downcount=1;
int encodercount=1;

int slowspeedsteps = 5;   // 1 step = .04mm 25 steps = 1mm
int fastspeedsteps = 25;
float distance = 0;
float measurement;
int steppermotorsteps;



// SETUP SETUP SETUP SETUP SETUP SETUP SETUP SETUP SETUP 

void setup() {

//Interrupts - CLK on int 1(pin3), Contact on int 0 (pin 2)

attachInterrupt(digitalPinToInterrupt(contactinterrupt),contactmade,CHANGE);
attachInterrupt(digitalPinToInterrupt (dtpinA),encoderrotate,CHANGE);

//  PINMODE SETUP
pinMode (contactled, OUTPUT);
pinMode (upbtn, INPUT);
pinMode (safetybtn, INPUT);
pinMode (downbtn, INPUT);
pinMode (zerobtn, INPUT);
pinMode (bottomlimit, INPUT);
pinMode (toplimit,INPUT);
pinMode (switchpin, INPUT_PULLUP);
pinMode (dtpinA,INPUT);
pinMode (clkpinB,INPUT);

pinMode (step, OUTPUT);
pinMode (dir, OUTPUT);
pinMode (MS1, OUTPUT);
pinMode (MS2, OUTPUT);
pinMode (MS3, OUTPUT);


// STEPPER MOTOR SETUP
 
// MS1(H) MS2(L) MS3(L) = half step mode.   5 steps = 0.1mm movement on 8mm lead screw

//digitalWrite (MS1, HIGH);
//digitalWrite (MS2, LOW);
//digitalWrite (MS3, LOW);
 
 //  SETUP SERIAL MONITOR

Serial.begin(9600);
Serial.print ("Serial port Initoated ");

//  INITIATE LCD PANEL 

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

lcd.setCursor(1,0);
lcd.print ("Height  : ");
lcd.setCursor (1,1);
lcd.print ("Encoder : ");

}


// VOID LOOP

void loop() {

//  If UP button an Safety Button are both pressed - initiate the GoToTop process

if(digitalRead(upbtn) == 0 && digitalRead(safetybtn) == 0) {
  Serial.println ("GO TO TOP initiated - Call");
  delay(250);
  //Takes router to the top when limit switch stops it or contact switch stops it//
    gototop();
 }

//  If DOEN button an Safety Button are both pressed - initiate the GoToBottom process


if(digitalRead(downbtn) == 0 && digitalRead(safetybtn) == 0) {
  Serial.println ("GO TO BOTTOM initiated - Call");
  gotobottominitiated == 0;
  delay(250);
  //Takes router to the bottom when limit switch stops it or contact switch stops it//
  gotobottom();
  }

if(digitalRead(upbtn) == 0 && digitalRead(safetybtn) == 1) {
  Serial.println ("GO UP initiated - Call");
  //Takes router to the bottom when limit switch stops it or contact switch stops it//
  Serial.println ("Go UP initiated - Call");
  upcount = 1;
  goup();
  }

if(digitalRead(downbtn) == 0 && digitalRead(safetybtn) == 1) {
  Serial.println ("GO DOWN initiated - Call");
  //Takes router to the bottom when limit switch stops it or contact switch stops it//
  Serial.println ("Go DOWN initiated - Call");
  godown();
  }


// Test for encoder button press
  
if (digitalRead(switchpin)== HIGH) {  
  Serial.print("Button pressed ");
  if (encoderspeed == "SLOW") {
    encoderspeed = "FAST";
    steppermotorsteps = fastspeedsteps;
   
  } else {
    encoderspeed = "SLOW";
    steppermotorsteps = slowspeedsteps;
  }
  lcd.setCursor (11,1);
  lcd.print (encoderspeed);

}  

//  TEST FOR THE zERO BUTTON

if (digitalRead(zerobtn) == 0) {
  Serial.println("in ZERO fuNCtion     ");
  distance = 0;
  displayheight();
}
 
}

// FUNCTIONS FUNCTIONS FUNCTIONS FUNCTIONS FUNCTIONS FUNCTIONS 

// MOVE THE ROUTER TO TOP OR UNTIL LIMIT SWITCH IS HIT OR ANOTHER BUTTIN IS PRESSED

void gototop() {
  gototopinitiated = 0;
  digitalWrite (dir, HIGH);
  //Serial.println ("GO TO TOP  Function ");
  // move stepper motor up 100 steps (4mm)--- update the display --- check if down butn or encoder have been pressed ...if so break 
  //int gttcounter =1;
  delay (1000);
  while (digitalRead(toplimit) == 1 && gototopinitiated == 0) {
   // Serial.print ("Stepping motor ");
      digitalWrite(step, HIGH);
      digitalWrite (step, LOW);
   //  Serial.print ("Go to top - prior checks  ");
    //  Serial.println (gttcounter);
      checkinterrupt();
   //   Serial.print ("Go to top - after checks  ");
   //   Serial.println (gttcounter);
   //   Serial.print ("GOTOTOP INITIATED  ");
   //   Serial.println (gototopinitiated);
    //  gttcounter++;
    }
  }

  void gotobottom() {
    gotobottominitiated = 0;
     digitalWrite (dir, LOW);
  Serial.println ("GO TO BOTTOM  Function ");
  //move stepper morot
   int gtbcounter=1;
   while (digitalRead(bottomlimit) == 1 && gotobottominitiated == 0) {
      //move stepper motor down
      Serial.print ("Go to bottom - prior checks  ");
      Serial.println (gtbcounter);
      digitalWrite(step, HIGH);
      digitalWrite (step, LOW);
      checkinterrupt();
      Serial.print ("Go to top - after checks  ");
      Serial.println (gtbcounter);
      gtbcounter++;
   }
}


void goup() {
  Serial.println ("GO UP  Function ");
   digitalWrite (dir, HIGH);
  while (digitalRead(upbtn) == 0 && digitalRead(toplimit) == 1 && digitalRead(contactinterrupt) == 1) { 
      //move stepper motoro up x steps 
      Serial.print ("moving motor Up ");
      Serial.println (upcount);
         digitalWrite(step, HIGH);
      digitalWrite (step, LOW);
      upcount++;
    }
      Serial.println ("Motor Stopped in GOUP function");
  }


void godown() {
  Serial.println ("GO DOWN  Function ");
   digitalWrite (dir, LOW);

  while (digitalRead(downbtn) == 0 && digitalRead(bottomlimit) == 1) { 
      //move stepper motoro down x steps 
      Serial.print ("moving motor down  ");
      Serial.println (downcount);
         digitalWrite(step, HIGH);
      digitalWrite (step, LOW);
     downcount++;
    }
      Serial.println  ("Motor Stopped in GO DOWN function"); 
  }


void encoderrotate() {
  // ROTARY DIRECT AND COUNT

  // Read the current state of dtpinA

  currentstatedtpinA = digitalRead(dtpinA);
 
  // If last and current state of outputA are different, then pulse occurred
  // React to only 1 state change to avoid double count
  if ((currentstatedtpinA==LOW) && (laststatedtpinA==HIGH)){

    if (digitalRead(clkpinB)==HIGH) {
      Serial.println ("Rotate left = ACW  ");
      encodercount--;
      Serial.print (encodercount);
    } 
    else {
      Serial.println ("Rotate right -CW  ");
      encodercount++;
      Serial.print (encodercount);
    }
  }
  laststatedtpinA = currentstatedtpinA;
}


void contactmade() {
  Serial.println ("Contact Made  Function zzzzzzzzzzzzzzzzzzzzzzzzzzzzz ");
  //stop stepper motor
  gototopinitiated=1;
  Serial.print ("LED PIN IS  ");
  Serial.print (ledstate);
  if (ledstate == "OFF") {
    digitalWrite(contactled, HIGH);
    ledstate = "ON";
    delay (300);
  } 
  else {
    Serial.print ("in Else ");
    digitalWrite(contactled, LOW);
    ledstate = "OFF";
  }
} 

void checkinterrupt() {

  //Serial.println ("Check Interrupt  Function ");
 // Serial.print ("!!!!!!!!!!!!!!!!!!!   ");
 // Serial.println(digitalRead(upbtn));
 
  if (digitalRead(upbtn) == 0 || digitalRead(downbtn) == 0 || digitalRead(safetybtn) == 0) { // || digitalRead(encodersw) == 1) {
        gototopinitiated = 1;
        gotobottominitiated = 1;
      }   
}

void displayheight() {

  lcd.setCursor(11,0);
  lcd.print (distance);
  lcd.setCursor(14,0);
  lcd.print ("mm");

}