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

#define OLED_RESET -1
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

#define PIEZO_PIN 8
#define VR_PIN A7
#define BUTTON_PIN A6

#define PWMA 6
#define AIN1 7
#define AIN2 4
#define PWMB 5
#define BIN1 2
#define BIN2 3

#define NOTE_C4 262
#define NOTE_E4 330
#define NOTE_G4 392
#define NOTE_A4 440

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
Adafruit_MCP3008 mcp;

static const unsigned char PROGMEM logo_bmp[] = {
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0xff, 0xff, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1e, 0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1c, 0x00, 0x00, 0x78, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x00, 0x00, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x38, 0x03, 0x80, 0x1c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x70, 0x03, 0x80, 0x1e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xf0, 0x03, 0x80, 0x0e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xe0, 0x03, 0x80, 0x0f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xe0, 0x03, 0x80, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xc0, 0x03, 0x80, 0x07, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xc0, 0x03, 0x80, 0x03, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0x80, 0x03, 0x80, 0x01, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0x00, 0x03, 0x80, 0x01, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0x00, 0x03, 0x80, 0x00, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x03, 0x80, 0x00, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x3f, 0xff, 0xfe, 0x00, 0x03, 0x80, 0x00, 0x7f, 0xff, 0xfe, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x7f, 0xff, 0xfe, 0x00, 0x03, 0x80, 0x00, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x7f, 0xff, 0xff, 0x00, 0x03, 0x80, 0x00, 0xe0, 0x00, 0x07, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0xf0, 0x00, 0x07, 0x80, 0x03, 0x80, 0x01, 0xe0, 0x00, 0x07, 0x80, 0x00, 0x00,
  0x00, 0x00, 0x01, 0xe0, 0x00, 0x07, 0x80, 0x03, 0x80, 0x01, 0xc0, 0x00, 0x03, 0x80, 0x00, 0x00,
  0x00, 0x00, 0x01, 0xc0, 0x00, 0x03, 0xc0, 0x03, 0x80, 0x03, 0x80, 0x7f, 0x01, 0xc0, 0x00, 0x00,
  0x00, 0x00, 0x03, 0xcf, 0xff, 0xf1, 0xc0, 0x03, 0x80, 0x07, 0x83, 0xff, 0xc1, 0xe0, 0x00, 0x00,
  0x00, 0x00, 0x03, 0x8f, 0xff, 0xf1, 0xe0, 0x03, 0x80, 0x07, 0x07, 0xc3, 0xe0, 0xe0, 0x00, 0x00,
  0x00, 0x00, 0x07, 0x8f, 0xff, 0xf0, 0xe0, 0x03, 0x80, 0x0f, 0x0f, 0x00, 0xf0, 0xf0, 0x00, 0x00,
  0x00, 0x00, 0x07, 0x0e, 0x00, 0x00, 0xf0, 0x03, 0x80, 0x0e, 0x0e, 0x00, 0x70, 0x70, 0x00, 0x00,
  0x00, 0x00, 0x0e, 0x0e, 0x00, 0x00, 0x78, 0x00, 0x00, 0x1e, 0x0e, 0x00, 0x00, 0x78, 0x00, 0x00,
  0x00, 0x00, 0x1e, 0x0e, 0x00, 0x00, 0x38, 0x00, 0x00, 0x3c, 0x0e, 0x00, 0x00, 0x3c, 0x00, 0x00,
  0x00, 0x00, 0x1c, 0x0e, 0x00, 0x00, 0x3c, 0x00, 0x00, 0x38, 0x0f, 0x00, 0x00, 0x1c, 0x00, 0x00,
  0x00, 0x00, 0x3c, 0x0e, 0x00, 0x00, 0x1c, 0x00, 0x00, 0x78, 0x07, 0x80, 0x00, 0x1e, 0x00, 0x00,
  0x00, 0x00, 0x38, 0x0e, 0x00, 0x00, 0x1e, 0x00, 0x00, 0x70, 0x03, 0xf8, 0x00, 0x0e, 0x00, 0x00,
  0x00, 0x00, 0x78, 0x0e, 0x00, 0x00, 0x0f, 0xff, 0xff, 0xf0, 0x01, 0xff, 0x80, 0x0f, 0x00, 0x00,
  0x00, 0x00, 0x70, 0x0f, 0xff, 0xe0, 0x0f, 0xff, 0xff, 0xe0, 0x00, 0x3f, 0xe0, 0x07, 0x00, 0x00,
  0x00, 0x00, 0xe0, 0x0f, 0xff, 0xe0, 0x07, 0xff, 0xff, 0xe0, 0x00, 0x03, 0xf0, 0x0f, 0x00, 0x00,
  0x00, 0x00, 0xe0, 0x0e, 0x00, 0x00, 0x07, 0x00, 0x00, 0xf0, 0x00, 0x00, 0x70, 0x0e, 0x00, 0x00,
  0x00, 0x00, 0x70, 0x0e, 0x00, 0x00, 0x0f, 0x00, 0x00, 0x78, 0x00, 0x00, 0x38, 0x1e, 0x00, 0x00,
  0x00, 0x00, 0x78, 0x0e, 0x00, 0x00, 0x0e, 0x00, 0x00, 0x38, 0x00, 0x00, 0x38, 0x1c, 0x00, 0x00,
  0x00, 0x00, 0x38, 0x0e, 0x00, 0x00, 0x1e, 0x00, 0x00, 0x3c, 0x1c, 0x00, 0x38, 0x38, 0x00, 0x00,
  0x00, 0x00, 0x3c, 0x0e, 0x00, 0x00, 0x1c, 0x00, 0x00, 0x1c, 0x1e, 0x00, 0x70, 0x78, 0x00, 0x00,
  0x00, 0x00, 0x1c, 0x0e, 0x00, 0x00, 0x3c, 0x00, 0x00, 0x1e, 0x0f, 0x00, 0xf0, 0x70, 0x00, 0x00,
  0x00, 0x00, 0x1e, 0x0e, 0x00, 0x00, 0x38, 0x00, 0x00, 0x0e, 0x07, 0xe7, 0xe0, 0xf0, 0x00, 0x00,
  0x00, 0x00, 0x0e, 0x0f, 0xff, 0xf0, 0x70, 0x00, 0x00, 0x0f, 0x03, 0xff, 0xc0, 0xe0, 0x00, 0x00,
  0x00, 0x00, 0x07, 0x0f, 0xff, 0xf8, 0xf0, 0x00, 0x00, 0x07, 0x80, 0x7e, 0x01, 0xe0, 0x00, 0x00,
  0x00, 0x00, 0x07, 0x8f, 0xff, 0xf0, 0xe0, 0x00, 0x00, 0x03, 0x80, 0x00, 0x03, 0xc0, 0x00, 0x00,
  0x00, 0x00, 0x03, 0x80, 0x00, 0x01, 0xe0, 0x00, 0x00, 0x03, 0xc0, 0x00, 0x03, 0x80, 0x00, 0x00,
  0x00, 0x00, 0x03, 0xc0, 0x00, 0x01, 0xc0, 0x00, 0x00, 0x01, 0xc0, 0x00, 0x07, 0x80, 0x00, 0x00,
  0x00, 0x00, 0x01, 0xc0, 0x00, 0x03, 0xc0, 0x00, 0x00, 0x01, 0xe0, 0x00, 0x07, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x01, 0xe0, 0x00, 0x03, 0x80, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0xe0, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x7f, 0xff, 0xfe, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x7f, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x7f, 0xff, 0xfe, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x7f, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x3f, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};

