#include <Arduino.h>
#include <WiFi.h>
#include <Firebase_ESP_Client.h>
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <PZEM004Tv30.h>
#include <WiFiManager.h>
#include "addons/TokenHelper.h"
#include <NTPClient.h>
#include <WiFiUdp.h>

// Firebase Credentials
#define API_KEY "AIzaSyCfdJ7v2WdONyDVSgpQQCyzi__tvklcxIk"
#define DATABASE_URL "https://inputkwh-b45d4-default-rtdb.asia-southeast1.firebasedatabase.app/"
#define USER_EMAIL "nurilakbar23@gmail.com"
#define USER_PASSWORD "akugaktau"

// Relay Pins
const int relay1Pin = 5;
const int relay2Pin = 18;
const int relay3Pin = 19;
const int relay4Pin = 23;

// Servo Parameters
#define SERVOMAX  600 // 60 degrees
#define SERVOMIN  500 // 40 degrees
#define STEPS 20      // Number of steps to slow down servo movement

// Define servo positions for keypad (4x3 matrix)
const int servoPins[4][3] = {
  {0, 1, 2},
  {3, 4, 5},
  {6, 7, 8},
  {9, 10, 11}
};

// Define keypad layout
const char keypad[4][3] = {
  {'1', '2', '3'},
  {'4', '5', '6'},
  {'7', '8', '9'},
  {'*', '0', '#'}
};

// Constants for delay
const int SERVO_DELAY = 20; // Delay between each servo step
const int BUTTON_DELAY = 500; // Delay between button press

FirebaseData fbdoChannels;
FirebaseData fbdoTokens;
FirebaseData fbdo;
FirebaseAuth auth;
FirebaseConfig config;
Adafruit_PWMServoDriver pca9685 = Adafruit_PWMServoDriver();

String previousTokens = ""; // Menyimpan token sebelumnya

// Declare PZEM sensor object
#define RXD2 16
#define TXD2 17
HardwareSerial SerialPZEM(2); // Create HardwareSerial object for Serial2
PZEM004Tv30 pzem(SerialPZEM, RXD2, TXD2); // Initialize PZEM004Tv30 object

unsigned long sendDataPrevMillis = 0;
unsigned long lastDataSentMillis = 0;
bool signupOK = false;

WiFiUDP ntpUDP;
NTPClient timeClient(ntpUDP, "pool.ntp.org", 25200); // NTPClient object for WIB timezone (UTC+7)

void channelStreamCallback(FirebaseStream data) {
  Serial.printf("Channel stream path: %s\n", data.streamPath().c_str());
  Serial.printf("Event path: %s\n", data.dataPath().c_str());
  Serial.printf("Data type: %s\n", data.dataType().c_str());
  Serial.printf("Event type: %s\n", data.eventType().c_str());
  Serial.printf("Data: %s\n", data.stringData().c_str());

  String jsonData = data.stringData();
  Serial.printf("Received channel data: %s\n", jsonData.c_str());

  // Parse JSON
  FirebaseJson json;
  json.setJsonData(jsonData);
  FirebaseJsonData result;

  // Process each channel
  if (json.get(result, "channel1")) {
    String channel1Value = result.stringValue;
    controlRelay(relay1Pin, channel1Value);
  }

  if (json.get(result, "channel2")) {
    String channel2Value = result.stringValue;
    controlRelay(relay2Pin, channel2Value);
  }

  if (json.get(result, "channel3")) {
    String channel3Value = result.stringValue;
    controlRelay(relay3Pin, channel3Value);
  }

  if (json.get(result, "channel4")) {
    String channel4Value = result.stringValue;
    controlRelay(relay4Pin, channel4Value);
  }
}

void tokenStreamCallback(FirebaseStream data) {
  Serial.printf("Token stream path: %s\n", data.streamPath().c_str());
  Serial.printf("Event path: %s\n", data.dataPath().c_str());
  Serial.printf("Data type: %s\n", data.dataType().c_str());
  Serial.printf("Event type: %s\n", data.eventType().c_str());
  Serial.printf("Data: %s\n", data.stringData().c_str());

  String tokens = data.stringData();
  Serial.printf("Received tokens: %s\n", tokens.c_str());
  Serial.printf("Tokens length: %d\n", tokens.length());
  processTokens(tokens);
}

void streamTimeoutCallback(bool timeout) {
  if (timeout)
    Serial.println("Stream timeout, resuming...");
  else
    Serial.println("Stream resumed");
}

