//my line follower code basic

#define sensorNum 8
#define maxSpeed 240
int rotationSpeed=200;



 int blackLimit[sensorNum];
 int digitalReading[sensorNum];
const int motorPin1=6,motorPin2=5;        //right motor
const int motorPin3=9,motorPin4=10;     //left motor

const int fSonarTrig = 8;
const int fSonarEcho = 7;

float error, prevError=0;
float mappedValue, targetValue = 7;      //changed ferom 4.5 to 9

float safety=0.35; //fixed

float kp=40;                             //496
float kd=3;                       //180
float kt;

int motorResponse;
float correction;

int leftSpeed,rightSpeed;
int digitalValue;
int allBlack=0;
int CRotateKey=0,ACRotateKey=0;

float lastAct;
int time=3;
int danceDelay = 350;

int prev, curr, diff;
int leftSide,rightSide;
int leftIR,rightIR;
int allBlackcount=0;
int stopThreshold = 14;
int fDistance;
int obsKey;

void setup()
{
  //initialize for IR pins
  for(int i = 0; i < sensorNum; i++)
  {
    pinMode(A0 + i, INPUT);
  }

  //initialize motor pins
  pinMode(motorPin1, OUTPUT);
  pinMode(motorPin2, OUTPUT);
  pinMode(motorPin3, OUTPUT);
  pinMode(motorPin4, OUTPUT);
  delay(1000);
calibration();

  Serial.begin(9600);
}


void loop() {

      //sensorRead();
sensorMapping();
Serial.println(mappedValue);

fDistance = trigger(fSonarTrig, fSonarEcho);

if(fDistance < 10)

{
  obsKey = 1;
  for(int i = 0; i < 3; i++)
  {

    fDistance = trigger(fSonarTrig, fSonarEcho);
    if(fDistance > 10)
    {
      obsKey = 0;
    }
  }
}


while(obsKey == 1)
{
  brake();
  fDistance = trigger(fSonarTrig, fSonarEcho);
  if(fDistance > 10)
  {
    obsKey = 0;
  }
}

 if(allBlack == 1)            //depend upon track for acute angle
{
  //rotate
  allBlackcount++;             //  ive to check it for my bot
}
else
{
  allBlackcount = 0;
}


if(allBlackcount > stopThreshold)
{
  brake();
}

if(leftSide == 1)
{
  plannedACRotate();
  delay(500);
}

                            //ei tukun ei sudhu



if(mappedValue != 100)
{
  pid();
  motor(leftSpeed, rightSpeed);
}

else
{
  if(leftIR == 0 && rightIR == 1)
  {
    plannedCRotate();
    while(digitalReading[3] != 1)
    {
      sensorMapping();
    }
    rightIR = 0;
    pid();
    motor(leftSpeed, rightSpeed);
  }

  else if(leftIR == 1 && rightIR == 0)
  {
     plannedACRotate();
         while(digitalReading[3] != 1)
    {
      sensorMapping();
    }
    leftIR = 0;
    pid();
    motor(leftSpeed, rightSpeed);
  }

  else if (leftIR == 0 && rightIR == 0)
  {
    motor(maxSpeed, maxSpeed);
  }
}



}



