#include <SoftwareSerial.h>
#include <TimerOne.h>

SoftwareSerial mySerial(10, 11);

const int pingPin = 12;  // ขาเชื่อมต่อส่งสัญญาณจาก Ultrasonic
const int inPin = 13;    // ขาเชื่อมต่อรับสัญญาณจาก Ultrasonic
#define RPWM 5
#define LPWM 6
int h1 = 1;
bool systemActive = true;  // ตัวแปรสำหรับเปิด/ปิดระบบ
int motorSpeed1 = 255; // ความเร็วของมอเตอร์ (ค่าตั้งแต่ 0 ถึง 255)
int motorSpeed2 = 0;

volatile unsigned long timerCounter = 0; // ตัวแปรสำหรับนับเวลา
const long interval = 100000; // หน่วงเวลา 100 ms (100,000 ไมโครวินาที)
bool shouldUpdate = false; // ตัวแปรตรวจสอบการอัปเดต

void setup() {
  Serial.begin(9600);  // เริ่มการเชื่อมต่อ Serial
  mySerial.begin(9600);
  
  pinMode(RPWM, OUTPUT);  // กำหนดขา PWM สำหรับควบคุมมอเตอร์
  pinMode(LPWM, OUTPUT);
  
  // สั่งให้มอเตอร์หยุดในตอนเริ่มต้น
  analogWrite(RPWM, 0);  // ใช้ analogWrite แทน digitalWrite
  analogWrite(LPWM, 0);

  Timer1.initialize(interval); // ตั้งค่า Timer1 ให้หน่วงเวลา
  Timer1.attachInterrupt(timerIsr); // กำหนด ISR สำหรับ Timer1
}

void loop() {
  // ตรวจสอบว่าระบบเปิดทำงานหรือไม่
  if (systemActive && shouldUpdate) {
    shouldUpdate = false; // รีเซ็ตการอัปเดต
    long cm = getDistance();  // เรียกใช้ฟังก์ชันเพื่อวัดระยะทาง

    if (cm <= 5) {  // ถ้าระยะทางน้อยเท่ากว่า 5 cm
      // สั่งให้มอเตอร์หยุด
      analogWrite(RPWM, 0);
      analogWrite(LPWM, 0);
      
      mySerial.write(h1);  // ส่งข้อมูลผ่าน SoftwareSerial

      systemActive = false;  // ปิดระบบ (หยุดการทำงานของ Ultrasonic sensor และมอเตอร์)
    } else {
      // สั่งให้มอเตอร์หมุนไปด้านหน้า 
      analogWrite(RPWM, 0);  // ตั้งให้มอเตอร์หมุนไปด้านหน้า (LOW ขา RPWM)
      analogWrite(LPWM, motorSpeed1);  // กำหนดความเร็วที่ขา LPWM
    }

    Serial.print(cm);  // แสดงระยะทาง
    Serial.print(" cm");
    Serial.println();
  }
  
  // ตรวจสอบว่ามีข้อมูลเข้ามาผ่าน SoftwareSerial หรือไม่
  if (mySerial.available()) {
    char received = mySerial.read();  // อ่านข้อมูลที่ได้รับ
    if (received == 'R') {  // ถ้าข้อมูลที่ได้รับคือ 'R' ให้เปิดระบบใหม่
      systemActive = true;  // เปิดระบบ
    }
  }
}

// ฟังก์ชัน ISR ที่จะถูกเรียกทุกครั้งที่ Timer1 หมดเวลา
void timerIsr() {
  timerCounter++; // นับเวลาเพิ่มขึ้น
  if (timerCounter >= 1) { // ตรวจสอบเวลาที่หน่วง (1 สำหรับ 100 ms)
    shouldUpdate = true; // อนุญาตให้ทำการอัปเดต
    timerCounter = 0; // รีเซ็ตตัวนับ
  }
}

// ฟังก์ชันสำหรับวัดระยะทางโดยใช้ Ultrasonic sensor
long getDistance() {
  long duration, cm;

  // ส่งสัญญาณเสียงจาก ultrasonic
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);

  // รับสัญญาณตอบกลับ
  pinMode(inPin, INPUT);
  duration = pulseIn(inPin, HIGH);  // วัดเวลาที่สัญญาณเดินทางไปกลับ

  // คำนวณระยะทางเป็นเซนติเมตร
  cm = microsecondsToCentimeters(duration);

  return cm;  // ส่งค่าระยะทางกลับไปยังฟังก์ชันที่เรียก
}

// ฟังก์ชันสำหรับคำนวณระยะทางจากเวลาที่วัดได้
long microsecondsToCentimeters(long microseconds) {
  // ความเร็วของเสียงในอากาศคือ 340 m/s หรือ 29 ไมโครวินาทีต่อเซนติเมตร
  // การเดินทางไปกลับของสัญญาณเสียงคือสองเท่าของระยะทางจริง ดังนั้นต้องหารด้วย 2
  return microseconds / 29 / 2;
}