/*
 * SensorProcessing.cpp
 *
 *  Created on: 7 Mar 2022
 *      Author: Robin Marquardt
 */

#include "SensorProcessing.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_) : savedMagFieldEst{0, 0, 0} {
  validMagField = false;
  validGcLatitude = false;
}

SensorProcessing::~SensorProcessing() {}

bool 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,
                                  const double gpsLatitude, const double gpsLongitude,
                                  const double gpsAltitude, bool gpsValid, double *magFieldEst,
                                  bool *outputValid, double *magFieldModel,
                                  bool *magFieldModelValid, double *magneticFieldVectorDerivative,
                                  bool *magneticFieldVectorDerivativeValid) {
  if (!mgm0valid && !mgm1valid && !mgm2valid && !mgm3valid && !mgm4valid) {
    *outputValid = false;
    validMagField = false;
    return false;
  }
  // Transforming Values to the Body Frame (actually it is the geometry frame atm)
  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};

  bool validUnit[5] = {false, false, false, false, false};
  uint8_t validCount = 0;
  if (mgm0valid) {
    MatrixOperations<float>::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0Value,
                                      mgm0ValueBody, 3, 3, 1);
    validCount += 1;
    validUnit[0] = true;
  }
  if (mgm1valid) {
    MatrixOperations<float>::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1Value,
                                      mgm1ValueBody, 3, 3, 1);
    validCount += 1;
    validUnit[1] = true;
  }
  if (mgm2valid) {
    MatrixOperations<float>::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2Value,
                                      mgm2ValueBody, 3, 3, 1);
    validCount += 1;
    validUnit[2] = true;
  }
  if (mgm3valid) {
    MatrixOperations<float>::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3Value,
                                      mgm3ValueBody, 3, 3, 1);
    validCount += 1;
    validUnit[3] = true;
  }
  if (mgm4valid) {
    MatrixOperations<float>::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4Value,
                                      mgm4ValueBody, 3, 3, 1);
    validCount += 1;
    validUnit[4] = true;
  }

  /* -------- MagFieldEst: Middle Value ------- */
  float mgmValues[3][5] = {
      {mgm0ValueBody[0], mgm1ValueBody[0], mgm2ValueBody[0], mgm3ValueBody[0], mgm4ValueBody[0]},
      {mgm0ValueBody[1], mgm1ValueBody[1], mgm2ValueBody[1], mgm3ValueBody[1], mgm4ValueBody[1]},
      {mgm0ValueBody[2], mgm1ValueBody[2], mgm2ValueBody[2], mgm3ValueBody[2], mgm4ValueBody[2]}};
  double mgmValidValues[3][validCount];
  uint8_t j = 0;
  for (uint8_t i = 0; i < validCount; i++) {
    if (validUnit[i]) {
      mgmValidValues[0][j] = mgmValues[0][i];
      mgmValidValues[1][j] = mgmValues[1][i];
      mgmValidValues[2][j] = mgmValues[2][i];
      j += 1;
    }
  }
  // Selection Sort
  double mgmValidValuesSort[3][validCount];
  MathOperations<double>::selectionSort(*mgmValidValues, *mgmValidValuesSort, 3, validCount);

  uint8_t n = ceil(validCount / 2);
  magFieldEst[0] = mgmValidValuesSort[0][n];
  magFieldEst[1] = mgmValidValuesSort[1][n];
  magFieldEst[2] = mgmValidValuesSort[2][n];
  validMagField = true;

  //-----------------------Mag Rate Computation ---------------------------------------------------
  double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst);
  for (uint8_t i = 0; i < 3; i++) {
    magneticFieldVectorDerivative[i] = (magFieldEst[i] - savedMagFieldEst[i]) / timeDiff;
    savedMagFieldEst[i] = magFieldEst[i];
  }

  *magneticFieldVectorDerivativeValid = true;
  if (timeOfSavedMagFieldEst.tv_sec == 0) {
    magneticFieldVectorDerivative[0] = 0;
    magneticFieldVectorDerivative[1] = 0;
    magneticFieldVectorDerivative[2] = 0;
    *magneticFieldVectorDerivativeValid = false;
  }

  timeOfSavedMagFieldEst = timeOfMgmMeasurement;

  *outputValid = true;

  // ---------------- IGRF- 13 Implementation here ------------------------------------------------
  if (!gpsValid) {
    *magFieldModelValid = false;
  } else {
    // 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(gpsLongitude, gpsLatitude, gpsAltitude, timeOfMgmMeasurement,
                        magFieldModel);
    *magFieldModelValid = false;
  }

  return true;
}

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, double *sunDirEst,
    bool *sunDirEstValid, double *sunVectorInertial, bool *sunVectorInertialValid,
    double *sunVectorDerivative, bool *sunVectorDerivativeValid) {
  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) {
    *sunDirEstValid = false;
    return;
  } else {
    // 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);
    }
    if (sus1valid) {
      MatrixOperations<float>::multiply(
          susParameters->sus1orientationMatrix[0],
          susConverter.getSunVectorSensorFrame(sus1Value, susParameters->sus1coeffAlpha,
                                               susParameters->sus1coeffBeta),
          sus1VecBody, 3, 3, 1);
    }
    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];
      }
    }
    VectorOperations<double>::normalize(susMeanValue, sunDirEst, 3);
    *sunDirEstValid = true;
  }

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

  double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst);
  for (uint8_t i = 0; i < 3; i++) {
    sunVectorDerivative[i] = (sunDirEst[i] - savedSunVector[i]) / timeDiff;
    savedSunVector[i] = sunDirEst[i];
  }

  *sunVectorDerivativeValid = true;
  if (timeOfSavedSusDirEst.tv_sec == 0) {
    sunVectorDerivative[0] = 0;
    sunVectorDerivative[1] = 0;
    sunVectorDerivative[2] = 0;
    *sunVectorDerivativeValid = false;
  }

  timeOfSavedSusDirEst = timeOfSusMeasurement;

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

  // Julean Centuries
  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;

  sunVectorInertial[0] = cos(eclipticLongitude);
  sunVectorInertial[1] = sin(eclipticLongitude) * cos(epsilon);
  sunVectorInertial[2] = sin(eclipticLongitude) * sin(epsilon);

  *sunVectorInertialValid = true;
}

