int r[5],m,t;
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));
  }
}
void 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],HIGH);
  pulseIn(color[sensor][4],LOW);
  int c=pulseIn(color[sensor][4],HIGH);
  return 0; // l mofrood return c green walla l2
}
void intersection(int dir)
{
  motors(0,0,0,0);
  if (readCol(dir))
  {
    motors(F,f,F,f);
    delay(100);
    findLine(dir);
  }
  else
  {
    motors(F,f,F,f);
    delay(100);
    linefollower();
    if (!m)
    findLine(dir);
    else
    motors(F,f,F,f);
  }
}
bool intersection2(int dep)
{
  bool found=1;
  motors(0,0,0,0);
  bool a=readCol(right),b=readCol(left);
  if (a && b)
  findLine(right);
  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)
    return 0;
    motors(B,f,B,f);
    delay(50);
    found=intersection2(dep+1);
  }
  if (!dep && !found)
  {
    motors(F,f,F,f);
    delay(250);
  }
}
int popcount(int mask)
{
  int c=0;
  while (mask)
  {
    mask&=(mask-1);
    c++;
  }
  return (c>2);
}
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()
{
  m=0;
  linefollower();
  t=((t>>1)|(popcount(m)<<2));
  if ((bool)(t&(1<<2))!=(bool)(t&(1<<1)))
  {
    motors(F,f,F,f);
    delay(50);
    linefollower();
    t=((t>>1)|(popcount(m)<<2));
    motors(B,f,B,f);
    delay(50);
  }
  if (popcount(t))
  {
    for (int i=0;i<5;i++)
    m^=(1<<i);
  }
  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);
}