/*
 *  YDLIDAR SYSTEM
 *  YDLIDAR Arduino 
 *
 *  Copyright 2015 - 2018 EAI TEAM
 *  http://www.eaibot.com
 * 
 * https://github.com/EAIBOT/ydlidar_arduino/tree/master
 *
 *
 *
 * modified V/2025 by Florian Schäffer, https://www.blafusel.de
 * https://github.com/blaschlumpf/ydlidar_x4pro/tree/master
 * 
 */
 
#include "YDLidar.h"
YDLidar lidar;      // You need to create an driver instance 


#include <GyverOLED.h>    // https://github.com/GyverLibs/GyverOLED     ### not the best one but small. sometimes pixel drawn around a line or boy
GyverOLED<SSD1306_128x64, OLED_NO_BUFFER> oled;

#define YDLIDAR_MOTOR_SCTP 3      // The PWM pin for control the speed of YDLIDAR's motor. 
#define YDLIDAR_BAURATE 128000        // LIDAR Baudrate
#define YDLIDAR_MINDIST 120.0         // min Distance in mm
#define YDLIDAR_MAXDIST 10000.0       // 10m
#define YDLIDAR_M_CTR_VOLT 2.0      // Motor speed control voltage. lower voltage => faster rotation

#define OLED_WIDTH 128
#define OLED_HEIGHT 64
#define OLED_Range 300.0
#define OLED_DEGREE  (OLED_WIDTH/360.0)    // pixel per degree
//#define OLED_PIX ((OLED_HEIGHT - 15) / YDLIDAR_MAXDIST)     // display height - top and bottm border per mm
#define OLED_PIX ((OLED_HEIGHT - 15) / OLED_Range)     // display only within range
#define DIST_ALERT 250        // objects in this range will throw alert

bool isScanning = false;   
unsigned long myTimer = millis();
unsigned long myTimeout = 5000;     // clear screen every ms

void setup() {
  lidar.begin(Serial, YDLIDAR_BAURATE);     // bind the YDLIDAR driver to the arduino hardware serial
  pinMode(YDLIDAR_MOTOR_SCTP, OUTPUT);      // PWM output 
  setMotorSpeed(0);     //full speed motor

  oled.init();
  oled.clear();
  oled.line (0, 3, 127, 3);
  oled.line (OLED_DEGREE * 90, 0, OLED_DEGREE * 90, 2);
  oled.line (OLED_DEGREE * 180, 0, OLED_DEGREE * 180, 2);
  oled.line (OLED_DEGREE * 270, 0, OLED_DEGREE * 270, 2);
  //oled.update();
}

void loop() 
{
  float distance = 0;
  float angle = 0;

  if(isScanning)
  {
    if (millis() > (myTimeout + myTimer)) 
    {
      oled.rect(0, 10, OLED_WIDTH-2, OLED_HEIGHT-1, 1);
      delay (10);
      oled.rect(0, 10, OLED_WIDTH-1, OLED_HEIGHT-1, 0);
      myTimer = millis();
      oled.update();

    }
    if (lidar.waitScanDot() == RESULT_OK) 
    {
        distance = lidar.getCurrentScanPoint().distance; //distance value in mm unit
        angle    = lidar.getCurrentScanPoint().angle; //anglue value in degree
        //byte  quality  = lidar.getCurrentScanPoint().quality; //quality of the current measurement
        //bool  startBit = lidar.getCurrentScanPoint().startBit;

        if ((distance > YDLIDAR_MINDIST) &&  (distance <= YDLIDAR_MAXDIST))  
        {
          //Serial.print("angle:");
          Serial.print(angle);
          //Serial.print("distance:");
          Serial.print("       ");
          Serial.print(distance);
          Serial.print(" mm       ");
          //Serial.print(quality);
          Serial.println();

          //if (distance <= OLED_Range)    // only dots within range
            oled.dot(OLED_DEGREE * angle, OLED_PIX * distance, 1);

          if (((angle >= 0) && (angle <= 30)) || ((angle >= 330) && (angle <= 360)))
          {
            if (distance <= DIST_ALERT)
            {
              oled.setCursorXY(50, 55);
              oled.print("ALERT");
            }  
          }
        }
    }
    else
    {
        //Serial.println(" YDLIDAR get Scandata failed!!");
    }
  }
  else
  {
    setMotorSpeed(0);     
    restartScan();
  }
}

void setMotorSpeed(float vol)
{
  analogWrite(YDLIDAR_MOTOR_SCTP, (uint8_t)(51*vol));
}

void restartScan()
{
  device_info deviceinfo;
  if (lidar.getDeviceInfo(deviceinfo, 100) == RESULT_OK) 
  {
    Serial.println();
    Serial.println();
    Serial.print("Modell: ");
    Serial.println(deviceinfo.model);

    uint16_t maxv = (uint16_t)(deviceinfo.firmware_version>>8);
    uint16_t midv = (uint16_t)(deviceinfo.firmware_version&0xff)/10;
    uint16_t minv = (uint16_t)(deviceinfo.firmware_version&0xff)%10;
    if(midv==0){
      midv = minv;
      minv = 0;
    }

    Serial.print("Firmware version: ");
    Serial.print(maxv,DEC);
    Serial.print(".");
    Serial.print(midv,DEC);
    Serial.print(".");
    Serial.println(minv,DEC);

    Serial.print("Hardware version: ");
    Serial.println((uint16_t)deviceinfo.hardware_version,DEC);

    Serial.print("Serial: ");
    for (int i=0;i<16;i++){
      Serial.print(deviceinfo.serialnum[i]&0xff, DEC);
    }

    Serial.println();
    Serial.println();
    delay(100);

    isScanning = true;
    
    setMotorSpeed(YDLIDAR_M_CTR_VOLT);    //start motor 
    Serial.println("Now YDLIDAR is scanning ...\r\n");
  }else
  {
    Serial.println("YDLIDAR get DeviceInfo Error!\r\n");
    setMotorSpeed(0);     
  }
}

