eive-obsw/mission/controller/acs/Guidance.cpp

#include "Guidance.h"

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

#include <filesystem>

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

Guidance::Guidance(AcsParameters *acsParameters_) : acsParameters(*acsParameters_) {}

Guidance::~Guidance() {}

void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double targetQuat[4],
                                       double targetSatRotRate[3]) {
  //-------------------------------------------------------------------------------------
  //	Calculation of target quaternion to groundstation or given latitude, longitude and altitude
  //-------------------------------------------------------------------------------------
  // transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
  double targetE[3] = {0, 0, 0};

  MathOperations<double>::cartesianFromLatLongAlt(
      acsParameters.targetModeControllerParameters.latitudeTgt,
      acsParameters.targetModeControllerParameters.longitudeTgt,
      acsParameters.targetModeControllerParameters.altitudeTgt, targetE);

  //  transformation between ECEF and ECI frame
  double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
  MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);

  double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);

  //  target direction in the ECI frame
  double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI = {0, 0, 0};
  MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
  MatrixOperations<double>::multiply(*dcmIE, targetE, targetI, 3, 3, 1);
  VectorOperations<double>::subtract(targetI, posSatI, &targetDirI, 3);

  //	rotation quaternion from two vectors
  double refDirB[3] = {0, 0, 0};
  std::memcpy(refDirB, acsParameters.targetModeControllerParameters.refDirection,
              3 * sizeof(double));
  double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
  double normRefDirB = VectorOperations<double>::norm(refDir, 3);
  double crossDir[3] = {0, 0, 0};
  double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
  VectorOperations<double>::cross(noramlizedTargetDirB, refDir, crossDir);
  targetQuat[0] = crossDir[0];
  targetQuat[1] = crossDir[1];
  targetQuat[2] = crossDir[2];
  targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections);
  VectorOperations<double>::normalize(targetQuat, targetQuat, 4);

  //-------------------------------------------------------------------------------------
  //	Calculation of reference rotation rate
  //-------------------------------------------------------------------------------------
  double velSatB[3] = {0, 0, 0}, velSatBPart1[3] = {0, 0, 0}, velSatBPart2[3] = {0, 0, 0};
  //	Velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E
  MatrixOperations<double>::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1);
  double dcmBEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  MatrixOperations<double>::multiply(*dcmBJ, *dcmJEDot, *dcmBEDot, 3, 3, 3);
  MatrixOperations<double>::multiply(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1);
  VectorOperations<double>::add(velSatBPart1, velSatBPart2, velSatB, 3);

  double normVelSatB = VectorOperations<double>::norm(velSatB, 3);
  double normRefSatRate = normVelSatB / normTargetDirB;

  double satRateDir[3] = {0, 0, 0};
  VectorOperations<double>::cross(velSatB, targetDirB, satRateDir);
  VectorOperations<double>::normalize(satRateDir, satRateDir, 3);
  VectorOperations<double>::mulScalar(satRateDir, normRefSatRate, refSatRate, 3);

  //-------------------------------------------------------------------------------------
  //	Calculation of reference rotation rate in case of star tracker blinding
  //-------------------------------------------------------------------------------------
  if (acsParameters.targetModeControllerParameters.avoidBlindStr) {
    double sunDirB[3] = {0, 0, 0};

    if (susDataProcessed->sunIjkModel.isValid()) {
      double sunDirJ[3] = {0, 0, 0};
      std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
      MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
    } else {
      std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double));
    }

    double exclAngle = acsParameters.strParameters.exclusionAngle,
           blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart,
           blindEnd = acsParameters.targetModeControllerParameters.blindAvoidStop;
    double sightAngleSun =
        VectorOperations<double>::dot(acsParameters.strParameters.boresightAxis, sunDirB);

    if (!(strBlindAvoidFlag)) {
      double critSightAngle = blindStart * exclAngle;

      if (sightAngleSun < critSightAngle) {
        strBlindAvoidFlag = true;
      }

    }

    else {
      if (sightAngleSun < blindEnd * exclAngle) {
        double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate;
        double blindRefRate[3] = {0, 0, 0};

        if (sunDirB[1] < 0) {
          blindRefRate[0] = normBlindRefRate;
          blindRefRate[1] = 0;
          blindRefRate[2] = 0;
        } else {
          blindRefRate[0] = -normBlindRefRate;
          blindRefRate[1] = 0;
          blindRefRate[2] = 0;
        }

        VectorOperations<double>::add(blindRefRate, refSatRate, refSatRate, 3);

      } else {
        strBlindAvoidFlag = false;
      }
    }
  }
}

