#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Stepper.h>
#include <Servo.h>

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

Adafruit_MPU6050 mpu;

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

Servo drill;

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

int selectbtn=2;
int backbtn=3;

int menuindex = 0;
volatile bool selectpressed = false;
volatile bool backpressed = false;

bool menuchanged = true;

String direction;

int depth=0;
int menusize=3;

int step=5;
int dir=4;

char* menuitems[] = 
{
  "1. Control Drill",
  "2. View Readings",
  "3. Option 3"
};

int buzz= 12;
int ledr= 9;
int ledb= 11;
int ledg= 10;

bool jammed = false;

void selectISR() 
{
  selectpressed = true;
}

void backISR() 
{
  backpressed = true;
}

void showmenu()
{
    display.clearDisplay();
    display.setCursor(0,0);

    for(int i=0; i<menusize; i++)
    {
        display.setCursor(0, i*16);

        if(i == menuindex)
            display.print(">");
        else
            display.print(" ");

        display.setCursor(10, i*16);
        display.print(menuitems[i]);
    }

    display.display();
}

void setup() 
{
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);  
  display.clearDisplay();

  display.setTextSize(1);
  display.setTextColor(WHITE);

display.display();

pinMode(selectbtn, INPUT_PULLUP);
pinMode(backbtn, INPUT_PULLUP);

attachInterrupt(digitalPinToInterrupt(selectbtn), selectISR, FALLING);
attachInterrupt(digitalPinToInterrupt(backbtn), backISR, FALLING);

pinMode(step, OUTPUT);
pinMode(dir, OUTPUT);

Serial.begin(9600);

drill.attach(8); 

pinMode(buzz, OUTPUT);
pinMode(ledg, OUTPUT);
pinMode(ledb, OUTPUT);
pinMode(ledr, OUTPUT); 

if (!mpu.begin()) {
  while (1);
}

}

void moveStepper(int direc)
{
  digitalWrite(dir, direc);

  digitalWrite(step, HIGH);
  digitalWrite(step, LOW);
}

void controldrillmode()
{
    backpressed = false;

    while(!backpressed)
    {
      

        int y = analogRead(A0);
        int x = analogRead(A1);

      if (jammed)
      {

    if(y > 700)
    {
        moveStepper(LOW);
        depth--;
        direction = "Retracting";

        if(depth <= 0)
        {
            depth = 0;
            jammed = false;

            display.clearDisplay();
            display.setCursor(0,20);
            display.println("DRILL CLEARED");
            display.display();

            delay(1000);

            backpressed = true;
            showmenu();
        }

    }
    else if(y < 300)
    {
        direction = "DRILL STUCK";
    }
      }

        else
        {
        if(y > 700)
        {
            moveStepper(HIGH);

            depth++;
            direction = "Upwards";
        }
        else if(y < 300)
        {
             moveStepper(LOW);

            depth--;
            direction = "Downwards";
        }
        else
        {
            direction = "Stationary";
        }
        }
        

    delay(1000);   


        int angle = map(x,0,1023,0,180);
      
        drill.write((int)angle);

        display.clearDisplay();

        display.setCursor(0,0);
        display.print("Depth: ");
        display.println(depth);

        display.setCursor(0,16);
        display.print("Tilt: ");
        display.println(angle);

        display.setCursor(0,32);
        display.print("Status:");

        display.setCursor(0,48);
        display.println(direction);

        display.display();

        
sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);

float gyroX = g.gyro.x;
float gyroY = g.gyro.y;

float jerk = sqrt((gyroX * gyroX) + (gyroY * gyroY));

digitalWrite(ledr, HIGH);
digitalWrite(ledg, HIGH);
digitalWrite(ledb, HIGH);
digitalWrite(buzz, LOW);

if (jerk < 0.3)
{
  digitalWrite(ledb, LOW);
  digitalWrite(ledr, HIGH); 
  digitalWrite(ledg, HIGH);
 

}

else if (jerk < 0.7)
{
  digitalWrite(ledg, LOW);
  digitalWrite(ledr, HIGH); 
  digitalWrite(ledb, HIGH);

}

else if (jerk > 0.7 && jammed == false)
{
  jammed = true;

  digitalWrite(ledr, LOW);
  digitalWrite(ledb, HIGH); 
  digitalWrite(ledg, HIGH);

digitalWrite(buzz, HIGH);

   display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);

  display.setCursor(10, 20);
  display.println("HARD ROCK HIT!!");
  display.display();

delay(1500);

selectpressed = false;
backpressed = false;

showmenu();

digitalWrite(buzz, LOW);
  digitalWrite(ledr, HIGH); 
  digitalWrite(ledg, HIGH); 
  digitalWrite(ledb, HIGH); 
  break;

    }
}
}



void loop() 
{
  int y = analogRead(A0);

  if (y < 300) 
  {
    menuindex--;
if (menuindex < 0) 
{menuindex = menusize - 1;}
    menuchanged = true;
  }

  if (y > 700) 
  {
    menuindex++;
    if (menuindex >= menusize)
    { menuindex = 0;}
    menuchanged = true;
  }

if(menuchanged)
{
    showmenu();
    menuchanged = false;
}

  if (selectpressed) 
  {
    selectpressed = false;

    if (jammed == true && menuindex==0)
{
  display.clearDisplay();
  display.setCursor(0,20);
  display.println("WARNING: DRILL STUCK");
  display.display();

  delay(1500);

  controldrillmode();
  
}

else if(menuindex == 0)
    {
        controldrillmode();
        }
    }



if (backpressed) 
  {
    backpressed = false;
}

}