#include <NewPing.h>
#include <SoftwareSerial.h>
#define TRIGGER_PIN  3 // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     2 // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 600 // Maximum distance we want to ping for (in meters).
#define LED 4
#define STM_LED_PIN 0
#define Speed_RX_PIN 1 // pin tied to receive speed from
#define Buzz 5

SoftwareSerial Serial(Speed_RX_PIN, -1);

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE * 100); // NewPing setup of pins and maximum distance in centimeters.

void setup() {
  pinMode(LED, OUTPUT);
  pinMode(STM_LED_PIN, INPUT_PULLUP);
  pinMode(Buzz, OUTPUT);
}

void loop() {
  unsigned int distance = getDistance();
  unsigned int rearVehiculeSpeed = getSpeed();
  unsigned int breakPoint = getBreakingDistance(rearVehiculeSpeed);

  if(distance < 305 ){ 
    digitalWrite(LED, HIGH);
  }else if( distance > 350){
    digitalWrite(LED, LOW);
  }

  if(distance < breakPoint){
    digitalWrite(LED, HIGH);
    digitalWrite(Buzz, HIGH);
    delay(500);
    digitalWrite(LED, LOW);
    digitalWrite(Buzz, LOW);
    delay(500);
    digitalWrite(LED, HIGH);
    digitalWrite(Buzz, HIGH);
    delay(500);
    digitalWrite(LED, LOW);
    digitalWrite(Buzz, LOW);

  }

  if(digitalRead(STM_LED_PIN) == LOW ){
    digitalWrite(LED, HIGH);
  }else{
    digitalWrite(LED,LOW);
  }

}

unsigned int getHelmetSpeed(){ //return in Meter per second
  return 0;
}

unsigned int getSpeed(){ //return in Meters per second
  int helmetSpeed = getHelmetSpeed();
  int startPoint = getDistance(); //init point
  delay(1000);
  int stopPoint = getDistance(); //second point
  int rearVehiculeSpeed = (startPoint - stopPoint) + helmetSpeed;
  if(rearVehiculeSpeed < 0){
    rearVehiculeSpeed = 0;
  }
  return rearVehiculeSpeed;
}

unsigned int getBreakingDistance(int rearVehiculeSpeed){ //return Meters
  int speedIn_KM = rearVehiculeSpeed / 1000; //turn in Kilometer
  int breakDistance = ((rearVehiculeSpeed / 10) * (rearVehiculeSpeed / 10)) *2;
  return breakDistance + 50; //adds 50 meters
}

unsigned int getDistance(){ //return Meter
  delay(50); // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
  unsigned int distance_cm = sonar.ping_cm(); // Send ping, get distance in centimeters.
  unsigned int distance_m = distance_cm / 100.0;

  return distance_m;
}