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


#include "ActuatorCmd.h"
#include "util/MathOperations.h"
#include "util/CholeskyDecomposition.h"
#include <cmath>
#include <fsfw/globalfunctions/constants.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>

ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) {
	acsParameters = *acsParameters_;
}

ActuatorCmd::~ActuatorCmd(){

}

void ActuatorCmd::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) {
  using namespace Math;
  // Scaling the commanded torque to a maximum value
  double torque[4] = {0, 0, 0, 0};
  double maxTrq = acsParameters.rwHandlingParameters.maxTrq;
  VectorOperations<double>::add(rwTrqIn, rwTrqNS, torque, 4);

  double maxValue = 0;
  for (int i = 0; i < 4; i++) {  // size of torque, always 4 ?
    if (abs(torque[i]) > maxValue) {
      maxValue = abs(torque[i]);
    }
  }

  if (maxValue > maxTrq) {
    double scalingFactor = maxTrq / maxValue;
    VectorOperations<double>::mulScalar(torque, scalingFactor, torque, 4);
  }

  // Calculating the commanded speed in RPM for every reaction wheel
  double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
  double deltaSpeed[4] = {0, 0, 0, 0};
  double commandTime = acsParameters.onBoardParams.sampleTime,
         inertiaWheel = acsParameters.rwHandlingParameters.inertiaWheel;
  double radToRpm = 60 / (2 * PI);  // factor for conversion to RPM
  //	 W_RW = Torque_RW / I_RW * delta t [rad/s]
  double factor = commandTime / inertiaWheel * radToRpm;
  VectorOperations<double>::mulScalar(torque, factor, deltaSpeed, 4);
  VectorOperations<double>::add(speedRws, deltaSpeed, rwCmdSpeed, 4);
}

void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits) {

//	Convert to Unit frame
	MatrixOperations<double>::multiply(*acsParameters.magnetorquesParameter.inverseAlignment, dipolMoment, dipolMomentUnits, 3, 3, 1);
//  Scaling along largest element if dipol exceeds maximum
	double maxDipol = acsParameters.magnetorquesParameter.DipolMax;
	double maxValue = 0;
	for (int i = 0; i < 3; i++) {
		if (abs(dipolMomentUnits[i]) > maxDipol) {
			maxValue = abs(dipolMomentUnits[i]);
		}
	}

	if (maxValue > maxDipol) {

		double scalingFactor = maxDipol / maxValue;
		VectorOperations<double>::mulScalar(dipolMomentUnits, scalingFactor, dipolMomentUnits, 3);

	}
}