#define trigPin 6    //pin digitali attenzione alle maiuscole!
#define  echoPin 4    // pin digitali
#define ks 1.7e-2  //velocità del suono divisa per 2 per tempo in centimetri su microsecondi
#define LUNGHEZZA 50
#define pingiallo 7
#define pinrosso 5
#define pinverde 3
#define velocitabassa 0.8
#define velocitalta  2.
#define TEMPO_ACCENSIONE 5000
#define pinvar 10
int duration;  // definizione di variabile finisce con ;
float distance; // definizione variabile distanza
int bright;

void trig() { 
  digitalWrite(trigPin , LOW); // spegne trigPin, aspetta 2 micro secondi e lo accende per 10 micro secondi poi lo rispegne
  delayMicroseconds(2);
  digitalWrite(trigPin , HIGH); 
  delayMicroseconds (10);
  digitalWrite(trigPin , LOW); 
}

float d0;

void setup() {
  Serial.begin(9600);     //apre canale di comunicazione seriale attraverso usb alla velocità di 9600 di default
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode (pingiallo,OUTPUT);
  pinMode (pinrosso,OUTPUT);
  pinMode (pinverde,OUTPUT);
  pinMode ( pinvar, OUTPUT);
}

void loop() {
  float t0;
  distance = 0.;
  int i = 0;
  while(i < 10) {
    trig();  // fa partire il treno d'onda
    duration = pulseIn(echoPin , HIGH);
    float d = duration * ks;  // d=v*t
    distance += d; // incrementa la distanza di d
    i++; // aumenta i di 1
  }
  distance /= i; // distanza diviso i
  if (distance > 30) {
    d0 = distance;
    t0 = millis();
  }

  int dmax = 80;
  if (distance > dmax) {
    bright = 0;
  } else if (distance > 10) {
    bright = 255 *(dmax- distance)/(dmax-10.);
  } else {
    bright = 255;
  }
  analogWrite(pinvar,bright);

  
  Serial.println(distance);
  if (distance < 10) {
    float speed = (d0 - distance)/(millis() - t0)*10; // risultato velocità in m/s 10^-2 diviso 10^-3
    Serial.println(speed,5); // con 5 cifre decimali
    
    if (speed < velocitabassa){
      digitalWrite(pingiallo , HIGH); // accende giallo se velocità < di velocitabassa
      delay(TEMPO_ACCENSIONE); // ASPETTA il tempo di accensione 
      digitalWrite(pingiallo, LOW);  //e poi spegni
    } else if (speed< velocitalta){
      digitalWrite(pinrosso , HIGH);
      delay(TEMPO_ACCENSIONE);
      digitalWrite(pinrosso, LOW);
    } else {
      digitalWrite(pinverde , HIGH);
      delay(TEMPO_ACCENSIONE);
      digitalWrite(pinverde, LOW);
    }
    while (1) {   // ciclo infinito PER STOPPARE TUTTO
      
    }

  
  }

  delay(1); // aspetta 2 ms per stampare nuovi dati
}