#include "Navigation.h"

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

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

Navigation::Navigation() {}

Navigation::~Navigation() {}

ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
                                  acsctrl::GyrDataProcessed *gyrDataProcessed,
                                  acsctrl::MgmDataProcessed *mgmDataProcessed,
                                  acsctrl::SusDataProcessed *susDataProcessed,
                                  acsctrl::MekfData *mekfData, AcsParameters *acsParameters) {
  double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
                      sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
  bool quatIBValid = sensorValues->strSet.isTrustWorthy.value;

  if (mekfStatus == MultiplicativeKalmanFilter::MEKF_UNINITIALIZED) {
    mekfStatus = multiplicativeKalmanFilter.init(
        mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(),
        susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(),
        susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(),
        mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData,
        acsParameters);
    return mekfStatus;
  } else {
    mekfStatus = multiplicativeKalmanFilter.mekfEst(
        quatIB, quatIBValid, gyrDataProcessed->gyrVecTot.value,
        gyrDataProcessed->gyrVecTot.isValid(), mgmDataProcessed->mgmVecTot.value,
        mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value,
        susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value,
        susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value,
        mgmDataProcessed->magIgrfModel.isValid(), mekfData, acsParameters);
    return mekfStatus;
  }
}

void Navigation::resetMekf(acsctrl::MekfData *mekfData) {
  mekfStatus = multiplicativeKalmanFilter.reset(mekfData);
}

void Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed) {
  double position[3] = {0, 0, 0};
  double velocity[3] = {0, 0, 0};
  ReturnValue_t result = sgp4Propagator.propagate(position, velocity, now, 0);

  if (result == returnvalue::OK) {
    {
      PoolReadGuard pg(gpsDataProcessed);
      if (pg.getReadResult() == returnvalue::OK) {
        gpsDataProcessed->source = acs::GpsSource::SPG4;
        gpsDataProcessed->source.setValid(true);
        std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double));
        gpsDataProcessed->gpsPosition.setValid(true);
        std::memcpy(gpsDataProcessed->gpsVelocity.value, velocity, 3 * sizeof(double));
        gpsDataProcessed->gpsVelocity.setValid(true);
      }
    }
  } else {
    {
      PoolReadGuard pg(gpsDataProcessed);
      if (pg.getReadResult() == returnvalue::OK) {
        gpsDataProcessed->source = acs::GpsSource::NONE;
        gpsDataProcessed->source.setValid(true);
        std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double));
        gpsDataProcessed->gpsPosition.setValid(false);
        std::memcpy(gpsDataProcessed->gpsVelocity.value, velocity, 3 * sizeof(double));
        gpsDataProcessed->gpsVelocity.setValid(false);
      }
    }
  }
}

ReturnValue_t Navigation::updateTle(const uint8_t *line1, const uint8_t *line2) {
  return sgp4Propagator.initialize(line1, line2);
}