#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>
#include "DHT.h"

#define DHTPIN 2        // Pin untuk sensor DHT22
#define DHTTYPE DHT22   // Tipe sensor DHT22
#define SWITCHPIN 7     // Pin untuk saklar

// Inisialisasi objek
DHT dht(DHTPIN, DHTTYPE);
LiquidCrystal_I2C lcd(0x27, 16, 2);
Servo wiperServo;

// Variabel PID
float setpoint = 80; // Target kelembapan (contoh: 80%)
float input = 0;     // Nilai aktual dari sensor
float output = 0;    // Output ke servo
float lastError = 0; // Error sebelumnya
float integral = 0;  // Integral error

// Konstanta PID
float Kp = 2.0;
float Ki = 0.5;
float Kd = 1.0;

// Variabel gerakan wiper
bool moveRight = true;
int servoAngle = 0;

void setup() {
  pinMode(SWITCHPIN, INPUT_PULLUP); // Inisialisasi pin saklar dengan pull-up internal

  dht.begin();
  lcd.init();
  lcd.backlight();
  wiperServo.attach(9);
  wiperServo.write(0);

  Serial.begin(9600); // Untuk komunikasi MATLAB

  // Pesan awal
  lcd.setCursor(0, 0);
  lcd.print("PID Wiper");
  delay(2000);
}

void loop() {
  // Baca status saklar
  bool systemActive = digitalRead(SWITCHPIN) == LOW; // LOW berarti saklar aktif

  if (systemActive) {
    // Membaca nilai kelembapan
    input = dht.readHumidity();

    // Menghitung error
    float error = setpoint - input;

    // Hitung PID
    integral += error; // Integral error
    float derivative = error - lastError; // Derivatif error

    output = Kp * error + Ki * integral + Kd * derivative;

    // Batasi output dalam rentang servo (0-180 derajat)
    output = constrain(output, 0, 180);

    // Tampilkan data di LCD
    lcd.setCursor(0, 0);
    if (input > setpoint) {
      lcd.print("Hujan       ");

      // Gerakkan wiper ke kanan dan kiri dengan cepat
      if (moveRight) {
        servoAngle += 10; // Langkah besar untuk cepat
        if (servoAngle >= 180) {
          moveRight = false;
        }
      } else {
        servoAngle -= 10;
        if (servoAngle <= 0) {
          moveRight = true;
        }
      }
      wiperServo.write(servoAngle);
      delay(50); // Kecepatan gerakan wiper

    } else {
      lcd.print("Cerah       ");
      wiperServo.write(0); // Servo kembali ke posisi awal saat cerah
    }

    lcd.setCursor(0, 1);
    lcd.print("Hum: ");
    lcd.print(input, 1);
    lcd.print("% Out: ");
    lcd.print(output, 0);

    // Kirim data ke MATLAB
    Serial.print("Input: ");
    Serial.print(input);
    Serial.print(", Output: ");
    Serial.println(output);

    lastError = error; // Simpan error terakhir

  } else {
    // Jika sistem tidak aktif
    lcd.setCursor(0, 0);
    lcd.print("Sistem Off  ");
    lcd.setCursor(0, 1);
    lcd.print("             ");

    wiperServo.write(0); // Servo kembali ke posisi awal
    integral = 0; // Reset integral untuk menghindari akumulasi error
    lastError = 0; // Reset error terakhir
    delay(100); // Tambahkan sedikit delay untuk mengurangi pembacaan berulang
  }
}