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

#define WIDTH 128 // ความกว้างของจอ OLED ในพิกเซล
#define HEIGHT 64 // ความสูงของจอ OLED ในพิกเซล
#define RESET -1 // ขา reset เป็น -1 ถ้าใช้ร่วมกับขา Arduino reset
Adafruit_SSD1306 OLED(WIDTH, HEIGHT, &Wire, RESET);

#define sw1 2
#define sw2 15
#define sw3 4
int sw1State;
int sw2State;
int sw3State;
int lastsw1State = HIGH;
int lastsw2State = HIGH;
int lastsw3State = HIGH;
unsigned long lastDebounceTime = 0;
unsigned long debounceDelay = 50;

byte count = 0;

float temp;
float humi;

#define TRIGPIN 19
#define ECHOPIN 18

#include <DHT.h>
#define dhtpin 16
#define dhttype DHT22
DHT dht(dhtpin, dhttype);

long duration;
float distance;

bool sw2Pressed = false;

const int servoPin = 27;
Servo servo;

#define pot 25

float val;
int degree;

void setup() {
  servo.attach(servoPin, 500, 2400);
  pinMode(TRIGPIN, OUTPUT);
  pinMode(ECHOPIN, INPUT);
  OLED.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  OLED.clearDisplay();
  Serial.begin(115200);
  dht.begin();
  OLED.setTextSize(2);
  OLED.setTextColor(WHITE,BLACK);
  OLED.setCursor(52, 2);
  OLED.setTextSize(1);
  OLED.print("Menu");
  OLED.setCursor(8, 15);
  OLED.print("1 Temp");
  OLED.setCursor(8, 25);
  OLED.print("2 Humi");
  OLED.setCursor(8, 35);
  OLED.print("3 Distance");
  OLED.setCursor(8, 45);
  OLED.print("4 Position");
  OLED.display();
}

void loop() {
  humi = dht.readHumidity();
  temp = dht.readTemperature();

  digitalWrite(TRIGPIN, LOW);
  delayMicroseconds(5);
  digitalWrite(TRIGPIN, HIGH);
  delayMicroseconds(5);
  digitalWrite(TRIGPIN, LOW);

  duration = pulseIn(ECHOPIN, HIGH);
  distance = (duration/2) * 0.0343;

  servo.write(degree);

  // SW1 on/off
  int reading = digitalRead(sw1);

  if (reading != lastsw1State) {
    lastDebounceTime = millis();
  }

  if ((millis() - lastDebounceTime) > debounceDelay && !sw2Pressed) {
    if (reading != sw1State) {
      sw1State = reading;
      if (sw1State == LOW) {
        count++;
        if (count > 4) {
          count = 1;
        }
        switch (count) {
          case 1 : {
            datatemp();
          }
          break;
          case 2 : {
            datahumi();
          } 
          break;
          case 3 : {
            datadistance();
          }
          break;
          case 4 : {
            dataservo();
          }
          break;
        }
      }
    }
  }
  lastsw1State = reading;

  // SW2 on/off
  if (count > 0 ) { 
    int reading2 = digitalRead(sw2);

    if (reading2 != lastsw2State) {
      lastDebounceTime = millis();
    }

    if ((millis() - lastDebounceTime) > debounceDelay) {
      if (reading2 != sw2State) {
        sw2State = reading2;
        if (sw2State == LOW) {
          sw2Pressed = true;
          switch (count) {
            case 1 : {
              tmp();
            }
            break;
            case 2 : {
              hmi();
            } 
            break;
            case 3 : {
              dista();
            }
            break;
            case 4 : {
              val = analogRead(pot);
              degree = map(val, 0 , 4095, 0, 180);
              srvo();
            }
            break;
          }
        }
      }
    }
    lastsw2State = reading2;
  }


  // SW3 on/off
  int reading3 = digitalRead(sw3);

  if (reading3 != lastsw3State) {
    lastDebounceTime = millis();
  }

  if ((millis() - lastDebounceTime) > debounceDelay) {
    if (reading3 != sw3State) {
      sw3State = reading3;
      if (sw3State == LOW) {
        home();
        sw2Pressed = false;
      }
    }
  }
  lastsw3State = reading3;
}

void datatemp() {
  OLED.clearDisplay();
  OLED.setTextSize(2);
  OLED.setTextColor(WHITE,BLACK);
  OLED.setCursor(52, 2);
  OLED.setTextSize(1);
  OLED.print("Menu");
  OLED.setTextColor(BLACK,WHITE);
  OLED.setCursor(8, 15);
  OLED.print("1 Temp");
  OLED.setTextColor(WHITE,BLACK);
  OLED.setCursor(8, 25);
  OLED.print("2 Humi");
  OLED.setTextColor(WHITE,BLACK);
  OLED.setCursor(8, 35);
  OLED.print("3 Distance");
  OLED.setTextColor(WHITE,BLACK);
  OLED.setCursor(8, 45);
  OLED.print("4 Position");
  OLED.display();
}

