#include <Wire.h>
#include <LIDARLite.h>


LIDARLite myLidarLite;

const float wn = 10;
int mode = -1;
int rawdata;
float coords, dist, vel, tstart;
float prev_dist, prev_vel, dtime;
unsigned long myTime, prevTime;



void setup()
{

    Serial.begin(115200);
  
  // check serial communication - acknowldegement routine
    Serial.println('a'); // sending a character to PC
    char a = 'b';
    while (a !='a')
    {
      //wait for a specific character from the pc
    a = Serial.read();
    }

  
 // Serial.begin(115200); // Initialize serial connection to display distance readings

  /*
    begin(int configuration, bool fasti2c, char lidarliteAddress)

    Starts the sensor and I2C.

    Parameters
    ----------------------------------------------------------------------------
    configuration: Default 0. Selects one of several preset configurations.
    fasti2c: Default 100 kHz. I2C base frequency.
      If true I2C frequency is set to 400kHz.
    lidarliteAddress: Default 0x62. Fill in new address here if changed. See
      operating manual for instructions.
  */
  myLidarLite.begin(0, true); // Set configuration to default and I2C to 400 kHz


  

  /*
    configure(int configuration, char lidarliteAddress)

    Selects one of several preset configurations.

    Parameters
    ----------------------------------------------------------------------------
    configuration:  Default 0.
      0: Default mode, balanced performance.
      1: Short range, high speed. Uses 0x1d maximum acquisition count.
      2: Default range, higher speed short range. Turns on quick termination
          detection for faster measurements at short range (with decreased
          accuracy)
      3: Maximum range. Uses 0xff maximum acquisition count.
      4: High sensitivity detection. Overrides default valid measurement detection
          algorithm, and uses a threshold value for high sensitivity and noise.
      5: Low sensitivity detection. Overrides default valid measurement detection
          algorithm, and uses a threshold value for low sensitivity and noise.
    lidarliteAddress: Default 0x62. Fill in new address here if changed. See
      operating manual for instructions.
  */
  myLidarLite.configure(0); // Change this number to try out alternate configurations

  prev_dist = 0;
  rawdata = myLidarLite.distance();
  prev_dist = (float)rawdata;
  dist = 0;
  prev_vel = 0.;
  vel = 0;
  prevTime = millis();

  for(int i = 0; i < 10; i++){
    myTime = millis();
    dtime = float(myTime-prevTime)/1000;
    prevTime = myTime;
    rawdata = myLidarLite.distance();
    coords = (float)rawdata;
    vel = (prev_vel + dtime*sq(wn)*(coords-prev_dist))/(1+ dtime*2*wn+sq(dtime*wn));
    dist = prev_dist + vel*dtime;
    prev_vel = vel;
    prev_dist = dist;
  }
 
}

void loop()
{



  if (Serial.available() > 0 ) 
 {

    mode = Serial.read();

   if (mode=='T'){ // Transmit data - 5 data pairs are sent per request

      for(int n=0; n<5; n++) {
        Serial.println(dist);
        //Serial.println(vel);

        for(int i = 0; i < 5; i++){
          myTime = millis();
          dtime = float(myTime-prevTime)/1000;
          prevTime = myTime;
          rawdata = myLidarLite.distance();
          coords = (float)rawdata;
          vel = (prev_vel + dtime*sq(wn)*(coords-prev_dist))/(1+ dtime*2*wn+sq(dtime*wn));
          dist = prev_dist + vel*dtime;
          prev_vel = vel;
          prev_dist = dist;
        }
      }
   }
  }
  else {
  /*
    distance(bool biasCorrection, char lidarliteAddress)

    Take a distance measurement and read the result.

    Parameters
    ----------------------------------------------------------------------------
    biasCorrection: Default true. Take aquisition with receiver bias
      correction. If set to false measurements will be faster. Receiver bias
      correction must be performed periodically. (e.g. 1 out of every 100
      readings).
    lidarliteAddress: Default 0x62. Fill in new address here if changed. See
      operating manual for instructions.
  */
  for(int i = 0; i < 10; i++){
    myTime = millis();
    dtime = float(myTime-prevTime)/1000;
    prevTime = myTime;
    rawdata = myLidarLite.distance();
    coords = (float)rawdata;
    vel = (prev_vel + dtime*sq(wn)*(coords-prev_dist))/(1+ dtime*2*wn+sq(dtime*wn));
    dist = prev_dist + vel*dtime;
    prev_vel = vel;
    prev_dist = dist;
 }

 }
}