class Thermistor
{
  int analogPin;

  public:
  Thermistor(int analogPin)
  {
    this->analogPin = analogPin;
    pinMode(analogPin, INPUT);
  }

  double getDegrees()
  {
    double temp = log(10000.0 * (1024.0 / analogRead(this->analogPin) - 1));
    temp = 1 / (0.001129148 + (0.000234125 + (0.0000000876741 * temp * temp)) * temp) - 273.15;

    return temp;
  }
};

Thermistor thermistor(15);

class HCSR04
{
  int pinTrigger;
  int pinEcho;
  double soundMetresPerMillisecond = 0.343;

  double getMetersPerMillisecond()
  {
    return (331 + 0.6 * thermistor.getDegrees()) / 1000.0;
  }

  public:
  HCSR04(int pinTrigger, int pinEcho)
  {
    this->pinTrigger = pinTrigger;
    this->pinEcho = pinEcho;

    pinMode(this->pinTrigger, OUTPUT);
    pinMode(this->pinEcho, INPUT);
  }

  double getMilliseconds()
  {
    digitalWrite(this->pinTrigger, LOW);
    delayMicroseconds(10);
    digitalWrite(this->pinTrigger, HIGH);
    delayMicroseconds(10);
    digitalWrite(this->pinTrigger, LOW);

    return pulseIn(this->pinEcho, HIGH) / 1000.0;
  }

  double getMetres()
  {
    return this->getMilliseconds() * this->getMetersPerMillisecond() * 0.5;
  }

  double getCentimeters()
  {
    return this->getMetres() * 100;
  }
};

class Clock
{
  int intervalPerSecond;
  int timeStart = millis();
  
  public:
  Clock(int intervalPerSecond)
  {
    this->intervalPerSecond = intervalPerSecond;
  }

  void reset()
  {
    this->timeStart = millis();
  }

  void wait()
  {
    int wait = 1000 / this->intervalPerSecond - (millis() - this->timeStart);
    wait = wait > 0 ? wait : 0;
    
    delay(wait);
  }
};

class Measurement
{
  double tolerance = 0.0;
  double average = 0.0;
  
  public:
  Measurement(HCSR04 sensor, int passes = 10)
  {
    double nearest = 99999.0;
    double furthest = 0.0;
    double distancesSum = 0.0;
  
    for (int i = 0; i < passes; ++i) {
      double measurement = sensor.getCentimeters();

      distancesSum += measurement;
  
      if (measurement > furthest) {
        furthest = measurement;
      }
  
      if (measurement < nearest) {
        nearest = measurement;
      }
    }
  
    this->tolerance = (furthest - nearest) * 1.3;
    this->average = distancesSum / passes;
  }

  double getTolerance()
  {
    return this->tolerance;
  }

  double getAverage()
  {
    return this->average;
  }
};

const int PIN_OUTPUT = 3;

HCSR04 sensor(2, 14);
Clock measureClock(4);
double distanceWall;
double lastDist;
double tolerance = 20.0;
double toleranceWall = 20.0;
bool active = true;
int intervalPerSecond = 4;

void scan(double dist) {
  if (dist < 0) {
    return;
  }
  
  if (active) {
    double difference = dist - lastDist;

    if (difference > tolerance || difference < -tolerance) {
      lastDist = dist;
      digitalWrite(PIN_OUTPUT, HIGH);
      delay(1000);
      digitalWrite(PIN_OUTPUT, LOW);
      active = false;
    }
  } else {
    double difference = dist - distanceWall;

    if (difference <= toleranceWall && difference >= -toleranceWall) {
      active = true;
      digitalWrite(PIN_OUTPUT, LOW);
    }
  }
}

void setup() {
  pinMode(PIN_OUTPUT, OUTPUT);
  digitalWrite(PIN_OUTPUT, LOW);

  delay(3000);

  Measurement measurement(sensor);

  tolerance = measurement.getTolerance();
  toleranceWall = measurement.getTolerance();

  distanceWall = measurement.getAverage();
  lastDist = distanceWall;
}

void loop() {
  measureClock.reset();
  
  double dist = sensor.getCentimeters();

  scan(dist);

  lastDist = dist;

  measureClock.wait();
}