int menuIndex = 0;
const int totalMenus = 4;
int previousMenuIndex = -1;

uint8_t Unit_sensor = 8;
uint16_t sensor_pin[8];
uint32_t Last_Position;
float kp, ki, kd, p, i, d, fl, m1, m2;
int error, last, sumerror, power;
int minValue[8];
int maxValue[8];
float P_value = 0;
float D_value = 0;
int S_value = 0;
int PD_Save = 0;
bool PD_while = true;

int position() {
  readADC();
  bool In_line = false;
  long Average = 0, sum = 0;
  for (uint8_t i = 0; i < Unit_sensor; i++) {
    long value = map(long(sensor_pin[i]), minValue[i], maxValue[i], 1000, 0);
    if (value > 1000) {
      value = 1000;
    }
    if (value < 0) {
      value = 0;
    }
    if (value > 200) {
      In_line = true;
    }
    if (value > 50) {
      Average += (long)value * (i * 1000);
      sum += value;
    }
  }
  if (In_line == false) {
    if (Last_Position < (Unit_sensor - 1) * 1000 / 2) {
      return 0;
    } else {
      return (Unit_sensor - 1) * 1000;
    }
  } else {
    Last_Position = Average / sum;
  }
  return Last_Position;
}

void setup() {
  mcp.begin(13, 11, 12, 10);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.drawBitmap(
    (display.width() - SCREEN_WIDTH) / 2,
    (display.height() - SCREEN_HEIGHT) / 2,
    logo_bmp, SCREEN_WIDTH, SCREEN_HEIGHT, 1);
  display.display();
  loadPDFromEEPROM();
  for (int i = 0; i < 8; i++) {
    EEPROM.get(i * 4, minValue[i]);
    EEPROM.get((i * 4) + 2, maxValue[i]);
  }
  pinMode(VR_PIN, INPUT);
  pinMode(BUTTON_PIN, INPUT);
  pinMode(PIEZO_PIN, OUTPUT);
  pinMode(PWMA, OUTPUT);
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(PWMB, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
  playStartUpTune();
  display.clearDisplay();
  motorA(0);
  motorB(0);
}

void loop() {
  int vrValue = analogRead(VR_PIN);
  int buttonValue = analogRead(BUTTON_PIN);
  menuIndex = map(vrValue, 0, 1023, 0, totalMenus - 1);
  if (menuIndex != previousMenuIndex) {
    playMenuChangeSound();
    previousMenuIndex = menuIndex;
  }
  displayMenu(menuIndex);
  if (buttonValue > 500) {
    playConfirmSound();
    executeMenuAction(menuIndex);
    delay(500);
  }
}

void readADC() {
  for (int i = 0; i < 8; i++) {
    sensor_pin[i] = mcp.readADC(i);
  }
}

void motorA(int speed) {
  speed = speed * 2.55;
  if (speed >= 255) {
    speed = 255;
  } else if (speed <= -255) {
    speed = -255;
  }
  boolean inPin1;
  boolean inPin2;
  if (speed >= 0) {
    inPin1 = HIGH;
    inPin2 = LOW;
  } else if (speed < 0) {
    inPin1 = LOW;
    inPin2 = HIGH;
    speed = speed * -1;
  }
  digitalWrite(AIN1, inPin1);
  digitalWrite(AIN2, inPin2);
  analogWrite(PWMA, speed);
}

void motorB(int speed) {
  speed = speed * 2.55;
  if (speed >= 255) {
    speed = 255;
  } else if (speed <= -255) {
    speed = -255;
  }
  boolean inPin1;
  boolean inPin2;
  if (speed >= 0) {
    inPin1 = HIGH;
    inPin2 = LOW;
  } else if (speed < 0) {
    inPin1 = LOW;
    inPin2 = HIGH;
    speed = speed * -1;
  }
  digitalWrite(BIN1, inPin1);
  digitalWrite(BIN2, inPin2);
  analogWrite(PWMB, speed);
}

void displayMenu(int selectedIndex) {
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(42, 0);
  display.println("MENU");
  display.setTextSize(1);
  display.setCursor(0, 20);
  if (selectedIndex == 0) display.setTextColor(SSD1306_BLACK, SSD1306_WHITE);
  display.println("1. RUN");
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 30);
  if (selectedIndex == 1) display.setTextColor(SSD1306_BLACK, SSD1306_WHITE);
  display.println("2. Calibrate");
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 40);
  if (selectedIndex == 2) display.setTextColor(SSD1306_BLACK, SSD1306_WHITE);
  display.println("3. PD Tuning");
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 50);
  if (selectedIndex == 3) display.setTextColor(SSD1306_BLACK, SSD1306_WHITE);
  display.println("4. Reset");
  display.setTextColor(SSD1306_WHITE);
  display.display();
}

