#pragma once

#include <ControlledMotor.h>
#include <Placement.h>
#include <Vector2.h>

#include <list>

struct MotorPlacement {
  Motor* motor;
  ControlledMotor* controlledMotor;
  Vector2 position;
};

class Propulsion {
 public:
  /// @brief Setup sensing
  Propulsion();

  void Update();

  void AddMotors(MotorPlacement* motors, unsigned int motorCount);
  void AddMotors(Placement* motors, unsigned int motorCount);

  void SetDiffDriveSpeed(float leftSpeed, float rightSpeed);
  void SetDiffDriveVelocities(float leftVelocity, float rightVelocity);

  void SetTwistSpeed(float forward, float yaw);
  void SetTwistSpeed(float forward, float yaw, float pitch);
  void SetTwistVelocity(float forward, float yaw);

  // Think: drones
  void SetLinearSpeed(Vector3 direction);

 protected:
  unsigned long lastMillis;
  MotorPlacement* motors = nullptr;
  Placement* placement = nullptr;
  unsigned int motorCount = 0;
};