#pragma once

#include "Activation.h"
#include "Perception.h"
#include "Placement.h"
#include "Propulsion.h"

class Waypoint {
 public:
  Waypoint(float forwardDistance, float rotation) {
    this->point = Vector3(0, 0, forwardDistance);
    this->distance = forwardDistance;
    this->rotation = rotation;
  }
  float distance = 0;
  Vector3 point = Vector3(0, 0, 0);
  float rotation = 0;
};
class Trajectory {
 public:
  Trajectory(){};
  Trajectory(Waypoint* waypoints, unsigned int waypointCount) {
    this->waypoints = waypoints;
    this->waypointCount = waypointCount;
  }

  Waypoint* waypoints;
  unsigned int waypointCount;
};

class Acceleration {
 public:
  float GetMagnitude() { return 0; };
};

class Roboid {
 public:
  Roboid();
  Roboid(Placement configuration[], unsigned int thingCount);

  Perception perception;
  Propulsion propulsion;
  Acceleration acceleration;

  Placement* configuration;
  unsigned int thingCount;

  void Update();

  // bool Drive(float forwardDistance);
  bool Drive(Waypoint* waypoint);
  void FollowTrajectory(Trajectory* Trajectory);

 public:
  Trajectory* trajectory;
  unsigned int waypointIx = 0;
};