#define btnPin1  2 //button 1 is connected to pin D2
#define btnPin2  3 //button 2 is connected to pin D3
#define btnPin3  4 //button 3 is connected to pin D4
#define potIn    A0  //Potentiometer is connected to pin A0
#define ledRed   5 //led 1 is connected to pin D2
#define ledGreen 6 //led 2 is connected to pin D2
#define ADDRESS  0x3C //I2C addrress for the OLED display


//MAXIMUM SPEED SET HERE
#define maximumSpeed  100 //set your max speed
#define angleVal  819   //angle value for mode 3


//All the libraries are included here
#include "AccelStepper.h"
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>



const unsigned char handImg [] PROGMEM = {
  // 'hand-logo-icon-png-svg, 60x64px
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x78, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfc, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x01, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xfc, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x01, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xe1, 0xfc, 0x1e, 0x00, 0x00, 0x00, 
  0x00, 0x03, 0xf1, 0xfc, 0x3f, 0x00, 0x00, 0x00, 0x00, 0x03, 0xf1, 0xfc, 0x3f, 0x80, 0x00, 0x00, 
  0x00, 0x03, 0xf1, 0xfc, 0x3f, 0x80, 0x00, 0x00, 0x00, 0x03, 0xf9, 0xfc, 0x3f, 0x00, 0x00, 0x00, 
  0x00, 0x03, 0xf9, 0xfc, 0x3f, 0x00, 0x00, 0x00, 0x00, 0x03, 0xf9, 0xfc, 0x7f, 0x00, 0x00, 0x00, 
  0x00, 0x03, 0xf9, 0xfc, 0x7f, 0x00, 0x00, 0x00, 0x00, 0x03, 0xf9, 0xfc, 0x7f, 0x00, 0x00, 0x00, 
  0x00, 0x03, 0xf9, 0xfc, 0x7f, 0x00, 0x00, 0x00, 0x00, 0xc1, 0xfd, 0xfc, 0x7f, 0x00, 0x00, 0x00, 
  0x03, 0xf1, 0xfd, 0xfe, 0x7f, 0x00, 0x00, 0x00, 0x03, 0xf1, 0xfd, 0xfe, 0xfe, 0x00, 0x00, 0x00, 
  0x03, 0xf9, 0xfd, 0xfe, 0xfe, 0x00, 0x00, 0x00, 0x03, 0xf9, 0xfd, 0xfe, 0xfe, 0x00, 0x00, 0x00, 
  0x03, 0xf9, 0xfd, 0xfe, 0xfe, 0x00, 0x00, 0x00, 0x01, 0xfd, 0xff, 0xfe, 0xfe, 0x00, 0x00, 0x00, 
  0x01, 0xfc, 0xff, 0xfe, 0xfe, 0x00, 0x00, 0x00, 0x01, 0xfc, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00, 
  0x01, 0xfe, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0xfe, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00, 
  0x00, 0xff, 0xff, 0xff, 0xfc, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xfc, 0x01, 0xf0, 0x00, 
  0x00, 0x7f, 0xff, 0xff, 0xfc, 0x03, 0xf8, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xfc, 0x07, 0xfc, 0x00, 
  0x00, 0x7f, 0xff, 0xff, 0xfc, 0x0f, 0xfc, 0x00, 0x00, 0x3f, 0xff, 0xff, 0xfc, 0x1f, 0xfc, 0x00, 
  0x00, 0x3f, 0xff, 0xff, 0xfc, 0x3f, 0xfc, 0x00, 0x00, 0x3f, 0xff, 0xff, 0xfe, 0x7f, 0xfc, 0x00, 
  0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xf8, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x00, 
  0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xc0, 0x00, 
  0x00, 0x0f, 0xff, 0xff, 0xff, 0xff, 0x80, 0x00, 0x00, 0x0f, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 
  0x00, 0x0f, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x07, 0xff, 0xff, 0xff, 0xfc, 0x00, 0x00, 
  0x00, 0x07, 0xff, 0xff, 0xff, 0xf8, 0x00, 0x00, 0x00, 0x07, 0xff, 0xff, 0xff, 0xf0, 0x00, 0x00, 
  0x00, 0x03, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x03, 0xff, 0xff, 0xff, 0xc0, 0x00, 0x00, 
  0x00, 0x01, 0xff, 0xff, 0xff, 0x80, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x7f, 0xff, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xff, 0xf8, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x0f, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xff, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};

//Setup BNO055 sensorr
Adafruit_BNO055 bno = Adafruit_BNO055(1,0x29);