void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3],
                                      double targetQuat[4], double targetSatRotRate[3]) {
  //-------------------------------------------------------------------------------------
  //	Calculation of target quaternion for target pointing
  //-------------------------------------------------------------------------------------
  // transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
  double targetE[3] = {0, 0, 0};
  MathOperations<double>::cartesianFromLatLongAlt(
      acsParameters.targetModeControllerParameters.latitudeTgt,
      acsParameters.targetModeControllerParameters.longitudeTgt,
      acsParameters.targetModeControllerParameters.altitudeTgt, targetE);
  double targetDirE[3] = {0, 0, 0};
  VectorOperations<double>::subtract(targetE, posSatE, targetDirE, 3);

  // transformation between ECEF and ECI frame
  double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
  MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);

  double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);

  // target direction in the ECI frame
  double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI[3] = {0, 0, 0};
  MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
  MatrixOperations<double>::multiply(*dcmIE, targetE, targetI, 3, 3, 1);
  VectorOperations<double>::subtract(targetI, posSatI, targetDirI, 3);

  // x-axis aligned with target direction
  // this aligns with the camera, E- and S-band antennas
  double xAxis[3] = {0, 0, 0};
  VectorOperations<double>::normalize(targetDirI, xAxis, 3);

  // transform velocity into inertial frame
  double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
  MatrixOperations<double>::multiply(*dcmIE, velSatE, velPart1, 3, 3, 1);
  MatrixOperations<double>::multiply(*dcmIEDot, posSatE, velPart2, 3, 3, 1);
  VectorOperations<double>::add(velPart1, velPart2, velocityI, 3);

  // orbital normal vector of target and velocity vector
  double orbitalNormalI[3] = {0, 0, 0};
  VectorOperations<double>::cross(posSatI, velocityI, orbitalNormalI);
  VectorOperations<double>::normalize(orbitalNormalI, orbitalNormalI, 3);

  // y-axis of satellite in orbit plane so that z-axis is parallel to long side of picture
  // resolution
  double yAxis[3] = {0, 0, 0};
  VectorOperations<double>::cross(orbitalNormalI, xAxis, yAxis);
  VectorOperations<double>::normalize(yAxis, yAxis, 3);

  // z-axis completes RHS
  double zAxis[3] = {0, 0, 0};
  VectorOperations<double>::cross(xAxis, yAxis, zAxis);

  // join transformation matrix
  double dcmIX[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
                        {xAxis[1], yAxis[1], zAxis[1]},
                        {xAxis[2], yAxis[2], zAxis[2]}};
  QuaternionOperations::fromDcm(dcmIX, targetQuat);

  int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
  targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
}

