#include "Roboid.h"
#include <Arduino.h>

Roboid::Roboid() {
  // this->configuration = nullptr;
  // this->thingCount = 0;
}

// Roboid::Roboid(Placement configuration[], unsigned int thingCount) {
//   // this->configuration = configuration;
//   // this->thingCount = thingCount;

//   perception.AddSensors(configuration, thingCount);
//   propulsion.AddMotors(configuration, thingCount);
// }

Roboid::Roboid(Perception* perception, Propulsion* propulsion) {
  this->perception = perception;
  this->propulsion = propulsion;
}

bool Roboid::Drive(Waypoint* waypoint, float currentTimeMs) {
  bool finished =
      propulsion->Drive(waypoint->point, waypoint->rotation, currentTimeMs);
  return finished;
}

void Roboid::FollowTrajectory(Trajectory* trajectory) {
  this->trajectory = trajectory;
  this->waypointIx = 0;
}

void Roboid::Update(float currentTimeMs) {
  if (this->trajectory == nullptr)
    return;

  Waypoint* waypoint = &this->trajectory->waypoints[this->waypointIx];
  if (Drive(waypoint, currentTimeMs)) {
    this->waypointIx++;
    if (this->waypointIx >= this->trajectory->waypointCount) {
      this->trajectory = nullptr;
      this->waypointIx = 0;
      // stop
      // propulsion.SetTwistSpeed(0, 0);
    }
  }
}