#include<AFMotor.h>
#include <Servo.h>

Servo servo;
AF_DCMotor Motor1(1);
AF_DCMotor Motor3(3);
AF_DCMotor Motor2(2);
AF_DCMotor Motor4(4);

int motor_speed = 200;
int IRSensorleft = A2; // connect ir sensor to arduino pin 2
int IRSensorright = A1;

void setup() 
{
  servo.attach(10);
  Serial.begin(9600);
  pinMode (IRSensorleft, INPUT); // sensor pin INPUT
  pinMode (IRSensorright, INPUT); // sensor pin INPUT
}

void loop()
{
  int statusSleft = digitalRead (IRSensorleft);
  int statusSright = digitalRead (IRSensorright);

  Serial.print("left ");
  Serial.println(statusSleft);
  Serial.print("right ");
  Serial.println(statusSright);
  //high -> no light, stop
  
  if (statusSleft==0 && statusSright==0)
  {
    Serial.println("stop!!");
    stopMove(250);
    moveBackward(1200);
    stopMove(250);
    moveForward();
    turn(180, 2950);
    turn(100, 0);    
  }
  else if(statusSleft==0 && statusSright==1)
  {
    moveForward();
    turn(170, 400);
    turn(100, 500);
    //moveBackward(250);
    //turnLeft(2000);
  }
  else{
    moveForward();
    //delay(1000);
    
  }
  delay(50);
}

void moveForward()
{
    Motor1.setSpeed(motor_speed);  //define motor1 speed:
    Motor1.run(FORWARD);   //rotate motor1 clockwise:
    Motor2.setSpeed(motor_speed);  //define motor1 speed:
    Motor2.run(FORWARD);   //rotate motor1 clockwise:
    Motor3.setSpeed(motor_speed);  //define motor1 speed:
    Motor3.run(FORWARD);   //rotate motor1 clockwise:
    Motor4.setSpeed(motor_speed);  //define motor1 speed:
    Motor4.run(FORWARD);   //rotate motor1 clockwise:
}

void stopMove(int duration)
{
    Motor1.setSpeed(0);    //define motor2 speed:
    Motor1.run(RELEASE);   //stop motor2:
    Motor3.setSpeed(0);    //define motor2 speed:
    Motor3.run(RELEASE);   //stop motor2:
    Motor2.setSpeed(0);    //define motor2 speed:
    Motor2.run(RELEASE);   //stop motor2:
    Motor4.setSpeed(0);    //define motor2 speed:
    Motor4.run(RELEASE);   //stop motor2:
    delay(duration);
}

void moveBackward(int duration)
{
    Motor1.setSpeed(motor_speed);  //define motor1 speed:
    Motor1.run(BACKWARD);   //rotate motor1 clockwise:
    Motor2.setSpeed(motor_speed);  //define motor1 speed:
    Motor2.run(BACKWARD);   //rotate motor1 clockwise:
    Motor3.setSpeed(motor_speed);  //define motor1 speed:
    Motor3.run(BACKWARD);   //rotate motor1 clockwise:
    Motor4.setSpeed(motor_speed);  //define motor1 speed:
    Motor4.run(BACKWARD);   //rotate motor1 clockwise:
    delay(duration);
}

void turn(int angle, int duration)
{
  servo.write(angle);
  delay(duration);
}