void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3],
                               double targetQuat[4], double targetSatRotRate[3]) {
  //-------------------------------------------------------------------------------------
  //	Calculation of target quaternion for ground station pointing
  //-------------------------------------------------------------------------------------
  // transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
  double groundStationE[3] = {0, 0, 0};

  MathOperations<double>::cartesianFromLatLongAlt(
      acsParameters.gsTargetModeControllerParameters.latitudeTgt,
      acsParameters.gsTargetModeControllerParameters.longitudeTgt,
      acsParameters.gsTargetModeControllerParameters.altitudeTgt, groundStationE);
  double targetDirE[3] = {0, 0, 0};
  VectorOperations<double>::subtract(groundStationE, posSatE, targetDirE, 3);

  // transformation between ECEF and ECI frame
  double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
  MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);

  double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);

  // target direction in the ECI frame
  double posSatI[3] = {0, 0, 0}, groundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0};
  MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
  MatrixOperations<double>::multiply(*dcmIE, groundStationE, groundStationI, 3, 3, 1);
  VectorOperations<double>::subtract(groundStationI, posSatI, groundStationDirI, 3);

  // x-axis aligned with target direction
  // this aligns with the camera, E- and S-band antennas
  double xAxis[3] = {0, 0, 0};
  VectorOperations<double>::normalize(groundStationDirI, xAxis, 3);

  // get sun vector model in ECI
  VectorOperations<double>::normalize(sunDirI, sunDirI, 3);

  // calculate z-axis as projection of sun vector into plane defined by x-axis as normal vector
  // z = sPerpenticular =  s - sParallel = s - (x*s)/norm(x)^2 * x
  double xDotS = VectorOperations<double>::dot(xAxis, sunDirI);
  xDotS /= pow(VectorOperations<double>::norm(xAxis, 3), 2);
  double sunParallel[3], zAxis[3];
  VectorOperations<double>::mulScalar(xAxis, xDotS, sunParallel, 3);
  VectorOperations<double>::subtract(sunDirI, sunParallel, zAxis, 3);
  VectorOperations<double>::normalize(zAxis, zAxis, 3);

  // y-axis completes RHS
  double yAxis[3];
  VectorOperations<double>::cross(zAxis, xAxis, yAxis);
  VectorOperations<double>::normalize(yAxis, yAxis, 3);

  // join transformation matrix
  double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
                         {xAxis[1], yAxis[1], zAxis[1]},
                         {xAxis[2], yAxis[2], zAxis[2]}};
  QuaternionOperations::fromDcm(dcmTgt, targetQuat);

  int8_t timeElapsedMax = acsParameters.gsTargetModeControllerParameters.timeElapsedMax;
  targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
}

void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]) {
  //-------------------------------------------------------------------------------------
  //	Calculation of target quaternion to sun
  //-------------------------------------------------------------------------------------
  //  positive z-Axis of EIVE in direction of sun
  double zAxis[3] = {0, 0, 0};
  VectorOperations<double>::normalize(sunDirI, zAxis, 3);

  //  assign helper vector (north pole inertial)
  double helperVec[3] = {0, 0, 1};

  //  construct y-axis from helper vector and z-axis
  double yAxis[3] = {0, 0, 0};
  VectorOperations<double>::cross(zAxis, helperVec, yAxis);
  VectorOperations<double>::normalize(yAxis, yAxis, 3);

  //  x-axis completes RHS
  double xAxis[3] = {0, 0, 0};
  VectorOperations<double>::cross(yAxis, zAxis, xAxis);
  VectorOperations<double>::normalize(xAxis, xAxis, 3);

  // join transformation matrix
  double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
                         {xAxis[1], yAxis[1], zAxis[1]},
                         {xAxis[2], yAxis[2], zAxis[2]}};
  QuaternionOperations::fromDcm(dcmTgt, targetQuat);

  //----------------------------------------------------------------------------
  //	Calculation of reference rotation rate
  //----------------------------------------------------------------------------
  refSatRate[0] = 0;
  refSatRate[1] = 0;
  refSatRate[2] = 0;
}

void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4],
                                      double refSatRate[3]) {  // old version of Nadir Pointing
  //-------------------------------------------------------------------------------------
  //	Calculation of target quaternion for Nadir pointing
  //-------------------------------------------------------------------------------------
  double targetDirE[3] = {0, 0, 0};
  VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);

  //	Transformation between ECEF and ECI frame
  double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
  MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);

  double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);

  //	Transformation between ECEF and Body frame
  double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  double quatBJ[4] = {0, 0, 0, 0};
  std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
  QuaternionOperations::toDcm(quatBJ, dcmBJ);
  MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3);

  //	Target Direction in the body frame
  double targetDirB[3] = {0, 0, 0};
  MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);

  //	rotation quaternion from two vectors
  double refDir[3] = {0, 0, 0};
  refDir[0] = acsParameters.nadirModeControllerParameters.refDirection[0];
  refDir[1] = acsParameters.nadirModeControllerParameters.refDirection[1];
  refDir[2] = acsParameters.nadirModeControllerParameters.refDirection[2];
  double noramlizedTargetDirB[3] = {0, 0, 0};
  VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
  VectorOperations<double>::normalize(refDir, refDir, 3);
  double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
  double normRefDir = VectorOperations<double>::norm(refDir, 3);
  double crossDir[3] = {0, 0, 0};
  double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
  VectorOperations<double>::cross(noramlizedTargetDirB, refDir, crossDir);
  targetQuat[0] = crossDir[0];
  targetQuat[1] = crossDir[1];
  targetQuat[2] = crossDir[2];
  targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections);
  VectorOperations<double>::normalize(targetQuat, targetQuat, 4);

  //-------------------------------------------------------------------------------------
  //	Calculation of reference rotation rate
  //-------------------------------------------------------------------------------------
  refSatRate[0] = 0;
  refSatRate[1] = 0;
  refSatRate[2] = 0;
}

