#include <ESP32Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
//#include <Arduino.h>
#include "DHT.h"

#define waistPin 18
#define shoulderPin 19
#define elbowPin 25
#define handPin 26
#define gripperPin 27
float angleW, angleS, angleE, angleR, angleG;
Servo Servo_waist, Servo_shoulder, Servo_elbow, Servo_hand, Servo_gripper;
// กำหนดขาที่ต่อกับ DHT22
#define DHTPIN 4      // ใช้ GPIO4 (เปลี่ยนได้ตามต้องการ)
#define DHTTYPE DHT22 // กำหนดชนิดเซ็นเซอร์
DHT dht(DHTPIN, DHTTYPE); // สร้างออบเจ็กต์ DHT
///////////////////kkkkk/////////////////////////
LiquidCrystal_I2C lcd(0x27, 20, 4); // Set the LCD address to 0x27 or 0x3F for a 16 chars and 2 line display

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  dht.begin(); // เริ่มต้นเซ็นเซอร์
  //////////////////////////K//////////////////////
  lcd.init();   
  lcd.backlight(); // Turn on the blacklight and print a message.
  lcd.setCursor(3, 0); // ไปที่ตัวอักษรที่ 0 แถวที่ 1
  lcd.print("My name is Kan");
  ///////////////////////K/////////////////////////
  Servo_waist.attach(waistPin, 500, 2500);
  Servo_shoulder.attach(shoulderPin, 500, 2500);
  Servo_elbow.attach(elbowPin, 500, 2500);
  Servo_hand.attach(handPin, 500, 2500);
  Servo_gripper.attach(gripperPin, 500, 2500);
  Serial.println("My name is Kan");
  delay(200);
}

void loop() {
  // put your main code here, to run repeatedly:
  if (Serial.available() > 0) {
      //stringPosition_check();
      String input = Serial.readStringUntil('\n'); 
      checkAngle(input, 'W', angleW);
      checkAngle(input, 'S', angleS);
      checkAngle(input, 'E', angleE);
      checkAngle(input, 'R', angleR);
      checkAngle(input, 'G', angleG);
      /////// Display ///////////////////////
      Serial.println("--------------------------");
      Serial.println("W angle:"+String(angleW));
      Serial.println("S angle:"+String(angleS));
      Serial.println("E angle:"+String(angleE));
      Serial.println("R angle:"+String(angleR));
      Serial.println("G angle:"+String(angleG));
      Serial.println("--------------------------");
      ////// LCD display 
      lcd.setCursor(0, 1); // ไปที่ตัวอักษรที่ 0 แถวที่ 1
      lcd.print("W:"+String(angleW)+" ");
      lcd.setCursor(0, 2); // ไปที่ตัวอักษรที่ 0 แถวที่ 1
      lcd.print("S:"+String(angleS)+" ");
      lcd.setCursor(0, 3); // ไปที่ตัวอักษรที่ 0 แถวที่ 1
      lcd.print("E:"+String(angleE)+" "); 
      lcd.setCursor(6, 1); // ไปที่ตัวอักษรที่ 0 แถวที่ 1
      lcd.print("R:"+String(angleR)+" ");
      lcd.setCursor(6, 2); // ไปที่ตัวอักษรที่ 0 แถวที่ 1
      lcd.print("G:"+String(angleG)+" ");
      ///////////////////K///////////////////////
	}
  Servo_waist.write(angleW);
  Servo_shoulder.write(angleS);
  Servo_elbow.write(angleE);
  Servo_hand.write(angleR);
  Servo_gripper.write(angleG);
  delay(10); // this speeds up the simulation
  DHT_Read();
}

void checkAngle(String input, char key, float &updateAngle){
	int startIndex=input.indexOf(key);
	if (startIndex!=-1){
		int endIndex=input.indexOf(' ', startIndex); // ตำแหน่งช่องว่างถัดไป
		updateAngle=input.substring(startIndex+1, endIndex).toInt();
	}
}

void stringPosition_check(){
    String input = Serial.readStringUntil('\n'); 
		int position = input.indexOf('X');  // ค้นหาตัว 'X' ในข้อความ
    Serial.println("Input command:"+String(input));
		Serial.println(position);  // พิมพ์ค่า -1
    delay(10);
}
  
void valueCheck(){
    String input = Serial.readStringUntil('\n');
		int startIndex = input.indexOf('W')+ 1; // ตำแหน่งหลังตัว 'W’
		int endIndex = input.indexOf(' ', startIndex); // ตำแหน่งช่องว่างถัดไป
		int angle = input.substring(startIndex, endIndex).toInt();
		Serial.println("Angle ของ W คือ: " + String(angle));
    delay(10);
}


void DHT_Read(){
  // หน่วงเล็กน้อยก่อนอ่านค่า (DHT22 อ่านค่าถี่เกินไปไม่ได้)
  delay(2000);

  // อ่านค่าอุณหภูมิและความชื้น
  float h = dht.readHumidity();
  float t = dht.readTemperature();      // องศาเซลเซียส
  float f = dht.readTemperature(true);  // องศาฟาเรนไฮต์

  // ตรวจสอบการอ่านค่า
  if (isnan(h) || isnan(t) || isnan(f)) {
    Serial.println("❌ อ่านค่า DHT22 ไม่สำเร็จ!");
    return;
  }

  // คำนวณ Heat Index
  float hif = dht.computeHeatIndex(f, h);
  float hic = dht.computeHeatIndex(t, h, false);

  // แสดงผล
  Serial.print("ความชื้น: ");
  Serial.print(h);
  Serial.print(" %\t");

  Serial.print("อุณหภูมิ: ");
  Serial.print(t);
  Serial.print(" °C ");
  Serial.print(f);
  Serial.print(" °F\t");

  Serial.print("Heat index: ");
  Serial.print(hic);
  Serial.println(" °C");

}