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

/*
 * Guidance.cpp
 *
 *  Created on: 6 Jun 2022
 *      Author: Robin Marquardt
 */

#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 "string.h"
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"

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

Guidance::~Guidance() {}

void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) {
  for (int i = 0; i < 3; i++) {
    sunTargetSafe[i] = acsParameters.safeModeControllerParameters.sunTargetDir[i];
    satRateSafe[i] = acsParameters.safeModeControllerParameters.satRateRef[i];
  }

  //	memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24);
}

void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
                                       acsctrl::SusDataProcessed *susDataProcessed,
                                       acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
                                       double targetQuat[4], double refSatRate[3]) {
  //-------------------------------------------------------------------------------------
  //	Calculation of target quaternion to groundstation or given latitude, longitude and altitude
  //-------------------------------------------------------------------------------------
  //	Transform longitude, latitude and altitude to cartesian coordiantes (earth
  // fixed/centered frame)
  double groundStationCart[3] = {0, 0, 0};

  MathOperations<double>::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs,
                                                  acsParameters.groundStationParameters.longitudeGs,
                                                  acsParameters.groundStationParameters.altitudeGs,
                                                  groundStationCart);

  //	Position of the satellite in the earth/fixed frame via GPS
  double posSatE[3] = {0, 0, 0};
  double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
  double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
  MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
                                                  sensorValues->gpsSet.altitude.value, posSatE);

  //	Target direction in the ECEF frame
  double targetDirE[3] = {0, 0, 0};
  VectorOperations<double>::subtract(groundStationCart, posSatE, targetDirE, 3);

  //	Transformation between ECEF and IJK 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.targetModeControllerParameters.refDirection[0];
  refDir[1] = acsParameters.targetModeControllerParameters.refDirection[1];
  refDir[2] = acsParameters.targetModeControllerParameters.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
  //-------------------------------------------------------------------------------------
  double velSatE[3] = {0, 0, 0};
  std::memcpy(velSatE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
  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(ACS::SensorValues *sensorValues,
                                      acsctrl::GpsDataProcessed *gpsDataProcessed,
                                      acsctrl::MekfData *mekfData, timeval now,
                                      double targetQuat[4], double refSatRate[3]) {
  //-------------------------------------------------------------------------------------
  //	Calculation of target quaternion for target pointing
  //-------------------------------------------------------------------------------------
  //	Transform longitude, latitude and altitude to cartesian coordiantes (earth
  // fixed/centered frame)
  double groundStationCart[3] = {0, 0, 0};

  MathOperations<double>::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs,
                                                  acsParameters.groundStationParameters.longitudeGs,
                                                  acsParameters.groundStationParameters.altitudeGs,
                                                  groundStationCart);
  //	Position of the satellite in the earth/fixed frame via GPS
  double posSatE[3] = {0, 0, 0};
  double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
  double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
  MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
                                                  sensorValues->gpsSet.altitude.value, posSatE);
  double targetDirE[3] = {0, 0, 0};
  VectorOperations<double>::subtract(groundStationCart, posSatE, targetDirE, 3);

  //	Transformation between ECEF and IJK 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);

  //	Target Direction and position vector in the inertial frame
  double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0};
  MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1);
  MatrixOperations<double>::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1);

  // negative x-Axis aligned with target (Camera/E-band transmitter position)
  double xAxis[3] = {0, 0, 0};
  VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
  VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);

  // Transform velocity into inertial frame
  double velocityE[3];
  std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
  double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
  MatrixOperations<double>::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1);
  MatrixOperations<double>::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1);
  VectorOperations<double>::add(velPart1, velPart2, velocityJ, 3);

  // orbital normal vector
  double orbitalNormalJ[3] = {0, 0, 0};
  VectorOperations<double>::cross(posSatJ, velocityJ, orbitalNormalJ);
  VectorOperations<double>::normalize(orbitalNormalJ, orbitalNormalJ, 3);

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

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

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

  //-------------------------------------------------------------------------------------
  //	Calculation of reference rotation rate
  //-------------------------------------------------------------------------------------
  double timeElapsed =
      now.tv_sec + now.tv_usec * pow(10, -6) -
      (timeSavedQuaternionNadir.tv_sec +
       timeSavedQuaternionNadir.tv_usec * pow((double)timeSavedQuaternionNadir.tv_usec, -6));
  if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) {
    double qDiff[4] = {0, 0, 0, 0};
    VectorOperations<double>::subtract(quatInertialTarget, savedQuaternionNadir, 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, omegaRefSavedNadir, refSatRate, 3);
    omegaRefSavedNadir[0] = omegaRefNew[0];
    omegaRefSavedNadir[1] = omegaRefNew[1];
    omegaRefSavedNadir[2] = omegaRefNew[2];
  } else {
    refSatRate[0] = 0;
    refSatRate[1] = 0;
    refSatRate[2] = 0;
  }

  timeSavedQuaternionNadir = now;
  savedQuaternionNadir[0] = quatInertialTarget[0];
  savedQuaternionNadir[1] = quatInertialTarget[1];
  savedQuaternionNadir[2] = quatInertialTarget[2];
  savedQuaternionNadir[3] = quatInertialTarget[3];

  // Transform in system relative to satellite frame
  double quatBJ[4] = {0, 0, 0, 0};
  std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
  QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
}

