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

// Define OLED display dimensions and pins
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Define pins for ultrasonic sensor
const int trigPin = 12;
const int echoPin = 8;

// Define servo pin and create servo object
const int servoPin = 6;
Servo servo;

// Define pins for Inputs
const int Button = 2;
const int Pot = A0;

// Define pins for LED's
const int Red = 3;
const int Green = 4;

int distanceThreshold = 10; // Initial distance threshold for servo closing
int maxDistance = 400; // Maximum distance the sensor can read
int lastDistance_cm = 0;
int lastPercentage = 0;
int potValue = 0; // Store the value of the potentiometer

bool manualControl = false; // Flag to indicate manual control of servo

unsigned long lastButtonPressTime = 0; // Variable to store the time of the last button press
bool buttonState = HIGH; // Initial state of the button

void setup() {
  // Set up the serial communication
  Serial.begin(9600);

  // Set up the ultrasonic sensor pins
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Initialize OLED display
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }

  // Initialize servo
  servo.attach(servoPin);

  // Set up Input pins
  pinMode(Button, INPUT);
  pinMode(Pot, INPUT);

  // Set up LED pins
  pinMode(Red, OUTPUT);
  pinMode(Green, OUTPUT);

  // Clear the display buffer
  display.clearDisplay();

  // Display initialization message
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("Water Level"); 
  display.setCursor(0, 10);
  display.println("Sensor & Control");
  display.display();
  delay(2000); // Pause for 2 seconds
}

void loop() {
    // Variables to store distance and duration
  int distance_cm;
  long duration;

  // Clear the trigPin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);

  // Send a 10us pulse to trigger the ultrasonic module
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Measure the duration of the pulse returned by the ultrasonic sensor
  duration = pulseIn(echoPin, HIGH);

  // Calculate distance in centimeters
  distance_cm = duration * 0.034 / 2;
  
  // Check for button press with debounce
  int buttonReading = digitalRead(Button);
  if (buttonReading == LOW && buttonState == HIGH && (millis() - lastButtonPressTime) > 200) {
    manualControl = !manualControl; // Toggle manual control mode
    if (manualControl) {
      digitalWrite(Red, LOW); // Turn off Red LED
      digitalWrite(Green, HIGH); // Turn on Green LED
    } else {
      digitalWrite(Green, LOW); // Turn off Green LED
      digitalWrite(Red, HIGH); // Turn on Red LED
    }
    lastButtonPressTime = millis(); // Update last button press time
  }
  buttonState = buttonReading;

  // Read potentiometer value
  potValue = analogRead(Pot);
  
  // Calculate the mapped servo position for manual control mode or threshold distance for automatic control mode
  int servoPos;
  int thresholdDistance; // Store the mapped potentiometer value for automatic control mode

  if (manualControl) {
    // Manual control mode
    // Map potentiometer value to the restricted range of the servo (40 to 0 degrees)
    servoPos = map(potValue, 0, 1023, 0, 40);
    // Set servo position
    servo.write(servoPos);

    // Calculate percentage based on servo position
    int servoPercentage = map(servoPos, 0, 40, 0, 100);

    // Update display
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(0, 0);
    display.println(manualControl ? "Manual Mode" : "Automatic Mode"); // Update mode display
    display.setCursor(0, 10);
    display.println("Target Distance:");
    display.setCursor(100, 10);
    display.println(thresholdDistance);
    display.setCursor(0, 20);
    display.println("Actual Distance:");
    display.setCursor(100, 20);
    display.println(distance_cm);
    //display.setCursor(0, 40);
    //display.print("Gate Angle:");
    //display.print(servoPos);
    //display.print(" degrees");
    display.setCursor(0, 50);
    display.print("Gate Open:");
    display.print(servoPercentage);
    display.println("%");
    display.display();
  } else {
    // Automatic control mode
    // Map potentiometer value to the range of the ultrasonic sensor (2 to 400 cm)
    thresholdDistance = map(potValue, 0, 1023, 2, maxDistance);

    // Control servo based on automatic mode
    if (distance_cm >= thresholdDistance) {
        // Close the servo (assuming 40 degrees is closed position)
        servo.write(40);
    } else {
        // Open the servo (assuming 0 degrees is open position)
        servo.write(0);
    }

    // Calculate percentage
    // Ensure servo position stays within the expected range
    int servoPos2 = constrain(servo.read(), 40, 0); // Read the current servo position and constrain it between 0 and 40
    int servoPercentage2 = map(servoPos2, 40, 0, 0, 100); // Calculate percentage based on servo position

    // Update display
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(0, 0);
    display.println(manualControl ? "Manual Mode" : "Automatic Mode"); // Update mode display
    display.setCursor(0, 10);
    display.println("Target Distance:");
    display.setCursor(100, 10);
    display.println(thresholdDistance);
    display.setCursor(0, 20);
    display.println("Actual Distance:");
    display.setCursor(100, 20);
    display.println(distance_cm);
    display.setCursor(0, 40);
    //display.print("Gate Angle:");
    //display.print(servoPos2);
    //display.print(" degrees");
    display.setCursor(0, 50);
    display.print("Gate Open:");
    display.print(servoPercentage2);
    display.println("%");
    display.display();


    // Update last values
    lastDistance_cm = distance_cm;
    //lastPercentage = servoPercentage;

  }

  // Wait before taking the next measurement
  delay(500);
}