#include <LiquidCrystal_I2C.h>     // if you don´t have I2C version of the display, use LiquidCrystal.h library instead

LiquidCrystal_I2C lcd(0x27,16,2);  // set the LCD address to 0x27 for a 16 chars and 2 line display
//pins
int encPhaseA_1 = 2;       // pin for encoder 1 phase A
int encPhaseB_1 = 3;       // pin for encoder 1 phase B
int LEDmark = 12 ;          //pin for LED
int blackButtonPin = 4;    // pin for black button
int switchPin = 0;   // pin for green button


//variables
unsigned long timePassed = 0;           // Time in msec since buttom press
unsigned long timeStarted;              // Time is msec when button pressed.
bool callibrate = 0;                     // the callibration state 
float angle = 0;
 static bool prevPhaseA;
 static bool prevPhaseB;
 bool currentPhaseA;
 bool currentPhaseB;
volatile int encCount_1 = 0;            // Counts made for encoder 1
int A = 1;            // for switch between right and left
int B = 90;
void setup() {
  // put your setup code here, to run once:
 // Setup Encoder 1
  pinMode(encPhaseA_1, INPUT);         // Initiates Encoder 1 Phase A
  pinMode(encPhaseB_1, INPUT);         // Initiates Encoder 1 Phase B
  pinMode(LEDmark, OUTPUT);         // Initiates Encoder 1 Phase B
  // Setup Button box
  pinMode(blackButtonPin, INPUT);      // Initiates Black Button
  pinMode(switchPin, INPUT);      // Initiates Green Button  
  attachInterrupt(digitalPinToInterrupt(encPhaseA_1),encInc_1,CHANGE); // Create interrupt for change in encoder 1 phase A
  attachInterrupt(digitalPinToInterrupt(encPhaseB_1),encInc_1,CHANGE); // Create interrupt for change in encoder 1 phase B
Serial.begin(9600);
 lcd.init();                       // initialize the 16x2 lcd module
Serial.println("Begin");
}

void loop() {
  // put your main code here, to run repeatedly:
Serial.println("LED turn on");
digitalWrite(LEDmark,HIGH); 

while (callibrate == 0){
    lcd.setCursor(0, 0);
  lcd.print("Welcome!!!!!!!");
  if (digitalRead(switchPin) == HIGH){
     lcd.setCursor(0, 1);
  lcd.print("Right");
}
else{
  lcd.setCursor(0, 1);
  lcd.print("Left ");
}
  if(digitalRead(blackButtonPin) == HIGH) {      //Check for the black button to be pressed.
      timeStarted = millis();
      do{timePassed = millis() - timeStarted;
      lcd.setCursor(0, 0);
      lcd.print("Callibrating!");
      lcd.print(100*timePassed/3000);}
       while (digitalRead(blackButtonPin) == HIGH && timePassed<3000);
        if (timePassed >=3000){
          encCount_1 = 0;
          callibrate = 1;
          lcd.setCursor(0, 0);
          lcd.print("Callibrated!");
          Serial.print("Callibrated");
          delay(1000);}
          lcd.clear();
          }               
    else {                                     //Else no button pressed.
      callibrate = 0;                       //Set button pressed to 0
    }
 if (callibrate == 1){
              break;
             }             
}
 lcd.setCursor(0, 0);
 lcd.println("Angle:00");
while (callibrate == 1){
//convert encoder count to angles
if (digitalRead(switchPin) == HIGH){
  A = -1;
   lcd.setCursor(0, 1);
  lcd.print("Right");
}
else{
  A = 1;
   lcd.setCursor(0, 1);
  lcd.print("Left ");
}
 angle = 90 - (B + A*encCount_1*0.7);
 lcd.setCursor(0, 0);
 lcd.print("Angle: ");
 lcd.println(angle);
 Serial.print(encCount_1);
 Serial.print(",");
 Serial.print(A);
 Serial.print(",");
 Serial.println(angle);
   if(digitalRead(blackButtonPin) == HIGH) {      //Check for the black button to be pressed.
    angle = 90 - (B + A*encCount_1*0.7);
    lcd.setCursor(0, 0);
    lcd.print("Angle: ");
    lcd.println(angle);
    Serial.print(encCount_1);
    Serial.print(",");
    Serial.print(A);
    Serial.print(",");
    Serial.println(angle);
    timeStarted = millis();
      do{timePassed = millis() - timeStarted;}
       while (digitalRead(blackButtonPin) == HIGH && timePassed<3000);
        if (timePassed >=3000){
          callibrate =0;
          lcd.setCursor(0, 0);
          lcd.print("TESTOVER!!!!!!!!");
          Serial.println("Test over");
          delay(1000);
          lcd.clear();
          }
        else {                                     //Else no button pressed.
      callibrate = 1;                       //Set button pressed to 0
    }
        }
}}

// ISR for counting the encoder 1 value
void encInc_1() {
     
   currentPhaseA = digitalRead(encPhaseA_1);                                       // Get current value of phase A and phase B
   currentPhaseB = digitalRead(encPhaseB_1); 
  
   if (currentPhaseA != prevPhaseA) {                                              // Check for a change in Phase A then apply count logic depending on state of Phase A and Phase B
   if (digitalRead(encPhaseA_1) == HIGH && digitalRead(encPhaseB_1) == HIGH) {
    encCount_1 ++;
   }
   else if (digitalRead(encPhaseA_1) == HIGH && digitalRead(encPhaseB_1) == LOW) {
    encCount_1 --;
   }
   else if (digitalRead(encPhaseA_1) == LOW && digitalRead(encPhaseB_1) == HIGH) {
    encCount_1 --;
   }
   else {
    encCount_1 ++;
   }
   }
   else if (currentPhaseB != prevPhaseB) {                                         // Check for a change in Phase B then apply count logic depending on state of Phase A and Phase B
    if (digitalRead(encPhaseA_1) == HIGH && digitalRead(encPhaseB_1) == HIGH) {
    encCount_1 --;
   }
   else if (digitalRead(encPhaseA_1) == HIGH && digitalRead(encPhaseB_1) == LOW) {
    encCount_1 ++;
   }
   else if (digitalRead(encPhaseA_1) == LOW && digitalRead(encPhaseB_1) == HIGH) {
    encCount_1 ++;
   }
   else {
    encCount_1 --;
   }
   }
   prevPhaseA = currentPhaseA; 
   prevPhaseB = currentPhaseB;  
}