//p158ct_LightSeekingShieldBotct
//Roam towards light & away fm shade (ct version)

#include <Servo.h>;

Servo servoL;
Servo servoR;

int pinServoL = 13;
int pinServoR = 12;

void setup()
{
  tone(4, 3000, 500);                       // play tone for 500 ms
  delay(300);                             // WHY NEED THIS??
  Serial.begin(9600);
  
  servoL.attach(pinServoL);
  servoR.attach(pinServoR);
}

void loop() 
{
  float tL = float(rcTime(8));            // range: 0 (bright) - 75k (lo light)
  float tR = float(rcTime(6));            // 8 flashlight to 55k (hand covering it)
  
  float tL2, tR2;
  int speedL, speedR;
  float percR, percL, avgRL, diffRL, avgTRL;
  float Kavg = 4.0;
  float Kdiff = 2.0;
  
  tL2 = 1/tL;                           // 1/8 - bright flashlight, ~0 = low light; 
  tR2 = 1/tR;                           // invert - so more lite = more signal
  avgTRL = 800.0*(tR2 + tL2)/2.0;       // get avg of L & R, (0 to 100.0)
  
  percR = 100*tL/(tR + tL);            // % of light to R sensor (0 - 100)
  percL = (100 - percR);               // % of light to L sensor (0 - 100)
  avgRL = (percR + percL)/2;           // avg of both L & R (0-100)
  diffRL = (percR - percL);            //-100 L lite <---> +100 R lite (-100 to +100)
  
  speedL = (Kdiff*diffRL) + (Kavg*avgTRL);        //-200 back <--> +200 fwd
  speedR = (-Kdiff*diffRL) + (Kavg*avgTRL);       // 1.9* 100 = 1900 )   + (4400 *50) = 
  
  speedL = constrain(speedL, -200, 200);
  speedR = constrain(speedR, -200, 200);
  
  // percL      percR 
  /*   
  Serial.print(percL);
  Serial.print("   ");
  Serial.print(percR);
  Serial.print("   ");
  Serial.print(avgTRL);
  Serial.print("   ");
  Serial.print(speedL);
  Serial.print("   ");
  Serial.println(speedR);
  delay(500);
  */
  
  maneuver((int)speedL, (int)speedR, 20);       // 20 = delay time s.t. freq = 50 Hz
}

long rcTime(int pin)  // measure light by measuring RC decay time
{
  pinMode(pin, OUTPUT);                     //set I/O pin to OUTPUT
  digitalWrite(pin, HIGH);                  //send HIGH to pin
  delay(1);                                 //wait to charge C
  pinMode(pin, INPUT);                      //change I/O pin to input (so you can read voltage)
  digitalWrite(pin, LOW);                   //when chg pin from output high to Input -> it adds R=10k, which messes up V reading, this cmd removes the R
  long time = micros();                     // read the current time
  while(digitalRead(pin));                  // wait for V to decay to below 2.1V
  time = micros() - time;                   // check time again ("micros()") & subtr fm old time
  return time;                              // return the decay time calculated (more t = less lite)
                                            // returns 8 (flashlite), 1.5k-2k (room lite), 16-20k covered 
}

void maneuver(int speedL, int speedR, int msTime)
{
  //1300 CW to 1700 CCW
  servoL.writeMicroseconds(1500 + speedL);    //+200 fwd, -200 back
  servoR.writeMicroseconds(1500 - speedR);    //+200 fwd, -200 back
  if(msTime == -1)
  {
    servoL.detach();
    servoR.detach();
  }
  delay(msTime);
}