void datahumi() {
  OLED.clearDisplay();
  OLED.setTextSize(2);
  OLED.setTextColor(WHITE,BLACK);
  OLED.setCursor(52, 2);
  OLED.setTextSize(1);
  OLED.print("Menu");
  OLED.setCursor(8, 15);
  OLED.print("1 Temp");
  OLED.setTextColor(BLACK,WHITE);
  OLED.setCursor(8, 25);
  OLED.print("2 Humi");
  OLED.setTextColor(WHITE,BLACK);
  OLED.setCursor(8, 35);
  OLED.print("3 Distance");
  OLED.setTextColor(WHITE,BLACK);
  OLED.setCursor(8, 45);
  OLED.print("4 Position");
  OLED.display();
}

void datadistance() {
  OLED.clearDisplay();
  OLED.setTextSize(2);
  OLED.setTextColor(WHITE,BLACK);
  OLED.setCursor(52, 2);
  OLED.setTextSize(1);
  OLED.print("Menu");
  OLED.setCursor(8, 15);
  OLED.print("1 Temp");
  OLED.setTextColor(WHITE,BLACK);
  OLED.setCursor(8, 25);
  OLED.print("2 Humi");
  OLED.setTextColor(BLACK,WHITE);
  OLED.setCursor(8, 35);
  OLED.print("3 Distance");
  OLED.setTextColor(WHITE,BLACK);
  OLED.setCursor(8, 45);
  OLED.print("4 Position");
  OLED.display();
}

void dataservo() {
  OLED.clearDisplay();
  OLED.setTextSize(2);
  OLED.setTextColor(WHITE,BLACK);
  OLED.setCursor(52, 2);
  OLED.setTextSize(1);
  OLED.print("Menu");
  OLED.setCursor(8, 15);
  OLED.print("1 Temp");
  OLED.setTextColor(WHITE,BLACK);
  OLED.setCursor(8, 25);
  OLED.print("2 Humi");
  OLED.setTextColor(WHITE,BLACK);
  OLED.setCursor(8, 35);
  OLED.print("3 Distance");
  OLED.setTextColor(BLACK,WHITE);
  OLED.setCursor(8, 45);
  OLED.print("4 Position");
  OLED.display();
}

void home() {
  OLED.clearDisplay();
  OLED.setTextSize(2);
  OLED.setTextColor(WHITE,BLACK);
  OLED.setCursor(52, 2);
  OLED.setTextSize(1);
  OLED.print("Menu");
  OLED.setCursor(8, 15);
  OLED.print("1 Temp");
  OLED.setCursor(8, 25);
  OLED.print("2 Humi");
  OLED.setCursor(8, 35);
  OLED.print("3 Distance");
  OLED.setCursor(8, 45);
  OLED.print("4 Position");
  OLED.display();
  count = 0;
}

void tmp() {
  OLED.clearDisplay();
  OLED.setTextSize(1);
  OLED.setCursor(35,0);
  OLED.print("Temperature");
  OLED.setTextSize(2);
  OLED.setCursor(30,30);
  OLED.print(temp);
  OLED.setCursor(100,30);
  OLED.print("c");
  OLED.drawCircle(95,30,3,WHITE);
  OLED.display();
}

void hmi() {
  OLED.clearDisplay();
  OLED.setTextSize(1);
  OLED.setCursor(40,0);
  OLED.print("Humidity");
  OLED.setTextSize(2);
  OLED.setCursor(15,30);
  OLED.print(humi);
  OLED.setCursor(90,30);
  OLED.print("%RH");
  OLED.display();
}

void dista() {
  OLED.clearDisplay();
  OLED.setTextSize(1);
  OLED.setTextColor(WHITE,BLACK);
  OLED.setCursor(40,0);
  OLED.print("distance");
  OLED.setTextSize(2);
  OLED.setCursor(30,30);
  OLED.print(int(distance));
  OLED.setTextSize(2);
  OLED.setCursor(70,30);
  OLED.print("cm");
  OLED.display();
}

void srvo() {
  OLED.clearDisplay();
  OLED.setTextSize(1);
  OLED.setTextColor(WHITE,BLACK);
  OLED.setCursor(40,0);
  OLED.print("Position");
  OLED.setTextSize(2);
  OLED.setCursor(50,30);
  OLED.print(degree);
  OLED.display();
}