void setup() {
  Serial.begin(115200);

  // Inisialisasi pin relay sebagai output
  pinMode(relay1Pin, OUTPUT);
  pinMode(relay2Pin, OUTPUT);
  pinMode(relay3Pin, OUTPUT);
  pinMode(relay4Pin, OUTPUT);

  // Inisialisasi relay ke posisi mati (off)
  digitalWrite(relay1Pin, HIGH);
  digitalWrite(relay2Pin, HIGH);
  digitalWrite(relay3Pin, HIGH);
  digitalWrite(relay4Pin, HIGH);

  WiFiManager wifiManager;

  wifiManager.setConfigPortalTimeout(180); // Set timeout portal ke 3 menit

  // Coba terhubung ke Wi-Fi
  if (!wifiManager.autoConnect("MAIN-KWH","tugasakhir")) {
    Serial.println("Failed to connect to WiFi, entering config mode");
    // Jika gagal terhubung ke Wi-Fi, masuk ke mode konfigurasi
    wifiManager.startConfigPortal("AutoConnectAP");
  }

  // Periksa koneksi WiFi
  if (WiFi.status() != WL_CONNECTED) {
    Serial.println("Failed to connect to WiFi");
    return;
  } else {
    Serial.println("Connected to WiFi");
  }

  config.api_key = API_KEY;
  config.database_url = DATABASE_URL;
  config.token_status_callback = tokenStatusCallback; // Callback status token

  auth.user.email = USER_EMAIL;
  auth.user.password = USER_PASSWORD;

  // Mendaftar (Sign up)
  if (Firebase.signUp(&config, &auth, "", "")) {
    Serial.println("Sign up successful");
    signupOK = true;
  } else {
    Serial.println("Sign up failed");
    Serial.println(config.signer.signupError.message.c_str());
  }

  // Mulai koneksi Firebase
  Firebase.begin(&config, &auth);
  Firebase.reconnectNetwork(true);

  // Mulai stream Firebase untuk channel
  if (!Firebase.RTDB.beginStream(&fbdoChannels, "/channels")) {
    Serial.printf("Failed to start channels stream, %s\n", fbdoChannels.errorReason().c_str());
  } else {
    Serial.println("Channels stream started successfully!");
    Firebase.RTDB.setStreamCallback(&fbdoChannels, channelStreamCallback, streamTimeoutCallback);
  }

  // Mulai stream Firebase untuk token
  if (!Firebase.RTDB.beginStream(&fbdoTokens, "/tokens")) {
    Serial.printf("Failed to start tokens stream, %s\n", fbdoTokens.errorReason().c_str());
  } else {
    Serial.println("Tokens stream started successfully!");
    Firebase.RTDB.setStreamCallback(&fbdoTokens, tokenStreamCallback, streamTimeoutCallback);
  }

  // Setup PCA9685
  pca9685.begin();
  pca9685.setPWMFreq(60); // Analog servos work at ~60 Hz
  delay(10);

  // Pindahkan semua servos ke posisi awal (60 derajat)
  for (int i = 0; i < 12; i++) {
    pca9685.setPWM(i, 0, SERVOMAX);
  }

  // Set posisi awal untuk servos '1', '4', '9', dan '#' tanpa bergerak
  setInitialPosition('1');
  setInitialPosition('4');
  setInitialPosition('9');
  setInitialPosition('#');

  // Inisialisasi NTP client
  timeClient.begin();
  timeClient.update();
}


void loop() {
  // Periksa koneksi WiFi dan kesiapan Firebase
  if (WiFi.status() == WL_CONNECTED && Firebase.ready() && signupOK) {
    unsigned long currentMillis = millis();

    // Periksa apakah sudah waktunya mengirim data sensor
    if (currentMillis - sendDataPrevMillis >= 1000) { // Kirim data setiap detik
      sendDataPrevMillis = currentMillis;

      // Baca data dari sensor PZEM
      float voltage = pzem.voltage();
      float current = pzem.current();
      float power = pzem.power();
      float energy = pzem.energy();
      float frequency = pzem.frequency();
      float pf = pzem.pf();
      float va = voltage * current; // Menghitung daya semu (VA)

      // Kirim data ke Firebase RTDB untuk path "sensor" (tidak diubah)
      if (Firebase.RTDB.setFloat(&fbdo, "sensor/voltage", voltage) &&
          Firebase.RTDB.setFloat(&fbdo, "sensor/current", current) &&
          Firebase.RTDB.setFloat(&fbdo, "sensor/power", power) &&
          Firebase.RTDB.setFloat(&fbdo, "sensor/energy", energy) &&
          Firebase.RTDB.setFloat(&fbdo, "sensor/frequency", frequency) &&
          Firebase.RTDB.setFloat(&fbdo, "sensor/pf", pf) &&
          Firebase.RTDB.setFloat(&fbdo, "sensor/va", va)) {
        Serial.println("Data sent successfully to sensor");
      } else {
        Serial.println("Failed to send data to sensor");
        Serial.println(fbdo.errorReason()); // Tampilkan alasan kegagalan
      }
    }

    // Periksa apakah sudah waktunya mengirim data-listrik
    if (currentMillis - lastDataSentMillis >= 60000) { // Kirim data setiap menit (60,000 milidetik)
      lastDataSentMillis = currentMillis;

      // Update NTP client to get current time
      timeClient.update();

      // Get formatted date and time
      String formattedTime = timeClient.getFormattedTime();
      time_t rawTime = timeClient.getEpochTime();
      struct tm *timeInfo = localtime(&rawTime);
      String year = String(timeInfo->tm_year + 1900);
      String month = String(timeInfo->tm_mon + 1);
      String day = String(timeInfo->tm_mday);
      String hour = formattedTime.substring(0, 2);
      String minute = formattedTime.substring(3, 5);

      // Combine date and time into a single field
      String dateTime = day + "/" + month + "/" + year + " " + hour + ":" + minute;

      // Buat objek JSON untuk data-listrik
      FirebaseJson json;
      json.set("dateTime", dateTime);
      json.set("voltage", pzem.voltage());
      json.set("current", pzem.current());
      json.set("power", pzem.power());
      json.set("energy", pzem.energy());
      json.set("frequency", pzem.frequency());
      json.set("pf", pzem.pf());
      json.set("va", pzem.voltage() * pzem.current()); // Menambahkan data VA ke objek JSON

      // Buat key baru dengan menggunakan timestamp
      String timestamp = String(rawTime);

      // Kirim data ke Firebase RTDB menggunakan push untuk menambahkan data baru di path "data-listrik" dengan key timestamp
      if (Firebase.RTDB.setJSON(&fbdo, "data-listrik/" + timestamp, &json)) {
        Serial.println("Data sent successfully to data-listrik");
      } else {
        Serial.println("Failed to send data to data-listrik");
        Serial.println(fbdo.errorReason()); // Tampilkan alasan kegagalan
      }
    }
  } else {
    // Reconnect WiFi and Firebase if not connected
    WiFi.reconnect();
    Firebase.reconnectWiFi(true);
    delay(2000);
  }
  delay(10);  // Add a short delay to prevent WDT reset
}

