/* 
 VernierMotionDetector.ino
===========================
 https://learn.sparkfun.com/tutorials/vernier-shield-hookup-guide/example-3---motion-detector
 Modified from Vernier example code: VernierMotionDetector (v 2013.11)
 Takes data from a Vernier Motion Detector connected to BTD connector on 
 SparkFun Vernier Interface Shield.
 This sketch measures the time taken for the ultrasound to return (in microseconds)
 and then calculates the corresponding distance (based on the speed of ultrasound
 in air) and displays the distance (in cm) on the Serial Monitor. 
 The data is displayed to the serial monitor as a tab delimitted format. Change the 
 delimiter variable to a comma ',' for Comma-Separated-Value (CSV) format.
 Here is how the Vernier Motion Detector works:
 - when the INIT pin (Arduino Pin 3 or 7) is pulled high, this triggers the 
 ultrasound pulse
 - program then starts timing but then delays 0.882 ms (blanking time),
 0.882 ms is the time it takes ultrasound to travel 15 cm twice (round trip))
 assuming a speed of 340 m/s
 - the program then monitors ECHO pin (Arduino Pin 2 or 6), waiting for it to '
 go high. This happens when an echo is detected.
 See www.vernier.com/arduino for more information.
 */
#define speedOfSound 340 // speed of sound in m/s

int duration = 0;         // data collection duration in milliseconds

const int triggerPin1 = 3; // trigger (INIT) pin -> pin 3 for Dig 1, pin 7 for Dig 2
const int echoPin1 = 2;    // echo pin -----------> pin 2 for Dig 1, pin 6 for Dig 2
const int triggerPin2 = 7; // trigger (INIT) pin -> pin 3 for Dig 1, pin 7 for Dig 2
const int echoPin2 = 6;    // echo pin -----------> pin 2 for Dig 1, pin 6 for Dig 2

// Variables used in the code for calculations
unsigned long resetPeriod;
unsigned long startTime;
unsigned long timeRef1;    // reference for starting time
unsigned long timeRef2;    // reference for starting time
unsigned long echoTime1;   // time it take echo to return
unsigned long echoTime2;   // time it take echo to return
unsigned int timeOut;     // used to calculate the "time-out" if an echo is not detected
float distance1;           // distance in meters
float distance2;           // distance in meters
bool waiting1 = false;
bool waiting2 = false;

void setup() 
{
  // initialize the Ping pin as an output:
  pinMode(triggerPin1, OUTPUT);
  pinMode(triggerPin2, OUTPUT);
  digitalWrite(triggerPin1, LOW);
  digitalWrite(triggerPin2, LOW);
  // initialize echo pin as input:
  pinMode(echoPin1, INPUT); 
  pinMode(echoPin2, INPUT); 
  
  duration = 5000;
  timeOut = 6000000/speedOfSound; // calculation of roundtrip timeOut 3 meters * 2 at 340 m/s
  resetPeriod = 882; // "blanking time"
  Serial.begin(9600);   // initialize serial communication at 9600 bits per second:
  Serial.println(0.000, 3);
}

void sendPulse1()
{
  digitalWrite(triggerPin1, HIGH); 
  timeRef1 = micros(); 
  waiting1 = true;
  digitalWrite(triggerPin1, LOW);

}
void sendPulse2()
{
  digitalWrite(triggerPin2, HIGH);
  timeRef2 = micros();  
  waiting2 = true;
  digitalWrite(triggerPin2, LOW);
}

void loop() 
{
  if (duration > 0)
  {
    int now = (micros() - startTime)/1000;
    if (now < duration)
    {
      if (waiting1)
      {
        unsigned long sincePing1 = micros() - timeRef1;
        if (digitalRead(echoPin1) == HIGH && sincePing1 > resetPeriod || sincePing1 > timeOut)
        {
          echoTime1 = sincePing1 / 2;
          distance1 = (float) (speedOfSound * echoTime1) / (1000000);
          Serial.print("a:");
          Serial.println(distance1, 3);
          waiting1 = false;
        }
      }
      else
      {
        sendPulse1();
      }
      if (waiting2)
      {
        unsigned long sincePing2 = micros() - timeRef2;
        if (digitalRead(echoPin2) == HIGH && sincePing2 > resetPeriod || sincePing2 > timeOut)
        {
          echoTime2 = sincePing2 / 2;
          distance2 = (float) (speedOfSound * echoTime2) / (1000000);
          Serial.print("b:");
          Serial.println(distance2, 3);
          waiting2 = false;
        }
      }
      else
      {
        sendPulse2();
      }
    }
    else
    {
      Serial.println("Done");
      duration = 0;
      waiting1 = false;
      waiting2 = false;
    }
  }

  if (Serial.available() > 0) 
  {
    String inString = Serial.readString();
    duration = inString.toInt();
    startTime = micros();
  }
}