#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Keypad.h>
#include <Servo.h>
#include "HX711.h"

LiquidCrystal_I2C lcd(0x27, 16, 2); 
HX711 scale;

// --- Servos ---
Servo servoDest;      // 44
Servo servoSizeBKK;   // 45
Servo servoSizePTM;   // 40
Servo servoSizeNON;   // 42
Servo servoSizeNKP;   // 38

// --- LEDs ---
const int ledRed    = 39; 
const int ledGreen  = 37; 
const int ledBlue   = 35; 
const int ledYellow = 33; 
const int ledOrange = 31; 

// --- Steppers (Step, Dir) ---
const int stepMain = 53; const int dirMain = 52; 
const int stepBKK  = 51; const int dirBKK  = 50; 
const int stepPTM  = 49; const int dirPTM  = 48; 
const int stepNON  = 47; const int dirNON  = 46; 
const int stepNKP  = 41; const int dirNKP  = 43; 

// --- Sensors & Buttons ---
const int trigPin = 8;
const int echoPin = 9;
const int btnStart = 6; 
const int btnStop  = 7; 

byte rowPins[4] = {5, 4, 3, 2}; 
byte colPins[4] = {A3, A2, A1, A0}; 
char keys[4][4] = {{'1','2','3','A'},{'4','5','6','B'},{'7','8','9','C'},{'*','0','#','D'}};
Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, 4, 4);

void setup() {
  Serial.begin(9600);
  lcd.init();
  lcd.backlight();
  
  // เริ่มต้น Load Cell (DT=10, SCK=11)
  scale.begin(10, 11);
  scale.set_scale(420.0); // ปรับค่านี้ตามการ Calibration จริง
  scale.tare(); 

  pinMode(btnStart, INPUT_PULLUP);
  pinMode(btnStop, INPUT_PULLUP);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  pinMode(ledRed, OUTPUT); pinMode(ledGreen, OUTPUT);
  pinMode(ledBlue, OUTPUT); pinMode(ledYellow, OUTPUT);
  pinMode(ledOrange, OUTPUT);

  int steps[] = {53, 51, 49, 47, 41};
  int dirs[]  = {52, 50, 48, 46, 43};
  for(int i=0; i<5; i++) { pinMode(steps[i], OUTPUT); pinMode(dirs[i], OUTPUT); }

  servoDest.attach(44);
  servoSizeBKK.attach(45); servoSizePTM.attach(40);
  servoSizeNON.attach(42); servoSizeNKP.attach(38);

  resetAll(); 
}

void loop() {
  if (digitalRead(btnStart) == LOW) {
    delay(250); 
    digitalWrite(ledRed, HIGH); 
    mainProcess();
  }
  if (digitalRead(btnStop) == LOW) { resetAll(); }
}

void mainProcess() {
  lcd.clear();
  lcd.print("Select Dest:");
  
  char key = 0;
  while (key == 0) {
    key = keypad.getKey();
    if (digitalRead(btnStop) == LOW) { resetAll(); return; }
  }

  // --- 1. ข้อมูลจังหวัดและราคาต้นทาง ---
  String pName = "";
  int pDest = 0;
  if (key == '1')      { pName = "BKK"; pDest = 20; }
  else if (key == '2') { pName = "PTM"; pDest = 25; }
  else if (key == '3') { pName = "NON"; pDest = 25; }
  else if (key == '4') { pName = "NKP"; pDest = 30; }
  else { resetAll(); return; }

  // --- 2. สายพานหลักหมุน + ไฟเขียว ---
  digitalWrite(ledGreen, HIGH); 
  runStepper(stepMain, dirMain, 5000); 

  // --- 3. ชั่งน้ำหนักและคำนวณราคา (Weight) ---
  lcd.clear(); lcd.print("Weighing...");
  float weight = (scale.is_ready()) ? scale.get_units(10) : 0;
  if (weight < 0) weight = 0;
  
  int pWeight = (weight <= 500) ? 15 : (weight <= 1000 ? 30 : 50);

  // --- 4. วัดขนาดและคำนวณราคา (Size) ---
  long dist = readUltrasonic();
  String szLabel = "";
  int pSize = 0;
  int szAngle = 0;

  if (dist < 10)      { szLabel = "S"; pSize = 10; szAngle = 45; }
  else if (dist < 25) { szLabel = "M"; pSize = 20; szAngle = 90; }
  else                { szLabel = "L"; pSize = 35; szAngle = 135; }

  // --- 5. Servo จังหวัด + ไฟน้ำเงิน ---
  digitalWrite(ledBlue, HIGH); 
  servoDest.write((key - '0') * 40);
  delay(1000);

  // --- 6. มอเตอร์จังหวัดหมุน + ไฟเหลือง ---
  digitalWrite(ledYellow, HIGH); 
  if (key == '1')      runStepper(stepBKK, dirBKK, 5000);
  else if (key == '2') runStepper(stepPTM, dirPTM, 5000); 
  else if (key == '3') runStepper(stepNON, dirNON, 5000);
  else if (key == '4') runStepper(stepNKP, dirNKP, 5000);

  // --- 7. Servo คัดขนาด + ไฟส้ม ---
  digitalWrite(ledOrange, HIGH); 
  if (key == '1')      servoSizeBKK.write(szAngle);
  else if (key == '2') servoSizePTM.write(szAngle);
  else if (key == '3') servoSizeNON.write(szAngle);
  else if (key == '4') servoSizeNKP.write(szAngle);

  // --- 8. สรุปราคาสุทธิและแสดงผล 10 วินาที ---
  int total = pDest + pWeight + pSize;

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(pName + "(" + szLabel + ")");
  lcd.setCursor(0, 1);
  lcd.print("Price: " + String(total) + " THB");

  delay(10000); // แสดงผลค้างไว้นานขึ้นตามที่ขอ
  resetAll(); 
}

void runStepper(int sPin, int dPin, int ms) {
  digitalWrite(dPin, HIGH);
  unsigned long st = millis();
  while (millis() - st < ms) {
    digitalWrite(sPin, HIGH);
    delayMicroseconds(800);
    digitalWrite(sPin, LOW);
    delayMicroseconds(800);
    if (digitalRead(btnStop) == LOW) break;
  }
}

long readUltrasonic() {
  digitalWrite(trigPin, LOW); delayMicroseconds(2);
  digitalWrite(trigPin, HIGH); delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  return pulseIn(echoPin, HIGH) * 0.034 / 2;
}

void resetAll() {
  digitalWrite(ledRed, LOW); digitalWrite(ledGreen, LOW);
  digitalWrite(ledBlue, LOW); digitalWrite(ledYellow, LOW);
  digitalWrite(ledOrange, LOW);
  servoDest.write(0);
  servoSizeBKK.write(0); servoSizePTM.write(0);
  servoSizeNON.write(0); servoSizeNKP.write(0);
  if (scale.is_ready()) scale.tare(); 
  lcd.clear();
  lcd.print("Ready to Start");
}