//Lab6.ino
#define STATE1 1
#define STATE2 2
#define STATE3 3
#define STATE4 4

#define CYC_CNT 100 
const int PWMchL = 5;     // enable for L motor  (PWM 500Hz)
const int DOutIN1 =  2;      // LIN1 for L motor 
const int DOutIN2 =  3;      // LIN2 for L motor

const int PWMchR = 6;      // enable for R motor (PWM 500Hz)
const int DOutIN3 =  4;      // RIN1 for R motor
const int DOutIN4 =  7;      // RIN2 for R motor

const int PWMG1 = 9;      // enable for R motor (PWM 500Hz)
const int DOutIN5 =  8;      // RIN1 for R motor
const int DOutIN6 =  11;      // RIN2 for R motor

const int PWMG2 = 10;      // enable for R motor (PWM 500Hz)
//const int DOutA =  4;      // RIN1 for R motor
//const int DOutB =  7;      // RIN2 for R motor


const int DIn1 =  12;      // the number of the LED pin
const int DIn2 =  13;      // the number of the LED pin
const int DIn3 =  A2;      // the number of the LED pin
const int DIn4 =  A3;      // the number of the LED pin
const int DIn5 =  A4;      // the number of the LED pin
const int DIn6 =  A5;      // the number of the LED pin

const int AInMTRL =  A0;      // PWML for adj
const int AInMTRR =  A1;      // PWMR for adj

// io assignment
int counter=0;
byte IN_sensor=0b00000000;

int outputValueL;
int outputValueR;

int sensorValueL;
int sensorValueR;

unsigned char state=1;

void setup() 
{
  pinMode(DIn1, INPUT);
  pinMode(DIn2, INPUT);
  pinMode(DIn3, INPUT);
  pinMode(DIn4, INPUT);
  pinMode(DIn5, INPUT);
  pinMode(DIn6, INPUT);
 
  pinMode(DOutIN1, OUTPUT);
  pinMode(DOutIN2, OUTPUT);
  pinMode(DOutIN3, OUTPUT);
  pinMode(DOutIN4, OUTPUT);
  pinMode(DOutIN5, OUTPUT);
  pinMode(DOutIN6, OUTPUT);
  analogWrite(PWMchL, 200);
  analogWrite(PWMchR, 200);
   Serial.begin(9600);   
}

/****************************************************************************************
  Truth Table approach
****************************************************************************************/

void LogicTable()
{
  switch (IN_sensor)  // 0b(S4)(S3)(S2)(S1)
  {
    case 0b1111 : LM1(1);LM2(0);RM1(1);RM2(0);
                    break;
    case 0b1110 : LM1(1);LM2(0);RM1(0);RM2(0);
                    break;
    case 0b1101 : LM1(0);LM2(0);RM1(1);RM2(0);
                    break;
    case 0b0001 : LM1(0);LM2(0);RM1(1);RM2(0);
                    break;  
    case 0b0010 : LM1(1);LM2(0);RM1(0);RM2(0);
                    break;             
    case 0b0011 : LM1(1);LM2(0);RM1(1);RM2(0);
                    break;
    case 0b0101 : LM1(0);LM2(0);RM1(1);RM2(0);
                    break;  
    case 0b0110 : LM1(1);LM2(0);RM1(0);RM2(0);
                    break;             
    case 0b0111 : LM1(1);LM2(0);RM1(1);RM2(0);
                    break;
         default :  LM1(0);LM2(0);RM1(0);RM2(0);
                   break;
  }  
}

void stop_motors()
 {
   LM1(0);LM2(0);RM1(0);RM2(0);
 }

// Ldir1=1, Ldir2=0 left motor forward; 
// Ldir1=0, Ldir2=1 left motor backward;
// Rdir1=1, Rdir2=0 right motor forward; 
// Rdir1=0, Rdir2=1 right motor backward;
// Lspeed, Rspeed, 0 - 255; 0 stop, 255 fastest speed;
// time, delay time in mili seconds;
void motors(boolean Ldir1,boolean Ldir2,boolean Rdir1,boolean Rdir2,unsigned char Lspeed,unsigned char Rspeed,int time)
{
 analogWrite(PWMchL, Lspeed); 
 analogWrite(PWMchR, Rspeed);  
 if (Ldir1==1) LM1(1);
 else LM1(0);
 if (Ldir2==1) LM2(1);
 else LM2(0);
 if (Rdir1==1) RM1(1);
 else RM1(0);
 if (Rdir2==1) RM2(1);
 else RM2(0);
 delay(time);
 stop_motors();
}

