#include <QTRSensors.h>
#include <Servo.h>

const int pingPin = 1;
const int echoPin = 0;
int powerPinL= 12; //Servo (wheel)
int powerPinR= 13; //Servo (wheel)
bool obstacle = false;
const uint8_t SensorCount = 8;
uint16_t sensorValues[SensorCount];

const float speedsLeft[]= {92,93};
const float speedsRight[]= {91,90};

Servo servL;
Servo servR;
QTRSensors qtr;


void setup(){
  Serial.begin(9600);
  calibrate();
  servL.attach(powerPinL);
  servR.attach(powerPinR);
  pinMode(pingPin, OUTPUT);
  pinMode(echoPin, INPUT);
}

float pos= -1;

int lastAction= -1;
void loop(){
  uint16_t position = qtr.readLineBlack(sensorValues);
  pos= evalPos(sensorValues, pos);
  Serial.print("pos " + String(pos) + " ");


  //obstacle = detectObstacle();

  if(obstacle == true)
  {
    Serial.println("Turning");
    avoidObstacle();
    return;
  }

  if(pos > -1 and pos < 4.5){
    //Serial.println("left");
    doTurn("left",int(2*(4.5-pos)+0.5),3);
    lastAction= 0;
    return;
  }
  if(pos > -1 and pos > 4.5){
    //Serial.println("right");
    doTurn("right",int(2*(pos-4.5)+0.5),3);
    lastAction= 1;
    return;
  }

  //Serial.println("straight");
  if(lastAction == 0){doTurn("right",int(2*(pos-4.5)+0.5),4); delay(50);}
  if(lastAction == 1){doTurn("left",int(2*(4.5-pos)+0.5),4); delay(50); }
  doStraight(8);
  lastAction = -1;


  delay(20);
}




void avoidObstacle(){

 int val = 10;
 int baseVal = 4;

 //turn left
 servR.write(30);
 servL.write(92+baseVal);
 delay(750);

 //go straight
 servR.write(91-val);
 servL.write(92+val);
 delay(500);

 //turn right
 servR.write(91-baseVal);
 servL.write(180);
 delay(1000);

 //go straight
 servR.write(91-val);
 servL.write(92+val);
 delay(800);

 //turn right
 servR.write(91-baseVal);
 servL.write(180);
 delay(1000);

 //go straight
 servR.write(91-val);
 servL.write(92+val);
 delay(750);

 //turn left
 servR.write(30);
 servL.write(92+baseVal);
 delay(750);

 //go straight
 servR.write(91-val);
 servL.write(92+val);
 delay(100);

}

void doStraight(int val){
 servR.write(91-val);
 servL.write(92+val);
}

//10 degrees
void doTurn(String dir, int val, int baseVal){
  if(dir == "right"){
    servR.write(91-val-baseVal);
    servL.write(92+baseVal);
  }

  if(dir == "left"){
    servR.write(91-baseVal);
    servL.write(92+val+baseVal);
  }
}


bool detectObstacle(){
  unsigned long threshold= 1500;

  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);

  unsigned long duration = pulseIn(echoPin, HIGH);
  Serial.print(duration);
  Serial.println(" ms");

  if(duration < threshold && duration > 0){ return true; }
  return false;
}



float evalPos(int sensorValues[], float lastPos){
  int lastActiveSensor= -1;
  float numActive= 0;
  float activeSum= 0;
  int threshold= 400;

  for (uint8_t i = 0; i < SensorCount; i++){
    Serial.print(sensorValues[i]);
    Serial.print('\t');


  if(sensorValues[i] > threshold){
      if(lastActiveSensor == -1 or lastActiveSensor == i-1){
        numActive++;
        lastActiveSensor= i;
        activeSum+= i+1;

        threshold= sensorValues[i];
      }
      else{//Serial.println("Failed consec. check.");
      return -1;
      }
    }
  }

//  if(numActive <= 0){ if(lastPos > 0){return lastPos;} else{return 4.5;} }
  return float(lastActiveSensor);
  float avgPos= -1;
//  if(numActive > 0){ avgPos= activeSum / numActive; }
//    else{
//    if (lastPos < 2){
//      avgPos= 0;
//      }
//    if (lastPos > 6)
//      avgPos= 8;
//    }
  if(avgPos > 3 and avgPos < 5){ avgPos=4.5; }
  Serial.println(String(avgPos) + " " + String(activeSum) + " " + String(numActive));
  //delay(50);

  return avgPos;
}


void calibrate(){
  Serial.println("beginnning");
  qtr.setTypeRC();
  qtr.setSensorPins((const uint8_t[]){3,4, 5, 6, 7, 8, 9,10}, SensorCount);
  qtr.setEmitterPin(3);
  delay(500);

  for (uint16_t i = 0; i < 160; i++){
    if(i%40 == 0){Serial.println(i);}
    qtr.calibrate();
  }

  Serial.println("finish");
  delay(500);
}