#include "SensorProcessing.h"

#include <fsfw/datapool/PoolReadGuard.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/timevalOperations.h>
#include <math.h>

#include "../controllerdefinitions/AcsCtrlDefinitions.h"
#include "Igrf13Model.h"
#include "util/MathOperations.h"

using namespace Math;

SensorProcessing::SensorProcessing(AcsParameters *acsParameters_)
    : savedMgmVecTot{0, 0, 0}, validMagField(false), validGcLatitude(false) {}

SensorProcessing::~SensorProcessing() {}

void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value,
                                  bool mgm1valid, const float *mgm2Value, bool mgm2valid,
                                  const float *mgm3Value, bool mgm3valid, const float *mgm4Value,
                                  bool mgm4valid, timeval timeOfMgmMeasurement,
                                  const AcsParameters::MgmHandlingParameters *mgmParameters,
                                  acsctrl::GpsDataProcessed *gpsDataProcessed,
                                  const double gpsAltitude, bool gpsValid,
                                  acsctrl::MgmDataProcessed *mgmDataProcessed) {
  if (!mgm0valid && !mgm1valid && !mgm2valid && !mgm3valid && !mgm4valid) {
    {
      PoolReadGuard pg(mgmDataProcessed);
      if (pg.getReadResult() == returnvalue::OK) {
        std::memcpy(mgmDataProcessed->mgm0vec.value, zeroVector, 3 * sizeof(float));
        std::memcpy(mgmDataProcessed->mgm1vec.value, zeroVector, 3 * sizeof(float));
        std::memcpy(mgmDataProcessed->mgm2vec.value, zeroVector, 3 * sizeof(float));
        std::memcpy(mgmDataProcessed->mgm3vec.value, zeroVector, 3 * sizeof(float));
        std::memcpy(mgmDataProcessed->mgm4vec.value, zeroVector, 3 * sizeof(float));
        std::memcpy(mgmDataProcessed->mgmVecTot.value, zeroVector, 3 * sizeof(float));
        std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, zeroVector, 3 * sizeof(float));
        std::memcpy(mgmDataProcessed->magIgrfModel.value, zeroVector, 3 * sizeof(double));
        mgmDataProcessed->setValidity(false, true);
      }
    }
    return;
  }
  float mgm0ValueNoBias[3] = {0, 0, 0}, mgm1ValueNoBias[3] = {0, 0, 0},
        mgm2ValueNoBias[3] = {0, 0, 0}, mgm3ValueNoBias[3] = {0, 0, 0},
        mgm4ValueNoBias[3] = {0, 0, 0};
  float mgm0ValueBody[3] = {0, 0, 0}, mgm1ValueBody[3] = {0, 0, 0}, mgm2ValueBody[3] = {0, 0, 0},
        mgm3ValueBody[3] = {0, 0, 0}, mgm4ValueBody[3] = {0, 0, 0};
  float mgm0ValueCalib[3] = {0, 0, 0}, mgm1ValueCalib[3] = {0, 0, 0}, mgm2ValueCalib[3] = {0, 0, 0},
        mgm3ValueCalib[3] = {0, 0, 0}, mgm4ValueCalib[3] = {0, 0, 0};
  float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0};

  if (mgm0valid) {
    MatrixOperations<float>::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0Value,
                                      mgm0ValueBody, 3, 3, 1);
    VectorOperations<float>::subtract(mgm0ValueBody, mgmParameters->mgm0hardIronOffset,
                                      mgm0ValueNoBias, 3);
    MatrixOperations<float>::multiply(mgmParameters->mgm0softIronInverse[0], mgm0ValueNoBias,
                                      mgm0ValueCalib, 3, 3, 1);
    for (uint8_t i = 0; i < 3; i++) {
      sensorFusionNumerator[i] += mgm0ValueCalib[i] / mgmParameters->mgm02variance[i];
      sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i];
    }
  }
  if (mgm1valid) {
    MatrixOperations<float>::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1Value,
                                      mgm1ValueBody, 3, 3, 1);
    VectorOperations<float>::subtract(mgm1ValueBody, mgmParameters->mgm1hardIronOffset,
                                      mgm1ValueNoBias, 3);
    MatrixOperations<float>::multiply(mgmParameters->mgm1softIronInverse[0], mgm1ValueNoBias,
                                      mgm1ValueCalib, 3, 3, 1);
    for (uint8_t i = 0; i < 3; i++) {
      sensorFusionNumerator[i] += mgm1ValueCalib[i] / mgmParameters->mgm13variance[i];
      sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i];
    }
  }
  if (mgm2valid) {
    MatrixOperations<float>::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2Value,
                                      mgm2ValueBody, 3, 3, 1);
    VectorOperations<float>::subtract(mgm2ValueBody, mgmParameters->mgm2hardIronOffset,
                                      mgm2ValueNoBias, 3);
    MatrixOperations<float>::multiply(mgmParameters->mgm2softIronInverse[0], mgm2ValueNoBias,
                                      mgm2ValueCalib, 3, 3, 1);
    for (uint8_t i = 0; i < 3; i++) {
      sensorFusionNumerator[i] += mgm2ValueCalib[i] / mgmParameters->mgm02variance[i];
      sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i];
    }
  }
  if (mgm3valid) {
    MatrixOperations<float>::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3Value,
                                      mgm3ValueBody, 3, 3, 1);
    VectorOperations<float>::subtract(mgm3ValueBody, mgmParameters->mgm3hardIronOffset,
                                      mgm3ValueNoBias, 3);
    MatrixOperations<float>::multiply(mgmParameters->mgm3softIronInverse[0], mgm3ValueNoBias,
                                      mgm3ValueCalib, 3, 3, 1);
    for (uint8_t i = 0; i < 3; i++) {
      sensorFusionNumerator[i] += mgm3ValueCalib[i] / mgmParameters->mgm13variance[i];
      sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i];
    }
  }
  if (mgm4valid) {
    float mgm4ValueUT[3];
    VectorOperations<float>::mulScalar(mgm4Value, 1e-3, mgm4ValueUT, 3);  // nT to uT
    MatrixOperations<float>::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueUT,
                                      mgm4ValueBody, 3, 3, 1);
    VectorOperations<float>::subtract(mgm4ValueBody, mgmParameters->mgm4hardIronOffset,
                                      mgm4ValueNoBias, 3);
    MatrixOperations<float>::multiply(mgmParameters->mgm4softIronInverse[0], mgm4ValueNoBias,
                                      mgm4ValueCalib, 3, 3, 1);

    for (uint8_t i = 0; i < 3; i++) {
      sensorFusionNumerator[i] += mgm4ValueCalib[i] / mgmParameters->mgm4variance[i];
      sensorFusionDenominator[i] += 1 / mgmParameters->mgm4variance[i];
    }
  }
  double mgmVecTot[3] = {0.0, 0.0, 0.0};
  for (uint8_t i = 0; i < 3; i++) {
    mgmVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i];
  }

  //-----------------------Mgm Rate Computation ---------------------------------------------------
  double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0};
  bool mgmVecTotDerivativeValid = false;
  double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst);
  if (timeOfSavedMagFieldEst.tv_sec != 0) {
    for (uint8_t i = 0; i < 3; i++) {
      mgmVecTotDerivative[i] = (mgmVecTot[i] - savedMgmVecTot[i]) / timeDiff;
      savedMgmVecTot[i] = mgmVecTot[i];
      mgmVecTotDerivativeValid = true;
    }
  }
  timeOfSavedMagFieldEst = timeOfMgmMeasurement;

  // ---------------- IGRF- 13 Implementation here ------------------------------------------------
  double magIgrfModel[3] = {0.0, 0.0, 0.0};
  if (gpsValid) {
    // Should be existing class object which will be called and modified here.
    Igrf13Model igrf13;
    // So the line above should not be done here. Update: Can be done here as long updated coffs
    //		stored in acsParameters ?
    igrf13.updateCoeffGH(timeOfMgmMeasurement);
    // maybe put a condition here, to only update after a full day, this
    // class function has around 700 steps to perform
    igrf13.magFieldComp(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value,
                        gpsAltitude, timeOfMgmMeasurement, magIgrfModel);
  }
  {
    PoolReadGuard pg(mgmDataProcessed);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(mgmDataProcessed->mgm0vec.value, mgm0ValueCalib, 3 * sizeof(float));
      mgmDataProcessed->mgm0vec.setValid(mgm0valid);
      std::memcpy(mgmDataProcessed->mgm1vec.value, mgm1ValueCalib, 3 * sizeof(float));
      mgmDataProcessed->mgm1vec.setValid(mgm1valid);
      std::memcpy(mgmDataProcessed->mgm2vec.value, mgm2ValueCalib, 3 * sizeof(float));
      mgmDataProcessed->mgm2vec.setValid(mgm2valid);
      std::memcpy(mgmDataProcessed->mgm3vec.value, mgm3ValueCalib, 3 * sizeof(float));
      mgmDataProcessed->mgm3vec.setValid(mgm3valid);
      std::memcpy(mgmDataProcessed->mgm4vec.value, mgm4ValueCalib, 3 * sizeof(float));
      mgmDataProcessed->mgm4vec.setValid(mgm4valid);
      std::memcpy(mgmDataProcessed->mgmVecTot.value, mgmVecTot, 3 * sizeof(double));
      mgmDataProcessed->mgmVecTot.setValid(true);
      std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, mgmVecTotDerivative,
                  3 * sizeof(double));
      mgmDataProcessed->mgmVecTotDerivative.setValid(mgmVecTotDerivativeValid);
      std::memcpy(mgmDataProcessed->magIgrfModel.value, magIgrfModel, 3 * sizeof(double));
      mgmDataProcessed->magIgrfModel.setValid(gpsValid);
      mgmDataProcessed->setValidity(true, false);
    }
  }
}