void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
                          acsctrl::SusDataProcessed *susDataProcessed,
                          acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
                          double targetQuat[4], double refSatRate[3]) {
  //-------------------------------------------------------------------------------------
  //	Calculation of target quaternion to sun
  //-------------------------------------------------------------------------------------
  double quatBJ[4] = {0, 0, 0, 0};
  double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
  QuaternionOperations::toDcm(quatBJ, dcmBJ);

  double sunDirJ[3] = {0, 0, 0}, sunDirB[3] = {0, 0, 0};
  if (susDataProcessed->sunIjkModel.isValid()) {
    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));
  }

  /*
  // ---------------------------------------------------------------------------
  // Old version of two vector quaternion (only one axis to align)
  // ---------------------------------------------------------------------------
  double sunRef[3] = {0, 0, 0};
  sunRef[0] = acsParameters.safeModeControllerParameters.sunTargetDir[0];
  sunRef[1] = acsParameters.safeModeControllerParameters.sunTargetDir[1];
  sunRef[2] = acsParameters.safeModeControllerParameters.sunTargetDir[2];

  double sunCross[3] = {0, 0, 0};
  VectorOperations<double>::cross(sunDirB, sunRef, sunCross);
  double normSunDir = VectorOperations<double>::norm(sunDirB, 3);
  double normSunRef = VectorOperations<double>::norm(sunRef, 3);
  double dotSun = VectorOperations<double>::dot(sunDirB, sunRef);

  targetQuat[0] = sunCross[0];
  targetQuat[1] = sunCross[1];
  targetQuat[2] = sunCross[2];
  targetQuat[3] = sqrt(pow(normSunDir,2) * pow(normSunRef,2) + dotSun);

  VectorOperations<double>::normalize(targetQuat, targetQuat, 4);
  */

  //----------------------------------------------------------------------------
  // New version
  //----------------------------------------------------------------------------
  //	Transformation between ECEF and IJK 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);

  // positive z-Axis of EIVE in direction of sun
  double zAxis[3] = {0, 0, 0};
  VectorOperations<double>::normalize(sunDirB, zAxis, 3);

  // Position of the satellite in the earth/fixed frame via GPS and body
  // velocity
  double posSatE[3] = {0, 0, 0};
  double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
  double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
  MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
                                                  sensorValues->gpsSet.altitude.value, posSatE);
  double velocityE[3];
  std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
  double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
  MatrixOperations<double>::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1);
  MatrixOperations<double>::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1);
  VectorOperations<double>::add(velPart1, velPart2, velocityJ, 3);
  double velocityB[3] = {0, 0, 0};
  MatrixOperations<double>::multiply(*dcmBJ, velocityJ, velocityB, 3, 3, 1);

  // Normale to velocity and sunDir
  double crossVelSun[3] = {0, 0, 0};
  VectorOperations<double>::cross(velocityB, sunDirB, crossVelSun);

  // y- Axis as cross of normal velSun and zAxis
  double yAxis[3] = {0, 0, 0};
  VectorOperations<double>::cross(crossVelSun, sunDirB, yAxis);
  VectorOperations<double>::normalize(yAxis, yAxis, 3);

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

  // Transformation matrix to Sun, no further transforamtions, reference is already
  // the EIVE body frame
  double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
                         {xAxis[1], yAxis[1], zAxis[1]},
                         {xAxis[2], yAxis[2], zAxis[2]}};
  double quatSun[4] = {0, 0, 0, 0};
  QuaternionOperations::fromDcm(dcmTgt, quatSun);

  targetQuat[0] = quatSun[0];
  targetQuat[1] = quatSun[1];
  targetQuat[2] = quatSun[2];
  targetQuat[3] = quatSun[3];

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