#define SCREEN_WIDTH 128 // OLED display width,  in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// declare an SSD1306 display object connected to I2C
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);






#define dirPin 12 //Dir pin of the motor driver is connected 
#define stepPin 11 //Dstepir pin of the motor driver is connected 


#define motorInterfaceType 1
AccelStepper stepper(AccelStepper::DRIVER, stepPin, dirPin); //Declare an accelstepper object connected to driver


//declared essencial verriables
int cnt1 = 1;
int val;
int lastVal = 0;

boolean is180 = false;
boolean is90 = false;
boolean is20 = false;

boolean isCal = false;
boolean isCal1 = false;

boolean isBreak = false;

int dir = 0;
int lastSpeed = 0;

int curPos = 0;
int Speed = 0;
int speedGraph = 0;

//calculating potentiometer value
int potVal(){
   return analogRead(potIn);
}



//getting button input
boolean isBtn3(){
   return !digitalRead(btnPin3);
}


//declaring essencial verriables
unsigned static long LastTimeConditionWasFalse = 0;
unsigned static long LastTimeConditionWasFalse1 = 0;
unsigned static long LastTimeConditionWasFalse2 = 0;
unsigned static long LastTimeConditionWasFalse3 = 0;
unsigned static long LastTimeConditionWasFalse4 = 0;




//getting all the button inputs from all three buttons
boolean isBtn1(){
    int a = digitalRead(btnPin1);
      if(a){
          LastTimeConditionWasFalse2 = millis();
          isCal = false;
      }
      else{
         isCal = true;
      }

       if (millis() - LastTimeConditionWasFalse2 >= 60){
          return true;
       }
       else{
         return false;
       }

}

boolean isBtn2(){
   int a = digitalRead(btnPin2);
      if(a){
          LastTimeConditionWasFalse3 = millis();
          isCal1 = false;
      }
      else{ 
         isCal1 = true;
      }

       if (millis() - LastTimeConditionWasFalse3 >= 60){
          return true;
       }
       else{
         return false;
       }

}




boolean isBtnHold1(){
     int a = digitalRead(btnPin1);
      if(a){
          LastTimeConditionWasFalse = millis();
      }

       if (millis() - LastTimeConditionWasFalse >= .5*1000){
          return true;
       }
       else{
         
         return false;
       }

      
  }

   boolean isBtnHold2(){
     int a = digitalRead(btnPin2);
      if(a){
          LastTimeConditionWasFalse1 = millis();
      }

       if (millis() - LastTimeConditionWasFalse1 >= .5*1000){
          return true;
       }
       else{
         return false;
       }

      
  }

    boolean isDualBtn(){
     int a = digitalRead(btnPin1);
      int b = digitalRead(btnPin2);
      if(a || b){
          LastTimeConditionWasFalse4 = millis();
      }

       if (millis() - LastTimeConditionWasFalse4 >= 30){
          return true;
       }
       else{
         return false;
       }

      
  }



//essencial boolean variables
  boolean isMode1 = true;
   boolean isMode2 = false;
    boolean isMode3 = false;
     boolean isMode4 = false;


void setup() {

  
  Serial.begin(112500); //Startig serial monitor
   if (!display.begin(SSD1306_SWITCHCAPVCC, ADDRESS)) { //initializing OLED display
    Serial.println(F("SSD1306 allocation failed"));
    while (true);
  }
  display.display();
  
  //setting all the button pins as INPUT
  pinMode(btnPin1, INPUT_PULLUP);
  pinMode(btnPin2, INPUT_PULLUP);
  pinMode(btnPin3, INPUT_PULLUP);
  

  //setting stepper motor's max speed and acceleration 
  stepper.setMaxSpeed(maximumSpeed);
   stepper.setAcceleration(100);
  


   if(!bno.begin()) //initializing Compus sensor
  {
    /* There was a problem detecting the BNO055 ... check your connections */
    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
    
  }
  

  bno.setExtCrystalUse(true);
  display.clearDisplay();




}

