#include <SoftwareSerial.h>

SoftwareSerial lidarSerial(4, 5);  // RX, TX

int distance = 0;

void setup() {
  Serial.begin(9600);
  lidarSerial.begin(115200);  // ความเร็ว Serial ของ TF-Luna
}

void loop() {
  static uint8_t recvData[9];
  static uint8_t dataIndex = 0;

  while (lidarSerial.available()) {
    uint8_t byteData = lidarSerial.read();

    if (dataIndex == 0 && byteData != 0x59) continue;  // Start Byte 1
    if (dataIndex == 1 && byteData != 0x59) {
      dataIndex = 0;  // Start Byte 2 ผิด ให้เริ่มต้นใหม่
      continue;
    }

    recvData[dataIndex++] = byteData;

    if (dataIndex == 9) {  // ครบ 9 ไบต์
      int checksum = 0;
      for (int i = 0; i < 8; i++) checksum += recvData[i];

      if (recvData[8] == (checksum & 0xFF)) {
        distance = recvData[2] + recvData[3] * 256;
        Serial.print("Distance: ");
        Serial.print(distance);
        Serial.println(" cm");
      } else {
       // Serial.println("Checksum Error");
      }
      dataIndex = 0;
    }
  }
}