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

// OLED display width and height (in pixels)
#define OLED_WIDTH 128
#define OLED_HEIGHT 64

// Define the pins for the ultrasonic sensor
//9,10,6,5
#define TRIG_PIN_SIDE_R 9
#define ECHO_PIN_SIDE_R 8
#define TRIG_PIN_SIDE_L 7
#define ECHO_PIN_SIDE_L 6
#define TRIG_PIN_HEIGHT 5
#define ECHO_PIN_HEIGHT 4
#define EXTERNAL_BUTTON_PIN 3 // Change to the pin you've connected the external button to

unsigned long previousMillis = 0; // Variable to store the last time distance was measured
const long interval = 2000; // Interval between distance measurements in milliseconds

const float initial_width_R = 9.875;
const float initial_width_L = 9.875;
const float initial_height = 10;
const float gate_width = 9.875;

// Initialize correction factors to 1
float correction_factor_SIDE_R = 1.0;
float correction_factor_SIDE_L = 1.0;
float correction_factor_HEIGHT = 1.0;
float Width = 0;
float Height = 0;
float Length = 0;
float Area = 0;


// Initialize the OLED display
#define SCREEN_ADDRESS 0x3C
Adafruit_SSD1306 display(OLED_WIDTH, OLED_HEIGHT, &Wire, -1);

//Function to read the distance from the Side Sensor
float mapDist(int trig, int echo) {
    //Send a pulse to trigger the ultrasonic sensor
    digitalWrite(trig, LOW);
    delayMicroseconds(2);
    digitalWrite(trig, HIGH);
    delayMicroseconds(10);
    digitalWrite(trig, LOW);
    
    // Measure the duration of the echo pulse
    long duration = pulseIn(echo, HIGH);
    float distance = duration / 74.0 / 2.0; // Speed of sound in inches per microsecond is approximately 0.0131 inches/µs (1/74 inch/µs), and we're interested in one-way distance
    return distance;
}

// Calculate distance with an applied correction factor and its value rounded to the nearest quarter and integer
#define ARRAY_SIZE 3

float RndDist[ARRAY_SIZE];

void RoundDist(float distance, float correction_factor, float *RndDist) {
    RndDist[0] = distance * correction_factor; // Store corrected distance at index 0
    RndDist[1] = (int)(((RndDist[0]) * 4.0) + 0.5) / 4.0; // Store quarter_dist at index 1
    RndDist[2] = round(RndDist[0]); // Store INT_dist at index 2
}


void setup() {
  // Initialize serial communication
  Serial.begin(9600);

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

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

  // Initialize the ultrasonic sensor pins
  pinMode(TRIG_PIN_SIDE_R, OUTPUT);
  pinMode(ECHO_PIN_SIDE_R, INPUT);
  pinMode(TRIG_PIN_SIDE_L, OUTPUT);
  pinMode(ECHO_PIN_SIDE_L, INPUT);
  pinMode(TRIG_PIN_HEIGHT, OUTPUT);
  pinMode(ECHO_PIN_HEIGHT, INPUT);

  // Initialize the external button pin
  pinMode(EXTERNAL_BUTTON_PIN, INPUT_PULLUP); // Set the external button pin as input with internal pull-up resistor
}