void loop() {
  // put your main code here, to run repeatedly:

mode1(); //mode one will run instantly

 

  if(isDualBtn()){ //if button 1 and 2 is pressed
     curPos = stepper.currentPosition();
     
       stepper.moveTo(curPos);
       
        while(true){
            if (stepper.distanceToGo() == 0){
              break;
          }
          stepper.run();
        }
     lastSpeed = map(potVal(), 0, 1024, 0, maximumSpeed);
      delay(400);
      display.clearDisplay();
       mode2(); //start mode 2

     
  } else if(isBtn2() || isBtn1()){ //if either of button 1 and 2 is pressed
       delay(400);
       display.clearDisplay();
       mode4(); //start mode 4
     
  }

  if(isBtn3()){ //if button 3 is prerssed, 
    delay(400);
          display.clearDisplay();
          display.setTextColor(SSD1306_WHITE);
          display.setCursor(57,15);
          display.setTextSize(3);
          
          display.cp437(true);   // Use correct CP437 character codes
          display.write(0x18);   // Print the o-with-umlauts
          display.drawCircle(display.width()/2, 50, 12, SSD1306_WHITE);
          display.display();
       isBreak = true;
       mode3(); //start mode 3
       
     
  }
  

}


void mode1(){ //mode 1 function 

    display.clearDisplay();
   Serial.println("Mode1");

    digitalWrite(ledGreen, HIGH);
    digitalWrite(ledRed, LOW);

    //writing on the display
    Speed = map(potVal(), 0, 1024, 0, maximumSpeed);
    speedGraph = map(potVal(), 0, 1024, 60, 3);
    stepper.setSpeed(Speed);
    display.drawRect(5, 3, 15, 60, 1);
    display.fillRect(5, speedGraph, 15, 60, 1);
    display.drawBitmap(40, 0, handImg, 60, 64, WHITE);

    display.display();


}

boolean isApplied = false;

void mode2(){ //mode 2 function
   while(true){
    
   digitalWrite(ledGreen, LOW);
    digitalWrite(ledRed, HIGH);
  


  
  
 



    
    if(isBtn3()){
      
       stepper.moveTo(curPos);
       stepper.setAcceleration(30);
       while(true){
          if (stepper.distanceToGo() == 0){
            break;
         }
         stepper.runSpeedToPosition();
       }
       cnt1++;
       delay(200);
       isApplied = false;
    }

    if(cnt1 == 1){
      
        stepper.setSpeed(Speed);
        

       if(!isApplied){
        
            display.clearDisplay();
            
               if(cnt1 == 1){
                  display.setTextSize(1); // Draw 2X-scale text
                        display.setTextColor(SSD1306_WHITE);
                         display.setCursor(56,27);
                        display.print("180");
              }
              else if(cnt1 == 2){
                  display.setTextSize(1); // Draw 2X-scale text
                        display.setTextColor(SSD1306_WHITE);
                         display.setCursor(56,27);
                        display.print("90");

              } else if(cnt1 == 3){
                
                  display.setTextSize(1); // Draw 2X-scale text
                        display.setTextColor(SSD1306_WHITE);
                         display.setCursor(56,27);
                        display.print("20");
              }
           
            display.drawRect(5, 3, 15, 60, 1);
            display.fillRect(5, speedGraph, 15, 60, 1);
            drawAngle();
            display.display();
             

         int steps = 50;
         stepper.moveTo(curPos+steps);
         isApplied = true;
       }
   

       
       if (stepper.distanceToGo() == 0){
            stepper.moveTo(-stepper.currentPosition());
       }
         
        stepper.runSpeedToPosition();

      
  
    }

    if(cnt1 == 2){
    
    stepper.setSpeed(Speed);
       if(!isApplied){
            display.clearDisplay();
                      if(cnt1 == 1){
              display.setTextSize(1); // Draw 2X-scale text
                    display.setTextColor(SSD1306_WHITE);
                    display.print("180");
          }
          else if(cnt1 == 2){
              display.setTextSize(1); // Draw 2X-scale text
                    display.setTextColor(SSD1306_WHITE);
                    display.print("90");

          } else if(cnt1 == 3){
              display.setTextSize(1); // Draw 2X-scale text
                    display.setTextColor(SSD1306_WHITE);
                    display.print("20");
          }
            display.drawRect(5, 3, 15, 60, 1);
            display.fillRect(5, speedGraph, 15, 60, 1);
            drawAngle();
            display.display();           
            

         int steps = 25;
         stepper.moveTo(curPos+steps);
         isApplied = true;
       }
       if (stepper.distanceToGo() == 0){
            stepper.moveTo(-stepper.currentPosition());
          }  
             stepper.runSpeedToPosition();
    }
    

    if(cnt1 == 3){
            
         
            stepper.setSpeed(Speed);
        int steps = 12;

       if(!isApplied){
            display.clearDisplay();

          if(cnt1 == 1){
              display.setTextSize(1); // Draw 2X-scale text
                    display.setTextColor(SSD1306_WHITE);
                    display.print("180");
          }
          else if(cnt1 == 2){
              display.setTextSize(1); // Draw 2X-scale text
                    display.setTextColor(SSD1306_WHITE);
                    display.print("90");

          } else if(cnt1 == 3){
              display.setTextSize(1); // Draw 2X-scale text
                    display.setTextColor(SSD1306_WHITE);
                    display.print("20");
          }
            display.drawRect(5, 3, 15, 60, 1);
            display.fillRect(5, speedGraph, 15, 60, 1);
            drawAngle();
            display.display();
        
         stepper.moveTo(curPos+steps);
         isApplied = true;
       }
      
       if (stepper.distanceToGo() == 0){
            stepper.moveTo(-stepper.currentPosition());

         }  
        stepper.runSpeedToPosition();
      
    }

    if(cnt1 == 4){
       cnt1 = 1;
    }

    if(isBtn1() && isBtn2()){
       cnt1 = 1;
       isApplied = false;
          stepper.moveTo(curPos);
          stepper.setAcceleration(30);
        while(true){
            if (stepper.distanceToGo() == 0){
              break;
          }
          stepper.runSpeedToPosition();
        }
        delay(400);
       break;
     
    }

  }
    
}

