eive-obsw/mission/controller/acs/util/MathOperations.h

#ifndef MATH_MATHOPERATIONS_H_
#define MATH_MATHOPERATIONS_H_

#include <fsfw/src/fsfw/globalfunctions/constants.h>
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/src/fsfw/globalfunctions/sign.h>
#include <fsfw/src/fsfw/serviceinterface.h>

#include <cmath>
#include <cstring>
#include <iostream>

template <typename T1, typename T2 = T1>
class MathOperations {
 public:
  static void skewMatrix(const T1 vector[], T2 *result) {
    // Input Dimension [3], Output [3][3]
    result[0] = 0;
    result[1] = -vector[2];
    result[2] = vector[1];
    result[3] = vector[2];
    result[4] = 0;
    result[5] = -vector[0];
    result[6] = -vector[1];
    result[7] = vector[0];
    result[8] = 0;
  }
  static void vecTransposeVecMatrix(const T1 vector1[], const T1 transposeVector2[], T2 *result,
                                    uint8_t size = 3) {
    // Looks like MatrixOpertions::multiply is able to do the same thing
    for (uint8_t resultColumn = 0; resultColumn < size; resultColumn++) {
      for (uint8_t resultRow = 0; resultRow < size; resultRow++) {
        result[resultColumn + size * resultRow] =
            vector1[resultRow] * transposeVector2[resultColumn];
      }
    }
    /*matrixSun[i][j] = sunEstB[i] * sunEstB[j];
     matrixMag[i][j] = magEstB[i] * magEstB[j];
     matrixSunMag[i][j] = sunEstB[i] * magEstB[j];
     matrixMagSun[i][j] = magEstB[i] * sunEstB[j];*/
  }

  static void selectionSort(const T1 *matrix, T1 *result, uint8_t rowSize, uint8_t colSize) {
    int min_idx;
    T1 temp;
    std::memcpy(result, matrix, rowSize * colSize * sizeof(*result));
    // One by one move boundary of unsorted subarray
    for (int k = 0; k < rowSize; k++) {
      for (int i = 0; i < colSize - 1; i++) {
        // Find the minimum element in unsorted array
        min_idx = i;
        for (int j = i + 1; j < colSize; j++) {
          if (result[j + k * colSize] < result[min_idx + k * colSize]) {
            min_idx = j;
          }
        }
        // Swap the found minimum element with the first element
        temp = result[i + k * colSize];
        result[i + k * colSize] = result[min_idx + k * colSize];
        result[min_idx + k * colSize] = temp;
      }
    }
  }

  static void convertDateToJD2000(const T1 time, T2 julianDate) {
    // time = { Y, M, D, h, m,s}
    // time in sec and microsec -> The Epoch (unixtime)
    julianDate = 1721013.5 + 367 * time[0] - floor(7 / 4 * (time[0] + (time[1] + 9) / 12)) +
                 floor(275 * time[1] / 9) + time[2] +
                 (60 * time[3] + time[4] + (time(5) / 60)) / 1440;
  }

  static T1 convertUnixToJD2000(timeval time) {
    // time = {{s},{us}}
    T1 julianDate2000;
    julianDate2000 = (time.tv_sec / 86400.0) + 2440587.5 - 2451545;
    return julianDate2000;
  }

  static void dcmFromQuat(const T1 vector[], T1 *outputDcm) {
    // convention q = [qx,qy,qz, qw]
    outputDcm[0] = pow(vector[0], 2) - pow(vector[1], 2) - pow(vector[2], 2) + pow(vector[3], 2);
    outputDcm[1] = 2 * (vector[0] * vector[1] + vector[2] * vector[3]);
    outputDcm[2] = 2 * (vector[0] * vector[2] - vector[1] * vector[3]);

    outputDcm[3] = 2 * (vector[1] * vector[0] - vector[2] * vector[3]);
    outputDcm[4] = -pow(vector[0], 2) + pow(vector[1], 2) - pow(vector[2], 2) + pow(vector[3], 2);
    outputDcm[5] = 2 * (vector[1] * vector[2] + vector[0] * vector[3]);

    outputDcm[6] = 2 * (vector[2] * vector[0] + vector[1] * vector[3]);
    outputDcm[7] = 2 * (vector[2] * vector[1] - vector[0] * vector[3]);
    outputDcm[8] = -pow(vector[0], 2) - pow(vector[1], 2) + pow(vector[2], 2) + pow(vector[3], 2);
  }

