#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);
}

bool Roboid::Drive(float forwardDistance) {
  bool finished = true;
  for (unsigned int motorIx = 0; motorIx < propulsion.motorCount; motorIx++) {
    bool motorFinished = false;
    Thing* thing = propulsion.placement[motorIx].thing;

    switch (thing->type) {
      case Thing::Type::ControlledMotor: {
        ControlledMotor* controlledMotor = (ControlledMotor*)thing;
        motorFinished = controlledMotor->Drive(forwardDistance);
        break;
      }
      case Thing::Type::Motor: {
        Motor* motor = (Motor*)thing;
        motorFinished = motor->Drive(forwardDistance);
        break;
      }
      default:
        break;
    }
    if (motorFinished == false) {
      finished = false;
    } else
      Serial.printf("Motor finished\n");
  }
  return finished;
}

float waypointArray[] = {1, -1};
void Roboid::FollowTrajectory(Trajectory* Trajectory) {
  this->waypoints = waypointArray;
  this->waypointCount = 2;
  this->waypointIx = 0;
}

void Roboid::Update() {
  if (this->waypoints == nullptr)
    return;

  // Serial.printf("Driving waypoints %d %f\n", this->waypointIx,
  //               waypoints[waypointIx]);
  if (Drive(this->waypoints[this->waypointIx])) {
    this->waypointIx++;
    if (this->waypointIx == this->waypointCount) {
      this->waypoints = nullptr;
      this->waypointIx = 0;
      this->waypointCount = 0;
      propulsion.SetTwistSpeed(0, 0);
    }
  }
}