#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Adafruit_TCS34725.h>
#include <ESP32Servo.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);

Adafruit_TCS34725 tcs = Adafruit_TCS34725(
  TCS34725_INTEGRATIONTIME_50MS,
  TCS34725_GAIN_4X
);

#define SRV1_OPEN 0
#define SRV1_CLOSE 150

#define SRV2_OPEN 0
#define SRV2_CLOSE 150

#define SRV3_OPEN 0
#define SRV3_CLOSE 150


Servo srv1, srv2, srv3;

const int pinSrv1  = 18;
const int pinSrv2  = 33;
const int pinSrv3  = 32;

const int pinIr1 = 12;
const int pinIr2 = 13;
const int pinIr3 = 25;
const int pinIr4 = 26;

const int pinRelay = 19;
const int pinStart = 15;
const int pinStop  = 23;

bool running = false;
unsigned long comboTimer = 0;
bool comboActive = false;

enum State {
  IDLE,
  SELECT_MODE,
  DORONG,
  WARNA
};

State state = IDLE;

// ===== REFERENSI KALIBRASI =====
float r_merah = 0.79, g_merah = 0.23, b_merah = 0.21;
float r_biru  = 0.32, g_biru  = 0.43, b_biru  = 0.37;
float r_putih = 0.40, g_putih = 0.41, b_putih = 0.31;

String lastWarna = "";
String warna = "NULL";
String lastLine1 = "";
String lastLine2 = "";

void setup() {
  Serial.begin(115200);
  Wire.begin(21, 22);
  
  pinMode(pinRelay, OUTPUT);
  pinMode(pinStart, INPUT_PULLUP);
  pinMode(pinStop, INPUT_PULLUP);
  pinMode(pinIr1, INPUT);
  pinMode(pinIr2, INPUT);
  pinMode(pinIr3, INPUT);
  pinMode(pinIr4, INPUT);
  
  srv1.attach(pinSrv1);
  srv2.attach(pinSrv2);
  srv3.attach(pinSrv3);

  lcd.init();
  lcd.backlight();

  /*if (!tcs.begin()) {
    lcd.setCursor(0,1);
    lcd.print("Sensor Warna Error!");
    while (1);
  }*/
}

void loop() {
  if (buttonPressed(pinStop) && running) {
    running = false;
    resetAll();
    lcds("Mini Conveyor", "Dihentikan");
    Serial.print("STOPPED");
    delay(2000);
  }
  switch (state) {
    case IDLE:
      //lcds("Mini Conveyor", "Kelompok 3");
      if (buttonPressed(pinStart)) {
        running = true;
        lcds("Mini Conveyor", "Dimulai");
        Serial.print("STARTED");
      } else {
        lcds("Kelompok", "3");
      }
      if (masukMode()) {
        state = SELECT_MODE;
      }
      break;
    case SELECT_MODE:
      if (masukMode()) {
        state = IDLE;
        break;
      }
      if (buttonPressed(pinStart)) {
        lcds("Mode Gay", "ON");
      }
      break;
  }
}

bool masukMode() {

  static bool lock = false;

  if (digitalRead(pinStart) == LOW && digitalRead(pinStop) == LOW) {

    if (!lock) {
      lock = true;
      return true;
    }

  } else {
    lock = false;
  }

  return false;
}

bool buttonPressed(int pin) {

  static unsigned long lastPress[40];
  const int debounce = 50;

  if (digitalRead(pin) == LOW) {

    if (millis() - lastPress[pin] > debounce) {
      lastPress[pin] = millis();
      return true;
    }

  }

  return false;
}

void upWarna() {
  float totalR=0, totalG=0, totalB=0;
  int samples = 10;

  for(int i=0;i<samples;i++){
    uint16_t r,g,b,c;
    tcs.getRawData(&r,&g,&b,&c);
    if(c==0) continue;

    totalR += (float)r/c;
    totalG += (float)g/c;
    totalB += (float)b/c;
    delay(20);
  }

  float rn = totalR/samples;
  float gn = totalG/samples;
  float bn = totalB/samples;

  float d_merah = sqrt(pow(rn-r_merah,2)+pow(gn-g_merah,2)+pow(bn-b_merah,2));
  float d_biru  = sqrt(pow(rn-r_biru,2)+pow(gn-g_biru,2)+pow(bn-b_biru,2));
  float d_putih = sqrt(pow(rn-r_putih,2)+pow(gn-g_putih,2)+pow(bn-b_putih,2));

  String warna;
  float batas = 0.50;

  if (d_merah > batas && d_biru > batas && d_putih > batas)
    warna = "NULL";
  else if (d_merah < d_biru && d_merah < d_putih)
    warna = "MERAH";
  else if (d_biru < d_merah && d_biru < d_putih)
    warna = "BIRU";
  else
    warna = "PUTIH";
}

void pusher(int val) {
  srv1.write(val);
}

void resetAll() {
  srv1.write(SRV1_OPEN);
  srv2.write(SRV2_OPEN);
  srv3.write(SRV3_OPEN);
  lcds("System", "Restarted!");
}

void lcds(String line1, String line2) {

  if (line1 != lastLine1) {
    lcd.setCursor(0, 0);
    lcd.print("                "); // hapus baris
    lcd.setCursor(0, 0);
    lcd.print(line1);
    lastLine1 = line1;
  }

  if (line2 != lastLine2) {
    lcd.setCursor(0, 1);
    lcd.print("                ");
    lcd.setCursor(0, 1);
    lcd.print(line2);
    lastLine2 = line2;
  }

}

bool aktif(int pin) {
  return digitalRead(pin) == LOW;
}