// LidarScanner.ino Arduino sketch
// http://www.qcontinuum.org/lidar

#include <Servo.h>
#include <LIDARLite.h>

Servo servoX;
Servo servoY;
LIDARLite lidar;

// Minimum and maximum servo angle in degrees
// Modify to avoid hitting limits imposed by pan/tilt bracket geometry
int minPosX = 40;
int maxPosX = 160;
int minPosY = 70;
int maxPosY = 120;

int lastPosX = 0;
int lastPosY = 0;
int loopCount = 0;
int posX = minPosX;
int posY = minPosY;
int radius = 0;
int lastRadius = 0;
float pi = 3.14159265;
float deg2rad = pi / 180.0;

void setup() {
  lidar.begin(0, true);
  lidar.configure(0);
  servoX.attach(2);
  servoY.attach(3);
  servoX.write(posX);
servoY.write(posY);
  Serial.begin(9600);
}

void loop() {
  for (posY = minPosY; posY <= maxPosY; posY++)               //Y軸上升至極值，完成一次掃描並停止動作
  {
    for (posX = minPosX; posX <= maxPosX; posX++)             //X軸旋轉至極值，Y軸向上旋轉一單位角度
    {
      posX = min(max(posX, minPosX), maxPosX);
      posY = min(max(posY, minPosY), maxPosY);
      bool moved = moveServos();
      loopCount += 1;

      if (loopCount % 100 == 0)    //每掃描100次重新校正一次
      {     
        radius = lidar.distance();
      }
else
      {
        radius = lidar.distance(false);
      }

      if (moved)
      {
        float azimuth = posX * deg2rad;
        float elevation = (180 - maxPosY + 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));  
      }
    }
  }
 
}