#include <WiFi.h>
#include <HTTPClient.h>
#include <ESP32Servo.h>
#include "HX711.h"

// =====================================================
// ================= WIFI ==============================
// =====================================================

const char* ssid     = "Fridho";
const char* password = "123456789.";

String serverURL = "http://10.10.1.112:5000";
String camURL    = "http://192.168.137.224";

// =====================================================
// ================= MOTOR =============================
// =====================================================

#define IN1 12
#define IN2 13
#define ENA 14

#define IR_START 5
#define IR_STOP  4

#define PWM_CHANNEL 7

bool motorOn   = false;
bool lastStart = HIGH;
bool lastStop  = HIGH;

int speedMotor = 200;

// =====================================================
// ================= LOADCELL ==========================
// =====================================================

#define DOUT 18
#define CLK  19

HX711 scale;

float calibration_factor = 942.0;
float stableWeight = 0;

// =====================================================
// ================= ROBOT ARM =========================
// =====================================================

Servo servo1, servo2, servo3, servo4, servo5, servo6;

#define SERVO1_PIN 25
#define SERVO2_PIN 26
#define SERVO3_PIN 27
#define SERVO4_PIN 33
#define SERVO5_PIN 22
#define SERVO6_PIN 21

int currentPos[6] = {137, 160, 90, 70, 55, 90};

// =====================================================
// ================= SERVO =============================
// =====================================================

void writeServo(int num, int ang) {

  switch (num) {

    case 0: servo1.write(ang); break;
    case 1: servo2.write(ang); break;
    case 2: servo3.write(ang); break;
    case 3: servo4.write(ang); break;
    case 4: servo5.write(ang); break;
    case 5: servo6.write(ang); break;
  }
}

float easing(float t) {

  return t * t * t * (t * (6 * t - 15) + 10);
}

// =====================================================
// ================= MOVE ALL ==========================
// =====================================================

void moveAll(int target[6], int duration) {

  int start[6];

  for (int i = 0; i < 6; i++) {
    start[i] = currentPos[i];
  }

  int steps = duration / 20;

  if (steps < 1) steps = 1;

  for (int s = 0; s <= steps; s++) {

    float t = easing((float)s / steps);

    for (int i = 0; i < 6; i++) {

      int pos = start[i] + (target[i] - start[i]) * t;

      writeServo(i, pos);

      currentPos[i] = pos;
    }

    delay(20);
  }
}

// =====================================================
// ================= MOVE SEQUENTIAL ===================
// =====================================================

void moveSequential(int target[6]) {

  for (int i = 1; i <= 5; i++) {

    int temp[6];

    for (int j = 0; j < 6; j++) {
      temp[j] = currentPos[j];
    }

    temp[i] = target[i];

    moveAll(temp, 500);
  }

  int temp[6];

  for (int j = 0; j < 6; j++) {
    temp[j] = currentPos[j];
  }

  temp[0] = target[0];

  moveAll(temp, 800);
}

// =====================================================
// ================= POSISI ============================
// =====================================================

void posisiAwal() {

  int t[6] = {137, 160, 90, 70, 55, 90};

  moveAll(t, 1200);
}

void posisiMati() {

  int t[6] = {137, 160, 60, 70, 55, 90};

  moveAll(t, 1200);
}

void posisiAntara() {

  int t[6] = {137,170,75,85,55,90};

  moveSequential(t);
}

// =====================================================
// ================= PROSES ROBOT ======================
// =====================================================