void SensorProcessing::processSus(
    const uint16_t *sus0Value, bool sus0valid, const uint16_t *sus1Value, bool sus1valid,
    const uint16_t *sus2Value, bool sus2valid, const uint16_t *sus3Value, bool sus3valid,
    const uint16_t *sus4Value, bool sus4valid, const uint16_t *sus5Value, bool sus5valid,
    const uint16_t *sus6Value, bool sus6valid, const uint16_t *sus7Value, bool sus7valid,
    const uint16_t *sus8Value, bool sus8valid, const uint16_t *sus9Value, bool sus9valid,
    const uint16_t *sus10Value, bool sus10valid, const uint16_t *sus11Value, bool sus11valid,
    timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters,
    const AcsParameters::SunModelParameters *sunModelParameters,
    acsctrl::SusDataProcessed *susDataProcessed) {
  if (sus0valid) {
    sus0valid = susConverter.checkSunSensorData(sus0Value);
  }
  if (sus1valid) {
    sus1valid = susConverter.checkSunSensorData(sus1Value);
  }
  if (sus2valid) {
    sus2valid = susConverter.checkSunSensorData(sus2Value);
  }
  if (sus3valid) {
    sus3valid = susConverter.checkSunSensorData(sus3Value);
  }
  if (sus4valid) {
    sus4valid = susConverter.checkSunSensorData(sus4Value);
  }
  if (sus5valid) {
    sus5valid = susConverter.checkSunSensorData(sus5Value);
  }
  if (sus6valid) {
    sus6valid = susConverter.checkSunSensorData(sus6Value);
  }
  if (sus7valid) {
    sus7valid = susConverter.checkSunSensorData(sus7Value);
  }
  if (sus8valid) {
    sus8valid = susConverter.checkSunSensorData(sus8Value);
  }
  if (sus9valid) {
    sus9valid = susConverter.checkSunSensorData(sus9Value);
  }
  if (sus10valid) {
    sus10valid = susConverter.checkSunSensorData(sus10Value);
  }
  if (sus11valid) {
    sus11valid = susConverter.checkSunSensorData(sus11Value);
  }

  if (!sus0valid && !sus1valid && !sus2valid && !sus3valid && !sus4valid && !sus5valid &&
      !sus6valid && !sus7valid && !sus8valid && !sus9valid && !sus10valid && !sus11valid) {
    {
      PoolReadGuard pg(susDataProcessed);
      if (pg.getReadResult() == returnvalue::OK) {
        std::memcpy(susDataProcessed->sus0vec.value, zeroVector, 3 * sizeof(float));
        std::memcpy(susDataProcessed->sus1vec.value, zeroVector, 3 * sizeof(float));
        std::memcpy(susDataProcessed->sus2vec.value, zeroVector, 3 * sizeof(float));
        std::memcpy(susDataProcessed->sus3vec.value, zeroVector, 3 * sizeof(float));
        std::memcpy(susDataProcessed->sus4vec.value, zeroVector, 3 * sizeof(float));
        std::memcpy(susDataProcessed->sus5vec.value, zeroVector, 3 * sizeof(float));
        std::memcpy(susDataProcessed->sus6vec.value, zeroVector, 3 * sizeof(float));
        std::memcpy(susDataProcessed->sus7vec.value, zeroVector, 3 * sizeof(float));
        std::memcpy(susDataProcessed->sus8vec.value, zeroVector, 3 * sizeof(float));
        std::memcpy(susDataProcessed->sus9vec.value, zeroVector, 3 * sizeof(float));
        std::memcpy(susDataProcessed->sus10vec.value, zeroVector, 3 * sizeof(float));
        std::memcpy(susDataProcessed->sus11vec.value, zeroVector, 3 * sizeof(float));
        std::memcpy(susDataProcessed->susVecTot.value, zeroVector, 3 * sizeof(float));
        std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVector, 3 * sizeof(float));
        std::memcpy(susDataProcessed->sunIjkModel.value, zeroVector, 3 * sizeof(double));
        susDataProcessed->setValidity(false, true);
      }
    }
    return;
  }
  // WARNING: NOT TRANSFORMED IN BODY FRAME YET
  // Transformation into Geomtry Frame
  float sus0VecBody[3] = {0, 0, 0}, sus1VecBody[3] = {0, 0, 0}, sus2VecBody[3] = {0, 0, 0},
        sus3VecBody[3] = {0, 0, 0}, sus4VecBody[3] = {0, 0, 0}, sus5VecBody[3] = {0, 0, 0},
        sus6VecBody[3] = {0, 0, 0}, sus7VecBody[3] = {0, 0, 0}, sus8VecBody[3] = {0, 0, 0},
        sus9VecBody[3] = {0, 0, 0}, sus10VecBody[3] = {0, 0, 0}, sus11VecBody[3] = {0, 0, 0};

  if (sus0valid) {
    MatrixOperations<float>::multiply(
        susParameters->sus0orientationMatrix[0],
        susConverter.getSunVectorSensorFrame(sus0Value, susParameters->sus0coeffAlpha,
                                             susParameters->sus0coeffBeta),
        sus0VecBody, 3, 3, 1);
  }
  {
    PoolReadGuard pg(susDataProcessed);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(susDataProcessed->sus0vec.value, sus0VecBody, 3 * sizeof(float));
      susDataProcessed->sus0vec.setValid(sus0valid);
      if (!sus0valid) {
        std::memcpy(susDataProcessed->sus0vec.value, zeroVector, 3 * sizeof(float));
      }
    }
  }
  if (sus1valid) {
    MatrixOperations<float>::multiply(
        susParameters->sus1orientationMatrix[0],
        susConverter.getSunVectorSensorFrame(sus1Value, susParameters->sus1coeffAlpha,
                                             susParameters->sus1coeffBeta),
        sus1VecBody, 3, 3, 1);
  }
  {
    PoolReadGuard pg(susDataProcessed);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(susDataProcessed->sus1vec.value, sus1VecBody, 3 * sizeof(float));
      susDataProcessed->sus1vec.setValid(sus1valid);
      if (!sus1valid) {
        std::memcpy(susDataProcessed->sus1vec.value, zeroVector, 3 * sizeof(float));
      }
    }
  }
  if (sus2valid) {
    MatrixOperations<float>::multiply(
        susParameters->sus2orientationMatrix[0],
        susConverter.getSunVectorSensorFrame(sus2Value, susParameters->sus2coeffAlpha,
                                             susParameters->sus2coeffBeta),
        sus2VecBody, 3, 3, 1);
  }
  if (sus3valid) {
    MatrixOperations<float>::multiply(
        susParameters->sus3orientationMatrix[0],
        susConverter.getSunVectorSensorFrame(sus3Value, susParameters->sus3coeffAlpha,
                                             susParameters->sus3coeffBeta),
        sus3VecBody, 3, 3, 1);
  }
  if (sus4valid) {
    MatrixOperations<float>::multiply(
        susParameters->sus4orientationMatrix[0],
        susConverter.getSunVectorSensorFrame(sus4Value, susParameters->sus4coeffAlpha,
                                             susParameters->sus4coeffBeta),
        sus4VecBody, 3, 3, 1);
  }
  if (sus5valid) {
    MatrixOperations<float>::multiply(
        susParameters->sus5orientationMatrix[0],
        susConverter.getSunVectorSensorFrame(sus5Value, susParameters->sus5coeffAlpha,
                                             susParameters->sus5coeffBeta),
        sus5VecBody, 3, 3, 1);
  }
  if (sus6valid) {
    MatrixOperations<float>::multiply(
        susParameters->sus6orientationMatrix[0],
        susConverter.getSunVectorSensorFrame(sus6Value, susParameters->sus6coeffAlpha,
                                             susParameters->sus6coeffBeta),
        sus6VecBody, 3, 3, 1);
  }
  if (sus7valid) {
    MatrixOperations<float>::multiply(
        susParameters->sus7orientationMatrix[0],
        susConverter.getSunVectorSensorFrame(sus7Value, susParameters->sus7coeffAlpha,
                                             susParameters->sus7coeffBeta),
        sus7VecBody, 3, 3, 1);
  }
  if (sus8valid) {
    MatrixOperations<float>::multiply(
        susParameters->sus8orientationMatrix[0],
        susConverter.getSunVectorSensorFrame(sus8Value, susParameters->sus8coeffAlpha,
                                             susParameters->sus8coeffBeta),
        sus8VecBody, 3, 3, 1);
  }
  if (sus9valid) {
    MatrixOperations<float>::multiply(
        susParameters->sus9orientationMatrix[0],
        susConverter.getSunVectorSensorFrame(sus9Value, susParameters->sus9coeffAlpha,
                                             susParameters->sus9coeffBeta),
        sus9VecBody, 3, 3, 1);
  }
  if (sus10valid) {
    MatrixOperations<float>::multiply(
        susParameters->sus10orientationMatrix[0],
        susConverter.getSunVectorSensorFrame(sus10Value, susParameters->sus10coeffAlpha,
                                             susParameters->sus10coeffBeta),
        sus10VecBody, 3, 3, 1);
  }
  if (sus11valid) {
    MatrixOperations<float>::multiply(
        susParameters->sus11orientationMatrix[0],
        susConverter.getSunVectorSensorFrame(sus11Value, susParameters->sus11coeffAlpha,
                                             susParameters->sus11coeffBeta),
        sus11VecBody, 3, 3, 1);
  }

  /* ------ Mean Value: susDirEst ------ */
  bool validIds[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid,  sus5valid,
                       sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid};
  float susVecBody[3][12] = {{sus0VecBody[0], sus1VecBody[0], sus2VecBody[0], sus3VecBody[0],
                              sus4VecBody[0], sus5VecBody[0], sus6VecBody[0], sus7VecBody[0],
                              sus8VecBody[0], sus9VecBody[0], sus10VecBody[0], sus11VecBody[0]},
                             {sus0VecBody[1], sus1VecBody[1], sus2VecBody[1], sus3VecBody[1],
                              sus4VecBody[1], sus5VecBody[1], sus6VecBody[1], sus7VecBody[1],
                              sus8VecBody[1], sus9VecBody[1], sus10VecBody[1], sus11VecBody[1]},
                             {sus0VecBody[2], sus1VecBody[2], sus2VecBody[2], sus3VecBody[2],
                              sus4VecBody[2], sus5VecBody[2], sus6VecBody[2], sus7VecBody[2],
                              sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[2]}};

  double susMeanValue[3] = {0, 0, 0};
  for (uint8_t i = 0; i < 12; i++) {
    if (validIds[i]) {
      susMeanValue[0] += susVecBody[0][i];
      susMeanValue[1] += susVecBody[1][i];
      susMeanValue[2] += susVecBody[2][i];
    }
  }
  double susVecTot[3] = {0.0, 0.0, 0.0};
  VectorOperations<double>::normalize(susMeanValue, susVecTot, 3);

  /* -------- Sun Derivatiative --------------------- */

  double susVecTotDerivative[3] = {0.0, 0.0, 0.0};
  bool susVecTotDerivativeValid = false;
  double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst);
  if (timeOfSavedSusDirEst.tv_sec != 0) {
    for (uint8_t i = 0; i < 3; i++) {
      susVecTotDerivative[i] = (susVecTot[i] - savedSusVecTot[i]) / timeDiff;
      savedSusVecTot[i] = susVecTot[i];
    }
  }
  timeOfSavedSusDirEst = timeOfSusMeasurement;

  /* -------- Sun Model Direction (IJK frame) ------- */
  // if (useSunModel) eventuell
  double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfSusMeasurement);

  // Julean Centuries
  double sunIjkModel[3] = {0.0, 0.0, 0.0};
  double JC2000 = JD2000 / 36525;

  double meanLongitude =
      (sunModelParameters->omega_0 + (sunModelParameters->domega) * JC2000) * PI / 180;
  double meanAnomaly = (sunModelParameters->m_0 + sunModelParameters->dm * JC2000) * PI / 180.;

  double eclipticLongitude = meanLongitude + sunModelParameters->p1 * sin(meanAnomaly) +
                             sunModelParameters->p2 * sin(2 * meanAnomaly);

  double epsilon = sunModelParameters->e - (sunModelParameters->e1) * JC2000;

  sunIjkModel[0] = cos(eclipticLongitude);
  sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon);
  sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon);
  {
    PoolReadGuard pg(susDataProcessed);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(susDataProcessed->sus0vec.value, sus0VecBody, 3 * sizeof(float));
      susDataProcessed->sus0vec.setValid(sus0valid);
      std::memcpy(susDataProcessed->sus1vec.value, sus1VecBody, 3 * sizeof(float));
      susDataProcessed->sus1vec.setValid(sus1valid);
      std::memcpy(susDataProcessed->sus2vec.value, sus2VecBody, 3 * sizeof(float));
      susDataProcessed->sus2vec.setValid(sus2valid);
      std::memcpy(susDataProcessed->sus3vec.value, sus3VecBody, 3 * sizeof(float));
      susDataProcessed->sus3vec.setValid(sus3valid);
      std::memcpy(susDataProcessed->sus4vec.value, sus4VecBody, 3 * sizeof(float));
      susDataProcessed->sus4vec.setValid(sus4valid);
      std::memcpy(susDataProcessed->sus5vec.value, sus5VecBody, 3 * sizeof(float));
      susDataProcessed->sus5vec.setValid(sus5valid);
      std::memcpy(susDataProcessed->sus6vec.value, sus6VecBody, 3 * sizeof(float));
      susDataProcessed->sus6vec.setValid(sus6valid);
      std::memcpy(susDataProcessed->sus7vec.value, sus7VecBody, 3 * sizeof(float));
      susDataProcessed->sus7vec.setValid(sus7valid);
      std::memcpy(susDataProcessed->sus8vec.value, sus8VecBody, 3 * sizeof(float));
      susDataProcessed->sus8vec.setValid(sus8valid);
      std::memcpy(susDataProcessed->sus9vec.value, sus9VecBody, 3 * sizeof(float));
      susDataProcessed->sus9vec.setValid(sus9valid);
      std::memcpy(susDataProcessed->sus10vec.value, sus10VecBody, 3 * sizeof(float));
      susDataProcessed->sus10vec.setValid(sus10valid);
      std::memcpy(susDataProcessed->sus11vec.value, sus11VecBody, 3 * sizeof(float));
      susDataProcessed->sus11vec.setValid(sus11valid);
      std::memcpy(susDataProcessed->susVecTot.value, susVecTot, 3 * sizeof(double));
      susDataProcessed->susVecTot.setValid(true);
      std::memcpy(susDataProcessed->susVecTotDerivative.value, susVecTotDerivative,
                  3 * sizeof(double));
      susDataProcessed->susVecTotDerivative.setValid(susVecTotDerivativeValid);
      std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double));
      susDataProcessed->sunIjkModel.setValid(true);
      susDataProcessed->setValidity(true, false);
    }
  }
}

