#include "TrackedObject.h"

#include <math.h>

InterestingThing::InterestingThing(Sensor *sensor, Polar position) {
  this->id = 0;
  this->confidence = maxConfidence;
  this->sensor = sensor;
  this->position = Spherical(position);
}

InterestingThing::InterestingThing(Sensor *sensor, Spherical position,
                                   Quaternion orientation) {
  this->id = 0;
  this->confidence = maxConfidence;
  this->sensor = sensor;
  this->position = position;
  this->orientation = orientation;
}

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

bool InterestingThing::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 InterestingThing::Refresh(Polar position) {
  this->position = Spherical(position);
  this->confidence = maxConfidence;
}

void InterestingThing::Refresh(Spherical position, Quaternion orientation) {
  this->position = position;
  this->orientation = orientation;
  this->confidence = maxConfidence;
}