RoboidControl-cpp/Perception.cpp

#include "Perception.h"
#include "ControlCore/LinearAlgebra/Angle.h"
#include "ControlCore/Thing.h"
#include "DistanceSensor.h"
#include "NetworkSync.h"
#include "Switch.h"

#include <math.h>

// #define RC_DEBUG
#ifdef RC_DEBUG
#include <Arduino.h>
#endif

unsigned char Perception::maxObjectCount = 7; // 7 is typically the maximum
                                              // number of object which can
                                              // be tracked by a human

Perception::Perception() {
  this->sensorCount = 0;
  this->sensors = new Sensor *[0];

  this->trackedObjects = new InterestingThing *[maxObjectCount];
  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++)
    this->trackedObjects[objIx] = nullptr;
}

Perception::Perception(Sensor **sensors, unsigned int sensorCount)
    : Perception() {
  this->sensorCount = sensorCount;
  this->sensors = new Sensor *[this->sensorCount];
  for (unsigned char sensorIx = 0; sensorIx < this->sensorCount; sensorIx++)
    this->sensors[sensorIx] = sensors[sensorIx];

  this->trackedObjects = new InterestingThing *[maxObjectCount];
  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++)
    this->trackedObjects[objIx] = nullptr;
}

unsigned int Perception::GetSensorCount() { return this->sensorCount; }

Sensor *Perception::GetSensor(unsigned int sensorId) {
  if (sensorId >= this->sensorCount)
    return nullptr;

  Sensor *sensor = this->sensors[sensorId];
  return sensor;
}

unsigned int Perception::AddSensor(Sensor *newSensor) {
  unsigned int newSensorCount = this->sensorCount + 1;
  Sensor **newSensors = new Sensor *[newSensorCount];
  for (unsigned char sensorIx = 0; sensorIx < this->sensorCount; sensorIx++)
    newSensors[sensorIx] = sensors[sensorIx];

  unsigned int sensorId = this->sensorCount;
  newSensors[sensorId] = newSensor;

  Sensor **oldSensors = this->sensors;
  this->sensors = newSensors;
  this->sensorCount = newSensorCount;
  delete[] oldSensors;
  return sensorId;
}

Sensor *Perception::FindSensorOfType(unsigned int sensorType) {
  for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
    Sensor *sensor = this->sensors[sensorIx];
    if (sensor->type == sensorType)
      return sensor;
  }

  return nullptr;
}

float GetPlaneDistance(InterestingThing *plane, float horizontalAngle,
                       float range) {
  float distance = plane->GetPosition().distance;
  float deltaAngle =
      AngleSingle::Normalize(
          AngleSingle::Degrees(
              plane->GetPosition().direction.horizontal.InDegrees() -
              horizontalAngle))
          .InDegrees();
  if (fabsf(deltaAngle) < fabsf(range)) {
    // distance = distance
    // printf("   plane distance = %f (%f-%f)+%f=%f", distance,
    //        (float)plane->position.horizontalAngle, horizontalAngle, range,
    //        deltaAngle);

  } else if (deltaAngle < -range) {
    float angle = deltaAngle + range;
    // printf("   plane distance < %f (%f-%f)+%f=%f %f", distance,
    //        (float)plane->position.horizontalAngle, horizontalAngle, range,
    //        angle, cosf(angle * Angle::Deg2Rad));
    if (angle > -90)
      distance = distance / cosf(angle * Passer::LinearAlgebra::Deg2Rad);
    else
      distance = 9999; // infinity?

  } else if (deltaAngle > range) {
    float angle = deltaAngle - range;
    // printf("   plane distance > %f (%f-%f)-%f=%f %f", distance,
    //        (float)plane->position.horizontalAngle, horizontalAngle, range,
    //        angle, cosf(angle * Angle::Deg2Rad));
    if (angle < 90)
      distance = distance / cosf(angle * Passer::LinearAlgebra::Deg2Rad);
    else
      distance = 9999; // infinity?
  }
  // printf("    distance = %f\n", distance);
  return distance;
}

float Perception::GetDistance(float horizontalDirection, float range) {
  float minDistance = INFINITY;
  if (range < 0)
    range = -range;

  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
    InterestingThing *obj = trackedObjects[objIx];
    if (obj == nullptr)
      continue;

    if (obj->type == 0x080) { // plane
      float planeDistance = GetPlaneDistance(obj, horizontalDirection, range);
      minDistance = fminf(minDistance, planeDistance);
    } else if (obj->GetPosition().direction.horizontal.InDegrees() >
                   horizontalDirection - range &&
               obj->GetPosition().direction.horizontal.InDegrees() <
                   horizontalDirection + range) {
      minDistance = fminf(minDistance, obj->GetPosition().distance);
    }
  }
  return minDistance;
}