void SensorProcessing::processGyr(
    const double gyr0axXvalue, bool gyr0axXvalid, const double gyr0axYvalue, bool gyr0axYvalid,
    const double gyr0axZvalue, bool gyr0axZvalid, const double gyr1axXvalue, bool gyr1axXvalid,
    const double gyr1axYvalue, bool gyr1axYvalid, const double gyr1axZvalue, bool gyr1axZvalid,
    const double gyr2axXvalue, bool gyr2axXvalid, const double gyr2axYvalue, bool gyr2axYvalid,
    const double gyr2axZvalue, bool gyr2axZvalid, const double gyr3axXvalue, bool gyr3axXvalid,
    const double gyr3axYvalue, bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid,
    timeval timeOfGyrMeasurement, const AcsParameters::GyrHandlingParameters *gyrParameters,
    acsctrl::GyrDataProcessed *gyrDataProcessed) {
  bool gyr0valid = (gyr0axXvalid && gyr0axYvalid && gyr0axZvalid);
  bool gyr1valid = (gyr1axXvalid && gyr1axYvalid && gyr1axZvalid);
  bool gyr2valid = (gyr2axXvalid && gyr2axYvalid && gyr2axZvalid);
  bool gyr3valid = (gyr3axXvalid && gyr3axYvalid && gyr3axZvalid);
  if (!gyr0valid && !gyr1valid && !gyr2valid && !gyr3valid) {
    {
      PoolReadGuard pg(gyrDataProcessed);
      if (pg.getReadResult() == returnvalue::OK) {
        std::memcpy(gyrDataProcessed->gyr0vec.value, zeroVector, 3 * sizeof(double));
        std::memcpy(gyrDataProcessed->gyr1vec.value, zeroVector, 3 * sizeof(double));
        std::memcpy(gyrDataProcessed->gyr2vec.value, zeroVector, 3 * sizeof(double));
        std::memcpy(gyrDataProcessed->gyr3vec.value, zeroVector, 3 * sizeof(double));
        std::memcpy(gyrDataProcessed->gyrVecTot.value, zeroVector, 3 * sizeof(double));
        gyrDataProcessed->setValidity(false, true);
      }
    }
    return;
  }
  // Transforming Values to the Body Frame (actually it is the geometry frame atm)
  double gyr0ValueBody[3] = {0, 0, 0}, gyr1ValueBody[3] = {0, 0, 0}, gyr2ValueBody[3] = {0, 0, 0},
         gyr3ValueBody[3] = {0, 0, 0};
  float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0};

  if (gyr0valid) {
    const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue};
    MatrixOperations<double>::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value,
                                       gyr0ValueBody, 3, 3, 1);
    for (uint8_t i = 0; i < 3; i++) {
      sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i];
      sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i];
    }
  }
  if (gyr1valid) {
    const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue};
    MatrixOperations<double>::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value,
                                       gyr1ValueBody, 3, 3, 1);
    for (uint8_t i = 0; i < 3; i++) {
      sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i];
      sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i];
    }
  }
  if (gyr2valid) {
    const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue};
    MatrixOperations<double>::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value,
                                       gyr2ValueBody, 3, 3, 1);
    for (uint8_t i = 0; i < 3; i++) {
      sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i];
      sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i];
    }
  }
  if (gyr3valid) {
    const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue};
    MatrixOperations<double>::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value,
                                       gyr3ValueBody, 3, 3, 1);
    for (uint8_t i = 0; i < 3; i++) {
      sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i];
      sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i];
    }
  }

  /* -------- SatRateEst: Middle Value ------- */
  // take ADIS measurements, if both avail
  // if just one ADIS measurement avail, perform sensor fusion
  double gyrVecTot[3] = {0.0, 0.0, 0.0};
  if ((gyr0valid && gyr2valid) && gyrParameters->preferAdis == gyrParameters->PreferAdis::YES) {
    double gyr02ValuesSum[3];
    VectorOperations<double>::add(gyr0ValueBody, gyr2ValueBody, gyr02ValuesSum, 3);
    VectorOperations<double>::mulScalar(gyr02ValuesSum, .5, gyrVecTot, 3);
  } else {
    for (uint8_t i = 0; i < 3; i++) {
      gyrVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i];
    }
  }
  {
    PoolReadGuard pg(gyrDataProcessed);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(gyrDataProcessed->gyr0vec.value, gyr0ValueBody, 3 * sizeof(double));
      gyrDataProcessed->gyr0vec.setValid(gyr0valid);
      std::memcpy(gyrDataProcessed->gyr1vec.value, gyr1ValueBody, 3 * sizeof(double));
      gyrDataProcessed->gyr1vec.setValid(gyr1valid);
      std::memcpy(gyrDataProcessed->gyr2vec.value, gyr2ValueBody, 3 * sizeof(double));
      gyrDataProcessed->gyr2vec.setValid(gyr2valid);
      std::memcpy(gyrDataProcessed->gyr3vec.value, gyr3ValueBody, 3 * sizeof(double));
      gyrDataProcessed->gyr3vec.setValid(gyr3valid);
      std::memcpy(gyrDataProcessed->gyrVecTot.value, gyrVecTot, 3 * sizeof(double));
      gyrDataProcessed->gyrVecTot.setValid(true);
      gyrDataProcessed->setValidity(true, false);
    }
  }
}

