// Highly speculative Partner Motion sketch using 2 motion detectors

#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 readEcho1()
{
  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;
  }
}
void readEcho2()
{
  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;
  }
}

void loop() 
{
  if (duration > 0)
  {
    int now = (micros() - startTime)/1000;
    if (now < duration)
    {
      if (waiting1)
      {
        readEcho1();
      }
      else
      {
        sendPulse1();
      }
      if (waiting2)
      {
        readEcho2();
      }
      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();
  }
}