#include "Roboid.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);
}

bool Roboid::Drive(float forwardDistance) {
  bool finished = true;
  for (unsigned int motorIx = 0; motorIx < propulsion.motorCount; motorIx++) {
    Motor* motor = (Motor*)propulsion.placement[motorIx].thing;
    if (motor == nullptr)
      continue;
    if (motor->isControlledMotor == false)
      continue;

    ControlledMotor* controlledMotor = (ControlledMotor*)motor;
    bool motorFinished = controlledMotor->Drive(forwardDistance);
    if (motorFinished == false)
      finished = false;
  }
  return finished;
}