void SensorProcessing::processGps(const double gps0latitude, const double gps0longitude,
                                  const bool validGps,
                                  acsctrl::GpsDataProcessed *gpsDataProcessed) {
  // name to convert not process
  double gdLongitude, gcLatitude;
  if (validGps) {
    // Transforming from Degree to Radians and calculation geocentric lattitude from geodetic
    gdLongitude = gps0longitude * PI / 180;
    double latitudeRad = gps0latitude * PI / 180;
    double eccentricityWgs84 = 0.0818195;
    double factor = 1 - pow(eccentricityWgs84, 2);
    gcLatitude = atan(factor * tan(latitudeRad));
    // validGcLatitude = true;
  }
  {
    PoolReadGuard pg(gpsDataProcessed);
    if (pg.getReadResult() == returnvalue::OK) {
      gpsDataProcessed->gdLongitude.value = gdLongitude;
      gpsDataProcessed->gcLatitude.value = gcLatitude;
      gpsDataProcessed->setValidity(validGps, validGps);
      if (!validGps) {
        gpsDataProcessed->gdLongitude.value = 0.0;
        gpsDataProcessed->gcLatitude.value = 0.0;
      }
    }
  }
}

void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
                               acsctrl::MgmDataProcessed *mgmDataProcessed,
                               acsctrl::SusDataProcessed *susDataProcessed,
                               acsctrl::GyrDataProcessed *gyrDataProcessed,
                               acsctrl::GpsDataProcessed *gpsDataProcessed,
                               const AcsParameters *acsParameters) {
  sensorValues->update();
  processGps(sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value,
             (sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() &&
              sensorValues->gpsSet.altitude.isValid()),
             gpsDataProcessed);

  processMgm(sensorValues->mgm0Lis3Set.fieldStrengths.value,
             sensorValues->mgm0Lis3Set.fieldStrengths.isValid(),
             sensorValues->mgm1Rm3100Set.fieldStrengths.value,
             sensorValues->mgm1Rm3100Set.fieldStrengths.isValid(),
             sensorValues->mgm2Lis3Set.fieldStrengths.value,
             sensorValues->mgm2Lis3Set.fieldStrengths.isValid(),
             sensorValues->mgm3Rm3100Set.fieldStrengths.value,
             sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(),
             sensorValues->imtqMgmSet.mtmRawNt.value, sensorValues->imtqMgmSet.mtmRawNt.isValid(),
             now, &acsParameters->mgmHandlingParameters, gpsDataProcessed,
             sensorValues->gpsSet.altitude.value,
             (sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() &&
              sensorValues->gpsSet.altitude.isValid()),
             mgmDataProcessed);

  processSus(sensorValues->susSets[0].channels.value, sensorValues->susSets[0].channels.isValid(),
             sensorValues->susSets[1].channels.value, sensorValues->susSets[1].channels.isValid(),
             sensorValues->susSets[2].channels.value, sensorValues->susSets[2].channels.isValid(),
             sensorValues->susSets[3].channels.value, sensorValues->susSets[3].channels.isValid(),
             sensorValues->susSets[4].channels.value, sensorValues->susSets[4].channels.isValid(),
             sensorValues->susSets[5].channels.value, sensorValues->susSets[5].channels.isValid(),
             sensorValues->susSets[6].channels.value, sensorValues->susSets[6].channels.isValid(),
             sensorValues->susSets[7].channels.value, sensorValues->susSets[7].channels.isValid(),
             sensorValues->susSets[8].channels.value, sensorValues->susSets[8].channels.isValid(),
             sensorValues->susSets[9].channels.value, sensorValues->susSets[9].channels.isValid(),
             sensorValues->susSets[10].channels.value, sensorValues->susSets[10].channels.isValid(),
             sensorValues->susSets[11].channels.value, sensorValues->susSets[11].channels.isValid(),
             now, &acsParameters->susHandlingParameters, &acsParameters->sunModelParameters,
             susDataProcessed);

  processGyr(
      sensorValues->gyr0AdisSet.angVelocX.value, sensorValues->gyr0AdisSet.angVelocX.isValid(),
      sensorValues->gyr0AdisSet.angVelocY.value, sensorValues->gyr0AdisSet.angVelocY.isValid(),
      sensorValues->gyr0AdisSet.angVelocZ.value, sensorValues->gyr0AdisSet.angVelocZ.isValid(),
      sensorValues->gyr1L3gSet.angVelocX.value, sensorValues->gyr1L3gSet.angVelocX.isValid(),
      sensorValues->gyr1L3gSet.angVelocY.value, sensorValues->gyr1L3gSet.angVelocY.isValid(),
      sensorValues->gyr1L3gSet.angVelocZ.value, sensorValues->gyr1L3gSet.angVelocZ.isValid(),
      sensorValues->gyr2AdisSet.angVelocX.value, sensorValues->gyr2AdisSet.angVelocX.isValid(),
      sensorValues->gyr2AdisSet.angVelocY.value, sensorValues->gyr2AdisSet.angVelocY.isValid(),
      sensorValues->gyr2AdisSet.angVelocZ.value, sensorValues->gyr2AdisSet.angVelocZ.isValid(),
      sensorValues->gyr3L3gSet.angVelocX.value, sensorValues->gyr3L3gSet.angVelocX.isValid(),
      sensorValues->gyr3L3gSet.angVelocY.value, sensorValues->gyr3L3gSet.angVelocY.isValid(),
      sensorValues->gyr3L3gSet.angVelocZ.value, sensorValues->gyr3L3gSet.angVelocZ.isValid(), now,
      &acsParameters->gyrHandlingParameters, gyrDataProcessed);
}