/*
 * SafeCtrl.cpp
 *
 *  Created on: 19 Apr 2022
 *      Author: Robin Marquardt
 */

#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 <math.h>

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

SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) {
  loadAcsParameters(acsParameters_);
  MatrixOperations<double>::multiplyScalar(*(inertiaEIVE->inertiaMatrix), 10, *gainMatrixInertia, 3,
                                           3);
}

SafeCtrl::~SafeCtrl() {}

void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
  safeModeControllerParameters = &(acsParameters_->safeModeControllerParameters);
  inertiaEIVE = &(acsParameters_->inertiaEIVE);
}

ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
                                 double *magFieldModel, bool magFieldModelValid,
                                 double *sunDirModel, bool sunDirModelValid, double *satRateMekf,
                                 bool rateMekfValid, double *sunDirRef, double *satRatRef,
                                 double *outputAngle, double *outputMagMomB, bool *outputValid) {
  if (!quatBJValid || !magFieldModelValid || !sunDirModelValid || !rateMekfValid) {
    *outputValid = false;
    return SAFECTRL_MEKF_INPUT_INVALID;
  }

  double kRate = 0, kAlign = 0;
  kRate = safeModeControllerParameters->k_rate_mekf;
  kAlign = safeModeControllerParameters->k_align_mekf;

  //	Calc sunDirB ,magFieldB with mekf output and model
  double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  MathOperations<double>::dcmFromQuat(quatBJ, *dcmBJ);
  double sunDirB[3] = {0, 0, 0}, magFieldB[3] = {0, 0, 0};
  MatrixOperations<double>::multiply(*dcmBJ, sunDirModel, sunDirB, 3, 3, 1);
  MatrixOperations<double>::multiply(*dcmBJ, magFieldModel, magFieldB, 3, 3, 1);

  double crossSun[3] = {0, 0, 0};

  VectorOperations<double>::cross(sunDirRef, sunDirB, crossSun);
  double normCrossSun = VectorOperations<double>::norm(crossSun, 3);

  //	calc angle alpha between sunDirRef and sunDIr
  double alpha = 0, dotSun = 0;
  dotSun = VectorOperations<double>::dot(sunDirRef, sunDirB);
  alpha = acos(dotSun);

  //	Law Torque calculations
  double torqueCmd[3] = {0, 0, 0}, torqueAlign[3] = {0, 0, 0}, torqueRate[3] = {0, 0, 0},
         torqueAll[3] = {0, 0, 0};

  double scalarFac = 0;
  scalarFac = kAlign * alpha / normCrossSun;
  VectorOperations<double>::mulScalar(crossSun, scalarFac, torqueAlign, 3);

  double rateSafeMode[3] = {0, 0, 0};
  VectorOperations<double>::subtract(satRateMekf, satRatRef, rateSafeMode, 3);
  VectorOperations<double>::mulScalar(rateSafeMode, -kRate, torqueRate, 3);

  VectorOperations<double>::add(torqueRate, torqueAlign, torqueAll, 3);
  //	Adding factor of inertia for axes
  MatrixOperations<double>::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1);

  // MagMom B (orthogonal torque)
  double torqueMgt[3] = {0, 0, 0};
  VectorOperations<double>::cross(magFieldB, torqueCmd, torqueMgt);
  double normMag = VectorOperations<double>::norm(magFieldB, 3);
  VectorOperations<double>::mulScalar(torqueMgt, 1 / pow(normMag, 2), outputMagMomB, 3);

  *outputAngle = alpha;
  *outputValid = true;

  return returnvalue::OK;
}

