// var pour servo
#include <Servo.h>
Servo moteur;
#define posAll 170 - 12
#define posEteint 170 + 12
unsigned int pos = posEteint;
#define servoPin 11

// var pour détection de lumière,
#define photocellPin A0
long synchroPhotocell = 0;
int lum = 0;
byte besoinLumiere = 0;

// var pour DEL de synchronisation pour la distance max
#define delPinBleu 2
long synchroDel;
bool etatDelBleu = 0;

// var pour nb de personnes
byte nbPers = 0;
byte persEntrant = 0;
byte persSortant = 0;
long synchroPersEntrant;
long synchroPersSortant;

// var pour les deux HC-SR04
#include <HCSR04.h>

#define trigPinTete 3
#define echoPinTete 4
HCSR04 sr04Tete(trigPinTete, echoPinTete);
int distanceMaxTete = 0;
int distanceInstTete;
int distanceMaxTeteMoyen[3];
#define trigPinBras 5
#define echoPinBras 7

HCSR04 sr04Bras(trigPinBras, echoPinBras);
int distanceMaxBras = 0;
int distanceInstBras;
int distanceMaxBrasMoyen[3];

long synchroDistance;
byte nbBoucle = 0;

void setup()
{
	Serial.begin(9600);
	pinMode(delPinBleu, OUTPUT);
	moteur.attach(servoPin);
	moteur.write(170 + 12); // peut differer selon la position du moteur
	synchroDel = millis();
	synchroDistance = millis();
	while (distanceMaxTete == 0 || distanceMaxBras == 0)
	{
		if (nbBoucle == 3)
		{
			if (abs(distanceMaxBrasMoyen[nbBoucle - 1] - distanceMaxBrasMoyen[nbBoucle - 2]) < 4 && (abs(distanceMaxBrasMoyen[nbBoucle - 2] - distanceMaxBrasMoyen[nbBoucle - 3]) < 4))
			{
				distanceMaxBras = distanceMaxBrasMoyen[1];
			}
			if (abs(distanceMaxTeteMoyen[nbBoucle - 1] - distanceMaxTeteMoyen[nbBoucle - 2]) < 4 && (abs(distanceMaxTeteMoyen[nbBoucle - 2] - distanceMaxTeteMoyen[nbBoucle - 3]) < 4))
			{
				distanceMaxTete = distanceMaxTeteMoyen[1];
			}
			nbBoucle = 0;
		}
		if (millis() - synchroDel >= 500)
		{
			digitalWrite(delPinBleu, etatDelBleu);
			etatDelBleu = !etatDelBleu;
			synchroDel = millis();
			lum = analogRead(photocellPin);
			Serial.print(lum);
			Serial.println("lux");
		}
		if (millis() - synchroDistance >= 1000)
		{
			distanceMaxTeteMoyen[nbBoucle] = sr04Tete.dist();
			distanceMaxBrasMoyen[nbBoucle] = sr04Bras.dist();
			Serial.print(distanceMaxTeteMoyen[0]);
			Serial.print(" ; ");
			Serial.print(distanceMaxTeteMoyen[1]);
			Serial.print(" ; ");
			Serial.println(distanceMaxTeteMoyen[2]);
			Serial.print(distanceMaxBrasMoyen[0]);
			Serial.print(" ; ");
			Serial.print(distanceMaxBrasMoyen[1]);
			Serial.print(" ; ");
			Serial.println(distanceMaxBrasMoyen[2]);
			Serial.print("boucle n°");
			Serial.println(nbBoucle);
			nbBoucle += 1;
			synchroDistance = millis();
		}
	}
	digitalWrite(delPinBleu, LOW);
	Serial.print("distance max tete :");
	Serial.println(distanceMaxTete);
	Serial.print("distance max bras :");
	Serial.print(distanceMaxBras);
}

void loop()
{
	lum = analogRead(photocellPin);
	if (lum <= 100)
	{
		besoinLumiere = 1;
	}
	else
	{
		besoinLumiere = 0;
	}
	if (besoinLumiere && pos == posEteint && nbPers > 0)
	{
		digitalWrite(delPinBleu, HIGH);
		delay(250);
		digitalWrite(delPinBleu, LOW);
		delay(250);
		digitalWrite(delPinBleu, HIGH);
		delay(200);
		digitalWrite(delPinBleu, LOW);
		for (pos = posEteint; pos > posAll; pos -= 1)
		{
			moteur.write(pos);
			delay(2); //==> lumière allumer
		}
		digitalWrite(delPinBleu, HIGH);
		delay(250);
		digitalWrite(delPinBleu, LOW);
	}
	distanceInstTete = sr04Tete.dist();
	if (distanceMaxTete - 40 > distanceInstTete)
	{
		Serial.print("distance tete :");
		Serial.println(distanceInstTete);
		synchroPersEntrant = millis();
		while (millis() - synchroPersEntrant <= 1000 && persEntrant == 0)
		{
			distanceInstBras = sr04Bras.dist();
			if (distanceMaxBras - 40 > distanceInstBras)
			{
				Serial.print("distance bras :");
				Serial.println(distanceInstBras);
				digitalWrite(delPinBleu, HIGH);
				persEntrant = 1;
				nbPers += 1;
				Serial.println("une personne entrante, il y a ");
				Serial.print(nbPers);
				Serial.println(" personne(s)");
				if (nbPers == 1 && besoinLumiere)
				{
					for (pos = posEteint; pos > posAll; pos -= 1)
					{
						moteur.write(pos);
						delay(2); //==> lumière allumer
					}
				}
			}
		}
	}
	if (persEntrant == 1)
	{
		persEntrant = 0;
		delay(1000);
		digitalWrite(delPinBleu, LOW);
	}
	distanceInstBras = sr04Bras.dist();
	if (distanceMaxBras - 40 > distanceInstBras)
	{
		Serial.print("distance bras :");
		Serial.println(distanceInstBras);
		synchroPersSortant = millis();
		while (millis() - synchroPersSortant <= 1000 && persSortant == 0)
		{
			distanceInstTete = sr04Tete.dist();
			if (distanceMaxTete - 40 > distanceInstTete && nbPers != 0)
			{
				Serial.print("distance tete :");
				Serial.println(distanceInstTete);
				digitalWrite(delPinBleu, HIGH);
				persSortant = 1;
				nbPers -= 1;
				Serial.print("une personne sortante, il y a ");
				Serial.print(nbPers);
				Serial.println(" personne(s)");
				if (nbPers == 0)
				{
					for (pos = posAll; pos < posEteint; pos += 1)
					{
						moteur.write(pos);
						delay(2); //==> lumière éteinte
					}
				}
			}
		}
	}
	if (persSortant == 1)
	{
		persSortant = 0;
		delay(400);
		digitalWrite(delPinBleu, LOW);
		delay(100);
		digitalWrite(delPinBleu, HIGH);
		delay(450);
		digitalWrite(delPinBleu, LOW);
	}
	if (millis() - synchroPhotocell >= 1000)
	{
		synchroPhotocell = millis();
		Serial.print(lum);
		Serial.println("lux");
	}
}