#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::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) {
  if (!quatBJValid || !magFieldModelValid || !sunDirModelValid || !rateMekfValid) {
    return SAFECTRL_MEKF_INPUT_INVALID;
  }

  double kRate = acsParameters->safeModeControllerParameters.k_rate_mekf;
  double kAlign = acsParameters->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);

  // change unit from uT to T
  VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldB, 3);

  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 dotSun = VectorOperations<double>::dot(sunDirRef, sunDirB);
  double 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 = 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>::multiplyScalar(*(acsParameters->inertiaEIVE.inertiaMatrix), 10,
                                           *gainMatrixInertia, 3,
                                           3);  // why only for mekf one and not for no mekf
  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;
  return returnvalue::OK;
}

// Will be the version in worst case scenario in event of no working MEKF
ReturnValue_t SafeCtrl::safeNoMekf(const double *magFieldB,
                                   const double *magneticFieldVectorDerivative,
                                   const double *sunVector, const double *sunvectorDerivative,
                                   double omegaRef, double *magMomB, double *spinAxis) {
  if (0) {
    return returnvalue::FAILED;
  }

  double magFieldBT[3] = {0, 0, 0};
  VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);

  double cmdParallel[3] = {0, 0, 0}, cmdAlign[3] = {0, 0, 0}, cmdOrtho[3] = {0, 0, 0},
         torqueCmd[3] = {0, 0, 0};

  bool valid;
  double omega =
      estimateRotationAroundSun(magFieldB, magneticFieldVectorDerivative, sunVector, &valid);

  if (valid) {
    VectorOperations<double>::mulScalar(
        sunVector,
        acsParameters->safeModeControllerParameters.k_parallel_no_mekf * (omegaRef - omega),
        cmdParallel, 3);
    omega = omega * sign<double>(omega);
  }

  VectorOperations<double>::cross(spinAxis, sunVector, cmdAlign);

  VectorOperations<double>::mulScalar(
      cmdAlign, acsParameters->safeModeControllerParameters.k_align_no_mekf, cmdAlign, 3);

  VectorOperations<double>::cross(sunvectorDerivative, sunVector, cmdOrtho);
  VectorOperations<double>::mulScalar(
      cmdOrtho, -acsParameters->safeModeControllerParameters.k_ortho_no_mekf, cmdOrtho, 3);

  // only spin up when the angle to the sun is less than a certain angle.
  // note that we check the cosin, thus "<"
  if (VectorOperations<double>::dot(spinAxis, sunVector) <
      acsParameters->safeModeControllerParameters.cosineStartSpin) {
    VectorOperations<double>::mulScalar(cmdParallel, 0, cmdParallel, 3);
  }

  for (uint8_t i = 0; i < 3; i++) {
    torqueCmd[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i];
  }

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

  *outputAngle = angleAlignErr;
  return returnvalue::OK;
}

double SafeCtrl::estimateRotationAroundSun(const double *magneticFieldVector,
                                           const double *magneticFieldVectorDerivative,
                                           const double *sunVector, bool *updated) {
  *updated = true;
  double vector[3];
  VectorOperations<double>::cross(magneticFieldVector, sunVector, vector);

  float magLength = VectorOperations<double>::norm(magneticFieldVector, 3);
  float length = VectorOperations<double>::norm(vector, 3);

  // only check if angle between B and sun is large enough
  if (length > (acsParameters->safeModeControllerParameters.sineCalculateOmegaSun * magLength)) {
    float omega = VectorOperations<double>::dot(magneticFieldVectorDerivative, vector);
    omega = omega / (length * length);
    lastCalculatedOmega = omega;
    return omega;
  } else {
    *updated = false;
    return lastCalculatedOmega;
  }
}