  static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt,
                                      T2 *cartesianOutput) {
    /* @brief:  cartesianFromLatLongAlt() - calculates cartesian coordinates in ECEF from latitude,
     * 								longitude and altitude
     * @param:  lat   				geodetic latitude [rad]
     * 			longi 				longitude [rad]
     * 			alt					altitude [m]
     * 			cartesianOutput 	Cartesian Coordinates in ECEF (3x1)
     * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.34ff
     * 			Landis Markley and John L. Crassidis*/
    double radiusPolar = 6356752.314;
    double radiusEqua = 6378137;

    double eccentricity = sqrt(1 - pow(radiusPolar, 2) / pow(radiusEqua, 2));
    double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity, 2) * pow(sin(lat), 2));

    cartesianOutput[0] = (auxRadius + alt) * cos(lat) * cos(longi);
    cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi);
    cartesianOutput[2] = ((1 - pow(eccentricity, 2)) * auxRadius + alt) * sin(lat);
  }

  static void latLongAltFromCartesian(const T1 *vector, T1 &latitude, T1 &longitude, T1 &altitude) {
    /* @brief:  latLongAltFromCartesian() - calculates latitude, longitude and altitude from
     * cartesian coordinates in ECEF
     * @param:  x   			x-value of position vector [m]
     * 			y 				y-value of position vector [m]
     * 			z				z-value of position vector [m]
     * 			latitude 		geodetic latitude [rad]
     * 			longitude		longitude [rad]
     * 			altitude		altitude [m]
     * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.35 f
     * 			Landis Markley and John L. Crassidis*/
    // From World Geodetic System the Earth Radii
    double a = 6378137.0;     // semimajor axis [m]
    double b = 6356752.3142;  // semiminor axis [m]

    // Calculation
    double e2 = 1 - pow(b, 2) / pow(a, 2);
    double epsilon2 = pow(a, 2) / pow(b, 2) - 1;
    double rho = sqrt(pow(vector[0], 2) + pow(vector[1], 2));
    double p = std::abs(vector[2]) / epsilon2;
    double s = pow(rho, 2) / (e2 * epsilon2);
    double q = pow(p, 2) - pow(b, 2) + s;
    double u = p / sqrt(q);
    double v = pow(b, 2) * pow(u, 2) / q;
    double P = 27 * v * s / q;
    double Q = pow(sqrt(P + 1) + sqrt(P), 2. / 3.);
    double t = (1 + Q + 1 / Q) / 6;
    double c = sqrt(pow(u, 2) - 1 + 2 * t);
    double w = (c - u) / 2;
    double d =
        sign(vector[2]) * sqrt(q) * (w + sqrt(sqrt(pow(t, 2) + v) - u * w - t / 2 - 1. / 4.));
    double N = a * sqrt(1 + epsilon2 * pow(d, 2) / pow(b, 2));
    latitude = asin((epsilon2 + 1) * d / N);
    altitude = rho * cos(latitude) + vector[2] * sin(latitude) - pow(a, 2) / N;
    longitude = atan2(vector[1], vector[0]);
  }

  static void dcmEJ(timeval time, T1 *outputDcmEJ, T1 *outputDotDcmEJ) {
    /* @brief:  dcmEJ() - calculates the transformation matrix between ECEF and ECI frame
     * @param:  time   			Current time
     * 			outputDcmEJ 	Transformation matrix from ECI (J) to ECEF (E) [3][3]
     * 			outputDotDcmEJ	Derivative of transformation matrix [3][3]
     * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff
     * 			Landis Markley and John L. Crassidis*/
    double JD2000Floor = 0;
    double JD2000 = convertUnixToJD2000(time);
    // Getting Julian Century from Day start : JD (Y,M,D,0,0,0)
    JD2000Floor = floor(JD2000);
    if ((JD2000 - JD2000Floor) < 0.5) {
      JD2000Floor -= 0.5;
    } else {
      JD2000Floor += 0.5;
    }

    double JC2000 = JD2000Floor / 36525;
    double sec = (JD2000 - JD2000Floor) * 86400;
    double gmst = 0;  // greenwich mean sidereal time
    gmst = 24110.54841 + 8640184.812866 * JC2000 + 0.093104 * pow(JC2000, 2) -
           0.0000062 * pow(JC2000, 3) + 1.002737909350795 * sec;
    double rest = gmst / 86400;
    double FloorRest = floor(rest);
    double secOfDay = rest - FloorRest;
    secOfDay *= 86400;
    gmst = secOfDay / 240 * M_PI / 180;

    outputDcmEJ[0] = cos(gmst);
    outputDcmEJ[1] = sin(gmst);
    outputDcmEJ[2] = 0;
    outputDcmEJ[3] = -sin(gmst);
    outputDcmEJ[4] = cos(gmst);
    outputDcmEJ[5] = 0;
    outputDcmEJ[6] = 0;
    outputDcmEJ[7] = 0;
    outputDcmEJ[8] = 1;

    //	Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION
    double dcmEJCalc[3][3] = {{outputDcmEJ[0], outputDcmEJ[1], outputDcmEJ[2]},
                              {outputDcmEJ[3], outputDcmEJ[4], outputDcmEJ[5]},
                              {outputDcmEJ[6], outputDcmEJ[7], outputDcmEJ[8]}};
    double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
    double omegaEarth = 0.000072921158553;
    double dotDcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
    MatrixOperations<double>::multiply(*dcmDot, *dcmEJCalc, *dotDcmEJ, 3, 3, 3);
    MatrixOperations<double>::multiplyScalar(*dotDcmEJ, omegaEarth, outputDotDcmEJ, 3, 3);
  }

  /* @brief:  ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame
   * 			give also the back the derivative of this matrix
   * @param:  unixTime   		Current time in Unix format
   * 			outputDcmEJ 	Transformation matrix from ECI (J) to ECEF (E) [3][3]
   * 			outputDotDcmEJ	Derivative of transformation matrix [3][3]
   * @source: Entwicklung einer Simulationsumgebung und robuster Algorithmen für das Lage- und
                          Orbitkontrollsystem der Kleinsatelliten Flying Laptop und PERSEUS, P.244ff
   * 			Oliver Zeile
   *
   https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110*/
  static void ecfToEciWithNutPre(timeval unixTime, T1 *outputDcmEJ, T1 *outputDotDcmEJ) {
    //		TT = UTC/Unix + 32.184s (TAI Difference) + 27 (Leap Seconds in UTC since 1972) + 10
    //(initial Offset) 		International Atomic Time (TAI)

    double JD2000UTC1 = convertUnixToJD2000(unixTime);

    //		Julian Date / century from TT
    timeval terestrialTime = unixTime;
    terestrialTime.tv_sec = unixTime.tv_sec + 32.184 + 37;
    double JD2000TT = convertUnixToJD2000(terestrialTime);
    double JC2000TT = JD2000TT / 36525;

    //-------------------------------------------------------------------------------------
    //		Calculation of Transformation from earth rotation Theta
    //-------------------------------------------------------------------------------------
    double theta[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
    //		Earth Rotation angle
    double era = 0;
    era = 2 * M_PI * (0.779057273264 + 1.00273781191135448 * JD2000UTC1);
    //		Greenwich Mean Sidereal Time
    double gmst2000 = 0.014506 + 4612.15739966 * JC2000TT + 1.39667721 * pow(JC2000TT, 2) -
                      0.00009344 * pow(JC2000TT, 3) + 0.00001882 * pow(JC2000TT, 4);
    double arcsecFactor = 1 * M_PI / (180 * 3600);
    gmst2000 *= arcsecFactor;
    gmst2000 += era;

    theta[0][0] = cos(gmst2000);
    theta[0][1] = sin(gmst2000);
    theta[0][2] = 0;
    theta[1][0] = -sin(gmst2000);
    theta[1][1] = cos(gmst2000);
    theta[1][2] = 0;
    theta[2][0] = 0;
    theta[2][1] = 0;
    theta[2][2] = 1;

    //-------------------------------------------------------------------------------------
    //		Calculation of Transformation from earth Precession P
    //-------------------------------------------------------------------------------------
    double precession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};

    double zeta = 2306.2181 * JC2000TT + 0.30188 * pow(JC2000TT, 2) + 0.017998 * pow(JC2000TT, 3);
    double theta2 = 2004.3109 * JC2000TT - 0.42665 * pow(JC2000TT, 2) - 0.041833 * pow(JC2000TT, 3);
    double ze = zeta + 0.79280 * pow(JC2000TT, 2) + 0.000205 * pow(JC2000TT, 3);

    zeta *= arcsecFactor;
    theta2 *= arcsecFactor;
    ze *= arcsecFactor;

    precession[0][0] = -sin(ze) * sin(zeta) + cos(ze) * cos(theta2) * cos(zeta);
    precession[1][0] = cos(ze) * sin(zeta) + sin(ze) * cos(theta2) * cos(zeta);
    precession[2][0] = sin(theta2) * cos(zeta);
    precession[0][1] = -sin(ze) * cos(zeta) - cos(ze) * cos(theta2) * sin(zeta);
    precession[1][1] = cos(ze) * cos(zeta) - sin(ze) * cos(theta2) * sin(zeta);
    precession[2][1] = -sin(theta2) * sin(zeta);
    precession[0][2] = -cos(ze) * sin(theta2);
    precession[1][2] = -sin(ze) * sin(theta2);
    precession[2][2] = cos(theta2);

    //-------------------------------------------------------------------------------------
    //		Calculation of Transformation from earth Nutation N
    //-------------------------------------------------------------------------------------
    double nutation[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
    //      lunar asc node
    double Om = 125 * 3600 + 2 * 60 + 40.28 - (1934 * 3600 + 8 * 60 + 10.539) * JC2000TT +
                7.455 * pow(JC2000TT, 2) + 0.008 * pow(JC2000TT, 3);
    Om *= arcsecFactor;
    //      delta psi approx
    double dp = -17.2 * arcsecFactor * sin(Om);

    //      delta eps approx
    double de = 9.203 * arcsecFactor * cos(Om);

    //        % true obliquity of the ecliptic eps p.71 (simplified)
    double e = 23.43929111 * M_PI / 180 - 46.8150 / 3600 * JC2000TT * M_PI / 180;

    nutation[0][0] = cos(dp);
    nutation[1][0] = cos(e + de) * sin(dp);
    nutation[2][0] = sin(e + de) * sin(dp);
    nutation[0][1] = -cos(e) * sin(dp);
    nutation[1][1] = cos(e) * cos(e + de) * cos(dp) + sin(e) * sin(e + de);
    nutation[2][1] = cos(e) * sin(e + de) * cos(dp) - sin(e) * cos(e + de);
    nutation[0][2] = -sin(e) * sin(dp);
    nutation[1][2] = sin(e) * cos(e + de) * cos(dp) - cos(e) * sin(e + de);
    nutation[2][2] = sin(e) * sin(e + de) * cos(dp) + cos(e) * cos(e + de);

    //-------------------------------------------------------------------------------------
    //		Calculation of Derivative of rotation matrix from earth
    //-------------------------------------------------------------------------------------
    double thetaDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
    double dotMatrix[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
    double omegaEarth = 0.000072921158553;
    MatrixOperations<double>::multiply(*dotMatrix, *theta, *thetaDot, 3, 3, 3);
    MatrixOperations<double>::multiplyScalar(*thetaDot, omegaEarth, *thetaDot, 3, 3);

    //-------------------------------------------------------------------------------------
    //		Calculation of transformation matrix and Derivative of transformation matrix
    //-------------------------------------------------------------------------------------
    double nutationPrecession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
    MatrixOperations<double>::multiply(*nutation, *precession, *nutationPrecession, 3, 3, 3);
    MatrixOperations<double>::multiply(*nutationPrecession, *theta, outputDcmEJ, 3, 3, 3);

    MatrixOperations<double>::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3);
  }
  static void inverseMatrixDimThree(const T1 *matrix, T1 *output) {
    int i, j;
    double determinant = 0;
    double mat[3][3] = {{matrix[0], matrix[1], matrix[2]},
                        {matrix[3], matrix[4], matrix[5]},
                        {matrix[6], matrix[7], matrix[8]}};

    for (i = 0; i < 3; i++) {
      determinant = determinant + (mat[0][i] * (mat[1][(i + 1) % 3] * mat[2][(i + 2) % 3] -
                                                mat[1][(i + 2) % 3] * mat[2][(i + 1) % 3]));
    }
    //		cout<<"\n\ndeterminant: "<<determinant;
    //		cout<<"\n\nInverse of matrix is: \n";
    for (i = 0; i < 3; i++) {
      for (j = 0; j < 3; j++) {
        output[i * 3 + j] = ((mat[(j + 1) % 3][(i + 1) % 3] * mat[(j + 2) % 3][(i + 2) % 3]) -
                             (mat[(j + 1) % 3][(i + 2) % 3] * mat[(j + 2) % 3][(i + 1) % 3])) /
                            determinant;
      }
    }
  }

  static float matrixDeterminant(const T1 *inputMatrix, uint8_t size) {
    /* do not use this. takes 300ms */
    float det = 0;
    T1 matrix[size][size], submatrix[size - 1][size - 1];
    for (uint8_t row = 0; row < size; row++) {
      for (uint8_t col = 0; col < size; col++) {
        matrix[row][col] = inputMatrix[row * size + col];
      }
    }
    if (size == 2)
      return ((matrix[0][0] * matrix[1][1]) - (matrix[1][0] * matrix[0][1]));
    else {
      for (uint8_t col = 0; col < size; col++) {
        int subRow = 0;
        for (uint8_t rowIndex = 1; rowIndex < size; rowIndex++) {
          int subCol = 0;
          for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
            if (colIndex == col) continue;
            submatrix[subRow][subCol] = matrix[rowIndex][colIndex];
            subCol++;
          }
          subRow++;
        }
        det += (pow(-1, col) * matrix[0][col] *
                MathOperations<T1>::matrixDeterminant(*submatrix, size - 1));
      }
    }
    return det;
  }

  static int inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) {
    // Stopwatch stopwatch;
    T1 matrix[size][size], identity[size][size];
    // reformat array to matrix
    for (uint8_t row = 0; row < size; row++) {
      for (uint8_t col = 0; col < size; col++) {
        matrix[row][col] = inputMatrix[row * size + col];
      }
    }
    // init identity matrix
    std::memset(identity, 0.0, sizeof(identity));
    for (uint8_t diag = 0; diag < size; diag++) {
      identity[diag][diag] = 1;
    }
    // gauss-jordan algo
    // sort matrix such as no diag entry shall be 0
    for (uint8_t row = 0; row < size; row++) {
      if (matrix[row][row] == 0.0) {
        bool swaped = false;
        uint8_t rowIndex = 0;
        while ((rowIndex < size) && !swaped) {
          if ((matrix[rowIndex][row] != 0.0) && (matrix[row][rowIndex] != 0.0)) {
            for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
              std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]);
              std::swap(identity[row][colIndex], identity[rowIndex][colIndex]);
            }
            swaped = true;
          }
          rowIndex++;
        }
        if (!swaped) {
          return 1;  // matrix not invertible
        }
      }
    }

    for (int row = 0; row < size; row++) {
      if (matrix[row][row] == 0.0) {
        uint8_t rowIndex;
        if (row == 0) {
          rowIndex = size - 1;
        } else {
          rowIndex = row - 1;
        }
        for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
          std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]);
          std::swap(identity[row][colIndex], identity[rowIndex][colIndex]);
        }
        row--;
        if (row < 0) {
          return 1;  // Matrix is not invertible
        }
      }
    }
    // remove non diag elements in matrix (jordan)
    for (int row = 0; row < size; row++) {
      for (int rowIndex = 0; rowIndex < size; rowIndex++) {
        if (row != rowIndex) {
          double ratio = matrix[rowIndex][row] / matrix[row][row];
          for (int colIndex = 0; colIndex < size; colIndex++) {
            matrix[rowIndex][colIndex] -= ratio * matrix[row][colIndex];
            identity[rowIndex][colIndex] -= ratio * identity[row][colIndex];
          }
        }
      }
    }
    // normalize rows in matrix (gauss)
    for (int row = 0; row < size; row++) {
      for (int col = 0; col < size; col++) {
        identity[row][col] = identity[row][col] / matrix[row][row];
      }
    }
    std::memcpy(inverse, identity, sizeof(identity));
    return 0;  // successful inversion
  }

  static bool checkVectorIsFinite(const T1 *inputVector, uint8_t size) {
    for (uint8_t i = 0; i < size; i++) {
      if (not isfinite(inputVector[i])) {
        return false;
      }
    }
    return true;
  }

  static bool checkMatrixIsFinite(const T1 *inputMatrix, uint8_t rows, uint8_t cols) {
    for (uint8_t col = 0; col < cols; col++) {
      for (uint8_t row = 0; row < rows; row++) {
        if (not isfinite(inputMatrix[row * cols + cols])) {
          return false;
        }
      }
    }
    return true;
  }
};

#endif /* ACS_MATH_MATHOPERATIONS_H_ */