/* 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=A3;

int upbtn = 9;
int safetybtn = 8;
int downbtn = 7;

int zerobtn = 6;

int bottomlimit = 4;
int toplimit = 5;

// int switchpin = 5;
int dtpinA = 2;
int clkpinB = 13;

int contactinterrupt = 3;

const int step = 12;
const int dir = 11;
//const int MS1 = 33;
//const int MS2 = 35;
//const int MS3 = 37;



// 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);

int upcount=0;
int downcount=0;

//tet variables to be deleted 


int encodercount=1;

int slowspeedsteps = 25;   // 1 step = .04mm (fullstep) 25 steps =0.5mm in halfs steps 
int fastspeedsteps = 50;
float distance = 0;
int steps = 25; // number of steps moved between checking for interrup on gototop / bottom
int encodersteps = slowspeedsteps;
int incrementsteps = 25; // number of steps in each increment for goup go down functiosn (in steps for 25 for .5mm stages)
char str[6];



// 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_PULLUP);
pinMode (safetybtn, INPUT_PULLUP);
pinMode (downbtn, INPUT_PULLUP);
pinMode (zerobtn, INPUT_PULLUP);
pinMode (bottomlimit, INPUT_PULLUP);
pinMode (toplimit,INPUT_PULLUP);
//pinMode (switchpin, INPUT_PULLUP);
pinMode (dtpinA,INPUT_PULLUP);
pinMode (clkpinB, INPUT_PULLUP);


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.print (distance);
lcd.setCursor(14,0);
lcd.print("mm");
lcd.setCursor (1,1);
lcd.print ("Encoder: ");
lcd.setCursor(10,1);
lcd.print (encoderspeed);

}


// VOID LOOP

void loop() {
/*
int x = 0;

while (x < 500) {
  digitalWrite(dir,HIGH);
 digitalWrite(step,HIGH);
 Serial.println (x);
 Serial.println ("HIGH");
 delay(100);
 digitalWrite(step,LOW);
  Serial.println ("LOW");
 delay(100);
 x++;
}
*/


//  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 DOwN 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");
  delay(250);
  //Takes router to the bottom when limit switch stops it or contact switch stops it//
  gotobottom();
  }

//  If UP button an Safety ButtonIS NOT pressed - initiate the Go UP  process

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//
  goup();
  }

  //  If DOWN button an Safety Button IS NOT pressed - initiate theGO DOWN process

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


// Test for encoder button press
  
/*
if (digitalRead(dtpinA)==LOW) {  
// Serial.print("Button pressed ");
  delay (300);
  if (encoderspeed == "SLOW") {
    encoderspeed = "FAST";
    encodersteps = fastspeedsteps;
  } else {
    encoderspeed = "SLOW";
    encodersteps = slowspeedsteps;
  }
  lcd.setCursor (10,1);
  lcd.print (encoderspeed);
//  Serial.println (encodersteps);

}  */


//  TEST FOR THE ZERO BUTTON
//Serial.print ("Zero Button Reading ....");
//Serial.println (digitalRead(zerobtn));
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);
  delay (500);
  while (digitalRead(toplimit) == 1 && gototopinitiated == 0) {
    delay(1000);
    Serial.println(steps);
   for (int x=steps; x>0; x--){
      digitalWrite(step, HIGH);
      digitalWrite (step, LOW);
      Serial.print(x);
      Serial.print (" ");
  
    }
    Serial.println(" ");
    Serial.println(distance);
  distance = distance+(steps/25*.5);
  Serial.println(distance);
  displayheight();
  checkinterrupt();
  }
}

  void gotobottom() {

    gotobottominitiated = 0;
    digitalWrite (dir, LOW);  
    delay(500);
    while (digitalRead(bottomlimit) == 1 && gotobottominitiated == 0) {
      for (int x=steps; x>0; x--){
      digitalWrite(step, HIGH);
      digitalWrite (step, LOW);
            Serial.print(x);
      Serial.print (" ");
      }
        Serial.println(" ");
    Serial.println(distance);
    distance=distance-(steps/25*.5); 
    Serial.println(distance);
    displayheight();
    checkinterrupt();
     
  }
}


void goup() {
//  Serial.println("In go up funtion");
  digitalWrite (dir, HIGH);
  while (digitalRead(upbtn) == 0 && digitalRead(toplimit) == 1) { 
    upcount = 1;
    while (upcount <= incrementsteps){
      digitalWrite(step, HIGH);
      digitalWrite (step, LOW);
      delay (100);
      upcount++;
    }
    distance=distance + 0.5;
    displayheight();
  }    
}     

void godown() {

  digitalWrite (dir, LOW);
  while (digitalRead(downbtn) == 0 && digitalRead(bottomlimit) == 1) { 
    downcount = 1;
    while (downcount <= incrementsteps){
      digitalWrite(step, HIGH);
      digitalWrite (step, LOW);
      delay (100);
      downcount++;
    }
    distance=distance - 0.5;
    displayheight();
  }    
}     


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) {
       digitalWrite (dir, LOW);
      for (int x=encodersteps; x>0; x--){
        digitalWrite(step, HIGH);
        digitalWrite (step, LOW);
      }
      distance=distance-(encodersteps/25*0.5);
      displayheight();
    } 
    else {
      digitalWrite (dir, HIGH);
      for (int x=encodersteps; x>0; x--){
          digitalWrite(step, HIGH);
          digitalWrite (step, LOW);
      }
      distance=distance+(encodersteps/25*0.5);
      displayheight();
    }
  }
  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() {

  if (digitalRead(upbtn) == 0 || digitalRead(downbtn) == 0 || digitalRead(safetybtn) == 0) { // || digitalRead(encodersw) == 1) {
        gototopinitiated = 1;
        gotobottominitiated = 1;
      }   
}

void displayheight() {

  dtostrf (distance, 6, 1, str);
  delay (100);
  lcd.setCursor(8,0);
  lcd.print (str);
  lcd.setCursor(14,0);
  lcd.print ("mm");
}