#include "SafeCtrl.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 <fsfw/globalfunctions/sign.h>
#include <math.h>

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

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

SafeCtrl::~SafeCtrl() {}

ReturnValue_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const ReturnValue_t mekfValid,
                                         const bool satRotRateValid, const bool sunDirValid) {
  if (not magFieldValid) {
    return SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
  } else if (mekfValid) {
    return SAFECTRL_USE_MEKF;
  } else if (satRotRateValid and sunDirValid) {
    return SAFECTRL_USE_NONMEKF;
  } else if (satRotRateValid and not sunDirValid) {
    return SAFECTRL_USE_DAMPING;
  } else {
    return SAFECTRL_NO_SENSORS_FOR_CONTROL;
  }
}

void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
                        const double *sunDirModelI, const double *quatBI, const double *sunDirRefB,
                        const double *satRotRateRefB, double *magMomB, double &errorAngle) {
  // convert magFieldB from uT to T
  double magFieldBT[3] = {0, 0, 0};
  VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);

  // convert sunDirModel to body rf
  double sunDirB[3] = {0, 0, 0};
  QuaternionOperations::multiplyVector(quatBI, sunDirModelI, sunDirB);

  // calculate angle alpha between sunDirRef and sunDir
  double dotSun = VectorOperations<double>::dot(sunDirRefB, sunDirB);
  errorAngle = acos(dotSun);

  // split rotational rate into parallel and orthogonal parts
  double satRotRateParallelB[3] = {0, 0, 0}, satRotRateOrthogonalB[3] = {0, 0, 0};
  double parallelLength = VectorOperations<double>::dot(satRotRateB, sunDirB) *
                          pow(VectorOperations<double>::norm(sunDirB, 3), -2);
  VectorOperations<double>::mulScalar(sunDirB, parallelLength, satRotRateParallelB, 3);
  VectorOperations<double>::subtract(satRotRateB, satRotRateParallelB, satRotRateOrthogonalB, 3);

  // calculate torque for parallel rotational rate
  double cmdParallel[3] = {0, 0, 0};
  if (errorAngle < (double)acsParameters->safeModeControllerParameters.angleStartSpin) {
    VectorOperations<double>::subtract(satRotRateRefB, satRotRateParallelB, cmdParallel, 3);
    VectorOperations<double>::mulScalar(
        cmdParallel, acsParameters->safeModeControllerParameters.k_parallelMekf, cmdParallel, 3);
  }

  // calculate torque for orthogonal rotational rate
  double cmdOrtho[3] = {0, 0, 0};
  VectorOperations<double>::mulScalar(satRotRateOrthogonalB,
                                      -acsParameters->safeModeControllerParameters.k_orthoMekf,
                                      cmdOrtho, 3);
  // calculate torque for alignment
  double cmdAlign[3] = {0, 0, 0}, crossAlign[3] = {0, 0, 0},
         alignFactor[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  MatrixOperations<double>::multiplyScalar(*acsParameters->inertiaEIVE.inertiaMatrix,
                                           acsParameters->safeModeControllerParameters.k_alignMekf,
                                           *alignFactor, 3, 3);
  VectorOperations<double>::cross(sunDirRefB, sunDirB, crossAlign);
  MatrixOperations<double>::multiply(*alignFactor, crossAlign, cmdAlign, 3, 3, 1);

  // sum of all torques
  double cmdTorque[3] = {0, 0, 0};
  for (uint8_t i = 0; i < 3; i++) {
    cmdTorque[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i];
  }

  // calculate magnetic moment to command
  double torqueMgt[3] = {0, 0, 0};
  VectorOperations<double>::cross(magFieldBT, cmdTorque, torqueMgt);
  double normMag = VectorOperations<double>::norm(magFieldB, 3);
  VectorOperations<double>::mulScalar(torqueMgt, pow(normMag, -2), magMomB, 3);
}

