#include <NewPing.h>

#define TRIGGER_PIN  5  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     4  // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 200 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.

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

int personCounter;
int currentState = 0;
int previousState = 0;
int maxDistanceRange = 100;
int minDistanceRange = 10;
int currentPingedDistance;
//int humanPing = sonar.ping_median(iterations);
int pingCounter;
int pingsOnObject;

//int objectInRange = currentPingedDistance <= maxDistanceRange && currentPingedDistance > minDistanceRange;

void setup() {
  Serial.begin(115200); // Open serial monitor at 115200 baud to see ping results.
}

void loop() {

  if (sonar.ping_cm() <= maxDistanceRange && sonar.ping_cm() > minDistanceRange) {

    pingsOnObject = 1;

    while (sonar.ping_cm() <= maxDistanceRange && sonar.ping_cm() > minDistanceRange)
    {
      delay(50);
      sonar.ping();
      Serial.print(sonar.ping_cm());
      pingsOnObject++;
      Serial.print("Pings");
      Serial.print(" - ");
      Serial.println(pingsOnObject);
      if (pingsOnObject >= 5)
      {
        break;
      }
      else {
        continue;
      }
    }


    if (pingsOnObject >= 5) {

      pingCounter = 1;
      currentState = 1;

      while (sonar.ping_cm() <= maxDistanceRange && sonar.ping_cm() > minDistanceRange) {
//        delay(500);
        pingCounter ++;
      }
      Serial.print("Pings On Person");
      Serial.print(" - ");
      Serial.println(pingCounter + 5);
    }
    else {

      currentState = 0;
    }

    if (currentState != previousState) {
      if (currentState == 1) {
        personCounter = personCounter + 1;

        Serial.print("Persons");
        Serial.print(" - ");
        Serial.println(personCounter);
      }
    }

  } else {

    pingsOnObject = 0;
  }


}