#include "ActuatorCmd.h"

#include <fsfw/globalfunctions/constants.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>

#include <cmath>

#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"

ActuatorCmd::ActuatorCmd() {}

ActuatorCmd::~ActuatorCmd() {}

void ActuatorCmd::scalingTorqueRws(double *rwTrq, double maxTorque) {
  uint8_t maxIdx = 0;
  VectorOperations<double>::maxAbsValue(rwTrq, 4, &maxIdx);
  double maxValue = rwTrq[maxIdx];

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

void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2,
                                int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed,
                                double sampleTime, int32_t maxRwSpeed, double inertiaWheel) {
  using namespace Math;

  // Calculating the commanded speed in RPM for every reaction wheel
  int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3};
  double deltaSpeed[4] = {0, 0, 0, 0};
  double radToRpm = 60 / (2 * PI);  // factor for conversion to RPM
  //	 W_RW = Torque_RW / I_RW * delta t [rad/s]
  double factor = sampleTime / inertiaWheel * radToRpm;
  int32_t deltaSpeedInt[4] = {0, 0, 0, 0};
  VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4);
  for (int i = 0; i < 4; i++) {
    deltaSpeedInt[i] = std::round(deltaSpeed[i]);
  }
  VectorOperations<int32_t>::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4);
  for (uint8_t i = 0; i < 4; i++) {
    if (rwCmdSpeed[i] > maxRwSpeed) {
      rwCmdSpeed[i] = maxRwSpeed;
    } else if (rwCmdSpeed[i] < -maxRwSpeed) {
      rwCmdSpeed[i] = -maxRwSpeed;
    }
  }
  VectorOperations<int32_t>::mulScalar(rwCmdSpeed, 10, rwCmdSpeed, 4);
}

void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator,
                              const double *inverseAlignment, double maxDipol) {
  // Convert to actuator frame
  double dipolMomentActuatorDouble[3] = {0, 0, 0};
  MatrixOperations<double>::multiply(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 3, 3,
                                     1);
  // Scaling along largest element if dipol exceeds maximum
  uint8_t maxIdx = 0;
  VectorOperations<double>::maxAbsValue(dipolMomentActuatorDouble, 3, &maxIdx);
  double maxAbsValue = abs(dipolMomentActuatorDouble[maxIdx]);
  if (maxAbsValue > maxDipol) {
    double scalingFactor = maxDipol / maxAbsValue;
    VectorOperations<double>::mulScalar(dipolMomentActuatorDouble, scalingFactor,
                                        dipolMomentActuatorDouble, 3);
  }
  // scale dipole from 1 Am^2 to 1e^-4 Am^2
  VectorOperations<double>::mulScalar(dipolMomentActuatorDouble, 1e4, dipolMomentActuatorDouble, 3);
  for (int i = 0; i < 3; i++) {
    dipolMomentActuator[i] = std::round(dipolMomentActuatorDouble[i]);
  }
}