#include "UltrasonicSensor.h"

#include <Arduino.h>
#include <iostream>

namespace RoboidControl {
namespace Arduino {

UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent)
    : Thing(parent) {
  this->type = Type::DistanceSensor;
  this->name = "Ultrasonic sensor";
  this->pinTrigger = config.trigger;
  this->pinEcho = config.echo;

  pinMode(pinTrigger, OUTPUT);  // configure the trigger pin to output mode
  pinMode(pinEcho, INPUT);      // configure the echo pin to input mode
}

float UltrasonicSensor::GetDistance() {
  // Start the ultrasonic 'ping'
  digitalWrite(pinTrigger, LOW);
  delayMicroseconds(2);
  digitalWrite(pinTrigger, HIGH);
  delayMicroseconds(10);
  digitalWrite(pinTrigger, LOW);

  // Measure the duration of the pulse on the echo pin
  unsigned long duration_us =
      pulseIn(pinEcho, HIGH, 10000);  // the result is in microseconds

  // Calculate the distance:
  // * Duration should be divided by 2, because the ping goes to the object
  // and back again. The distance to the object is therefore half the duration
  // of the pulse: duration_us /= 2;
  // * Then we need to convert from microseconds to seconds: duration_sec =
  // duration_us / 1000000;
  // * Now we calculate the distance based on the speed of sound (340 m/s):
  // distance = duration_sec * 340;
  // * The result calculation is therefore:
  this->distance = (float)duration_us / 2 / 1000000 * 340;

  // Serial.println(this->distance);

  // std::cout << "US distance " << this->distance << std::endl;

  // Filter faulty measurements. The sensor can often give values > 30 m which
  // are not correct
  // if (distance > 30)
  //   distance = 0;

  return this->distance;
}

void UltrasonicSensor::Update(bool recursive) {
  GetDistance();
  Thing::Update(recursive);
}

#pragma region Distance sensor

UltrasonicSensor::DistanceSensor::DistanceSensor(
    UltrasonicSensor::Configuration config,
    Thing* parent)
    : RoboidControl::DistanceSensor(parent), ultrasonic(config, this) {}

void UltrasonicSensor::DistanceSensor::Update(bool recursive) {
  RoboidControl::DistanceSensor::Update(recursive);
  this->ultrasonic.Update(false);
  if (this->ultrasonic.distance > 0)
    this->internalDistance = this->ultrasonic.distance;
  else
#if ARDUNIO
    this->internalDistance = INFINITY;
#else
    this->internalDistance = std::numeric_limits<float>::infinity();
#endif
}

#pragma endregion Distance sensor

#pragma region Touch sensor

UltrasonicSensor::TouchSensor::TouchSensor(Configuration config, Thing* parent)
    : RoboidControl::TouchSensor(parent), ultrasonic(config, this) {}

void UltrasonicSensor::TouchSensor::Update(bool recursive) {
  RoboidControl::TouchSensor::Update(recursive);
  this->ultrasonic.Update(false);
  this->internalTouch = (this->ultrasonic.distance > 0 &&
                         this->ultrasonic.distance <= this->touchDistance);
}

#pragma endregion Touch sensor

}  // namespace Arduino
}  // namespace RoboidControl