void displayCalibrate() {
  display.clearDisplay();
  display.setTextColor(SSD1306_BLACK, SSD1306_WHITE);
  display.fillRect(0, 0, display.width(), 8, SSD1306_WHITE);
  display.setCursor(38, 0);
  display.println("MIN Value");
  display.setTextColor(SSD1306_WHITE);
  for (int i = 0; i < 8; i++) {
    int x = (i % 4) * 34;
    int y = (i / 4) * 11 + 11;
    display.setCursor(x, y);
    display.println(minValue[i]);
  }
  display.setTextColor(SSD1306_BLACK, SSD1306_WHITE);
  display.fillRect(0, 33, display.width(), 8, SSD1306_WHITE);
  display.setCursor(38, 33);
  display.println("MAX Value");
  display.setTextColor(SSD1306_WHITE);
  for (int i = 0; i < 8; i++) {
    int x = (i % 4) * 34;
    int y = (i / 4) * 11 + 44;
    display.setCursor(x, y);
    display.println(maxValue[i]);
  }
  display.display();
}

void executeMenuAction(int selectedIndex) {
  if (selectedIndex == 0) {
    while (true) {
      int buttonValue = analogRead(BUTTON_PIN);
      if (buttonValue > 500) {
        playConfirmSound();
        while (true) {
          int buttonValue = analogRead(BUTTON_PIN);
          if (buttonValue > 500) {
            playConfirmSound();
            display.clearDisplay();
            display.setTextSize(2);
            display.setTextColor(SSD1306_WHITE);
            display.setCursor(0, 0);
            display.println("RUN");
            display.setCursor(0, 20);
            display.println("PROGRAM...");
            display.display();
            while (true) {
              power = S_value;
              kp = P_value;
              kd = D_value;
              error = position() - 3500;
              p = error * kp;
              d = (error - last) * kd;
              fl = p + d;
              last = error;
              m1 = power - fl;
              m2 = power + fl;
              if (m1 <= -100) m1 = -100;
              if (m1 >= 100) m1 = 100;
              if (m2 <= -100) m2 = -100;
              if (m2 >= 100) m2 = 100;
              motorA(m1);
              motorB(m2);
            }
          }
          display.clearDisplay();
          for (int i = 0; i < 8; i++) {
            int sensorValue = mcp.readADC(i);
            int barHeight = map(sensorValue, 0, 1023, 40, 0);
            int barY = 40 - barHeight;
            display.fillRect(i * (12 + 4), barY, 12, abs(barHeight), SSD1306_WHITE);
          }
          for (int i = 0; i < 8; i++) {
            int x = (i % 4) * 34;
            int y = (i / 4) * 11 + 44;
            display.setCursor(x, y);
            display.println(mcp.readADC(i));
          }
          display.display();
        }
      }
      display.clearDisplay();
      display.setTextColor(SSD1306_BLACK, SSD1306_WHITE);
      display.fillRect(0, 0, display.width(), 8, SSD1306_WHITE);
      display.setCursor(0, 0);
      display.println("MIN Value EEPROM");
      display.setTextColor(SSD1306_WHITE);
      for (int i = 0; i < 8; i++) {
        int x = (i % 4) * 34;
        int y = (i / 4) * 11 + 11;
        display.setCursor(x, y);
        display.println(minValue[i]);
      }
      display.setTextColor(SSD1306_BLACK, SSD1306_WHITE);
      display.fillRect(0, 33, display.width(), 8, SSD1306_WHITE);
      display.setCursor(0, 33);
      display.println("MAX Value EEPROM");
      display.setTextColor(SSD1306_WHITE);
      for (int i = 0; i < 8; i++) {
        int x = (i % 4) * 34;
        int y = (i / 4) * 11 + 44;
        display.setCursor(x, y);
        display.println(maxValue[i]);
      }
      display.display();
    }
  } else if (selectedIndex == 1) {
    for (int i = 0; i < 8; i++) {
      minValue[i] = 1023;
      maxValue[i] = 0;
    }
    while (true) {
      int buttonValue = analogRead(BUTTON_PIN);
      if (buttonValue > 500) {
        for (int i = 0; i < 8; i++) {
          EEPROM.put(i * 4, minValue[i]);
          EEPROM.put((i * 4) + 2, maxValue[i]);
        }
        playConfirmSound();
        break;
      }
      for (int i = 0; i < 8; i++) {
        int sensorValue = mcp.readADC(i);
        minValue[i] = min(minValue[i], sensorValue);
        maxValue[i] = max(maxValue[i], sensorValue);
      }
      displayCalibrate();
    }
    display.clearDisplay();
    display.setTextSize(2);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(0, 0);
    display.println("SAVE");
    display.setCursor(0, 20);
    display.println("EEPROM...");
    display.display();
    delay(1000);
  } else if (selectedIndex == 2) {
    PD_while = true;
    int menuIndex = 0;
    while (PD_while) {
      int vrValue = analogRead(VR_PIN);
      int buttonValue = analogRead(BUTTON_PIN);
      float newValue = map(vrValue, 0, 1023, 0, 299) / 1000.000;
      switch (menuIndex) {
        case 0:
          P_value = newValue;
          break;
        case 1:
          D_value = newValue;
          break;
        case 2:
          S_value = map(vrValue, 0, 1023, 0, 100);
          break;
      }
      display.clearDisplay();
      display.setTextSize(2);
      display.setTextColor(SSD1306_WHITE);
      display.setCursor(0, 0);
      display.println("PD Tuning");
      display.setTextSize(1);
      display.setCursor(0, 20);
      if (menuIndex == 0) display.setTextColor(SSD1306_BLACK, SSD1306_WHITE);
      display.print("1. P = ");
      display.println(P_value, 3);
      display.setTextColor(SSD1306_WHITE);
      display.setCursor(0, 30);
      if (menuIndex == 1) display.setTextColor(SSD1306_BLACK, SSD1306_WHITE);
      display.print("2. D = ");
      display.println(D_value, 3);
      display.setTextColor(SSD1306_WHITE);
      display.setCursor(0, 40);
      if (menuIndex == 2) display.setTextColor(SSD1306_BLACK, SSD1306_WHITE);
      display.print("3. S = ");
      display.println(S_value);
      display.setTextColor(SSD1306_WHITE);
      display.display();
      if (buttonValue > 500) {
        playMenuChangeSound();
        while (analogRead(BUTTON_PIN) > 500) {
          PD_Save++;
          if (PD_Save > 13000) {
            playConfirmSound();
            savePDToEEPROM();
            display.clearDisplay();
            display.setTextSize(2);
            display.setCursor(0, 0);
            display.println("SAVE");
            display.setCursor(0, 20);
            display.println("EEPROM...");
            display.display();
            delay(1000);
            PD_while = false;
            break;
          }
        }
        menuIndex++;
        if (menuIndex > 2) {
          menuIndex = 0;
        }
        delay(150);
        PD_Save = 0;
      }
    }
  } else if (selectedIndex == 3) {
    display.clearDisplay();
    display.setTextSize(2);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(0, 0);
    display.println("EEPROM");
    display.setCursor(0, 20);
    display.println("Clear...");
    display.display();
    delay(1000);
    for (int i = 0; i < EEPROM.length(); i++) {
      EEPROM.write(i, 0);
    }
    playConfirmSound();
    display.clearDisplay();
    display.setTextSize(2);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(0, 0);
    display.println("EEPROM");
    display.setCursor(0, 20);
    display.println("Clear OK!!");
    display.display();
    delay(1000);
  }
}

void playStartUpTune() {
  tone(PIEZO_PIN, NOTE_E4, 500);
  delay(400);
  tone(PIEZO_PIN, NOTE_C4, 500);
  delay(400);
  tone(PIEZO_PIN, NOTE_G4, 500);
  delay(400);
  tone(PIEZO_PIN, NOTE_E4, 500);
  delay(400);
}

void playMenuChangeSound() {
  tone(PIEZO_PIN, NOTE_A4, 100);
  delay(100);
  noTone(PIEZO_PIN);
}

void playConfirmSound() {
  tone(PIEZO_PIN, NOTE_C4, 200);
  delay(200);
  tone(PIEZO_PIN, NOTE_E4, 200);
  delay(200);
  noTone(PIEZO_PIN);
}

void savePDToEEPROM() {
  EEPROM.put(50, P_value);
  EEPROM.put(54, D_value);
  EEPROM.put(58, S_value);
}

void loadPDFromEEPROM() {
  EEPROM.get(50, P_value);
  EEPROM.get(54, D_value);
  EEPROM.get(58, S_value);
}