#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Keypad.h>
#include <AccelStepper.h>


//Przerwanie 
#define interruptPin 2

#define czujnikPin A3
#define kalibracjaBtn A1


//EKRAN
LiquidCrystal_I2C lcd(0x27,16,2);  // Setting the chip address to 0x27

//KLAWIATURA                                                      
const byte ROWS = 4; // ile wierszy                                       
const byte COLS = 4; //ile kolumn
uint8_t rowPins[ROWS] = { 10, 9, 8, 7 }; // Pins connected to R1, R2, R3, R4
uint8_t colPins[COLS] = { 6, 5, 4, 3 }; // Pins connected to C1, C2, C3, C4
char keys[ROWS][COLS] = { //mapowanie klawiatury
  {'1','2','3','A'},
  {'4','5','6','B'},
  {'7','8','9','C'},
  {'*','0','#','D'}
};

Keypad klawiatura = Keypad( makeKeymap(keys), rowPins, colPins, ROWS, COLS ); //inicjalizacja klawiatury

long pozZad=0;
float pozZadMm=0;
float pozAktMm=0;
long pozAkt=0;

int set=0;
int buf=0;
int dec=0;
bool kalibracja=0;
bool prevRunning=false;
bool running=false;


//STEPER
#define enablePin 13
#define dirPin 12
#define stepPin 11
AccelStepper stepper(1, stepPin, dirPin);

bool motorStop=true;


//*******************************KALIBRACJA**************//
void kalibruj() {
//***PRĘDKOŚCI DO KALIBRACJI***//
  stepper.setMaxSpeed(2500);
  stepper.setSpeed(2000);
  stepper.setAcceleration(400);

  lcd.setCursor(4,0); 
  lcd.print("KALIBRUJ");
  lcd.setCursor(3,1);
  lcd.print("URZADZENIE");
  
int menu=0;
while(!kalibracja){
  switch(menu){
    case 0:


    if(digitalRead(kalibracjaBtn)==0){
      lcd.clear();
      lcd.setCursor(3,0);
      lcd.print("KALIBRACJA");
      stepper.moveTo(-200000);
      menu=1;
    }
    break;

    case 1:
    if(digitalRead(czujnikPin)==0){
      stepper.run();
    //stepper.setSpeed(-1200);
    //stepper.runSpeed();
    
    } else {
      lcd.setCursor(0,1);
      lcd.print("Czujnik=TRUE ");
      stepper.setCurrentPosition(0);
      stepper.runToNewPosition(-400);
      stepper.setSpeed(100);
      menu=2;
    }
    break;

    case 2:
    if(digitalRead(czujnikPin)==1){
    stepper.runSpeed();
    } else {
      //PARAMETRY PRACY SILNIKA//
      stepper.setMaxSpeed(2000);
      stepper.setAcceleration(400);
      stepper.setSpeed(200);
      stepper.setCurrentPosition(0);
      lcd.clear();
      int i=0;
      while (i<2) {
        lcd.setCursor(0,3);
        lcd.print("KALIBRACJA UDANA");
        delay(1000);
        lcd.setCursor(0,3);
        lcd.print("                ");
        delay(1000);
        i++;
      }
      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print("Zad:0");
      lcd.setCursor(0,1);
      lcd.print("Akt:0");
      kalibracja=1;
    }
    break;

  }
}

    return 0;
 }  


void setup() {
  Serial.begin(9600);
  stepper.setEnablePin(enablePin);
  stepper.setPinsInverted(false,false,true); // piny silnika
  stepper.enableOutputs();

  attachInterrupt(digitalPinToInterrupt(interruptPin), stop, FALLING);
  
  lcd.init();
  lcd.clear();         
  lcd.backlight();
  pinMode(enablePin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(stepPin, OUTPUT);
  pinMode(interruptPin, INPUT_PULLUP);
  pinMode(kalibracjaBtn, INPUT_PULLUP);
  pinMode(czujnikPin, INPUT);

  delay(10);

  kalibruj();

}

void loop() {
char klawisz = klawiatura.getKey();
  
  if (klawisz){
    buf= klawisz - '0';
    if (buf<10 && buf >=0 ) { //wcisnieto cyfre
      if (pozZadMm <2000 && dec <2 ){
        if(dec==0){
          klawisz = klawisz -'0';
      	  pozZadMm=pozZadMm * 10 + klawisz;
          lcd.setCursor(4,0);
          lcd.print("            ");
          lcd.setCursor(4,0);
          lcd.print(pozZadMm);
        } else {
          klawisz = klawisz -'0';
          pozZadMm=pozZadMm * 10 + klawisz;
          pozZadMm=pozZadMm/10;
          lcd.setCursor(4,0);
          lcd.print("            ");
          lcd.setCursor(4,0);
          lcd.print(pozZadMm);
          dec++;
        }
      	
      }
    }

    else {
      switch (klawisz){
        case 'A':
        stepper.stop();
        break;
        case 'C':
        	pozZadMm=0;
          dec=0;
          lcd.setCursor(4,0);
          lcd.print("            ");
          lcd.setCursor(4,0);
          lcd.print(pozZadMm);
        break;
        case 'B':
          pozZad=pozZadMm*100;
          stepper.stop();
          while(stepper.isRunning()){
            stepper.run();
          }
          stepper.moveTo(pozZad);
          lcd.setCursor(4,1);
          lcd.print("            ");
          lcd.setCursor(4,1);
          lcd.print("------------");
          motorStop=false;
        break;
        case '*':
          dec++;
        break;
      }
    }

  }
  
  if(!motorStop) {
    stepper.run();
  }

  
  running=stepper.isRunning();
  if( running==false && prevRunning==true ){
    pozAkt=stepper.currentPosition();
    pozAktMm=pozAkt;
    pozAktMm=pozAktMm/100;
    lcd.setCursor(4,1);
    lcd.print("            ");
    lcd.setCursor(4,1);
    lcd.print(pozAktMm);
  }
  prevRunning=running;
}

void stop(){
  motorStop=true;
  stepper.moveTo(stepper.currentPosition());
  //stepper.computeNewSpeed();
}