float Perception::GetDistanceOfType(unsigned char thingType,
                                    float horizontalAngle, float range) {
  float minDistance = INFINITY;
  if (range < 0)
    range = -range;

  for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
    InterestingThing *thing = trackedObjects[thingIx];
    if (thing == nullptr)
      continue;
    if (thing->type != thingType)
      continue;

    if (thing->type == 0x080) { // plane
      float planeDistance = GetPlaneDistance(thing, horizontalAngle, range);
      minDistance = fminf(minDistance, planeDistance);
    } else if (thing->GetPosition().direction.horizontal.InDegrees() >
                   horizontalAngle - range &&
               thing->GetPosition().direction.horizontal.InDegrees() <
                   horizontalAngle + range) {
      minDistance = fminf(minDistance, thing->GetPosition().distance);
    }
  }
  return minDistance;
}

float Perception::GetDistance(float horizontalDirection,
                              float verticalDirection, float range) {
  float minDistance = INFINITY;
  if (range < 0)
    range = -range;

  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
    InterestingThing *obj = trackedObjects[objIx];
    if (obj == nullptr)
      continue;
    if (obj->GetPosition().direction.horizontal.InDegrees() >
            horizontalDirection - range &&
        obj->GetPosition().direction.horizontal.InDegrees() <
            horizontalDirection + range) {
      minDistance = fminf(minDistance, obj->GetPosition().distance);
    }
  }

  return minDistance;
}
bool Perception::ObjectNearby(float direction, float range) {
  if (range < 0)
    range = -range;

  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
    InterestingThing *obj = trackedObjects[objIx];
    if (obj == nullptr)
      continue;

    if (obj->GetPosition().direction.horizontal.InDegrees() >
            direction - range &&
        obj->GetPosition().direction.horizontal.InDegrees() <
            direction + range) {
      if (obj->GetPosition().distance <= nearbyDistance)
        return true;
    }
  }
  return false;
}

InterestingThing *
Perception::AddTrackedObject(Sensor *sensor, Spherical16 position,
                             SwingTwist16 orientation, unsigned char thingType,
                             unsigned char thingId, unsigned char networkId) {
  InterestingThing *thing = new InterestingThing(sensor, position, orientation);
  if (thingId != 0x00)
    thing->id = thingId;
  thing->type = thingType;

  unsigned char farthestObjIx = 0;
  unsigned char availableSlotIx = 0;
  for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
    if (this->trackedObjects[thingIx] == nullptr) {
      availableSlotIx = thingIx;
    }
    // Do we see the same object?
    else {
      if (thing->IsTheSameAs(this->trackedObjects[thingIx])) {
        this->trackedObjects[thingIx]->Refresh(
            thing->GetPosition(), thing->GetOrientation()); //.ToQuaternion());
        delete thing;

        return this->trackedObjects[thingIx];
      }
      // Is this the fartest object we see?
      else if (this->trackedObjects[farthestObjIx] == nullptr ||
               (this->trackedObjects[thingIx]->GetPosition().distance >
                this->trackedObjects[farthestObjIx]->GetPosition().distance)) {
        farthestObjIx = thingIx;
      }
    }
  }

  // Check if an perception slot is available (we currently see less than the
  // max number of objects)
  if (availableSlotIx < maxObjectCount) {
    // a slot is available
    this->trackedObjects[availableSlotIx] = thing;
    thing->networkId = networkId;
    if (thingId == 0x00)
      thing->id = lastObjectId++; // availableSlotIx + 1;
    return thing;
  }
  // If this object is closer than the farthest object, then replace it
  else if (thing->GetPosition().distance <
           this->trackedObjects[farthestObjIx]->GetPosition().distance) {
    delete this->trackedObjects[farthestObjIx];
    this->trackedObjects[farthestObjIx] = thing;
    thing->networkId = networkId;
    if (thingId == 0x00)
      thing->id = lastObjectId++; // availableSlotIx + 1;
    return thing;
  } else {
    // No available slot, delete trackedobject
    delete thing;
    return nullptr;
  }
}

InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
                                               unsigned char networkId,
                                               unsigned char thingId,
                                               Spherical16 position,
                                               SwingTwist16 orientation) {
  InterestingThing *thing = FindTrackedObject(networkId, thingId);
  if (thing == nullptr) {
    thing = AddTrackedObject(sensor, position, orientation, 0xFF, thingId,
                             networkId);
    // Don't we set this above already?
    thing->networkId = networkId;
    thing->id = thingId;
    thing->type = 0xFF; // unknown
  }
  return thing;
}

InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
                                               Thing *orgThing) {
  InterestingThing *thing = new InterestingThing(
      sensor, orgThing->GetPosition(), orgThing->GetOrientation());
  thing->id = orgThing->id;
  thing->networkId = 0;
  thing->type = orgThing->type;
  thing->name = orgThing->name;
  thing->modelUrl = orgThing->modelUrl;
  thing->modelScale = orgThing->modelScale;

  unsigned char farthestObjIx = 0;
  unsigned char availableSlotIx = 0;
  for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
    if (this->trackedObjects[thingIx] == nullptr) {
      availableSlotIx = thingIx;
    }
    // Do we see the same object?
    else {
      if (thing->IsTheSameAs(this->trackedObjects[thingIx])) {
        this->trackedObjects[thingIx]->Refresh(
            thing->GetPosition(), thing->GetOrientation()); //.ToQuaternion());
        delete thing;

        return this->trackedObjects[thingIx];
      }
      // Is this the fartest object we see?
      else if (this->trackedObjects[farthestObjIx] == nullptr ||
               (this->trackedObjects[thingIx]->GetPosition().distance >
                this->trackedObjects[farthestObjIx]->GetPosition().distance)) {
        farthestObjIx = thingIx;
      }
    }
  }

  // Check if an perception slot is available (we currently see less than the
  // max number of objects)
  if (availableSlotIx < maxObjectCount) {
    // a slot is available
    this->trackedObjects[availableSlotIx] = thing;
    if (thing->id == 0x00)
      thing->id = lastObjectId++; // availableSlotIx + 1;
    return thing;
  }
  // If this object is closer than the farthest object, then replace it
  else if (thing->GetPosition().distance <
           this->trackedObjects[farthestObjIx]->GetPosition().distance) {
    delete this->trackedObjects[farthestObjIx];
    this->trackedObjects[farthestObjIx] = thing;
    if (thing->id == 0x00)
      thing->id = lastObjectId++; // availableSlotIx + 1;
    return thing;
  } else {
    return nullptr;
  }
}

bool Perception::IsInteresting(float distance) {
  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
    InterestingThing *thing = this->trackedObjects[objIx];
    if (thing == nullptr)
      return true;
    if (thing->GetPosition().distance > distance)
      return true;
  }
  return false;
}

InterestingThing *Perception::FindTrackedObject(char objectId) {
  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
    InterestingThing *thing = this->trackedObjects[objIx];
    if (thing == nullptr)
      continue;
    if (thing->id == objectId) {
      return thing;
    }
  }
  return nullptr;
}

InterestingThing *Perception::FindTrackedObject(unsigned char networkId,
                                                unsigned char objectId) {
  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
    InterestingThing *thing = this->trackedObjects[objIx];
    if (thing == nullptr)
      continue;

    if (thing->networkId == networkId && thing->id == objectId) {
      return thing;
    }
  }
  return nullptr;
}

unsigned char Perception::TrackedObjectCount() {
  unsigned char objectCount = 0;
  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
    if (this->trackedObjects[objIx] != nullptr)
      objectCount++;
  }
  return objectCount;
}

InterestingThing **Perception::GetTrackedObjects() {
  return this->trackedObjects;
}

unsigned char Perception::ThingsOfType(unsigned char thingType,
                                       InterestingThing *buffer[],
                                       unsigned char bufferSize) {
  unsigned char thingCount = 0;
  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
    InterestingThing *thing = this->trackedObjects[objIx];
    if (thing == nullptr)
      continue;

    // printf("[%d/%d]%d  ", thing->networkId, thing->id, thing->type);
    if (thing->type == thingType) {
      buffer[thingCount] = thing;
      thingCount++;
      if (thingCount >= bufferSize) {
        // printf("\n");
        return bufferSize;
      }
    }
  }
  // printf("\n");
  return thingCount;
}

InterestingThing *Perception::ThingOfType(unsigned char thingType) {
  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
    InterestingThing *thing = this->trackedObjects[objIx];
    if (thing == nullptr)
      continue;

    if (thing->type == thingType)
      return thing;
  }
  return nullptr;
}

InterestingThing *Perception::GetMostInterestingThing() {
  if (this->trackedObjects == nullptr)
    return nullptr;

  InterestingThing *closestObject = nullptr;
  float closestDistance = INFINITY;
  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
    InterestingThing *obj = this->trackedObjects[objIx];
    if (obj != nullptr) {
      if (obj->GetPosition().distance < closestDistance) {
        closestObject = obj;
        closestDistance = obj->GetPosition().distance;
      }
    }
  }
  return closestObject;
}

