#include <SPI.h>
#include <LoRa.h>
#include <ArduinoJson.h>
#include "HX711.h"

// ==========================================
// ==========================================
// PIN DEFINITIONS (LoRa SPI - Custom Layout)
// ==========================================
#define LORA_SCK  22  // D22
#define LORA_MISO 21  // D21
#define LORA_MOSI 19  // D19
#define LORA_SS   18  // D18
#define LORA_RST  23  // D23
#define LORA_DIO0 34  // D34 (Sisi Kiri, Input-Only, Sangat Aman)

// ==========================================
// PIN DEFINITIONS (SENSOR & LED - SISI KANAN ESP32)
// ==========================================
// SENSOR (Dipindah ke KIRI karena D15 & D2 adalah Strapping Pin)
#define TRIG_PIN   25  // D25
#define ECHO_PIN   26  // D26

// LED & BUZZER (SISI KANAN - Aman)
#define LED_BLUE   17  // TX2 (D17)
#define LED_GREEN  16  // RX2 (D16)
#define LED_RED    4   // D4
#define BUZZER_PIN 15  // D15 (Buzzer untuk indikator LoRa)

// ==========================================
// PIN DEFINITIONS (LOADCELL - SISI KIRI ESP32)
// ==========================================
// Dipindah ke D32 & D33 karena D18 & D19 sekarang dipakai LoRa
#define HX711_DT   32  // D32
#define HX711_SCK  33  // D33

// ==========================================
// SETTINGS
// ==========================================
#define MIN_DIST  2.0
#define MAX_DIST  200.0
#define TIMEOUT_MS 3000

// ==========================================
// KALIBRASI HC-SR04
// ==========================================
#define TINGGI_ALAT 79.27 // Tinggi sensor dari lantai ke alas (cm) - diukur manual
float SENSOR_OFFSET = -4.94; // Offset koreksi hardware sensor (dari kalibrasi)
float TINGGI_OFFSET = 0.0;  // Offset koreksi tambahan jika ada selisih pembacaan

// ==========================================
// KALIBRASI LOAD CELL
// ==========================================
float calibration_factor = -103.8; // Dari hasil kalibrasi (loadcell_ultrasonik_kalibrasi.ino)
float weightThreshold = 2.0; // Ambang batas dianggap kosong (gram)

unsigned long emptyStartTime = 0;
bool isEmptyScale = false;

// ==========================================
// VARIABLES
// ==========================================
bool isConnected = false;
bool isBalitaSelected = false;
unsigned long lastHeartbeatTime = 0;
bool isLedLocked = false;
char jsonBuffer[800];

// Measuring Variables
float lastDistance = 0;
unsigned long lastChangeTime = 0;
unsigned long lastScaleRead = 0;

// Worker Modes
enum WorkMode { MODE_MEASURE, MODE_ANALYZE };
WorkMode currentMode = MODE_MEASURE;

// Objects
HX711 scale;

// Packet Structure
typedef struct struct_message {
    char text[220];  // Dikurangi untuk memberi ruang pada data sinyal
    int msgType;     // 0=Request, 1=Response, 2=Ping, 3=Pong
    int packetId;    
    int totalPackets;
    int senderId;    // 1=Node1, 2=Node2, 3=Repeater
    int rssi_gw;     // Sinyal Hop 1 (A -> GW)
    float snr_gw;
    int rssi_b;      // Sinyal Hop 2 (GW -> B)
    float snr_b;
} struct_message;

struct_message myData;
struct_message recvData;

// ==========================================
// HELPER FUNCTIONS
// ==========================================
void setStatus(int r, int g, int b) {
  digitalWrite(LED_RED, r);
  digitalWrite(LED_GREEN, g);
  digitalWrite(LED_BLUE, b);
}

// Send Data via LoRa
bool sendDataLoRa(struct_message *data) {
  // Bunyikan Buzzer saat mengirim data
  digitalWrite(BUZZER_PIN, HIGH);
  delay(50);
  digitalWrite(BUZZER_PIN, LOW);

  LoRa.beginPacket();
  LoRa.write((uint8_t*)data, sizeof(struct_message));
  LoRa.endPacket();
  return true; 
}

