#pragma once
#include "ControlledMotor.h"
#include "Placement.h"
#include "Quadcopter.h"
#include "Vector2.h"

#include <time.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
  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
  void SetLinearSpeed(Vector3 direction, float yawSpeed = 0.0F,
                      float rollSpeed = 0.0F);

  // Position control (or actually, distance control)
  bool Drive(Vector3 point, float rotation, float currentTimeMs);

  float GetOdometer();

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

protected:
  Quadcopter *quadcopter = nullptr;

  float maxSpeed = 1;

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

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