void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3],
                                           double targetQuat[4], double refSatRate[3]) {
  //-------------------------------------------------------------------------------------
  //	Calculation of target quaternion for Nadir pointing
  //-------------------------------------------------------------------------------------
  // transformation between ECEF and ECI frame
  double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
  MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);

  double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);

  // satellite position in inertial reference frame
  double posSatI[3] = {0, 0, 0};
  MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);

  // negative x-axis aligned with position vector
  // this aligns with the camera, E- and S-band antennas
  double xAxis[3] = {0, 0, 0};
  VectorOperations<double>::normalize(posSatI, xAxis, 3);
  VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);

  // make z-Axis parallel to major part of camera resolution
  double zAxis[3] = {0, 0, 0};
  double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
  MatrixOperations<double>::multiply(*dcmIE, velSatE, velPart1, 3, 3, 1);
  MatrixOperations<double>::multiply(*dcmIEDot, posSatE, velPart2, 3, 3, 1);
  VectorOperations<double>::add(velPart1, velPart2, velocityI, 3);
  VectorOperations<double>::cross(xAxis, velocityI, zAxis);
  VectorOperations<double>::normalize(zAxis, zAxis, 3);

  // y-Axis completes RHS
  double yAxis[3] = {0, 0, 0};
  VectorOperations<double>::cross(zAxis, xAxis, yAxis);

  // join transformation matrix
  double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
                         {xAxis[1], yAxis[1], zAxis[1]},
                         {xAxis[2], yAxis[2], zAxis[2]}};
  QuaternionOperations::fromDcm(dcmTgt, targetQuat);

  int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax;
  targetRotationRate(timeElapsedMax, now, targetQuat, refSatRate);
}

void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
                          double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
                          double errorQuat[4], double errorSatRotRate[3], double errorAngle) {
  // First calculate error quaternion between current and target orientation
  QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
  // Last calculate add rotation from reference quaternion
  QuaternionOperations::multiply(refQuat, errorQuat, errorQuat);
  // Keep scalar part of quaternion positive
  if (errorQuat[3] < 0) {
    VectorOperations<double>::mulScalar(errorQuat, -1, errorQuat, 4);
  }
  // Calculate error angle
  errorAngle = QuaternionOperations::getAngle(errorQuat, true);

  // Only give back error satellite rotational rate if orientation has already been aquired
  if (errorAngle < 2. / 180. * M_PI) {
    // First combine the target and reference satellite rotational rates
    double combinedRefSatRotRate[3] = {0, 0, 0};
    VectorOperations<double>::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3);
    // Then substract the combined required satellite rotational rates from the actual rate
    VectorOperations<double>::subtract(currentSatRotRate, combinedRefSatRotRate, errorSatRotRate,
                                       3);
  } else {
    // If orientation has not been aquired yet set satellite rotational rate to zero
    errorSatRotRate = 0;
  }

  // target flag in matlab, importance, does look like it only gives feedback if pointing control is
  // under 150 arcsec ??
}