void sensorMapping()
{
  int sum=0,count=0;
  for(int i = 0; i < sensorNum; i++)
  {
    if(analogRead(i) < blackLimit[i])
    {

      sum += i*2;
      count++;

      digitalReading[i] = 1;
    }

    else
    {
      digitalReading[i] = 0;
    }

  }

  if(count != 0)
  {
    mappedValue = sum/count;
  }
  else
    mappedValue = 100;

  if(digitalReading[0] || digitalReading[sensorNum - 1])
  {
    leftIR = digitalReading[0];
    rightIR = digitalReading[sensorNum-1];
  }

  if(count == sensorNum)
  {
    allBlack = 1;
  }
  else
allBlack = 0;


//loop case
if(digitalReading[3] || digitalReading [4] && digitalReading[7]
{ rightSide= 1;
}

 //turn full right or left depends upon track
   /* if(digitalReading[0] || digitaReading[1] || digitalReading[2])
  {
    leftSide = 1;
  }
  else
    leftSide = 0;
             if(digitalReading[5] || digitaReading[6] || digitalReading[7])
  {
    rightSide = 1;
  }
  else
    rightSide = 0;

*/

}




void pid()
{

  error=targetValue-mappedValue;
  correction=(kp*error)+(kd*(error-prevError));
  prevError=error;
  motorResponse=(int)correction;

 if(motorResponse>maxSpeed) motorResponse=maxSpeed;

if(motorResponse<-maxSpeed) motorResponse=-maxSpeed;

   if(motorResponse>0)
  {
    rightSpeed=maxSpeed;
    leftSpeed=maxSpeed-motorResponse;
  }
  else
  {
    rightSpeed=maxSpeed+ motorResponse;
    leftSpeed=maxSpeed;
  }

}




void motor(int left, int right)
{

  if(right>0)
  {
  analogWrite(motorPin1,right);
  analogWrite(motorPin2,0);
  }
  else
  {
    analogWrite(motorPin1,0);
    analogWrite(motorPin2,-right);
  }

  if(left>0)
  {
  analogWrite(motorPin3,left);
  analogWrite(motorPin4,0);
  }
  else
  {
   analogWrite(motorPin3,0);
   analogWrite(motorPin4,-left);
  }

 }



void brake(void)
{
  analogWrite(motorPin1, 0);
  analogWrite(motorPin2, 0);
  analogWrite(motorPin3, 0);
  analogWrite(motorPin4, 0);
}

void plannedACRotate()
{
  analogWrite(motorPin1,rotationSpeed);
  analogWrite(motorPin2, 0);
  analogWrite(motorPin3, 0);
  analogWrite(motorPin4,rotationSpeed);

}

void plannedCRotate()
{
  analogWrite(motorPin1,0);
  analogWrite(motorPin2, rotationSpeed);
  analogWrite(motorPin3, rotationSpeed);
  analogWrite(motorPin4,0);

}



/*void dance(void)
{

int loopCounter = (int) (danceDelay / diff);

  for(int i = loopCounter; i > 0; i--)
  {

  plannedCRotate();

  sensorMapping();

   if(mappedValue != 100)
   {
    pid();
    motor(leftSpeed, rightSpeed);
    return;
   }

  }

  for(int i = 2 * loopCounter; i > 0; i--)
  {
  plannedACRotate();

  sensorMapping();

   if(mappedValue != 100)
   {
    pid();
    motor(leftSpeed, rightSpeed);
    return;
   }

  }

} */


         //auto calibration
     void calibration()
{
  plannedCRotate();
  float upSum = 0,lowSum = 0;
  int sensorArray[sensorNum][2];

  for(int i = 0; i < sensorNum; i++)
    {
      sensorArray[i][0] = analogRead(A0+i);
      sensorArray[i][1] = analogRead(A0+i);
    }


  int loopCounter = (int)(time * 1000 / 2.5);
  while(loopCounter)
  {
    for(int i = 0; i < sensorNum; i++)
    {
      if(analogRead(A0+i)<sensorArray[i][0]) sensorArray[i][0]=analogRead(A0+i);
      if(analogRead(A0+i)>sensorArray[i][1]) sensorArray[i][1]=analogRead(A0+i);
    }
  loopCounter--;

  }

 for(int i=0; i < sensorNum; i++)
  blackLimit[i] = (int)(sensorArray[i][0] + safety * (sensorArray[i][1] - sensorArray[i][0]));
prev = millis();
sensorMapping ();
curr= millis();
diff = curr - prev;

  brake();
  delay(1000);

}



//sonar functions
long mstocm(long microseconds)
{

  return (microseconds*346.3)/2/10000;
}


int trigger(int trigPin,int echopin)
{
  digitalWrite(trigPin, LOW);

  digitalWrite(trigPin, HIGH);

  digitalWrite(trigPin, LOW);
  int distance =  mstocm(pulseIn(echopin, HIGH));
  return distance;
}