#include "PerceivedObject.h"

#include <math.h>

/***
 * Oject perception
 ***/

PerceivedObject::PerceivedObject() {
  this->id = 0;
  this->confidence = maxConfidence;
  this->position = Polar(0, INFINITY);
  this->radius = INFINITY;
}

PerceivedObject::PerceivedObject(Sensor *sensor, Polar position, float radius)
    : PerceivedObject() {
  this->sensor = sensor;
  this->position = position;
  this->radius = radius;
}

bool PerceivedObject::IsTheSameAs(PerceivedObject *otherObj) {
  if (id != 0 && id == otherObj->id)
    return true;
  if (fabsf(position.distance - otherObj->position.distance) > equalityDistance)
    return false;
  if (fabsf(position.angle - otherObj->position.angle) > equalityAngle)
    return false;
  return true;
}

bool PerceivedObject::DegradeConfidence(float deltaTime) {
  unsigned char confidenceDrop =
      (unsigned char)((float)confidenceDropSpeed * deltaTime);
  // Make sure the confidence always drops
  if (confidenceDrop == 0)
    confidenceDrop = 1;

  if (confidence <= confidenceDrop) {
    // object is dead
    confidence = 0;
    return false;
  } else {
    confidence -= confidenceDrop;
    return true;
  }
}

void PerceivedObject::Refresh(Polar position, float radius) {
  this->position = position;
  this->radius = radius;
  this->confidence = maxConfidence;
}