void loop()
{
  boolean LEFTM1,LEFTM2,RIGHTM1,RIGHTM2;
  unsigned char LM1,LM2,RM1,RM2,SPEED;
  int DELAY_TIME;
  
  if(S6()==1 && S5()==1)
  {
    GetInput();
    LogicTable();
  }
   else if(S6()==1 && S5()==0)
  {
    switch(state)
    {
  	case STATE1:
		LM1=1;
		LM2=0;
		RM1=1;
		RM2=0;
		SPEED=200;
		DELAY_TIME=2000;  // delay 2 seconds
		motors(LM1,LM2,RM1,RM2,SPEED,SPEED,DELAY_TIME);
                //
		state=STATE2;
		break;

	case STATE2:
		LM1=0;
		LM2=0;
		RM1=1;
		RM2=0;
		SPEED=200;
		DELAY_TIME=2000;  // delay 2 seconds
		motors(LM1,LM2,RM1,RM2,SPEED,SPEED,DELAY_TIME);
                //
                state=STATE3;
		break;

	case STATE3:
		LM1=1;
		LM2=0;
		RM1=0;
		RM2=0;
		SPEED=200;
		DELAY_TIME=2000;  // delay 2 seconds
		motors(LM1,LM2,RM1,RM2,SPEED,SPEED,DELAY_TIME);
                //
                state=STATE4;
		break;

	case STATE4:
		LM1=0;
		LM2=0;
		RM1=0;
		RM2=0;
		SPEED=200;
		DELAY_TIME=2000;  // delay 2 seconds
		motors(LM1,LM2,RM1,RM2,SPEED,SPEED,DELAY_TIME);
		//
		state=STATE1;
		break;
	default:
		state=STATE4;
		break;
      }

  }
  else if(S6()==0 && S5()==1)
  {
    switch(state)
    {
  	case STATE1:
		LM1=1;
		LM2=0;
		RM1=1;
		RM2=0;
		SPEED=200;
		DELAY_TIME=10;
		motors(LM1,LM2,RM1,RM2,SPEED,SPEED,DELAY_TIME);
                //
                if(S3()==0) state=STATE4;
		else if (S3()==1 && S2()==1 && S1()==0) state=STATE2;
                else if(S3()==1 && S2()==0 && S1()==1) state=STATE3;
                else if(S3()==1 && S2()==0 && S1()==0) state=STATE4;
		break;

	case STATE2:
		LM1=0;
                LM2=0;
                RM1=1;
                RM2=0;
                SPEED=200;
                DELAY_TIME=10;
		motors(LM1,LM2,RM1,RM2,SPEED,SPEED,DELAY_TIME);
                //
                if(S3()==0) state=STATE4;
		else if (S3()==1 && S2()==1 && S1()==1) state=STATE1;
                else if(S3()==1 && S2()==0 && S1()==1) state=STATE3;
                else if(S3()==1 && S2()==0 && S1()==0) state=STATE4;
		break;

	case STATE3:
		LM1=1;
		LM2=0;
		RM1=0;
		RM2=0;
		SPEED=200;
		DELAY_TIME=10;
		motors(LM1,LM2,RM1,RM2,SPEED,SPEED,DELAY_TIME);
                //
                if(S3()==0) state=STATE4;
		else if (S3()==1 && S2()==1 && S1()==1) state=STATE1;
                else if(S3()==1 && S2()==1 && S1()==0) state=STATE2;
                else if(S3()==1 && S2()==0 && S1()==0) state=STATE4;
		break;

	case STATE4:
		LM1=0;
		LM2=0;
		RM1=0;
		RM2=0;
		SPEED=200;
		DELAY_TIME=10;
		motors(LM1,LM2,RM1,RM2,SPEED,SPEED,DELAY_TIME);
		break;
	default:
		state=STATE1;
		break;
    }
  }
}

//------------------------------
void GetInput()
{
  IN_sensor=0b00000000;
  if (S1()==1)
    IN_sensor |= (0x01<<0);
  if (S2()==1)
    IN_sensor |= (0x01<<1);
  if (S3()==1)
    IN_sensor |= (0x01<<2);
  if (S4()==1)
    IN_sensor |= (0x01<<3);
  //if (S5()==1)
  //  IN_sensor |= (0x01<<4);
  //if (S6()==1)
  //  IN_sensor |= (0x01<<5);
}

int LM1(int state)
{
    if (state==1)
      digitalWrite(DOutIN1,HIGH);
    else
      digitalWrite(DOutIN1,LOW);  
}

void LM2(int state)
{
    if (state==1)
      digitalWrite(DOutIN2,HIGH);
    else
      digitalWrite(DOutIN2,LOW);  
}

void RM1(int state)
{
    if (state==1)
      digitalWrite(DOutIN3,HIGH);
    else
      digitalWrite(DOutIN3,LOW);  
}

void  RM2(int state)
{
    if (state==1)
      digitalWrite(DOutIN4,HIGH);
    else
      digitalWrite(DOutIN4,LOW);  
}

boolean S1()
{
   boolean sensor1;
   if(digitalRead(DIn1)==1) sensor1=true;else sensor1=false;
   return(sensor1);
}

boolean S2()
{
   boolean sensor2;
   if(digitalRead(DIn2)==1) sensor2=true;else sensor2=false;
   return(sensor2);
}

boolean S3()
{
   boolean sensor3;
   if(digitalRead(DIn3)==1) sensor3=true;else sensor3=false;
   return(sensor3);
}

boolean S4()
{
   boolean sensor4;
   if(digitalRead(DIn4)==1) sensor4=true;else sensor4=false;
   return(sensor4);
}

boolean S5()
{
   boolean sensor5;
   if(digitalRead(DIn5)==1) sensor5=true;else sensor5=false;
   return(sensor5);
}

boolean S6()
{
   boolean sensor6;
   if(digitalRead(DIn6)==1) sensor6=true;else sensor6=false;
   return(sensor6);
}