void loop() {
  unsigned long currentMillis = millis(); // Get the current time

  // Check if the external button is pressed
   if (digitalRead(EXTERNAL_BUTTON_PIN) == LOW) {
     // Button is pressed,
     // Call Distance Function to read distances from ultrasonic sensors
     float distance_SIDE_R = mapDist(TRIG_PIN_SIDE_R,ECHO_PIN_SIDE_R);
     float distance_SIDE_L = mapDist(TRIG_PIN_SIDE_L,ECHO_PIN_SIDE_L);
     float distance_HEIGHT = mapDist(TRIG_PIN_HEIGHT,ECHO_PIN_HEIGHT);

     // Calculate correction factor
     correction_factor_SIDE_R = initial_width_R / distance_SIDE_R;
     correction_factor_SIDE_L = initial_width_L / distance_SIDE_L;
     correction_factor_HEIGHT = initial_height / distance_HEIGHT;
   }

  // Check if it's time to measure the distance again
  if (currentMillis - previousMillis >= interval) {
    // Update the last time distance was measured
    previousMillis = currentMillis;

    // Call Distance Function to read distances (in inches) from ultrasonic sensors
    float distance_SIDE_R = mapDist(TRIG_PIN_SIDE_R,ECHO_PIN_SIDE_R);
    float distance_SIDE_L = mapDist(TRIG_PIN_SIDE_L,ECHO_PIN_SIDE_L);
    float distance_HEIGHT = mapDist(TRIG_PIN_HEIGHT,ECHO_PIN_HEIGHT);
    
    // Calculate distance with an applied correction factor and its value rounded to the nearest quarter and integer. (Calls struct function)
    RoundDist(distance_SIDE_R, correction_factor_SIDE_R, RndDist);
    float RndDistR = RndDist[0]; // Assign corrected distance for the right side
    float RndDistR_quarter = RndDist[1]; // Assign corrected quarter distance for the right side
    float RndDistR_int = RndDist[2]; // Assign corrected integer distance for the right side

    RoundDist(distance_SIDE_L, correction_factor_SIDE_L, RndDist);
    float RndDistL = RndDist[0]; // Assign corrected distance for the left side
    float RndDistL_quarter = RndDist[1]; // Assign corrected quarter distance for the left side
    float RndDistL_int = RndDist[2]; // Assign corrected integer distance for the left side

    RoundDist(distance_HEIGHT, correction_factor_HEIGHT, RndDist);
    float RndDistH = RndDist[0]; // Assign corrected distance for the height
    float RndDistH_quarter = RndDist[1]; // Assign corrected quarter distance for the height
    float RndDistH_int = RndDist[2]; // Assign corrected integer distance for the height

    //Box Dimension Calculations
    Width = gate_width-((distance_SIDE_R)+(distance_SIDE_L));
    Height = initial_height - distance_HEIGHT;
    
    Area = (Width)*(Height);


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

    // Display the Box Dimensions
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(0, 0);
    display.print("Width: ");
    display.print(Width, 2); // Display distance with 2 decimal places
    display.print(" in");

    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(0, 10);
    display.print("Height: ");
    display.print(Height, 2); // Display distance with 2 decimal places
    display.print(" in");

    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(0, 20);
    display.print("Length: ");
    display.print(Length, 2); // Display distance with 2 decimal places
    display.print(" in");

    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(0, 40);
    display.print("Area: ");
    display.print(Area, 2); // Display distance with 2 decimal places
    display.print(" squared in");

    // Display the content on the OLED display
    display.display();


    // SERIAL MONITOR
    Serial.println("");
    Serial.print("DISTANCE WIDTH R: ");
    Serial.print(distance_SIDE_R);
    Serial.print(", Width Dist: ");
    Serial.print(RndDistR);
    Serial.print(" inches, ");
    Serial.print("Rounded QTR: ");
    Serial.print(RndDistR_quarter);
    Serial.print(", Rounded INT: ");
    Serial.print(RndDistR_int);
    Serial.println(" inches");

    Serial.print("DISTANCE WIDTH L: ");
    Serial.print(distance_SIDE_L);
    Serial.print(", Width Dist: ");
    Serial.print(RndDistL);
    Serial.print(" inches, ");
    Serial.print("Rounded QTR: ");
    Serial.print(RndDistL_quarter);
    Serial.print(", Rounded INT: ");
    Serial.print(RndDistL_int);
    Serial.println(" inches");
    
    Serial.print("DISTANCE HEIGHT: ");
    Serial.print(distance_HEIGHT);
    Serial.print(", Height Dist: ");
    Serial.print(RndDistH);
    Serial.print(" inches, ");
    Serial.print("Rounded QTR: ");
    Serial.print(RndDistH_quarter);
    Serial.print(", Rounded INT: ");
    Serial.print(RndDistH_int);
    Serial.println(" inches");

    Serial.print("Box Dimensions: Width = ");
    Serial.print(Width);
    Serial.print(", Height = ");
    Serial.print(Height);
    Serial.print(", Area = ");
    Serial.print(Area);
    Serial.println("");
  }
}