void SensorProcessing::processRmu(const double rmu0Value[], bool rmu0valid,
                                  const double rmu1Value[], bool rmu1valid,
                                  const double rmu2Value[], bool rmu2valid,
                                  timeval timeOfrmuMeasurement,
                                  const AcsParameters::RmuHandlingParameters *rmuParameters,
                                  double *satRatEst, bool *satRateEstValid) {
  if (!rmu0valid && !rmu1valid && !rmu2valid) {
    *satRateEstValid = false;
    return;
  }
  // Transforming Values to the Body Frame (actually it is the geometry frame atm)
  double rmu0ValueBody[3] = {0, 0, 0}, rmu1ValueBody[3] = {0, 0, 0}, rmu2ValueBody[3] = {0, 0, 0};

  bool validUnit[3] = {false, false, false};
  uint8_t validCount = 0;
  if (rmu0valid) {
    MatrixOperations<double>::multiply(rmuParameters->rmu0orientationMatrix[0], rmu0Value,
                                       rmu0ValueBody, 3, 3, 1);
    validCount += 1;
    validUnit[0] = true;
  }
  if (rmu1valid) {
    MatrixOperations<double>::multiply(rmuParameters->rmu1orientationMatrix[0], rmu1Value,
                                       rmu1ValueBody, 3, 3, 1);
    validCount += 1;
    validUnit[1] = true;
  }
  if (rmu2valid) {
    MatrixOperations<double>::multiply(rmuParameters->rmu2orientationMatrix[0], rmu2Value,
                                       rmu2ValueBody, 3, 3, 1);
    validCount += 1;
    validUnit[2] = true;
  }

  /* -------- SatRateEst: Middle Value ------- */
  double rmuValues[3][3] = {{rmu0ValueBody[0], rmu1ValueBody[0], rmu2ValueBody[0]},
                            {rmu0ValueBody[1], rmu1ValueBody[1], rmu2ValueBody[1]},
                            {rmu0ValueBody[2], rmu1ValueBody[2], rmu2ValueBody[2]}};
  double rmuValidValues[3][validCount];
  uint8_t j = 0;
  for (uint8_t i = 0; i < validCount; i++) {
    if (validUnit[i]) {
      rmuValidValues[0][j] = rmuValues[0][i];
      rmuValidValues[1][j] = rmuValues[1][i];
      rmuValidValues[2][j] = rmuValues[2][i];
      j += 1;
    }
  }
  // Selection Sort
  double rmuValidValuesSort[3][validCount];
  MathOperations<double>::selectionSort(*rmuValidValues, *rmuValidValuesSort, 3, validCount);

  uint8_t n = ceil(validCount / 2);
  satRatEst[0] = rmuValidValuesSort[0][n];
  satRatEst[1] = rmuValidValuesSort[1][n];
  satRatEst[2] = rmuValidValuesSort[2][n];
  *satRateEstValid = true;
}

void SensorProcessing::processGps(const double gps0latitude, const double gps0longitude,
                                  const bool validGps, double *gcLatitude, double *gdLongitude) {
  // name to convert not process
  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;
  }
}

void SensorProcessing::process(acsctrl::SusDataRaw *susData, timeval now,
                               ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues,
                               const AcsParameters *acsParameters) {
  sensorValues->update();
  //  processGps(sensorValues->gps0latitude, sensorValues->gps0longitude, sensorValues->gps0Valid,
  //             &outputValues->gcLatitude, &outputValues->gdLongitude);

  outputValues->mgmUpdated = 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,
      outputValues->gcLatitude, outputValues->gdLongitude, sensorValues->gps0altitude,
      sensorValues->gps0Valid, outputValues->magFieldEst, &outputValues->magFieldEstValid,
      outputValues->magFieldModel, &outputValues->magFieldModelValid,
      outputValues->magneticFieldVectorDerivative,
      &outputValues->magneticFieldVectorDerivativeValid);  // VALID outputs- PoolVariable ?

  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,
             outputValues->sunDirEst, &outputValues->sunDirEstValid, outputValues->sunDirModel,
             &outputValues->sunDirModelValid, outputValues->sunVectorDerivative,
             &outputValues->sunVectorDerivativeValid);
  // VALID outputs ?

  //  processRmu(sensorValues->rmu0, sensorValues->rmu0Valid, sensorValues->rmu1,
  //             sensorValues->rmu1Valid, sensorValues->rmu2, sensorValues->rmu2Valid, now,
  //             &acsParameters->rmuHandlingParameters, outputValues->satRateEst,
  //             &outputValues->satRateEstValid);
}