//************************************************************************
// Borne Curieuse
// Programme arduino interface US
// Hackatlon pésdagogique - UBO Open Factory
// 20-21 décembre 2018
//************************************************************************


// setup pins and variables for DYP-ME007 sonar device
int echoPin = 2; // DYP-ME007 echo pin (digital 2)
int trigPin = 3; // DYP-ME007 trigger pin (digital 3)
int nbLed=60;

#define MODE_VEILLE 0
#define MODE_CATCH 1
#define MODE_ACTION 2

unsigned long pulseTime = 0; // stores the pulse in Micro Seconds
unsigned long distance = 0; // variable for storing the distance (cm)

// setup pins/values for LED
int veillePin = 7;
int catchPin = 8;
int actionPin = 9;
 
int mode=0;
int old_mode=0;
int nb_ech=5;
int iteration_catch=0;

void setup()
{
pinMode(veillePin, OUTPUT);
pinMode(catchPin, OUTPUT);
pinMode(actionPin, OUTPUT);   
pinMode(trigPin, OUTPUT); // set init pin 3 as output
pinMode(echoPin, INPUT); // set echo pin 2 as input

Serial.begin(9600); // pour pouvoir transmettre des infos par le port Série de l'ordi pour mise au point
}


void mesureDistance()
{
int ii;
unsigned long SommeDist=0;

for(ii=0;ii<nb_ech;ii++)
   {
   digitalWrite(trigPin, HIGH); // send 10 microsecond pulse
   delayMicroseconds(10); // wait 10 microseconds before turning off
   digitalWrite(trigPin, LOW); // stop sending the pulse
   pulseTime = pulseIn(echoPin, HIGH, 11600); // Mesure la durée en microsecondes de l'A/R du signal US, TimeOut de 11600 us 200x58 :Correspond à 200cm
   SommeDist = SommeDist + (pulseTime/58); // Distance (cm) = pulse time (microsecond) / 58 
   delay(100);  
   }
   distance=SommeDist/nb_ech;
}

void loop()
{
mesureDistance();
if (distance==0) mode=MODE_VEILLE;
if (mode==MODE_VEILLE)
   {
   iteration_catch=0;     
   if(old_mode!=MODE_VEILLE)
      //Serial.println("1");
      Serial.println("MODE_VEILLE");
   digitalWrite(veillePin,HIGH);
   digitalWrite(catchPin,LOW);
   digitalWrite(actionPin,LOW);
   if((distance<150) && (distance>10))
      {
      mode=MODE_CATCH;
      }
   old_mode=MODE_VEILLE;      
   }

else if (mode==MODE_CATCH)
   {
   if(old_mode!=MODE_CATCH)
      //Serial.println("2");
      Serial.println("MODE_CATCH");    
   digitalWrite(veillePin,LOW);
   digitalWrite(catchPin,HIGH);
   digitalWrite(actionPin,LOW);
   if((distance<50) && (distance>10))
      {
      iteration_catch++;
      if(iteration_catch>5)
         {
         mode=MODE_ACTION;
         }
      }
   old_mode=MODE_CATCH;
   }
else if (mode==MODE_ACTION)
   {
   iteration_catch=0; 
   if(old_mode!=MODE_ACTION)
      //Serial.println("3");
      Serial.println("MODE_ACTION");    
   digitalWrite(veillePin,LOW);
   digitalWrite(catchPin,LOW);
   digitalWrite(actionPin,HIGH);
   old_mode=MODE_ACTION;    
   }

//Serial.println(distance, DEC); // Envoie la valeur de la distance vers le PC
Serial.flush();
//delay(500);

}