int r[5],m,bm,col;
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));
  }
  if (m==0b00100 && bm==0b11011)
  col=0;
  if (m==0b11011 && bm==0b00100)
  col=1;
  if (col)
  {
    for (int i=0;i<5;i++)
    m^=(1<<i);
  }
}
void findLine(int dir)
{
  motors(F*!dir,f,F*dir,f);
  delay(100);
  linefollower();
  while (!r[2])
  linefollower();
}
bool readCol(int sensor)
{
  bool r=0;
  motors(B,f,B,f);
  delay(50);
  for (int i=0;i<3;i++)
  {
    motors(0,0,0,0);
    pulseIn(color[sensor][4],HIGH);
    pulseIn(color[sensor][4],LOW);
    int c=pulseIn(color[sensor][4],HIGH);
    r|=1; // l mofrood howwa c green walla l2 m4 1;
    motors(F,f,F,f);
    delay(50);
  }
  motors(B,f,B,f);
  delay(100);
  motors(0,0,0,0);
  return r;
}
void intersection(int dir)
{
  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()
{
  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
  motors(F,f,F,f);
}
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);
  bm=0b00100;
}
void loop()
{
  bm=m;
  m=0;
  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();
}