#pragma once

#include "Quadcopter.h"
#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 AddQuadcopter(Quadcopter* quadcopter);

  unsigned int GetMotorCount();
  Motor* GetMotor(unsigned int motorIx);

  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 SetTwistSpeed(Vector3 linear, float yaw);
  void SetTwistVelocity(float forward, float yaw);

  // Think: drones
  Quadcopter* GetQuadcopter();
  void SetLinearSpeed(Vector3 direction, float yawSpeed = 0.0F, float rollSpeed = 0.0F);

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

  Quadcopter* quadcopter = nullptr;
};