void Guidance::quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
                                      timeval now, double targetQuat[4],
                                      double refSatRate[3]) {  // old version of Nadir Pointing
  //-------------------------------------------------------------------------------------
  //	Calculation of target quaternion for Nadir pointing
  //-------------------------------------------------------------------------------------
  //	Position of the satellite in the earth/fixed frame via GPS
  double posSatE[3] = {0, 0, 0};
  double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
  double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
  MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
                                                  sensorValues->gpsSet.altitude.value, posSatE);
  double targetDirE[3] = {0, 0, 0};
  VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);

  //	Transformation between ECEF and IJK 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.targetModeControllerParameters.nadirRefDirection[0];
  refDir[1] = acsParameters.targetModeControllerParameters.nadirRefDirection[1];
  refDir[2] = acsParameters.targetModeControllerParameters.nadirRefDirection[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::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues,
                                     acsctrl::GpsDataProcessed *gpsDataProcessed,
                                     acsctrl::MekfData *mekfData, timeval now, double targetQuat[4],
                                     double refSatRate[3]) {
  //-------------------------------------------------------------------------------------
  //	Calculation of target quaternion for Nadir pointing
  //-------------------------------------------------------------------------------------
  //	Position of the satellite in the earth/fixed frame via GPS
  double posSatE[3] = {0, 0, 0};
  double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
  double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
  MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
                                                  sensorValues->gpsSet.altitude.value, posSatE);
  double targetDirE[3] = {0, 0, 0};
  VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);

  //	Transformation between ECEF and IJK 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);

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

  // negative x-Axis aligned with target (Camera position)
  double xAxis[3] = {0, 0, 0};
  VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
  VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);

  // z-Axis parallel to long side of picture resolution
  double zAxis[3] = {0, 0, 0}, velocityE[3];
  std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
  double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
  MatrixOperations<double>::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1);
  MatrixOperations<double>::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1);
  VectorOperations<double>::add(velPart1, velPart2, velocityJ, 3);
  VectorOperations<double>::cross(xAxis, velocityJ, zAxis);
  VectorOperations<double>::normalize(zAxis, zAxis, 3);

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

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

  //-------------------------------------------------------------------------------------
  //	Calculation of reference rotation rate
  //-------------------------------------------------------------------------------------
  double timeElapsed =
      now.tv_sec + now.tv_usec * pow(10, -6) -
      (timeSavedQuaternionNadir.tv_sec +
       timeSavedQuaternionNadir.tv_usec * pow((double)timeSavedQuaternionNadir.tv_usec, -6));
  if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) {
    double qDiff[4] = {0, 0, 0, 0};
    VectorOperations<double>::subtract(quatInertialTarget, savedQuaternionNadir, 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, omegaRefSavedNadir, refSatRate, 3);
    omegaRefSavedNadir[0] = omegaRefNew[0];
    omegaRefSavedNadir[1] = omegaRefNew[1];
    omegaRefSavedNadir[2] = omegaRefNew[2];
  } else {
    refSatRate[0] = 0;
    refSatRate[1] = 0;
    refSatRate[2] = 0;
  }

  timeSavedQuaternionNadir = now;
  savedQuaternionNadir[0] = quatInertialTarget[0];
  savedQuaternionNadir[1] = quatInertialTarget[1];
  savedQuaternionNadir[2] = quatInertialTarget[2];
  savedQuaternionNadir[3] = quatInertialTarget[3];

  // Transform in system relative to satellite frame
  double quatBJ[4] = {0, 0, 0, 0};
  std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
  QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
}

