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

#ifndef ACTUATORCMD_H_
#define ACTUATORCMD_H_


#include "AcsParameters.h"
#include "SensorProcessing.h"
#include "MultiplicativeKalmanFilter.h"
#include "SensorValues.h"
#include "OutputValues.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_ */