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

#include <time.h>

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

  void Update();

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

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

  // 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);

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

 protected:
  Quadcopter* quadcopter = nullptr;

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