void controlRelay(int relayPin, String state) {
  if (state == "On") {
    digitalWrite(relayPin, LOW); // Turn relay on
  } else {
    digitalWrite(relayPin, HIGH); // Turn relay off
  }
}

void processTokens(String tokens) {
  if (tokens.length() == 20) { // Jika panjang token adalah 20 karakter
    for (int i = 0; i < 20; i++) {
      char digit = tokens.charAt(i);
      pressKey(digit); // Tekan tombol sesuai digit token
      delay(BUTTON_DELAY); // Sesuaikan penundaan jika diperlukan agar keypad mendeteksi tekanan tombol
    }
    
    // Tekan tombol '#' setelah mengeksekusi 20 digit
    pressKey('#');
    delay(BUTTON_DELAY); // Tunda sebelum membaca token lagi
  } else {
    Serial.println("Invalid token length"); // Tampilkan pesan kesalahan jika panjang token tidak valid
  }
}

void pressKey(char key) {
  int startPos, endPos;
  
  if (key == '1' || key == '4' || key == '9' || key == '#') {
    startPos = SERVOMIN;  // 40 degrees
    endPos = SERVOMAX - 20;    // 60 degrees, kurangi 50 agar gerakan lebih kecil
  } else {
    startPos = SERVOMAX;  // 60 degrees
    endPos = SERVOMIN + 20;    // 40 degrees, tambahkan 50 agar gerakan lebih kecil
  }

  for (int row = 0; row < 4; row++) {
    for (int col = 0; col < 3; col++) {
      if (keypad[row][col] == key) {
        int servoNum = servoPins[row][col];
        
        // Move servo from startPos to endPos with steps
        for (int pulse = startPos; (startPos < endPos) ? (pulse <= endPos) : (pulse >= endPos); pulse += ((startPos < endPos) ? 1 : -1) * ((abs(endPos - startPos)) / STEPS)) {
          pca9685.setPWM(servoNum, 0, pulse);
          delay(SERVO_DELAY); // Delay to slow down servo movement
        }

        delay(BUTTON_DELAY); // Delay to simulate button press
        
        // Return servo from endPos to startPos with steps
        for (int pulse = endPos; (endPos < startPos) ? (pulse <= startPos) : (pulse >= startPos); pulse += ((endPos < startPos) ? 1 : -1) * ((abs(startPos - endPos)) / STEPS)) {
          pca9685.setPWM(servoNum, 0, pulse);
          delay(SERVO_DELAY); // Delay to slow down servo movement
        }
        
        delay(BUTTON_DELAY); // Delay after button press
        
        return;
      }
    }
  }
  Serial.printf("Button %c not found\n", key);
}

void setInitialPosition(char key) {
  for (int row = 0; row < 4; row++) {
    for (int col = 0; col < 3; col++) {
      if (keypad[row][col] == key) {
        int servoNum = servoPins[row][col];
        pca9685.setPWM(servoNum, 0, SERVOMIN); // Set to 40 degrees
        return;
      }
    }
  }
  Serial.printf("Button %c not found\n", key);
}