#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>

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(const int32_t speedRw0, const int32_t speedRw1,
                                const int32_t speedRw2, const int32_t speedRw3,
                                const double sampleTime, const double inertiaWheel,
                                const int32_t maxRwSpeed, const double *rwTorque,
                                int32_t *rwCmdSpeed) {
  // group RW speed values (in 0.1 [RPM]) in vector
  int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3};

  // calculate required RW speed as sum of current RW speed and RW speed delta
  // delta w_rw = delta t / I_RW * torque_RW [rad/s]
  double deltaSpeed[4] = {0, 0, 0, 0};
  const double factor = sampleTime / inertiaWheel * RAD_PER_SEC_TO_RPM * 10;
  VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4);

  // convert double to int32
  int32_t deltaSpeedInt[4] = {0, 0, 0, 0};
  for (int i = 0; i < 4; i++) {
    deltaSpeedInt[i] = std::round(deltaSpeed[i]);
  }

  // sum of current RW speed and RW speed delta
  VectorOperations<int32_t>::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4);

  // crop values which would exceed the maximum possible RPM
  for (uint8_t i = 0; i < 4; i++) {
    if (rwCmdSpeed[i] > maxRwSpeed) {
      rwCmdSpeed[i] = maxRwSpeed;
    } else if (rwCmdSpeed[i] < -maxRwSpeed) {
      rwCmdSpeed[i] = -maxRwSpeed;
    }
  }
}

void ActuatorCmd::cmdDipoleMtq(const double *inverseAlignment, const double maxDipole,
                               const double *dipoleMoment, int16_t *dipoleMomentActuator) {
  // convert to actuator frame
  double dipoleMomentActuatorDouble[3] = {0, 0, 0};
  MatrixOperations<double>::multiply(inverseAlignment, dipoleMoment, dipoleMomentActuatorDouble, 3,
                                     3, 1);
  // scaling along largest element if dipole exceeds maximum
  uint8_t maxIdx = 0;
  VectorOperations<double>::maxAbsValue(dipoleMomentActuatorDouble, 3, &maxIdx);
  double maxAbsValue = std::abs(dipoleMomentActuatorDouble[maxIdx]);
  if (maxAbsValue > maxDipole) {
    double scalingFactor = maxDipole / maxAbsValue;
    VectorOperations<double>::mulScalar(dipoleMomentActuatorDouble, scalingFactor,
                                        dipoleMomentActuatorDouble, 3);
  }
  // scale dipole from 1 Am^2 to 1e^-4 Am^2
  VectorOperations<double>::mulScalar(dipoleMomentActuatorDouble, 1e4, dipoleMomentActuatorDouble,
                                      3);
  for (int i = 0; i < 3; i++) {
    dipoleMomentActuator[i] = std::round(dipoleMomentActuatorDouble[i]);
  }
}