/*
----PinOut-----
D0- NA
D1- NA
D2- NA
D3- Common Trig
D4- Echo1 side 2
D5- Echo2 side 1
D6- Echo3 main
D7- Echo4 res
D8- Buzzer
D9- Servo1
D10- Servo2
D11- Servo3
D12- NA
D13-LED

*/
#include <Servo.h>
#define trig 3
#define echo1 4// side2 steam
#define echo2 5// side1 stram
#define echo3 6// main strem
#define echo4 7// res strem
#define led 13//indication LED
#define buzzer 8// sound Buzzer


Servo mains;//object for servo1
Servo side1;//object for servo2
Servo side2;//object for serv03

float duration[4]={0,0,0,0};
float distance[4]={0,0,0,0}; 
int mindis= 0;

int reslvl = 0;
int mainlvl = 0;
int s1lvl = 0;
int s2lvl = 0;

int minin(float array[],int size=4);

void setup() {

mains.attach(9);
side1.attach(10);
side2.attach(11);

allclose();



pinMode(trig,OUTPUT);
pinMode(echo1,INPUT);
pinMode(echo2,INPUT);
pinMode(echo3,INPUT);
pinMode(echo4,INPUT);
pinMode(led,OUTPUT);
pinMode(buzzer,OUTPUT);
digitalWrite(buzzer,LOW);
digitalWrite(led,LOW);
}

void loop() {
 getdata(echo1,0);
 delay(100);
 getdata(echo2,1);
 delay(100);
 getdata(echo3,2);
 delay(100);
 getdata(echo4,3);

 checklvl();

 if(reslvl>=1)
 {
  allclose();
  mindis=minin(distance);
  switch(mindis){
    case 0:
            side1.write(90);
            digitalWrite(led,HIGH);
            delay(200);
            break;
    case 1:         
            side2.write(90);
            digitalWrite(led,HIGH);
            delay(200);
            break;
    case 2:
            mains.write(90);
            digitalWrite(led,HIGH);
            delay(200);
            break;
    case 3:
            allclose();
            break;
    default:
            allclose();
            digitalWrite(led,LOW);
            break;
  }
 }
 else{
  allclose();
  digitalWrite(led,LOW);
 }

delay(100);
 }


void getdata(int pin,int var )
{
  digitalWrite(trig,LOW);
  delayMicroseconds(2);
  digitalWrite(trig,HIGH);
  delayMicroseconds(10);
  digitalWrite(trig,LOW);
  duration[var] = pulseIn(pin, HIGH);
  distance[var] = (duration[var]/2) / 29.1;
}

void checklvl(){
if(distance[3]<8 && distance[3]>=4 )// res lvl-1 reached
    reslvl=1; 
else if(distance[3]<4 && distance[3]>=2)// res lvl-2 reached
  reslvl=2; 
else if(distance[3]<=2)// res full
  reslvl=3; 
else
  reslvl=0;


if(distance[2]<4 && distance[2]>=2.5 )// main lvl-1 reached
    mainlvl = 1;
else if(distance[2]<2.5 && distance[2]>=2)// main lvl-2 reached
  mainlvl = 2; 
else if(distance[2]<=2)// main full
  mainlvl = 3; 
else
  mainlvl = 0;

if(distance[1]<4 && distance[1]>=2.5 )// stream1 lvl-1 reached
   s1lvl = 1;
else if(distance[1]<2.5 && distance[1]>=2)// stream1 lvl-2 reached
  s1lvl = 2;
else if(distance[1]<=2)// stream1 full
  s1lvl = 3;
else  
  s1lvl = 0;

if(distance[0]<4 && distance[0]>=2.5 )// stream2 lvl-1 reached
    s2lvl = 1;
else if(distance[0]<2.5 && distance[0]>=2)// stream2 lvl-2 reached
  s2lvl = 2; 
else if(distance[0]<=2)// stream2 full
  s2lvl = 3; 
else
  s2lvl = 0;

if((mainlvl>=2) && (reslvl>=2) && (s1lvl>=2) && (s2lvl>=2) )// all full
  digitalWrite(buzzer,HIGH);
else
  digitalWrite(buzzer,LOW);

}

void allclose()
{
    mains.write(7);//7
    side1.write(7);//all gates close position 7
    side2.write(5);//5
}


int minin(float array[],int size=4)
{
  int index = 0;

	for (int i = 1; i < size; i++)
	{
      if (array[i] < array[index])
			  index = i;
	}
  return index;
}