#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;
//   }
//   return finished;
// }

bool Roboid::Drive(Waypoint* waypoint) {
  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(waypoint->point.z);
        break;
      }
      case Thing::Type::Motor: {
        Motor* motor = (Motor*)thing;
        motorFinished = motor->Drive(waypoint->point.z);
        break;
      }
      default:
        break;
    }
    if (motorFinished == false)
      finished = false;
  }
  return finished;
}

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

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

  Waypoint* waypoint = &this->trajectory->waypoints[this->waypointIx];
  // Serial.printf("Driving waypoints %d:  %f %f\n", this->waypointIx,
  //               waypoint->point.z, waypoint->rotation);
  if (Drive(waypoint)) {
    this->waypointIx++;
    if (this->waypointIx == this->trajectory->waypointCount) {
      this->trajectory = nullptr;
      this->waypointIx = 0;
      propulsion.SetTwistSpeed(0, 0);
    }
  }
}