#include "Perception.h"
#include "Angle.h"
#include <Arduino.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();
  Serial.printf("New sensor ADDED %f \n", angle);
  Placement *newPlacement = new Placement(distanceSensor, angle);
  AddSensors(newPlacement, 1);

  return distanceSensor;
}