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

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

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

int tankheight = 20; //22cm
int fillheight = 20;  //in cm
int bufferheight = 3;
int data = 1;
int trig1 = 2, echo1 = 3;
int trig2 = 4, echo2 = 5;

int buttonPin = 8;
int buttonState;

void testdrawstyles(int a) {
  display.clearDisplay();
  display.setTextSize(2);             // Normal 1:1 pixel scale
  display.setTextColor(WHITE);        // Draw white text
  display.setCursor(0, 0);            // Start at top-left corner
  display.print(F("Level= "));
  display.setTextSize(2);             // Draw 2X-scale text
  display.setTextColor(WHITE);
  display.print(a);
  display.setTextSize(2);             // Draw 2X-scale text
  display.setTextColor(WHITE);
  display.print(F("%"));
  display.display();
  delay(500);
}

void setup() {
  Serial.begin(9600);
  pinMode(trig1, OUTPUT);
  pinMode(echo1, INPUT);
  pinMode(trig2, OUTPUT);
  pinMode(echo2, INPUT);
  pinMode(buttonPin, INPUT);
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;); // Don't proceed, loop forever
  }
  display.clearDisplay();
  display.drawPixel(10, 10, WHITE);
  display.display();
  delay(1000);
}



void loop() {
  long time1 = 0, height1 = 0, waterpercent1 = 0, waterheight1 = 0;
  long time2 = 0, height2 = 0, waterpercent2 = 0, waterheight2 = 0;
  // buttonState = digitalRead(buttonPin);
  // if (buttonState == HIGH) {
  //   Serial.println(buttonState);
  //   buttonState = LOW;
  //   data = data + 2;
  //   if(data > 7){
  //     data = 1;
  //   }
  // }
  // switch (data) {
  //   case 1:
  //     trig = data + 1;
  //     echo = data + 2;
  //     break;
  //   case 3:
  //     trig = data + 1;
  //     echo = data + 2;
  //     break;
  // }
  // digitalWrite(trig1, LOW);
  // delayMicroseconds(2);
  // digitalWrite(trig1, HIGH);
  // delayMicroseconds(10);
  // digitalWrite(trig1, LOW);
  // time1 = pulseIn(echo1, HIGH);
  // height1 = time1 / (29 * 2);
  // if (height1 <= 23 && height1 > 3) {
  //   waterheight1 = tankheight - (height1 - bufferheight);
  //   waterpercent1 = waterheight1 * 5;
  //   Serial.print(waterheight1);
  //   Serial.println(" cm");
  // }
  // else {
  //   Serial.println("Tank Full.");
  // }
  // digitalWrite(trig2, LOW);
  // delayMicroseconds(2);
  // digitalWrite(trig2, HIGH);
  // delayMicroseconds(10);
  // digitalWrite(trig2, LOW);
  // time2 = pulseIn(echo2, HIGH);
  // height2 = time2 / (29 * 2);
  // if (height2 <= 23 && height2 > 3) {
  //   waterheight2 = tankheight - (height2 - bufferheight);
  //   waterpercent2 = waterheight2 * 5;
  //   Serial.print(waterheight2);
  //   Serial.println(" cm");
  // }
  // else {
  //   Serial.println("Tank Full.");
  // }

  buttonState = digitalRead(buttonPin);
  Serial.println(buttonState);
  if (buttonState == HIGH) {
    Serial.println(buttonState);
    // buttonState = LOW;
    // Serial.println(buttonState);
  }

  // if (height <= 23) {
  //   height = height - bufferheight ;
  //   waterheight = tankheight - height;
  //   waterpercent = (waterheight / tankheight) * 100;
  //   Serial.println(waterheight + "dsf");
  // Serial.println(" cm");
  // if(){

  // }
  // }
  // else {
  //   if (height >= 0 && height < 5) {
  //     Serial.println("Tank Full.");
  //     waterpercent = 100;
  //     waterheight = 50;
  //   }
  //   else {
  //     Serial.println("No Range");
  //   }
  // }

  delay(2000);


  // testdrawstyles(waterpercent);

}