// ==========================================
// SETUP
// ==========================================
void setup() {
  Serial.begin(115200);
  
  // Sensor & LED Pins
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT_PULLDOWN); // Gunakan PULLDOWN agar tidak menangkap noise saat sensor tidak terhubung
  pinMode(LED_RED, OUTPUT);
  pinMode(LED_GREEN, OUTPUT);
  pinMode(LED_BLUE, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  digitalWrite(BUZZER_PIN, LOW);
  
  // Matikan semua LED di awal
  setStatus(0, 0, 0);

  // 1. Setup Loadcell (HX711)
  scale.begin(HX711_DT, HX711_SCK);
  scale.set_scale(calibration_factor);
  
  // Wajib: Beri Pull-Up pada pin DT agar jika timbangan tidak dicolok, dia akan diam di HIGH (Not Ready).
  // Jika tidak di-pullup, pin yang mengambang akan dianggap LOW (Ready) oleh ESP32.
  pinMode(HX711_DT, INPUT_PULLUP);

  // Beri waktu maksimal 3 detik untuk modul HX711 menyala dan siap (sama seperti kalibrasi)
  unsigned long startWait = millis();
  while (!scale.is_ready() && millis() - startWait < 3000) {
    delay(10);
  }
  
  // Cek apakah sensor timbangan benar-benar terhubung
  if (scale.is_ready()) {
    scale.tare(); // Zero the scale on startup
    digitalWrite(LED_GREEN, HIGH); // Hijau = HX711 Ready
  } else {
    Serial.println("Warning: HX711 Loadcell not detected or not ready yet!");
  }

  // 2. Setup SPI & LoRa
  SPI.begin(LORA_SCK, LORA_MISO, LORA_MOSI, LORA_SS);
  LoRa.setPins(LORA_SS, LORA_RST, LORA_DIO0);
  
  if (!LoRa.begin(433E6)) { 
    Serial.println("Error initializing LoRa");
  } else {
    digitalWrite(LED_RED, HIGH); // Merah = LoRa Ready
  }
  
  delay(500); // Jeda tambahan agar sensor ultrasonik stabil sebelum dites

  // 3. Cek Ultrasonik (Kirim 1x Ping)
  digitalWrite(TRIG_PIN, LOW); delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH); delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  long initDuration = pulseIn(ECHO_PIN, HIGH, 30000); 
  if (initDuration > 0) digitalWrite(LED_BLUE, HIGH); // Biru = Ultrasonik Ready
  
  Serial.println("LoRa Node A (ESP32-Integrated-Pro) Ready (V2 Piggybacking)");
}

