// LidarScanner code
// The original codes was created by  http://www.qcontinuum.org/lidar
//Modified by Sangay Penjor for fabacademy 2022 final project from Bhutan
#include <SoftwareSerial.h> //header file of software serial port
#include <Servo.h>

SoftwareSerial Serial1(3,2); //define software serial port name as Serial1 and define pin2 as RX and pin3 as TX
  int dist; //actual distance measurements of LiDAR
  int strength; //signal strength of LiDAR
  int check; //save check value
  int i;
  int uart[9]; //save data measured by LiDAR
  const int HEADER=0x59; //frame header of data package

int loopCount = 0;
int radius = 0;
int lastRadius = 0;

//LIDARLite lidar;
Servo servoX;  // create servo object to control a servo
Servo servoY;  // create servo object to control a servo
// twelve servo objects can be created on most boards 

int posX = 180;    // variable to store the servo position
int posY = 0;


int buttonPin = A0;
int buttonState = 0;
boolean scanning = false;

float pi = 3.14159265;
float deg2rad = pi / 180.0;

void setup() {
   pinMode(buttonPin, INPUT);
  servoX.attach(4);  // attaches the servo on pin 5 to the servo object
  servoY.attach(5);  // attaches the servo on pin 4 to the servo object
  Serial.begin(9600); //set bit rate of serial port connecting Arduino with computer
  Serial1.begin(115200); //set bit rate of serial port connecting LiDAR with Arduino
}

void loop() {
  buttonState = analogRead(buttonPin);
  if (buttonState > 100){
    scanning = true;
    //Serial.println("Button Pressed, Moving to Scanning Mode");
     }

  while (scanning){
   // Serial.println("Scan Started ....");
    servoHoming(); // change the values of Max and Min Position to set home
    servoRun();
    scanningLidar();
    delay(1000);
    
    }
 // Serial.println("Scanning Not started Yet, Press start Button to SCAN");

 } 
// function called at the begining of buttonPush to go to HOME position
void servoHoming(){
  posX = 180;  //homepoint/startpoint X
  posY = 0; //homepoint/startpoint Y
  servoX.write(posX);
  delay(200);
  servoY.write(posY);
  delay(200);
}

//Main code to run the motors and the scanning
void servoRun(){
  for (posY = 0; posY <= 120; posY += 2) { // goes from 0 degrees to 180 degrees // in steps of 1 degree
    for (posX = 0; posX <= 180; posX += 3) { // goes from 180 degrees to 0 degrees
      servoX.write(posX);              // tell servo to go to position in variable 'pos'
     // Serial.println(posX);
      servoY.write(posY);              // tell servo to go to position in variable 'pos'
     // Serial.println(posY);
     scanningLidar();
     delay(100);                       // waits 200 ms for the servo to complete one scan
    }
  //Serial.println("One Set of XY Scan Complete");  //prints everytime one Axis scan is complete 
  }
 // Serial.println("All Sets of XY Scan Complete"); 
  scanning = false; 
  servoHoming();
  //Serial.println("All Scan Complete, Moving to HOME");
}

//scanning function called by servoRun Funtion.
void scanningLidar()
{
   if (Serial1.available()) { //check if serial port has data input
   if(Serial1.read() == HEADER) { //assess data package frame header 0x59
    uart[0]=HEADER;
      if (Serial1.read() == HEADER) { //assess data package frame header 0x59
        uart[1] = HEADER;
          for (i = 2; i < 9; i++) { //save data in array 
            uart[i] = Serial1.read();
            }
       check = uart[0] + uart[1] + uart[2] + uart[3] + uart[4] + uart[5] + uart[6] + uart[7];
       if (uart[8] == (check & 0xff)){ //verify the received data as per protocol
       radius = uart[2] + uart[3] * 256; //calculate distance value
       //Serial.print("radius = ");
      // Serial.print(radius); //output measure distance value of LiDAR
      // Serial.print('\t');
        }
      }
    }
    //Serial.println("Taking Measurements from Lidar");
    TakeMeasurement();
    delay(200);
  }
}

//Measurement from lidar called everytime the motor moves by one angle.
void TakeMeasurement(){
  if (abs(radius - lastRadius) > 2)
    {
      lastRadius = radius;
      }
      
  
  if (scanning) {
    float azimuth = posX * deg2rad;
    float elevation = posY * deg2rad;
    double x = radius * sin(elevation) * cos(azimuth);
    double y = radius * sin(elevation) * sin(azimuth);
    double z = radius * cos(elevation);
    Serial.println(String(-x, 5) + " " + String(y, 5) + " " + String(-z, 5));
    }
  }

  //Thanks to Suhas Labade, Rico, Kamal and Tenzin for helping to code