void prosesRobot(char g) {

  unsigned long tStart = millis();

  posisiAwal();

  delay(300);

  // =================================================
  // POSISI AMBIL
  // =================================================

  int ambil_open[6] = {137,165,40,95,55,90};

  moveAll(ambil_open, 1200);

  delay(300);

      // =================================================
     // CAPIT TELUR
     // =================================================
    
    int servo4Grip = 95;
    
    // KHUSUS TL LEBIH NUNDUK
    if (g == 'T') {
      servo4Grip = 100;
    }
    
    int grip[6] = {
      137,
      165,
      40,
      servo4Grip,
      35,
      110
    };
    
    moveAll(grip, 700);

  // =================================================
  // ANGKAT TELUR
  // =================================================

  int angkat[6] = {137,170,75,85,35,110};

  moveAll(angkat, 1200);

  delay(300);

  unsigned long tAmbil = millis();

  // =================================================
  // GERAK KE GRADE
  // =================================================

  int baseMove[6];

  for (int i = 0; i < 6; i++) {
    baseMove[i] = currentPos[i];
  }

  if      (g == 'A') baseMove[0] = 80;
  else if (g == 'B') baseMove[0] = 50;
  else if (g == 'C') baseMove[0] = 10;
  else if (g == 'T') baseMove[0] = 180;

  int selisih = abs(currentPos[0] - baseMove[0]);

  int durasiGerak = selisih * 25;

  if (durasiGerak < 1200) {
    durasiGerak = 1200;
  }

  moveAll(baseMove, durasiGerak);

  delay(300);

  // =================================================
  // TURUNKAN TELUR
  // =================================================

  int turun[6] = {
    baseMove[0],
    130,
    60,
    50,
    35,
    110
  };

  moveAll(turun, 1400);

  delay(500);

  // =================================================
  // BUKA CAPIT
  // =================================================

  int lepas[6] = {
    baseMove[0],
    130,
    60,
    50,
    55,
    90
  };

  moveAll(lepas, 1500);

  delay(500);

  unsigned long tTaruh = millis();

  // =================================================
  // NAIK DULU
  // =================================================

  int amanBalik[6] = {
    baseMove[0],
    170,
    75,
    85,
    55,
    90
  };

  moveAll(amanBalik, 1400);

  delay(300);

  // =================================================
  // BALIK BASE
  // =================================================

  int baseBalik[6];

  for (int i = 0; i < 6; i++) {
    baseBalik[i] = amanBalik[i];
  }

  baseBalik[0] = 137;

  int selisihBalik = abs(currentPos[0] - baseBalik[0]);

  int durasiBalik = selisihBalik * 25;

  if (durasiBalik < 1200) {
    durasiBalik = 1200;
  }

  moveAll(baseBalik, durasiBalik);

  delay(300);

  // =================================================
  // POSISI STANDBY
  // =================================================

  int standby[6] = {
    137,
    160,
    90,
    70,
    55,
    90
  };

  moveAll(standby, 1500);

  delay(300);

  unsigned long tSelesai = millis();

  // =================================================
  // OUTPUT TIMER
  // =================================================

  float waktuAmbil =
    (tAmbil - tStart) / 1000.0;

  float waktuSortir =
    (tTaruh - tAmbil) / 1000.0;

  float waktuBalik =
    (tSelesai - tTaruh) / 1000.0;

  float totalWaktu =
    (tSelesai - tStart) / 1000.0;

  Serial.println("=================================");

  Serial.print("Grade Tujuan : ");
  Serial.println(g);

  Serial.print("Waktu ambil telur : ");
  Serial.print(waktuAmbil, 2);
  Serial.println(" detik");

  Serial.print("Waktu sortir + jatuhkan : ");
  Serial.print(waktuSortir, 2);
  Serial.println(" detik");

  Serial.print("Waktu balik standby : ");
  Serial.print(waktuBalik, 2);
  Serial.println(" detik");

  Serial.print("Total waktu robot : ");
  Serial.print(totalWaktu, 2);
  Serial.println(" detik");

  Serial.println("=================================");
}

// =====================================================
// ================= WIFI ==============================
// =====================================================

void connectWiFi() {

  WiFi.begin(ssid, password);

  Serial.print("Connecting WiFi");

  while (WiFi.status() != WL_CONNECTED) {

    delay(500);
    Serial.print(".");
  }

  Serial.println("");
  Serial.println("WiFi Connected!");
}

// =====================================================
// ================= AMBIL SPEED =======================
// =====================================================