void Perception::Update(unsigned long currentTimeMs) {
  float deltaTime = (float)(currentTimeMs - lastUpdateTimeMs) / 1000.0f;
  if (deltaTime <= 0)
    return;

  lastUpdateTimeMs = currentTimeMs;

  // Update sensing
  for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
    Sensor *sensor = sensors[sensorIx];
    if (sensor == nullptr)
      continue;

    if (sensor->type == (unsigned char)Thing::Type::DistanceSensor) {
      DistanceSensor *distanceSensor = (DistanceSensor *)sensor;

      float distance = distanceSensor->GetDistance();
      if (distance >= 0) {
        // Angle16 angle = sensor->position.horizontal;
        //  Polar position = Polar(angle, distance);
        //  Polar position = Polar(distance, angle.ToFloat());
        //  AddTrackedObject(distanceSensor, position);
        Spherical16 position =
            Spherical16(distance, sensor->GetPosition().direction);
        AddTrackedObject(distanceSensor, position);
      }

    } else if (sensor->type == (unsigned char)Thing::Type::Switch) {
      Switch *switchSensor = (Switch *)sensor;
      if (switchSensor->IsOn()) {
        // Polar position = Polar(sensor->position.angle, nearbyDistance);
        AngleSingle horizontal = AngleSingle::Degrees(horizontal.InDegrees());
        PolarSingle position = PolarSingle(nearbyDistance, horizontal);
        // AddTrackedObject(switchSensor, position);
      }
    } else {
      sensor->Update(currentTimeMs);
    }
  }

  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
    InterestingThing *thing = trackedObjects[objIx];
    if (thing == nullptr)
      continue;

    if (thing->DegradeConfidence(deltaTime) == false) {
      // delete obj
      if (roboid != nullptr && roboid->networkSync != nullptr) {
        // roboid->networkSync->SendDestroy((Passer::Control::Thing *)thing);
      }
      this->trackedObjects[objIx] = nullptr;
      delete thing;
    }
  }
}

void Perception::UpdatePose(Polar16 translation) {
  for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
    InterestingThing *thing = trackedObjects[thingIx];
    if (thing == nullptr)
      continue;

    // We only support translations in the horizontal plane at this moment...
    // This needs Spherical operator- to be implemented to work in 3d space
    if (thing->type == 0x80) { // plane
      // printf("[1/%d] %f (%f %f) ", thing->id, thing->position.distance,
      //        (float)thing->position.horizontalAngle,
      //        (float)thing->position.verticalAngle);
      // Update the closest point to the plane
      float angle =
          (float)thing->GetPosition().direction.horizontal.InDegrees() +
          translation.angle.InDegrees();
      angle = fabsf(angle);

      float deltaDistance =
          translation.distance * cosf(angle * Passer::LinearAlgebra::Deg2Rad);
      // printf(" | translate %f %f %f | ", (float)translation.distance,
      //        (float)angle, deltaDistance);
      Spherical16 position = thing->GetPosition();
      position.distance -= deltaDistance;
      thing->SetPosition(position); //.distance -= deltaDistance;
      // printf("-> %f (%f %f)\n", thing->position.distance,
      //        (float)thing->position.horizontalAngle,
      //        (float)thing->position.verticalAngle);
    } else {
      // Polar horizontalPosition = Polar(thing->position);
      // // obj->position.ProjectOnHorizontalPlane();
      // Spherical16 newPosition = Spherical16(horizontalPosition -
      // translation);

      Spherical16 translationS = Spherical16(
          translation.distance, Angle16::Degrees(translation.angle.InDegrees()),
          Angle16::Degrees(0));
      Spherical16 newPosition = thing->GetPosition() + translationS;

      Vector3 oldPos = thing->GetPosition().ToVector3();
      Vector3 newPos = newPosition.ToVector3();
      // printf(" update percepted position (%f 0 %f) -> (%f 0 %f)\n",
      //        oldPos.Right(), oldPos.Forward(), newPos.Right(),
      //        newPos.Forward());

      thing->SetPosition(newPosition);
    }
  }
}

void Perception::UpdatePose(SwingTwist16 rotation) {
  // only rotation around vertical axis is supported for now
  // float rotationAngle;
  // Vector3 rotationAxis;
  // rotation.ToAngleAxis(&rotationAngle, &rotationAxis);
  // Make sure rotation axis is positive
  // if (rotationAxis.Up() < 0)
  //   rotationAngle = -rotationAngle;
  Angle16 rotationAngle = rotation.swing.horizontal;

  for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
    InterestingThing *thing = trackedObjects[thingIx];
    if (thing == nullptr)
      continue;

    // printf("[1/%d] %f (%f %f) ", thing->id, thing->position.distance,
    //        (float)thing->position.horizontalAngle,
    //        (float)thing->position.verticalAngle);
    // printf("| rotate %f  | ", rotationAngle);

    Spherical16 newPosition = thing->GetPosition();
    newPosition.direction.horizontal - rotationAngle;
    thing->SetPosition(newPosition);
    // thing->position.direction.horizontal =
    //     thing->position.direction.horizontal - rotationAngle;

    // printf("-> %f (%f %f) \n", thing->position.distance,
    //        (float)thing->position.horizontalAngle,
    //        (float)thing->position.verticalAngle);
  }
}