#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);
  this->updated = true;
}

InterestingThing::InterestingThing(Sensor* sensor,
                                   Spherical position,
                                   Quaternion orientation) {
  this->id = 0;
  this->confidence = maxConfidence;
  this->sensor = sensor;
  this->position = position;
  float angle;
  Vector3 axis;
  orientation.ToAngleAxis(&angle, &axis);
  this->orientation = AngleAxis<float>(angle, axis);
  this->updated = true;
}

// #include <Arduino.h>
bool InterestingThing::IsTheSameAs(InterestingThing* otherObj) {
  if (id != 0 && id == otherObj->id)
    return true;
  if (type != otherObj->type)
    return false;

  float equalDistance = equalityDistance;
  float equalAngle = equalityAngle;
  if (otherObj->type == 0x80 || otherObj->id == 0x80) {
    equalDistance = 0.1f;
    equalAngle = 10;
  }
  // printf(" %d %f (%f %f) is same as %f (%f %f)\n", otherObj->type,
  //        otherObj->position.distance,
  //        (float)otherObj->position.horizontalAngle,
  //        (float)otherObj->position.verticalAngle, position.distance,
  //        (float)position.horizontalAngle, (float)position.verticalAngle);
  if (fabsf(position.distance - otherObj->position.distance) > equalDistance)
    return false;
  if (fabsf(position.horizontalAngle.ToFloat() -
            otherObj->position.horizontalAngle.ToFloat()) > equalAngle)
    return false;
  if (fabsf(position.verticalAngle.ToFloat() -
            otherObj->position.verticalAngle.ToFloat()) > equalAngle)
    return false;
  // printf(" -> yes ");
  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;
  this->updated = true;
}

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

  float angle;
  Vector3 axis;
  orientation.ToAngleAxis(&angle, &axis);
  this->orientation = AngleAxis<float>(angle, axis);
  this->confidence = maxConfidence;
  this->updated = true;
}