 //define PID variables
 double Setpoint_theta, Input_theta, Output_theta, Setpoint_dist, Input_dist, Output_dist;
 //specify initial PID links and tuning
-int KpT = 2;
-int KiT = 0;
-int KdT = 1;
-int KpD = 2;
-int KiD = 0;
-int KdD = 0;
+int KpT = 1.7;    //1.7
+int KiT = 0;      //0
+int KdT = 0.01;   //0.01
+int KpD = 1.1;      //1
+int KiD = 0.075;    //0
+int KdD = 0.97;   //0
 PID PID_theta(&Input_theta, &Output_theta, &Setpoint_theta, KpT, KiT, KdT, DIRECT);
 PID PID_dist(&Input_dist, &Output_dist, &Setpoint_dist, KpD, KiD, KdD, DIRECT);
 
-void getDist(double (&distArray)[4]);
-// Variables to store last state
-double lastDist0;
-double lastDist1;
-// Variables to store current distances
-double dist0;
-double dist1;
-
 //Spacing between LIDARs on vehicle
-double LIDARspacing = 20;
-
-// Power enable switches
-const int switch0 = 12;
-const int switch1 = 13;
-
-// # of LIDAR samples
-int samples = 5;
+double LIDARspacing = 22.5;
 
 // Set pi
 const float pi = 3.142;

#include <PID_v1.h>
#include <Servo.h>

#define    LIDARLite_ADDRESS   0x62          // Default I2C Address of LIDAR-Lite.
#define    RegisterMeasure     0x00          // Register to write to initiate ranging.
#define    MeasureValue        0x04          // Value to initiate ranging.
#define    RegisterHighLowB    0x8f          // Register to get both High and Low bytes in 1 call.

//set up servos
Servo steeringServo;

  PID_theta.SetMode(AUTOMATIC);
  PID_dist.SetMode(AUTOMATIC);

  // Servo range limits
  PID_theta.SetOutputLimits(-50, 50);
  PID_dist.SetOutputLimits(-50, 50);

  steeringServo.attach(steeringPin);
  motorServo.attach(motorPin);

  // Begin serial
  Serial.begin(57600);
  Serial.println("Serial begin");
  // Open and join irc bus as master
  Wire.begin();
  delay(100);

  for (int i = 0; i < 2; i++)   pinMode(lidar_pwr_en[i], OUTPUT);
  for (int i = 0; i < 2; i++) {
    for (int j = 0; j < 2; j++)  digitalWrite(lidar_pwr_en[j], LOW);
    digitalWrite(lidar_pwr_en[i], HIGH);
    while (1) {
      Wire.beginTransmission(LIDARLITE_ADDR);
      Wire.write(0x01);
      Wire.write(0xFF);
      if (Wire.endTransmission() == 0) break;
    }
  }

}

void loop() {
  now = millis();
  read_lidars();
  if (now - timestamp > 40) {
    timestamp = now;
//    dist[0] = 4.5 + dist[0] * (0.98); //scaling equation
    Input_theta = calcAngle(dist[0], dist[1]);
    Input_dist = calcPerpDist(dist[0], dist[1], Input_theta);
    Input_theta = Input_theta * 180 / pi;
//    Setpoint_theta = constrain(0.5*(Input_dist - Setpoint_dist), -45, 45);

    // Run the PID loops
    Serial.print(dist[0]);
    Serial.print(", ");
    Serial.print(dist[1]);
    Serial.print(", ");
    Serial.print(Input_theta);
    Serial.print(", ");
    Serial.println(Input_dist);
    PID_theta.Compute();
    PID_dist.Compute();

    motorServo.write(90  - 20 );
    steeringServo.write(90 + (Output_theta) - Output_dist); // + Output_theta - Output_dist
  } // (50/abs(Input_dist - Setpoint_dist)) * 
  delay(2);
}


//---------------------------------------------------------//

double calcAngle(double dist0, double dist1) {
  return  atan((dist0 - dist1) / LIDARspacing);
}

double calcPerpDist(double dist0, double dist1, double angle) {
  return ((dist0 + dist1) * cos(angle)) / 2;
}


void read_lidars() {
  //  Serial.print(lidar_state);
  switch (lidar_state) {
    case 0:
      for (int i = 0; i < 2; i++) digitalWrite(lidar_pwr_en[i], LOW);
      digitalWrite(lidar_pwr_en[enabled_lidar], HIGH);
      lidar_state = 1;
      state_cycle_count = 0;
      break;
    case 1:
      Wire.beginTransmission(LIDARLITE_ADDR);
      Wire.write(LIDARLITE_CMD_CTRL_ADDR);
      Wire.write(LIDARLITE_TRIG_VAL);
      if (Wire.endTransmission() == 0) lidar_state = 2;
      else state_cycle_count++;
      break;
    case 2:
      Wire.beginTransmission(LIDARLITE_ADDR);
      Wire.write(LIDARLITE_RANGE_ADDR);
      if (Wire.endTransmission() == 0) lidar_state = 3;
      else state_cycle_count++;
      break;
    case 3:
      if (Wire.requestFrom(LIDARLITE_ADDR, 2) >= 2) {
        uint16_t val = Wire.read() << 8 | Wire.read();
        if (val < 500 && val > 10) dist[enabled_lidar] = 0.2 * dist[enabled_lidar] + 0.8 * val;
       
        enabled_lidar = 1 - enabled_lidar;
        lidar_state = 0;
      } else state_cycle_count++;
      break;
  }
  if (state_cycle_count > 30) {
    lidar_state = 0;
    enabled_lidar = 1 - enabled_lidar;
    state_cycle_count = 0;
    Serial.println("RST");
  }
}

