#include "Sonar.h"

Sonar::Sonar(int trigPin, int echoPin, long maxDistCm)
  : _trigPin(trigPin), _echoPin(echoPin),
    _maxDistCm(maxDistCm), _lastDistance(0) {
  pinMode(_trigPin, OUTPUT);
  pinMode(_echoPin, INPUT);
}

long Sonar::measure() {
  digitalWrite(_trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(_trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(_trigPin, LOW);

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

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

  return _lastDistance;
}

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