void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) {
  for (int i = 0; i < 4; i++) {
    targetQuat[i] = acsParameters.inertialModeControllerParameters.tgtQuatInertial[i];
  }
  for (int i = 0; i < 3; i++) {
    refSatRate[i] = acsParameters.inertialModeControllerParameters.tgtRotRateInertial[i];
  }
}

void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3],
                          double quatErrorComplete[4], double quatError[3], double deltaRate[3]) {
  double quatRef[4] = {0, 0, 0, 0};
  quatRef[0] = acsParameters.targetModeControllerParameters.quatRef[0];
  quatRef[1] = acsParameters.targetModeControllerParameters.quatRef[1];
  quatRef[2] = acsParameters.targetModeControllerParameters.quatRef[2];
  quatRef[3] = acsParameters.targetModeControllerParameters.quatRef[3];

  double satRate[3] = {0, 0, 0};
  std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double));
  VectorOperations<double>::subtract(satRate, refSatRate, deltaRate, 3);
  // valid checks ?
  double quatErrorMtx[4][4] = {{quatRef[3], quatRef[2], -quatRef[1], -quatRef[0]},
                               {-quatRef[2], quatRef[3], quatRef[0], -quatRef[1]},
                               {quatRef[1], -quatRef[0], quatRef[3], -quatRef[2]},
                               {quatRef[0], -quatRef[1], quatRef[2], quatRef[3]}};

  MatrixOperations<double>::multiply(*quatErrorMtx, targetQuat, quatErrorComplete, 4, 4, 1);

  if (quatErrorComplete[3] < 0) {
    quatErrorComplete[3] *= -1;
  }

  quatError[0] = quatErrorComplete[0];
  quatError[1] = quatErrorComplete[1];
  quatError[2] = quatErrorComplete[2];

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