void drawAngle(){
    display.drawLine(100, 0, 64, display.height()-1, SSD1306_WHITE);
    display.drawLine(28, 0, 64, display.height()-1, SSD1306_WHITE);
    display.setCursor(56,27);
    display.display();
}



 int curPos1 = 0;


void mode3(){ //mode 3 function
    if(isBreak){

              val = 0;
             
              curPos1 = stepper.currentPosition();
              digitalWrite(ledGreen, LOW);
              digitalWrite(ledRed, HIGH);
             
              isBreak = false;
    }

  while(true){
   
     int stepPos;
      
           calVal();
           if(val > 0 && val < 176){
              stepPos = map(val, 0, 176, 0, angleVal);
           }
           else if(val < 0 && val > -176){
             stepPos = map(val, 0, -176, 0, -angleVal);
           }
           
 
      
           stepper.moveTo(curPos1+stepPos);
           stepper.runSpeedToPosition();
           
       



       if(isBtn3()){
        delay(400);
        isBreak = true;
         break;
       
       }
   
  

         
       
  
}
}

 long long x1 = 0;
 int cnt2 = 0;
 
void mode4(){ //mode 4 function
  while(true){
    Serial.println("Mode4");
  digitalWrite(ledGreen, LOW);
    digitalWrite(ledRed, HIGH);
    if(!cnt2){
       x1 = stepper.currentPosition();
       cnt2++;
    }
 
  
   
  
  
 
   if(isBtnHold1()){
          display.clearDisplay();
          display.setTextColor(SSD1306_WHITE);
          display.setCursor(57,27);
          display.setTextSize(3);
          display.clearDisplay();
          display.drawRect(5, 3, 15, 60, 1);
          display.fillRect(5, speedGraph, 15, 60, 1);     
          display.cp437(true);   // Use correct CP437 character codes
          display.write(0x1A);   // Print the o-with-umlauts
          display.display();
       while(isBtnHold1()){
           stepper.setSpeed(Speed);
           stepper.runSpeed();
       }
         
   }
   
 
   if(isBtnHold2()){
          display.clearDisplay();
          display.setTextColor(SSD1306_WHITE);
          display.setCursor(57,27);
          display.setTextSize(3);
          display.clearDisplay();
          display.drawRect(5, 3, 15, 60, 1);
          display.fillRect(5, speedGraph, 15, 60, 1);
          display.cp437(true);   // Use correct CP437 character codes
          display.write(0x1B);   // Print the o-with-umlauts
          display.display();
       while(isBtnHold2()){
            stepper.setSpeed(-Speed);
            stepper.runSpeed();
           
    }
   }
   


if(isBtn3()){
  
   cnt2 = 0;
   delay(400);
   break;
}
        
        Speed = map(potVal(), 0, 1024, 0, maximumSpeed);
        speedGraph = map(potVal(), 0, 1024, 60, 3);
        stepper.setSpeed(Speed);
  
 
  display.clearDisplay();
  display.drawRect(5, 3, 15, 60, 1);
  display.fillRect(5, speedGraph, 15, 60, 1);

  display.display();

  }
}



void getAxis(){
    imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
    dir = euler.x();
  
    
}

void calVal(){
    getAxis();
    if(dir > lastVal){
      val++;
      lastVal = dir;
    }

    else if(dir < lastVal){
       val--;
       lastVal = dir;
    }
}