#include "TrackedObject.h"

#include <math.h>

// InterestingThing::InterestingThing(Sensor *sensor, Polar position) : Thing()
// {
//   this->id = 0;
//   this->confidence = maxConfidence;
//   this->sensor = sensor;
//   this->position = Spherical16(position.distance,
//                                Angle16::Degrees(position.angle.InDegrees()),
//                                Angle16::Degrees(0));
//   this->updated = true;
// }

InterestingThing::InterestingThing(Sensor *sensor, Spherical16 position,
                                   SwingTwist16 orientation)
    : Thing() {
  // this->id = 0;
  this->confidence = maxConfidence;
  this->sensor = sensor;
  this->position = position;
  // float angle;
  // Vector3 axis;
  // orientation.ToAngleAxis(&angle, &axis);
  this->orientation = orientation; // AngleAxisOf<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.direction.horizontal.InDegrees() -
            otherObj->position.direction.horizontal.InDegrees()) > equalAngle)
    return false;
  if (fabsf(position.direction.vertical.InDegrees() -
            otherObj->position.direction.vertical.InDegrees()) > 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(Spherical16 position, SwingTwist16 orientation) {
  this->position = position;
  this->orientation = orientation;
  // float angle;
  // Vector3 axis;
  // orientation.ToAngleAxis(&angle, &axis);
  // this->orientation = AngleAxisOf<float>(angle, axis);
  this->confidence = maxConfidence;
  this->updated = true;
}