/*
 * ActuatorCmd.h
 *
 *  Created on: 4 Aug 2022
 *      Author: Robin Marquardt
 */

#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:	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 *rwTrqIn, const double *rwTrqNS,
                     double *rwCmdSpeed);

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

 protected:
 private:
  AcsParameters acsParameters;
};

#endif /* ACTUATORCMD_H_ */