#ifndef ACTUATORCMD_H_
#define ACTUATORCMD_H_

#include <cmath>

class ActuatorCmd {
 public:
  ActuatorCmd();
  virtual ~ActuatorCmd();

  /*
   * @brief:	scalingTorqueRws()	scales the torque via maximum part in case this part is
   * higher then the maximum torque
   * @param:	rwTrq	 			given torque for reaction wheels which will be
   * scaled if needed to be
   */
  void scalingTorqueRws(double *rwTrq, double maxTorque);

  /*
   * @brief:	cmdSpeedToRws()		Calculates the RPM for the reaction wheel configuration,
   * given the required torque calculated by the controller. Will also scale down the RPM of the
   * wheels if they exceed the maximum possible RPM
   * @param:	rwTrq 			    given torque from pointing controller
   * 			rwCmdSpeed			output revolutions per minute for every
   * reaction wheel
   */
  void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
                     const int32_t speedRw3, const double sampleTime, const double inertiaWheel,
                     const int32_t maxRwSpeed, const double *rwTorque, int32_t *rwCmdSpeed);

  /*
   * @brief:	cmdDipoleMtq()			gives the commanded dipole moment for the
   * magnetorquer
   *
   * @param:	dipoleMoment 			given dipole moment in spacecraft frame
   * 			dipoleMomentActuator	resulting dipole moment in actuator reference frame
   */
  void cmdDipoleMtq(const double *inverseAlignment, const double maxDipole,
                    const double *dipoleMoment, int16_t *dipoleMomentActuator);

 protected:
 private:
  static constexpr double RAD_PER_SEC_TO_RPM = 60 / (2 * M_PI);
};

#endif /* ACTUATORCMD_H_ */