#ifndef ACTUATORCMD_H_
#define ACTUATORCMD_H_

#include "AcsParameters.h"
#include "MultiplicativeKalmanFilter.h"
#include "SensorProcessing.h"
#include "SensorValues.h"

class ActuatorCmd {
 public:
  ActuatorCmd(AcsParameters *acsParameters_);  // Input mode ?
  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
   * 			rwTrqScaled			possible scaled torque
   */
  void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled);

  /*
   * @brief:	cmdSpeedToRws()		will set the maximum possible torque for the reaction
   * wheels, also will calculate the needed revolutions per minute for the RWs, which will be given
   * as Input to the RWs
   * @param:	rwTrqIn 			given torque from pointing controller
   * 			rwTrqNS				Nullspace torque
   * 			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 *rwTorque, double *rwCmdSpeed);

  /*
   * @brief:	cmdDipolMtq()		gives the commanded dipol moment for the magnetorques
   *
   * @param:	dipolMoment 		given dipol moment in spacecraft frame
   * 			dipolMomentUnits	resulting dipol moment for every unit
   */
  void cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits);

 protected:
 private:
  AcsParameters acsParameters;
};

#endif /* ACTUATORCMD_H_ */