#include <Servo.h>

Servo servo;

//FIX CONSTANT INTEGERS 
const int TrigPin  = 6;  // Arduino pin connected to Ultrasonic Sensor's TRIG pin
const int EchoPin  = 3;  // Arduino pin connected to Ultrasonic Sensor's ECHO pin
const int ServoPin = 2; // Arduino pin connected to Servo Motor's pin
const int Button1Pin = 4; // Button for selecting
const int Button2Pin = 5; // Menu Change Button

//VARYING INTEGERS
int LastButton1State = LOW; // Previous state from Button1
int LastButton2State = LOW; // Previous state from Button2
int CurrentButton1State;  // Current reading from the Button1
int CurrentButton2State;  // Current reading from the Button2
int LastServoState = 1; // Starting servo state
int CurrentServoState;  // Current servo state

int DistThresh = 150; // Threshold distance for the ultrasonic sensor to trigger the servo

volatile bool MoveServo = false; // Interrupt variable
int SerAng = 75;  // Starting servo angle
int SerSpd = 250; // Delay in the servo function

float duration_us, dist_cm; // Ultrasonic variables

//SETUP CODES 
void setup() {
  Serial.begin (9600);  // Serial monitor baud rate

  pinMode(Button1Pin, INPUT_PULLUP);  // Button 1 assigned as a pull up input
  pinMode(Button2Pin, INPUT_PULLUP);  // Button 2 assigned as a pull up input

  pinMode(TrigPin, OUTPUT);   // Ultrasonic's Trigger pin assigned as an output
  pinMode(EchoPin, INPUT);    // Ultrasonic's Echo pin assigned as an input

  attachInterrupt(digitalPinToInterrupt(EchoPin), motor55, FALLING); // Servo assigned as an interrupt function
  servo.attach(ServoPin);   //  Servo setup
  servo.write(75);
}

//RECCURRING CODE
void loop() {
  CurrentButton1State = digitalRead(Button1Pin);  // Data from Button1Pin stored in CurrentButton1State for state change function
  CurrentButton2State = digitalRead(Button2Pin);  // Data from Button2Pin stored in CurrentButton1State for state change function

//Button1 STATE CHANGE CODE
 if (LastButton1State == HIGH && CurrentButton1State == LOW) //
  Serial.println("The button1 is pressed"); // Enter button 1 function when pressed here
 else if (LastButton1State == LOW && CurrentButton1State == HIGH)
  Serial.println("The button1 is released"); // Enter button 1 function when released here
  LastButton1State = CurrentButton1State; // Current button 1 state is stored as LastButton1State
 //Button2 STATE CHANGE CODE
 if (LastButton2State == HIGH && CurrentButton2State == LOW)
  Serial.println("The button2 is pressed"); // Enter button 2 function when pressed here
 else if (LastButton2State == LOW && CurrentButton2State == HIGH)
  Serial.println("The button2 is released"); // Enter button 2 function when pressed here
 LastButton2State = CurrentButton2State; // Current button 2 state stored as LastButton2State


  digitalWrite(TrigPin, HIGH);  // Ultrasonic ping on
  delayMicroseconds(10);        // Ping delay
  digitalWrite(TrigPin, LOW);   // Ultrasonic ping off

  duration_us = pulseIn(EchoPin, HIGH); // Ultrasonic receiver on
  dist_cm = 0.017 * duration_us;        // Ultrasonic distance calculator

  //LEGACY CODE 
  //SerAng=constrain(SerAng,70, 90);
  //if(dist_cm < DistThresh)
  //  SerAng ++;
  //   else
  //   SerAng --;
  //servo.write(SerAng);
  MoveServo = false;
  
  //if(dist_cm < DistThresh && CurrentServoState == 0){ // if distance of Ultrasonic's receiver is less than the threshold and the current servo state = 0 do this:
   // CurrentServoState = 1;                  // Current servo state now = 1
    //LastServoState = CurrentServoState;     // Store current servo state in LastServoState
   // motor55();                              // go to function motor55()
   // }
   // else if (dist_cm > DistThresh && CurrentServoState == 1){ // if distance of Ultrasonic's receiver is greater than the threshold and the current servo state = 1 do this:
   // CurrentServoState = 0;                  // Current servo state is now = 0
   // LastServoState = CurrentServoState;     // Store current servo state in LastServoState
   // motor75();                              // go to function motor75()
   // }
}


void triggerMoveServo() { // Servo interrupt service routine
 MoveServo = true;
}


void motor55() { // Function to move servo to 55°
  //LEGACY CODE
  //SerAng=constrain(SerAng,55, 75);
  //SerAng ++;
  servo.write(55); // Move servo to 55°
  Serial.print("distance: "); // Print the word distance on the serial monitor
  Serial.print(dist_cm);      // Print dist_cm variable on the same line on the serial monitor
  Serial.println(" cm");       // Print the unit cm on the same line on the serial monitor. 'ln' creates a new line after

}


void motor75() {  // Function to move servo to 75°
  //LEGACY CODE
  //SerAng=constrain(SerAng,55, 75);
  //SerAng--;
  servo.write(75); // Move servo to 75°
  Serial.print("distance: "); // Print the word distance on the serial monitor
  Serial.print(dist_cm);      // Print dist_cm variable on the same line on the serial monitor
  Serial.println(" cm");      // Print the unit cm on the same line on the serial monitor. 'ln' creates a new line after
  delay(SerSpd);
}