// Will be the version in worst case scenario in event of no working MEKF (nor GYRs)
void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
                          bool sunRateBValid, double *magFieldB, bool magFieldBValid,
                          double *magRateB, bool magRateBValid, double *sunDirRef,
                          double *satRateRef, double *outputAngle, double *outputMagMomB,
                          bool *outputValid) {
  // Check for invalid Inputs
  if (!susDirBValid || !magFieldBValid || !magRateBValid) {
    *outputValid = false;
    return;
  }

  // normalize sunDir and magDir
  double magDirB[3] = {0, 0, 0};
  VectorOperations<double>::normalize(magFieldB, magDirB, 3);
  VectorOperations<double>::normalize(susDirB, susDirB, 3);

  // Cosinus angle between sunDir and magDir
  double cosAngleSunMag = VectorOperations<double>::dot(magDirB, susDirB);

  // Rate parallel to sun direction and magnetic field direction
  double rateParaSun = 0, rateParaMag = 0;
  double dotSunRateMag = 0, dotmagRateSun = 0, rateFactor = 0;
  dotSunRateMag = VectorOperations<double>::dot(sunRateB, magDirB);
  dotmagRateSun = VectorOperations<double>::dot(magRateB, susDirB);
  rateFactor = 1 - pow(cosAngleSunMag, 2);
  rateParaSun = (dotmagRateSun + cosAngleSunMag * dotSunRateMag) / rateFactor;
  rateParaMag = (dotSunRateMag + cosAngleSunMag * dotmagRateSun) / rateFactor;

  // Full rate or estimate
  double estSatRate[3] = {0, 0, 0};
  double estSatRateMag[3] = {0, 0, 0}, estSatRateSun[3] = {0, 0, 0};
  VectorOperations<double>::mulScalar(susDirB, rateParaSun, estSatRateSun, 3);
  VectorOperations<double>::add(sunRateB, estSatRateSun, estSatRateSun, 3);
  VectorOperations<double>::mulScalar(magDirB, rateParaMag, estSatRateMag, 3);
  VectorOperations<double>::add(magRateB, estSatRateMag, estSatRateMag, 3);
  VectorOperations<double>::add(estSatRateSun, estSatRateMag, estSatRate, 3);
  VectorOperations<double>::mulScalar(estSatRate, 0.5, estSatRate, 3);

  /* Only valid if angle between sun direction and magnetic field direction
   * is sufficiently large */
  double angleSunMag = acos(cosAngleSunMag);
  if (angleSunMag < safeModeControllerParameters->sunMagAngleMin) {
    return;
  }

  // Rate for Torque Calculation
  double diffRate[3] = {0, 0, 0}; /* ADD TO MONITORING */
  VectorOperations<double>::subtract(estSatRate, satRateRef, diffRate, 3);

  // Torque Align calculation
  double kRateNoMekf = 0, kAlignNoMekf = 0;
  kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf;
  kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf;

  double cosAngleAlignErr = VectorOperations<double>::dot(sunDirRef, susDirB);
  double crossSusSunRef[3] = {0, 0, 0};
  VectorOperations<double>::cross(sunDirRef, susDirB, crossSusSunRef);
  double sinAngleAlignErr = VectorOperations<double>::norm(crossSusSunRef, 3);

  double torqueAlign[3] = {0, 0, 0};
  double angleAlignErr = acos(cosAngleAlignErr);
  double torqueAlignFactor = kAlignNoMekf * angleAlignErr / sinAngleAlignErr;
  VectorOperations<double>::mulScalar(crossSusSunRef, torqueAlignFactor, torqueAlign, 3);

  // Torque Rate Calculations
  double torqueRate[3] = {0, 0, 0};
  VectorOperations<double>::mulScalar(diffRate, -kRateNoMekf, torqueRate, 3);

  // Final torque
  double torqueB[3] = {0, 0, 0}, torqueAlignRate[3] = {0, 0, 0};
  VectorOperations<double>::add(torqueRate, torqueAlign, torqueAlignRate, 3);
  MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), torqueAlignRate, torqueB, 3, 3,
                                     1);

  // Magnetic moment
  double magMomB[3] = {0, 0, 0};
  double crossMagFieldTorque[3] = {0, 0, 0};
  VectorOperations<double>::cross(magFieldB, torqueB, crossMagFieldTorque);
  double magMomFactor = pow(VectorOperations<double>::norm(magFieldB, 3), 2);
  VectorOperations<double>::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3);

  std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double));
  *outputAngle = angleAlignErr;
  *outputValid = true;
}