#include <Arduino.h>
#include "Sonar.h"

Sonar::Sonar(const int vccPin, const int trigPin, const int echoPin)
{
  vcc   = vccPin;
  trig  = trigPin;
  echo  = echoPin;

  pinMode(vcc,  OUTPUT);
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);

  digitalWrite(vcc, HIGH);
}


bool Sonar::measure()
{
  unsigned long timeStart = micros(); //  Record time at start of measurement.

  //  Fire sound.
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);

  //  Retrieve sound travel time, and calculate distance.
  unsigned long duration  = pulseIn(echo, HIGH);
  float distance          = (speedOfSound * duration) / 2;  //  (Distance needs to be halved, since the sound needs to travel to, and back from the object.

  //  If no previous distance measurement is saved, we can't calculate the velocity.
  if (!started)
  {
    started           = true;
    previousDistance  = distance;
    return;
  }

  //  Calculate velocity.
  float timePassed        = (micros() - timeStart) / 1000;  //  Calculate time passed in ms.
  float distanceTraveled  = distance - previousDistance;    //  Calculate distance traveled in cm.

  float velocity = distanceTraveled / (timePassed / 1000);  //  Calculate velocity in cm/ms.

  //  Print info. Only use for debugging, as this will clutter the serial port!
  /*
  Serial.print(" Distance is " + String(distance) +  ".\t Traveled " + String(distanceTraveled) + " cm," + "\t" + "after " + String(timePassed) + " milliseconds.");
  Serial.print("\t");
  Serial.print("Estimated velocity is at: " + String(velocity) + " cm/s.");
  Serial.println();
  */

  //  Cache values for next iteration.
  previousDistance = distance;

  //  If the velocity is within the margin of error, don't do anything.
  if (velocity < errorMargin) return false;

  //  Otherwise, send the information through the serial port.
  Serial.println("velocity:" + String(velocity));

  //  Return true.
  return true;
}