void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) {
  if (sensorValues->rw1Set.isValid() && sensorValues->rw2Set.isValid() &&
      sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) {
    rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0];
    rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1];
    rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2];
    rwPseudoInv[3] = acsParameters.rwMatrices.pseudoInverse[1][0];
    rwPseudoInv[4] = acsParameters.rwMatrices.pseudoInverse[1][1];
    rwPseudoInv[5] = acsParameters.rwMatrices.pseudoInverse[1][2];
    rwPseudoInv[6] = acsParameters.rwMatrices.pseudoInverse[2][0];
    rwPseudoInv[7] = acsParameters.rwMatrices.pseudoInverse[2][1];
    rwPseudoInv[8] = acsParameters.rwMatrices.pseudoInverse[2][2];
    rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0];
    rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1];
    rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2];

  }

  else if (!(sensorValues->rw1Set.isValid()) && sensorValues->rw2Set.isValid() &&
           sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) {
    rwPseudoInv[0] = acsParameters.rwMatrices.without0[0][0];
    rwPseudoInv[1] = acsParameters.rwMatrices.without0[0][1];
    rwPseudoInv[2] = acsParameters.rwMatrices.without0[0][2];
    rwPseudoInv[3] = acsParameters.rwMatrices.without0[1][0];
    rwPseudoInv[4] = acsParameters.rwMatrices.without0[1][1];
    rwPseudoInv[5] = acsParameters.rwMatrices.without0[1][2];
    rwPseudoInv[6] = acsParameters.rwMatrices.without0[2][0];
    rwPseudoInv[7] = acsParameters.rwMatrices.without0[2][1];
    rwPseudoInv[8] = acsParameters.rwMatrices.without0[2][2];
    rwPseudoInv[9] = acsParameters.rwMatrices.without0[3][0];
    rwPseudoInv[10] = acsParameters.rwMatrices.without0[3][1];
    rwPseudoInv[11] = acsParameters.rwMatrices.without0[3][2];
  }

  else if ((sensorValues->rw1Set.isValid()) && !(sensorValues->rw2Set.isValid()) &&
           sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) {
    rwPseudoInv[0] = acsParameters.rwMatrices.without1[0][0];
    rwPseudoInv[1] = acsParameters.rwMatrices.without1[0][1];
    rwPseudoInv[2] = acsParameters.rwMatrices.without1[0][2];
    rwPseudoInv[3] = acsParameters.rwMatrices.without1[1][0];
    rwPseudoInv[4] = acsParameters.rwMatrices.without1[1][1];
    rwPseudoInv[5] = acsParameters.rwMatrices.without1[1][2];
    rwPseudoInv[6] = acsParameters.rwMatrices.without1[2][0];
    rwPseudoInv[7] = acsParameters.rwMatrices.without1[2][1];
    rwPseudoInv[8] = acsParameters.rwMatrices.without1[2][2];
    rwPseudoInv[9] = acsParameters.rwMatrices.without1[3][0];
    rwPseudoInv[10] = acsParameters.rwMatrices.without1[3][1];
    rwPseudoInv[11] = acsParameters.rwMatrices.without1[3][2];
  }

  else if ((sensorValues->rw1Set.isValid()) && (sensorValues->rw2Set.isValid()) &&
           !(sensorValues->rw3Set.isValid()) && sensorValues->rw4Set.isValid()) {
    rwPseudoInv[0] = acsParameters.rwMatrices.without2[0][0];
    rwPseudoInv[1] = acsParameters.rwMatrices.without2[0][1];
    rwPseudoInv[2] = acsParameters.rwMatrices.without2[0][2];
    rwPseudoInv[3] = acsParameters.rwMatrices.without2[1][0];
    rwPseudoInv[4] = acsParameters.rwMatrices.without2[1][1];
    rwPseudoInv[5] = acsParameters.rwMatrices.without2[1][2];
    rwPseudoInv[6] = acsParameters.rwMatrices.without2[2][0];
    rwPseudoInv[7] = acsParameters.rwMatrices.without2[2][1];
    rwPseudoInv[8] = acsParameters.rwMatrices.without2[2][2];
    rwPseudoInv[9] = acsParameters.rwMatrices.without2[3][0];
    rwPseudoInv[10] = acsParameters.rwMatrices.without2[3][1];
    rwPseudoInv[11] = acsParameters.rwMatrices.without2[3][2];
  }

  else if ((sensorValues->rw1Set.isValid()) && (sensorValues->rw2Set.isValid()) &&
           (sensorValues->rw3Set.isValid()) && !(sensorValues->rw4Set.isValid())) {
    rwPseudoInv[0] = acsParameters.rwMatrices.without3[0][0];
    rwPseudoInv[1] = acsParameters.rwMatrices.without3[0][1];
    rwPseudoInv[2] = acsParameters.rwMatrices.without3[0][2];
    rwPseudoInv[3] = acsParameters.rwMatrices.without3[1][0];
    rwPseudoInv[4] = acsParameters.rwMatrices.without3[1][1];
    rwPseudoInv[5] = acsParameters.rwMatrices.without3[1][2];
    rwPseudoInv[6] = acsParameters.rwMatrices.without3[2][0];
    rwPseudoInv[7] = acsParameters.rwMatrices.without3[2][1];
    rwPseudoInv[8] = acsParameters.rwMatrices.without3[2][2];
    rwPseudoInv[9] = acsParameters.rwMatrices.without3[3][0];
    rwPseudoInv[10] = acsParameters.rwMatrices.without3[3][1];
    rwPseudoInv[11] = acsParameters.rwMatrices.without3[3][2];
  }

  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.
    rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0];
    rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1];
    rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2];
    rwPseudoInv[3] = acsParameters.rwMatrices.pseudoInverse[1][0];
    rwPseudoInv[4] = acsParameters.rwMatrices.pseudoInverse[1][1];
    rwPseudoInv[5] = acsParameters.rwMatrices.pseudoInverse[1][2];
    rwPseudoInv[6] = acsParameters.rwMatrices.pseudoInverse[2][0];
    rwPseudoInv[7] = acsParameters.rwMatrices.pseudoInverse[2][1];
    rwPseudoInv[8] = acsParameters.rwMatrices.pseudoInverse[2][2];
    rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0];
    rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1];
    rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2];
  }
}