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

#define pinButton 12
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define DHTPIN 8
#define DHTTYPE DHT22
#define triggerPin 13

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
Servo servo1;
Servo servo2;
Servo servo3;

DHT dht(DHTPIN, DHTTYPE);


unsigned long lasttime = 0,
              lasttime1= 0;
int detik = 0;
int menit = 0;
int step = 0;
float t ;
bool state = false;
bool state1 = false;
bool timestart = false;
void setup() 
{
  Serial.begin(115200);
  pinMode(pinButton, INPUT_PULLUP);
  pinMode(triggerPin, OUTPUT);
  dht.begin();
  servo1.attach(3);
  servo2.attach(5);
  servo3.attach(6);
  gerak_servo(0,0,0,100);
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C))
   { // Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }
  delay(1000);
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0, 0);
  // Display static text
  display.println("RANCANG");
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0, 30);
  // Display static text
  display.println("BANGUN");
  display.display(); 
  delay(1000);
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0, 0);
  // Display static text
  display.println("TISSUE");
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0, 30);
  // Display static text
  display.println("PROCESSOR");
  display.display(); 
  delay(1000);
}
void loop(){
  unsigned long starttime = millis();
  if(starttime - lasttime1 >500){
    lasttime1 = starttime;
    t = dht.readTemperature();
  }
  if(t>60){digitalWrite(triggerPin, HIGH);}else{digitalWrite(triggerPin, LOW);}
  if(digitalRead(pinButton)==1 && state==false){
    display.clearDisplay();
    display.setTextSize(2);
    display.setTextColor(WHITE);
    display.setCursor(0, 0);
  // Display static text
    display.println("PRESS");
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(0, 30);
  // Display static text
    display.println("START BUTTON");
    display.display();
  }
 
  if(digitalRead(pinButton)==0 || state==true){
    state=true;
    display.clearDisplay();
    display.setTextSize(2);
    display.setTextColor(WHITE);
    display.setCursor(0, 0);
    display.println("TIME");

    display.setTextSize(2);
    display.setTextColor(WHITE);
    display.setCursor(0, 30);
    display.print(menit);
    display.print(":");
    display.print(detik);

    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(90, 0);
    display.print("TEMP:");
    display.setTextSize(2);
    display.setTextColor(WHITE);
    display.setCursor(80, 20);
    display.print(t,0);
    display.print(" C");
    display.display();
  }
  if(state==true && step==0){gerak_servo_turun(45,45,90,500);step=1;timestart=true;}
  if(state==true && step==1 && timestart==false){gerak_servo_naik(45,0,0,45,500);step=2;}
  if(state==true && step==2){gerak_servo_turun(45,45,90,500);step=3;timestart=true;}
  if(state==true && step==3 && timestart==false){gerak_servo_naik(45,0,0,135,500);step=4;}
  if(state==true && step==4){gerak_servo_turun(45,45,90,500);step=5;timestart=true;}
  if(state==true && step==5 && timestart==false){gerak_servo_naik(45,0,0,180,500);step=6;}
  if(state==true && step==6){gerak_servo_turun(45,45,90,500);step=7;timestart=true;}
  if(state==true && step==7 && timestart==false){gerak_servo_naik(45,0,0,0,500);step=0;state=false;}
  if(detik>10 ){timestart=false;menit=0;detik=0;}
  if(starttime - lasttime >1000 && state==true && timestart==true){
    lasttime = starttime;
    detik++;
    if(detik>59){
      menit++;
      detik=0;
    }
    if(menit>59){
      menit=0;
    }
  }
}
void gerak_servo(int pos1, int pos2, int pos3, int interval){
    servo1.write(pos1);   
    delay(interval);
    servo2.write(pos2);   
    delay(interval);
    servo3.write(pos3);   
    delay(interval);
}
void gerak_servo_turun(int pos1, int pos2, int pos3, int interval){
    smoothMove1(pos1);   
    delay(interval);
    smoothMove2(pos2);   
    delay(interval);
    smoothMove1(pos3);   
    delay(interval);
}
void gerak_servo_naik(int pos1, int pos2, int pos3, int pos4, int interval){
    smoothMove1(pos1);   
    delay(interval);
    smoothMove2(pos2);   
    delay(interval);
    smoothMove1(pos3);   
    delay(interval);
    smoothMove3(pos4);   
    delay(interval);
}

void smoothMove1(int targetAngle) {
  int currentAngle = servo1.read();  // Membaca posisi awal servo
  if (currentAngle < targetAngle) {
    for (int pos = currentAngle; pos <= targetAngle; pos++) {
      servo1.write(pos);  // Menggerakkan servo ke posisi baru
      delay(15);  // Jeda untuk pergerakan halus
    }
  } else {
    for (int pos = currentAngle; pos >= targetAngle; pos--) {
      servo1.write(pos);  // Menggerakkan servo ke posisi baru
      delay(15);  // Jeda untuk pergerakan halus
    }
  }
}
void smoothMove2(int targetAngle) {
  int currentAngle = servo2.read();  // Membaca posisi awal servo
  if (currentAngle < targetAngle) {
    for (int pos = currentAngle; pos <= targetAngle; pos++) {
      servo2.write(pos);  // Menggerakkan servo ke posisi baru
      delay(15);  // Jeda untuk pergerakan halus
    }
  } else {
    for (int pos = currentAngle; pos >= targetAngle; pos--) {
      servo2.write(pos);  // Menggerakkan servo ke posisi baru
      delay(15);  // Jeda untuk pergerakan halus
    }
  }
}
void smoothMove3(int targetAngle) {
  int currentAngle = servo3.read();  // Membaca posisi awal servo
  if (currentAngle < targetAngle) {
    for (int pos = currentAngle; pos <= targetAngle; pos++) {
      servo3.write(pos);  // Menggerakkan servo ke posisi baru
      delay(15);  // Jeda untuk pergerakan halus
    }
  } else {
    for (int pos = currentAngle; pos >= targetAngle; pos--) {
      servo3.write(pos);  // Menggerakkan servo ke posisi baru
      delay(15);  // Jeda untuk pergerakan halus
    }
  }
}