void ambilSpeed() {

  if (WiFi.status() != WL_CONNECTED) return;

  HTTPClient http;

  http.begin(serverURL + "/getSpeed");

  int code = http.GET();

  if (code == 200) {

    String payload = http.getString();

    int idx = payload.indexOf(":");
    int end = payload.indexOf("}");

    if (idx != -1 && end != -1) {

      int newSpeed =
        payload.substring(idx + 1, end).toInt();

      if (newSpeed > 0 && newSpeed != speedMotor) {

        speedMotor = newSpeed;

        Serial.print("Speed update: ");
        Serial.println(speedMotor);
      }
    }
  }

  http.end();
}

// =====================================================
// ================= TRIGGER CAMERA ====================
// =====================================================

void triggerKamera() {

  if (WiFi.status() != WL_CONNECTED) return;

  HTTPClient http;

  http.begin(camURL + "/capture");

  http.setTimeout(8000);

  int code = http.GET();

  Serial.print("[CAM] Trigger status: ");
  Serial.println(code);

  http.end();
}

// =====================================================
// ================= JSON HELPER =======================
// =====================================================

String cariString(String body, String key) {

  String cari = "\"" + key + "\"";

  int idx = body.indexOf(cari);

  if (idx == -1) return "";

  int start =
    body.indexOf("\"", idx + cari.length() + 1);

  if (start == -1) return "";

  int end = body.indexOf("\"", start + 1);

  if (end == -1) return "";

  return body.substring(start + 1, end);
}

int cariInt(String body, String key) {

  String cari = "\"" + key + "\"";

  int idx = body.indexOf(cari);

  if (idx == -1) return -1;

  int pos = idx + cari.length();

  while (
    pos < (int)body.length() &&
    (body[pos] == ':' || body[pos] == ' ')
  ) pos++;

  String numStr = "";

  while (
    pos < (int)body.length() &&
    (isDigit(body[pos]) || body[pos] == '-')
  ) {

    numStr += body[pos];
    pos++;
  }

  if (numStr.length() == 0) return -1;

  return numStr.toInt();
}

// =====================================================
// ================= POLLING ===========================
// =====================================================

bool pollingHasilKamera(
  int &telurId,
  String &aiGrade
) {

  if (WiFi.status() != WL_CONNECTED) return false;

  unsigned long start = millis();

  while (millis() - start < 12000) {

    HTTPClient http;

    http.begin(serverURL + "/latest");

    http.setTimeout(5000);

    int code = http.GET();

    if (code == 200) {

      String body = http.getString();

      int id = cariInt(body, "telur_id");

      String grade = cariString(body, "grade");

      if (id > 0 && grade.length() > 0) {

        telurId = id;
        aiGrade = grade;

        http.end();

        return true;
      }
    }

    http.end();

    delay(500);
  }

  return false;
}

// =====================================================
// ================= UPDATE FINAL ======================
// =====================================================

String updateFinal(
  int telurId,
  float berat
) {

  if (WiFi.status() != WL_CONNECTED) return "TL";

  HTTPClient http;

  http.begin(serverURL + "/updateFinal");

  http.addHeader(
    "Content-Type",
    "application/json"
  );

  String json =
    "{\"id\":" + String(telurId) +
    ",\"berat\":" + String(berat, 2) + "}";

  int code = http.POST(json);

  String finalGrade = "C";

  if (code == 200) {

    String body = http.getString();

    String fg = cariString(body, "final");

    if (fg.length() > 0)
      finalGrade = fg;
  }

  http.end();

  return finalGrade;
}

// =====================================================
// ================= TIMBANG ===========================
// =====================================================

void timbangTelur() {

  float smoothed = 0;

  bool first = true;

  unsigned long startTime = millis();

  while (millis() - startTime < 3000) {

    float berat = scale.get_units(10);

    if (berat < 0)
      berat = -berat;

    if (first) {

      smoothed = berat;
      first = false;
    }

    else {

      smoothed =
        0.8 * berat +
        0.2 * smoothed;
    }

    delay(60);
  }

  stableWeight = smoothed;
}

// =====================================================
// ================= SETUP =============================
// =====================================================

