#include <WiFi.h>
#include <ESP32Servo.h>
#include "ThingsBoard.h"
#include <Wire.h>
#include "RTCDS1307.h"
#include <Adafruit_SSD1306.h>

#define WIFI_SSID           "Wokwi-GUEST"
#define WIFI_PASSWORD       ""
#define TOKEN               "Fish1234"   
#define THINGSBOARD_SERVER  "thingsboard.cloud"
#define SERIAL_DEBUG_BAUD   115200

#define TrigPin 15
#define EchPin 2
#define ServoPin 13
#define RLedPin 14
#define GLedPin 26
#define YLedPin 27
#define BuzzerPin 25
#define BLedPin 12

RTCDS1307 rtc(0x68);
Adafruit_SSD1306 display(128, 64, &Wire);
Servo myServo;
WiFiClient espClient;
ThingsBoard tb(espClient);

int status = WL_IDLE_STATUS;
uint8_t hour, minute, second;
bool period = 0, feed1 = false, feed2 = false, feed3 = false;
int statusPakan = 0;

void connectWifi() {
  // Loop until we're reconnected
  status = WiFi.status();
  if (status != WL_CONNECTED) {
    digitalWrite(BLedPin, LOW);
    Serial.println("Connecting to AP...");
    WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
    while (WiFi.status() != WL_CONNECTED) {
      delay(500);
      Serial.print(".");
    }
    Serial.println();
    Serial.println("Connected to AP");
    digitalWrite(BLedPin, HIGH);
  }
}

void setup() {
  // Allow allocation of all timers
  ESP32PWM::allocateTimer(0);
  ESP32PWM::allocateTimer(1);
  ESP32PWM::allocateTimer(2);
  ESP32PWM::allocateTimer(3);
  myServo.setPeriodHertz(50);    // standard 50 Hz servo
  myServo.attach(ServoPin, 500, 2400);
  
  pinMode(TrigPin, OUTPUT);
  pinMode(EchPin, INPUT);
  pinMode(RLedPin, OUTPUT);
  pinMode(YLedPin, OUTPUT);
  pinMode(GLedPin, OUTPUT);
  pinMode(BLedPin, OUTPUT);
  
  // Initialize serial for debugging
  Serial.begin(SERIAL_DEBUG_BAUD);
  Serial.println();

  rtc.begin();
  if (display.begin(SSD1306_SWITCHCAPVCC, 0x3c)) {
    display.clearDisplay();
    display.setTextColor(1);
    display.setTextSize(1);
  }

  //rtc.setDate(27, 7, 27);
  //rtc.setTime(21, 48, 20);
  //connectWifi();
}

void servoOpenClose(int duration) {
  myServo.write(180);
  delay(duration);
  myServo.write(90);
}

void loop() {
  rtc.getTime(hour, minute, second, period);
  delay(1000);
  digitalWrite(TrigPin, HIGH);
  delay(10);
  digitalWrite(TrigPin, LOW);
  int duration = pulseIn(EchPin, HIGH);
  int dist = duration / 58;
  if (dist < 20) { // If distance < 20 cm, turn on green LED
    digitalWrite(GLedPin, HIGH);
    digitalWrite(YLedPin, LOW);
    digitalWrite(RLedPin, LOW);
    statusPakan = 2;
  } else {
    if (dist < 40) { // If distance < 40 cm, turn on yellow LED and sound buzzer
      statusPakan = 1;
      digitalWrite(GLedPin, LOW);
      digitalWrite(YLedPin, HIGH);
      digitalWrite(RLedPin, LOW);  
      for (int i = 0; i < 3; i++) {
        tone(BuzzerPin, 500, 250);
        delay(350);
      }
      delay(1000);
    } else { // If distance >= 40 cm, turn on red LED and sound buzzer
      statusPakan = 0;
      digitalWrite(GLedPin, LOW);
      digitalWrite(YLedPin, LOW);
      digitalWrite(RLedPin, HIGH);
      for (int i = 0; i < 8; i++) {
        tone(BuzzerPin, 700, 250);
        delay(50);
      }      
    }
  }

  switch (hour) {
    case 7:
      if (feed1 == false) {
        feed1 = true;
        servoOpenClose(3000);
      }      
      break;
    case 12:
      if (feed2 == false) {
        feed2 = true;
        servoOpenClose(3000);
      }
      break;
    case 17:
      if (feed3 == false) {
        feed3 = true;
        servoOpenClose(3000);
      }
      break;
  }

  if (feed1 == true && feed2 == true && feed3 == true) {
    feed1 = false;
    feed2 = false;
    feed3 = false;
  }

  display.clearDisplay();
  display.setCursor(0, 0); 
  display.print("Jumlah Pakan : ");
  display.println(50 - dist, DEC);
  display.display();

  Serial.println(dist);
  connectWifi();

  if (!tb.connected()) {
    // Connect to ThingsBoard
    Serial.print("Connecting to: ");
    Serial.print(THINGSBOARD_SERVER);
    Serial.print(" with token: ");
    Serial.println(TOKEN);
    if (!tb.connect(THINGSBOARD_SERVER, TOKEN)) {
      Serial.println("Failed to connect");
      return;
    }
  }

  Serial.println("Sending data...");
  tb.sendTelemetryInt("Jumlah Pakan", 50 - dist);
  tb.sendTelemetryInt("Status Pakan", statusPakan);

  tb.loop();
}