#pragma once

#include "Motor.h"
#include "Placement.h"
#include "Vector2.h"

namespace Passer {
namespace RoboidControl {

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

  void Update(float currentTimeMs);

  void AddMotors(Placement* motors, unsigned int motorCount);
  // void AddQuadcopter(Quadcopter* quadcopter);
  // Quadcopter* GetQuadcopter();

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

  /// @brief Set the maximum linear speed.
  /// @param maxSpeed The new maximum linear speed
  /// For controlled motors, this is rpm.
  /// For calibrated controlled motors, this is m/s
  /// For uncontrolled motors this is a value between 0 and 1 where 1 is the
  /// maximum speed of the motor
  void SetMaxSpeed(float maxSpeed);

  // Velocity control
  virtual void SetTwistSpeed(float forward, float yaw = 0.0F);   // 2D
  virtual void SetTwistSpeed(Vector2 linear, float yaw = 0.0F);  // 2D plus
  virtual void SetTwistSpeed(Vector3 linear,
                             float yaw = 0.0F,
                             float pitch = 0.0F,
                             float roll = 0.0F);  // 3D

  Placement* placement = nullptr;
  unsigned int motorCount = 0;

 protected:
  float maxSpeed = 1;

  bool driving = false;
  float targetDistance = 0;
  time_t startTime;
  float lastUpdateTime;
};

}  // namespace RoboidControl
}  // namespace Passer
using namespace Passer::RoboidControl;