// ==========================================
// LOOP
// ==========================================
void loop() {
  // 1. CEK DATA DARI WEB APP (SERIAL) --------------------
  if (Serial.available() > 0) {
    String cmd = Serial.readStringUntil('\n');
    cmd.trim();
    
    if (cmd == "CMD:CONNECT") {
      isConnected = true;
      isBalitaSelected = false;
      lastHeartbeatTime = millis(); 
      isLedLocked = false;
      
      // Buzzer Beep Cepat saat Terkoneksi ke Web
      for(int i=0; i<3; i++) { 
        digitalWrite(BUZZER_PIN, HIGH); delay(100); 
        digitalWrite(BUZZER_PIN, LOW); delay(100); 
      }

    } else if (cmd == "CMD:PING") {
      if (!isConnected) isConnected = true;
      lastHeartbeatTime = millis();
      
    } else if (cmd == "CMD:DISCONNECT") {
      isConnected = false;
      isBalitaSelected = false;
      isLedLocked = false;
      
    } else if (cmd == "CMD:TARE") {
      scale.tare();
      Serial.println("TARE:OK");

    } else if (cmd == "CMD:LOCK_LED") {
      isLedLocked = true;
      
    } else if (cmd == "CMD:UNLOCK_LED") {
      isLedLocked = false;

    } else if (cmd == "CMD:SET_MODE:MEASURE") {
      currentMode = MODE_MEASURE;
      isLedLocked = false;
      Serial.println("MODE:MEASURE:OK");
      
    } else if (cmd == "CMD:SET_MODE:ANALYZE") {
      currentMode = MODE_ANALYZE;
      isLedLocked = true;
      Serial.println("MODE:ANALYZE:OK");
      
    } else if (cmd == "CMD:BALITA_SELECTED") {
      isBalitaSelected = true;
      Serial.println("BALITA:SELECTED:OK");
      
    } else if (cmd == "CMD:BALITA_UNSELECTED") {
      isBalitaSelected = false;
      Serial.println("BALITA:UNSELECTED:OK");

    } else if (cmd == "CMD:PING_LOCAL") {
      // 1. Cek Sensor Ultrasonik Lokal
      digitalWrite(TRIG_PIN, LOW); delayMicroseconds(2);
      digitalWrite(TRIG_PIN, HIGH); delayMicroseconds(10);
      digitalWrite(TRIG_PIN, LOW);
      long duration = pulseIn(ECHO_PIN, HIGH, 30000); 
      float distance = duration * 0.0343 / 2;

      // 2. Cek Loadcell Lokal
      bool loadcellReady = scale.is_ready();

      String resp = "PONG:A:READY";
      if (distance > MIN_DIST && distance < MAX_DIST) resp += " PONG:SENSOR:DETECTED";
      else resp += " PONG:SENSOR:EMPTY";
      
      if (loadcellReady) resp += " PONG:LOADCELL:READY";
      else resp += " PONG:LOADCELL:ERROR";

      Serial.println(resp);
      
    } else if (cmd == "CMD:PING_LORA") {
      // Konfirmasi ke Web App bahwa Node A (Local) juga Ready
      Serial.println("PONG:A:READY");

      // Ping Node B via LoRa
      myData.msgType = 2; // PING
      myData.packetId = 0;
      myData.totalPackets = 1;
      myData.senderId = 1; // Node 1
      strcpy(myData.text, "PING");
      sendDataLoRa(&myData);

    } else if (cmd.startsWith("CMD:SEND_DATA:")) {
      String json = cmd.substring(14);
      int totalLength = json.length();
      int chunkSize = 215; // Dikurangi agar muat di text[220]
      int totalPackets = (totalLength + chunkSize - 1) / chunkSize;
      
      for (int i = 0; i < totalPackets; i++) {
        memset(&myData, 0, sizeof(struct_message));
        myData.msgType = 0; // REQUEST
        myData.packetId = i;
        myData.totalPackets = totalPackets;
        myData.senderId = 1; // Node 1
        
        String chunk = json.substring(i * chunkSize, min((i + 1) * chunkSize, totalLength));
        strncpy(myData.text, chunk.c_str(), sizeof(myData.text) - 1);
        myData.text[sizeof(myData.text) - 1] = '\0';
        
        sendDataLoRa(&myData);
        
        if (i < totalPackets - 1) {
            delay(400); // Beri jeda 400ms agar Repeater sempat memantulkan chunk sebelumnya
        }
      }
    }
  }

  // 2. CEK DATA MASUK DARI LORA (NODE B) ------------------
  int packetSize = LoRa.parsePacket();
  if (packetSize == sizeof(struct_message)) {
    uint8_t buffer[sizeof(struct_message)];
    int i = 0;
    while (LoRa.available() && i < sizeof(struct_message)) {
      buffer[i] = LoRa.read();
      i++;
    }
    memcpy(&recvData, buffer, sizeof(struct_message));
    recvData.text[219] = '\0'; 
    
    // STRICT ROUTING: HANYA terima data dari Node Perantara (Repeater / ID 3)
    // Abaikan jika data datang langsung dari Node 2 atau pantulan Node 1 sendiri
    if (recvData.senderId != 3) return; 

    // Abaikan PING yang dipantulkan kembali oleh Repeater (agar Node A tidak beep dua kali)
    if (recvData.msgType == 2) return;

    // Abaikan REQUEST (Data Anda sendiri) yang dipantulkan kembali oleh Repeater
    if (recvData.msgType == 0) return;

    // Bunyikan Buzzer saat menerima data
    digitalWrite(BUZZER_PIN, HIGH);
    delay(50);
    digitalWrite(BUZZER_PIN, LOW);

    if (recvData.msgType == 1) { // RESPONSE (AI)
      if (recvData.packetId == 0) memset(jsonBuffer, 0, sizeof(jsonBuffer));
      int currentLen = strlen(jsonBuffer);
      int spaceLeft = sizeof(jsonBuffer) - currentLen - 1;
      if (spaceLeft > 0) strncat(jsonBuffer, recvData.text, spaceLeft);
      
      if (recvData.packetId == recvData.totalPackets - 1) {
          // Tangkap Downlink RSSI dan SNR
          int rssi_a = LoRa.packetRssi();
          float snr_a = LoRa.packetSnr();

          String payload = String(jsonBuffer);
          int lastBrace = payload.lastIndexOf('}');
          if (lastBrace != -1) {
              payload.remove(lastBrace);
              payload += ",\"rssi_a\":" + String(rssi_a) + 
                         ",\"snr_a\":" + String(snr_a) + 
                         ",\"rssi_gw\":" + String(recvData.rssi_gw) + 
                         ",\"snr_gw\":" + String(recvData.snr_gw) + "}";
          }

          Serial.print("AI_RESPONSE:");
          Serial.println(payload); 
          isLedLocked = true;
      }
    } else if (recvData.msgType == 3) { // PONG
      String status = String(recvData.text);
      if (status.indexOf("WIFI_ERROR") != -1) Serial.println("PONG:B:WIFI_ERROR");
      else if (status.indexOf("N8N_ERROR") != -1) Serial.println("PONG:B:N8N_ERROR");
      // Cek REPEATER_READY DULU sebelum READY, agar tidak salah tangkap (karena kata READY ada di dalamnya)
      else if (status.indexOf("REPEATER_READY") != -1) Serial.println("PONG:REPEATER:READY");
      else if (status.indexOf("READY") != -1) Serial.println("PONG:B:READY");
    }
  }

  // 3. LOGIKA BUZZER DISCONNECTED ---------------------
  static unsigned long lastBuzzerLoop = 0;
  
  if (!isConnected) {
    // Buzzer beep 2x cepat (Setiap 20 detik)
    if (millis() - lastBuzzerLoop > 20000) { 
      for (int i = 0; i < 2; i++) {
        digitalWrite(BUZZER_PIN, HIGH);
        delay(50); // Beep singkat
        digitalWrite(BUZZER_PIN, LOW);
        delay(50); // Jeda antar beep
      }
      lastBuzzerLoop = millis();
    }
  } else {
    // Dipastikan mati jika sudah connected
  }

  // 4. LOGIKA SENSOR (TINGGI & BERAT) ---------------------
  // --- MODE UKUR AKTIF ---
  if (!isLedLocked && isConnected && currentMode == MODE_MEASURE && isBalitaSelected) {
    if (millis() - lastScaleRead > 250) {
      lastScaleRead = millis();

      // 4a. Baca Tinggi (Ultrasonik)
      digitalWrite(TRIG_PIN, LOW); delayMicroseconds(2);
      digitalWrite(TRIG_PIN, HIGH); delayMicroseconds(10);
      digitalWrite(TRIG_PIN, LOW);
      long duration = pulseIn(ECHO_PIN, HIGH, 30000); 
      // Konversi ke cm + koreksi offset hardware sensor (dari kalibrasi)
      float jarak_sensor = (duration * 0.0343 / 2) + SENSOR_OFFSET;

      if (duration > 0 && jarak_sensor > MIN_DIST && jarak_sensor < MAX_DIST) {
        // Rumus: Tinggi Badan = Tinggi Alat - Jarak Sensor + Offset Koreksi
        float tinggi_badan = TINGGI_ALAT - jarak_sensor + TINGGI_OFFSET;
        if (tinggi_badan < 0) tinggi_badan = 0;

        Serial.print("TINGGI:");
        Serial.println(tinggi_badan);
        lastDistance = tinggi_badan;
      }

      // 4b. Baca Berat (HX711) - Cek langsung hardware setiap saat
      if (scale.is_ready()) {
        float weight_grams = scale.get_units(20); // Rata-rata 20 bacaan (lebih akurat, sesuai kalibrasi)
        
        // === LOGIKA AUTO-TARE (DARI KALIBRASI) ===
        if (abs(weight_grams) < weightThreshold) {
          if (!isEmptyScale) {
            emptyStartTime = millis();
            isEmptyScale = true;
          }
          // Jika kosong selama lebih dari 2 detik -> Tare otomatis
          if (millis() - emptyStartTime > 2000) {
            scale.tare();
            weight_grams = 0;
          }
        } else {
          isEmptyScale = false;
        }

        // Konversi ke KG sesuai file kalibrasi
        float weight_kg = weight_grams / 1000.0;
        if (weight_kg < 0) weight_kg = 0; // Prevent negative display

        Serial.print("BERAT:");
        Serial.println(weight_kg, 2); // Kirim 2 angka di belakang koma
      }
    }
  }

  // 4. TIMEOUT --------------------------------------------
  if (isConnected && (millis() - lastHeartbeatTime > TIMEOUT_MS)) {
    isConnected = false; 
    isLedLocked = false;
  }

  delay(30); 
}