void setup() {

  Serial.begin(115200);

  connectWiFi();

  // MOTOR

  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);

  pinMode(IR_START, INPUT);
  pinMode(IR_STOP, INPUT);

  ledcSetup(PWM_CHANNEL, 1000, 8);

  ledcAttachPin(ENA, PWM_CHANNEL);

  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);

  ledcWrite(PWM_CHANNEL, 0);

  // LOADCELL

  scale.begin(DOUT, CLK);

  while (!scale.is_ready()) {

    Serial.println("Menunggu HX711...");
    delay(500);
  }

  delay(2000);

  scale.set_scale(calibration_factor);

  scale.tare();

  // SERVO

  servo1.attach(SERVO1_PIN);
  servo2.attach(SERVO2_PIN);
  servo3.attach(SERVO3_PIN);
  servo4.attach(SERVO4_PIN);
  servo5.attach(SERVO5_PIN);
  servo6.attach(SERVO6_PIN);

  delay(2000);

  for (int i = 0; i < 6; i++) {
    writeServo(i, currentPos[i]);
  }

  delay(1000);

  posisiMati();

  delay(1000);

  posisiAwal();

  Serial.println("=== SISTEM SIAP ===");
}

// =====================================================
// ================= LOOP ==============================
// =====================================================

void loop() {

  ambilSpeed();

  bool startVal = digitalRead(IR_START);
  bool stopVal  = digitalRead(IR_STOP);

  // =================================================
  // IR START
  // =================================================

  if (
    lastStart == HIGH &&
    startVal == LOW &&
    !motorOn
  ) {

    Serial.println("IR1 → Conveyor ON");

    motorOn = true;

    ledcWrite(PWM_CHANNEL, speedMotor);

    delay(200);
  }

  // =================================================
  // IR STOP
  // =================================================

  if (
    lastStop == HIGH &&
    stopVal == LOW &&
    motorOn
  ) {

    Serial.println("IR2 → Conveyor OFF");

    motorOn = false;

    ledcWrite(PWM_CHANNEL, 0);

    delay(800);

    // =================================================
    // TIMBANG
    // =================================================

    timbangTelur();

    Serial.print("Berat: ");
    Serial.println(stableWeight);

    // =================================================
    // TRIGGER CAMERA
    // =================================================

    triggerKamera();

    delay(2500);

    // =================================================
    // POLLING HASIL AI
    // =================================================

    int telurId = -1;

    String aiGrade = "";

    bool berhasil =
      pollingHasilKamera(
        telurId,
        aiGrade
      );

    char finalChar;

    // =================================================
    // JIKA AI BERHASIL
    // =================================================

    if (berhasil) {

      // =============================================
      // OUTPUT HASIL AI
      // =============================================

      Serial.println("=================================");

      Serial.print("ID Telur     : ");
      Serial.println(telurId);

      Serial.print("Grade AI     : ");
      Serial.println(aiGrade);

      Serial.print("Berat Telur  : ");
      Serial.print(stableWeight);
      Serial.println(" gram");

      // =============================================
      // FINAL GRADE
      // =============================================

      String finalGrade =
        updateFinal(
          telurId,
          stableWeight
        );

      Serial.print("Final Grade  : ");
      Serial.println(finalGrade);

      Serial.println("=================================");

      // =============================================
      // KONVERSI FINAL GRADE
      // =============================================

      if      (finalGrade == "A")
        finalChar = 'A';

      else if (finalGrade == "B")
        finalChar = 'B';

      else if (finalGrade == "C")
        finalChar = 'C';

      else
        finalChar = 'T';
    }

    // =================================================
    // JIKA AI GAGAL
    // =================================================

    else {

      Serial.println("=================================");
      Serial.println("AI Tidak Merespon");
      Serial.println("Menggunakan Berat Saja");

      Serial.print("Berat Telur : ");
      Serial.print(stableWeight);
      Serial.println(" gram");

      if      (stableWeight >= 70)
        finalChar = 'A';

      else if (stableWeight >= 60)
        finalChar = 'B';

      else
        finalChar = 'C';

      Serial.print("Final Grade : ");
      Serial.println(finalChar);

      Serial.println("=================================");
    }

    // =================================================
    // ROBOT ARM
    // =================================================

    prosesRobot(finalChar);

    delay(1000);
  }

  lastStart = startVal;
  lastStop  = stopVal;

  delay(10);
}