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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
ST7032_asukiaaa lcd;

const int trigPin = 14;
const int echoPin = 27;
const int btnNext = 5;
const int btnAction = 18;
const int btnAverage = 19;

float memory[5] = {0, 0, 0, 0, 0};
int currentIndex = 0;

void setup() {
  Serial.begin(115200);
  
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(btnNext, INPUT_PULLUP);
  pinMode(btnAction, INPUT_PULLUP);
  pinMode(btnAverage, INPUT_PULLUP);

  if(!oled.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 failed"));
    for(;;);
  }
  oled.clearDisplay();
  oled.setTextColor(WHITE);

  lcd.begin(16, 2);
  lcd.setContrast(40);
  
  updateDisplays();
}

float getDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH);
  return duration * 0.034 / 2;
}

void updateDisplays() {
  oled.clearDisplay();
  oled.setTextSize(1);
  oled.setCursor(0,0);
  oled.print("Selected Cell: ");
  oled.setTextSize(2);
  oled.setCursor(50, 20);
  oled.print(currentIndex + 1);
  oled.display();

  lcd.setCursor(0, 0);
  for(int i = 0; i < 3; i++) {
    lcd.print(i+1); lcd.print(":"); lcd.print((int)memory[i]); lcd.print(" ");
  }
  lcd.setCursor(0, 1);
  for(int i = 3; i < 5; i++) {
    lcd.print(i+1); lcd.print(":"); lcd.print((int)memory[i]); lcd.print(" ");
  }
}

void showAverage() {
  float sum = 0;
  int count = 0;
  for(int i = 0; i < 5; i++) {
    if(memory[i] > 0) {
      sum += memory[i];
      count++;
    }
  }
  float avg = (count > 0) ? sum / count : 0;

  oled.clearDisplay();
  oled.setTextSize(1);
  oled.setCursor(0,0);
  oled.print("AVERAGE DISTANCE:");
  oled.setTextSize(2);
  oled.setCursor(10, 30);
  oled.print(avg);
  oled.print(" cm");
  oled.display();
  delay(3000);
  updateDisplays();
}

void loop() {
  if (digitalRead(btnNext) == LOW) {
    currentIndex = (currentIndex + 1) % 5;
    updateDisplays();
    delay(300);
  }

  if (digitalRead(btnAction) == LOW) {
    if (memory[currentIndex] == 0) {
      memory[currentIndex] = getDistance();
    } else {
      memory[currentIndex] = 0;
    }
    updateDisplays();
    delay(300);
  }

  if (digitalRead(btnAverage) == LOW) {
    showAverage();
    delay(300);
  }
}