void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB,
                           const double *sunDirB, const double *sunDirRefB,
                           const double *satRotRateRefB, double *magMomB, double &errorAngle) {
  // convert magFieldB from uT to T
  double magFieldBT[3] = {0, 0, 0};
  VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);

  // calculate angle alpha between sunDirRef and sunDir
  double dotSun = VectorOperations<double>::dot(sunDirRefB, sunDirB);
  errorAngle = acos(dotSun);

  // split rotational rate into parallel and orthogonal parts
  double satRotRateParallelB[3] = {0, 0, 0}, satRotRateOrthogonalB[3] = {0, 0, 0};
  double parallelLength = VectorOperations<double>::dot(satRotRateB, sunDirB) *
                          pow(VectorOperations<double>::norm(sunDirB, 3), -2);
  VectorOperations<double>::mulScalar(sunDirB, parallelLength, satRotRateParallelB, 3);
  VectorOperations<double>::subtract(satRotRateB, satRotRateParallelB, satRotRateOrthogonalB, 3);

  // calculate torque for parallel rotational rate
  double cmdParallel[3] = {0, 0, 0};
  if (errorAngle < (double)acsParameters->safeModeControllerParameters.angleStartSpin) {
    VectorOperations<double>::subtract(satRotRateRefB, satRotRateParallelB, cmdParallel, 3);
    VectorOperations<double>::mulScalar(
        cmdParallel, acsParameters->safeModeControllerParameters.k_parallelMekf, cmdParallel, 3);
  }

  // calculate torque for orthogonal rotational rate
  double cmdOrtho[3] = {0, 0, 0};
  VectorOperations<double>::mulScalar(satRotRateOrthogonalB,
                                      -acsParameters->safeModeControllerParameters.k_orthoMekf,
                                      cmdOrtho, 3);

  // calculate torque for alignment
  double cmdAlign[3] = {0, 0, 0}, crossAlign[3] = {0, 0, 0},
         alignFactor[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  MatrixOperations<double>::multiplyScalar(*acsParameters->inertiaEIVE.inertiaMatrix,
                                           acsParameters->safeModeControllerParameters.k_alignMekf,
                                           *alignFactor, 3, 3);
  VectorOperations<double>::cross(sunDirRefB, sunDirB, crossAlign);
  MatrixOperations<double>::multiply(*alignFactor, crossAlign, cmdAlign, 3, 3, 1);

  // sum of all torques
  double cmdTorque[3] = {0, 0, 0};
  for (uint8_t i = 0; i < 3; i++) {
    cmdTorque[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i];
  }

  // calculate magnetic moment to command
  double torqueMgt[3] = {0, 0, 0};
  VectorOperations<double>::cross(magFieldBT, cmdTorque, torqueMgt);
  double normMag = VectorOperations<double>::norm(magFieldB, 3);
  VectorOperations<double>::mulScalar(torqueMgt, pow(normMag, -2), magMomB, 3);
}

void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRateB,
                               const double *satRotRateRefB, double *magMomB, double &errorAngle) {
  // convert magFieldB from uT to T
  double magFieldBT[3] = {0, 0, 0};
  VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);

  // calculate torque for rate damping
  double cmdTorque[3] = {0, 0, 0}, diffSatRotRate[3] = {0, 0, 0};
  VectorOperations<double>::subtract(satRotRateRefB, satRotRateB, diffSatRotRate, 3);
  VectorOperations<double>::mulScalar(
      satRotRateB, acsParameters->safeModeControllerParameters.k_rateDamping, cmdTorque, 3);

  // calculate magnetic moment to command
  double torqueMgt[3] = {0, 0, 0};
  VectorOperations<double>::cross(magFieldBT, cmdTorque, torqueMgt);
  double normMag = VectorOperations<double>::norm(magFieldB, 3);
  VectorOperations<double>::mulScalar(torqueMgt, pow(normMag, -2), magMomB, 3);

  errorAngle = NAN;
}