int r[5],m;
int line[5]={} //line sensor pins;
int color[2][5]={{},{}} //s0,s1,s2,s3,out
#define left 1
#define right 0
#define f 60
#define F 1
#define B 0
void motors(int d1,int s1,int d2,int s2)
{
  analogWrite(2,s1*!d1);
  analogWrite(3,s1*d1);
  analogWrite(8,s2*d2);
  analogWrite(9,s2*!d2);
}
void linefollower()
{
  for (int i=0;i<5;i++)
  {
    r[i]=digitalRead(line[i]);
    m|=(r[i]<<(4-i));
  }
}
int findLine(int dir)
{
  motors(F*!dir,f,F*dir,f);
  delay(100);
  linefollower();
  while (!r[2])
  linefollower();
}
bool readCol(int sensor)
{
  pulseIn(color[sensor][4],LOW);
  int c=pulseIn(color[sensor][4]);
  return 0; // l mofrood return c green walla l2
}
void intersection(int dir)
{
  motors(0,0,0,0);
  if (readCol(dir))
  findline(dir);
  else
  {
    motors(F,f,F,f);
    delay(100);
    linefollower();
    if (!m)
    findLine(dir);
    else
    motors(F,f,F,f);
  }
}
void intersection2(int dep)
{
  motors(0,0,0,0);
  bool a=readCol(right),B=readCol(left);
  if (a && b)
  {
    motors(F,f,B,f);
    delay(200);
    while (!r[2])
    motors(F,f,B,f);
    motors(F,f,F,f);
  }
  else if (a && !b)
  {
    motors(F,f,F,f);
    delay(100);
    findLine(left);
  }
  else if (!a && b)
  {
    motors(F,f,F,f);
    delay(100);
    findLine(right);
  }
  else if (dep<3)
  {
    motors(B,f,B,f);
    delay(50);
    intersection2(dep+1);
  }
}
void setup()
{
  Serial.begin(9600);
  pinMode(2,OUTPUT);
  pinMode(3,OUTPUT);
  pinMode(8,OUTPUT);
  pinMode(9,OUTPUT);
  for (int i=0;i<5;i++)
  pinMode(line[i],INPUT);
  for (int i=0;i<4;i++)
  {
    pinMode(color[0][i],OUTPUT);
    pinMode(color[1][i],OUTPUT);
  }
  pinMode(color[0][4],INPUT);
  pinMode(color[1][4],INPUT);
}
void loop()
{
  linefollower();
  if (m==0b00000)
  motors(F,f,F,f);
  else if (m==0b00001)
  findLine(left);
  else if (m==0b00010)
  findLine(left);
  else if (m==0b00011)
  intersection(right);
  else if (m==0b00100)
  motors(F,f,F,f);
  else if (m==0b00101)
  intersection(right);
  else if (m==0b00110)
  intersection(right);
  else if (m==0b00111)
  intersection(right);
  else if (m==0b10000)
  findLine(right);
  else if (m==0b01000)
  findLine(left);
  else if (m==0b11000)
  intersection(left);
  else if (m==0b10100)
  intersection(left);
  else if (m==0b01100)
  intersection(left);
  else if (m==0b11100)
  intersection(left);
  else
  intersection2(0);
}