#include "Sonar.h"

Sonar::Sonar(int sharedPin, long maxDistCm)
  : _trigPin(sharedPin),
    _echoPin(sharedPin),
    _singlePinMode(true),
    _maxDistCm(maxDistCm),
    _lastDistance(0) {
}

long Sonar::measure() {
  if (_singlePinMode) {
    _lastDistance = _measureSinglePin();
  } else {
    digitalWrite(_trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(_trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(_trigPin, LOW);

    long duration = pulseIn(_echoPin, HIGH, 30000UL);
    _lastDistance = duration / 58;
  }

  if (_lastDistance > _maxDistCm) {
    _lastDistance = _maxDistCm;
  }

  return _lastDistance;
}

long Sonar::_measureSinglePin() {
  pinMode(_trigPin, OUTPUT);
  digitalWrite(_trigPin, LOW);
  delayMicroseconds(4);

  digitalWrite(_trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(_trigPin, LOW);

  pinMode(_echoPin, INPUT);

  delayMicroseconds(15);

  long duration = pulseIn(_echoPin, HIGH, 30000UL);

  return duration / 58;
}

long Sonar::getDistance() const {
  return _lastDistance;
}