void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
                          double targetSatRotRate[3], double errorQuat[4],
                          double errorSatRotRate[3], double errorAngle) {
  // First calculate error quaternion between current and target orientation
  QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
  // Keep scalar part of quaternion positive
  if (errorQuat[3] < 0) {
    VectorOperations<double>::mulScalar(errorQuat, -1, errorQuat, 4);
  }
  // Calculate error angle
  errorAngle = QuaternionOperations::getAngle(errorQuat, true);

  // Only give back error satellite rotational rate if orientation has already been aquired
  if (errorAngle < 2. / 180. * M_PI) {
    // Then substract the combined required satellite rotational rates from the actual rate
    VectorOperations<double>::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3);
  } else {
    // If orientation has not been aquired yet set satellite rotational rate to zero
    errorSatRotRate = 0;
  }

  // target flag in matlab, importance, does look like it only gives feedback if pointing control is
  // under 150 arcsec ??
}

void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
                                  double *refSatRate) {
  //-------------------------------------------------------------------------------------
  //	Calculation of target rotation rate
  //-------------------------------------------------------------------------------------
  double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) -
                       (timeSavedQuaternion.tv_sec +
                        timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6));
  if (timeElapsed < timeElapsedMax) {
    double qDiff[4] = {0, 0, 0, 0};
    VectorOperations<double>::subtract(quatInertialTarget, savedQuaternion, qDiff, 4);
    VectorOperations<double>::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4);

    double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]},
           qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]};
    double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0};
    VectorOperations<double>::cross(quatInertialTarget, qDiff, sum1);
    VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3);
    VectorOperations<double>::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3);
    VectorOperations<double>::add(sum1, sum2, sum, 3);
    VectorOperations<double>::subtract(sum, sum3, sum, 3);
    double omegaRefNew[3] = {0, 0, 0};
    VectorOperations<double>::mulScalar(sum, -2, omegaRefNew, 3);

    VectorOperations<double>::mulScalar(omegaRefNew, 2, refSatRate, 3);
    VectorOperations<double>::subtract(refSatRate, omegaRefSaved, refSatRate, 3);
    omegaRefSaved[0] = omegaRefNew[0];
    omegaRefSaved[1] = omegaRefNew[1];
    omegaRefSaved[2] = omegaRefNew[2];
  } else {
    refSatRate[0] = 0;
    refSatRate[1] = 0;
    refSatRate[2] = 0;
  }

  timeSavedQuaternion = now;
  savedQuaternion[0] = quatInertialTarget[0];
  savedQuaternion[1] = quatInertialTarget[1];
  savedQuaternion[2] = quatInertialTarget[2];
  savedQuaternion[3] = quatInertialTarget[3];
}

ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
                                                double *rwPseudoInv) {
  bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid());
  bool rw2valid = (sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid());
  bool rw3valid = (sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid());
  bool rw4valid = (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid());

  if (rw1valid && rw2valid && rw3valid && rw4valid) {
    std::memcpy(rwPseudoInv, acsParameters.rwMatrices.pseudoInverse, 12 * sizeof(double));
    return returnvalue::OK;
  } else if (!rw1valid && rw2valid && rw3valid && rw4valid) {
    std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without1, 12 * sizeof(double));
    return returnvalue::OK;
  } else if (rw1valid && !rw2valid && rw3valid && rw4valid) {
    std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without2, 12 * sizeof(double));
    return returnvalue::OK;
  } else if (rw1valid && rw2valid && !rw3valid && rw4valid) {
    std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without3, 12 * sizeof(double));
    return returnvalue::OK;
  } else if (rw1valid && rw2valid && rw3valid && !rw4valid) {
    std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without4, 12 * sizeof(double));
    return returnvalue::OK;
  } else {
    // 	    @note: This one takes the normal pseudoInverse of all four raction wheels valid.
    //		Does not make sense, but is implemented that way in MATLAB ?!
    //		Thought: It does not really play a role, because in case there are more then one
    //		reaction wheel invalid the pointing control is destined to fail.
    return returnvalue::FAILED;
  }
}

void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) {
  if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE) or
      not std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) {  // ToDo: if file does not exist anymore
    std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir,
                3 * sizeof(double));
  } else {
    std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop,
                3 * sizeof(double));
  }
  std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef,
              3 * sizeof(double));
}