#include "Perception.h"
#include <Arduino.h>
#include "Angle.h"

DistanceSensor* Perception::GetSensor(float angle) {
  angle = Angle::Normalize(angle);

  for (unsigned int ix = 0; ix < this->sensorCount; ix++) {
    Placement placement = this->sensorPlacements[ix];
    if (abs(placement.direction.y - angle) < 0.01F)
      return (DistanceSensor*)placement.thing;
  }

  DistanceSensor* distanceSensor = new DistanceSensor();
  Placement* newPlacement = new Placement(distanceSensor, angle);
  AddSensors(newPlacement, 1);

  return distanceSensor;
}