#include "FusedRotationEstimation.h"

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

void FusedRotationEstimation::estimateFusedRotationRate(
    acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
    acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues,
    const double timeDelta, acsctrl::FusedRotRateData *fusedRotRateData) {
  if (sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and
      sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid()) {
    double quatNew[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
                         sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
    if (VectorOperations<double>::norm(quatOld, 4) != 0 and timeDelta != 0) {
      estimateFusedRotationRateStr(quatNew, timeDelta, fusedRotRateData);
    } else {
      estimateFusedRotationRateSafe(susDataProcessed, mgmDataProcessed, gyrDataProcessed,
                                    fusedRotRateData);
    }
    std::memcpy(quatOld, quatNew, sizeof(quatOld));
  } else {
    std::memcpy(quatOld, ZERO_VEC4, sizeof(quatOld));
    estimateFusedRotationRateSafe(susDataProcessed, mgmDataProcessed, gyrDataProcessed,
                                  fusedRotRateData);
  }
}

void FusedRotationEstimation::estimateFusedRotationRateStr(
    double *quatNew, const double timeDelta, acsctrl::FusedRotRateData *fusedRotRateData) {
  double quatOldInv[4] = {0, 0, 0, 0};
  double quatDelta[4] = {0, 0, 0, 0};

  QuaternionOperations::inverse(quatOld, quatOldInv);
  QuaternionOperations::multiply(quatNew, quatOldInv, quatDelta);
  QuaternionOperations::normalize(quatDelta);

  double rotVec[3] = {0, 0, 0};
  double angle = QuaternionOperations::getAngle(quatDelta);
  if (angle == 0.0) {
    {
      PoolReadGuard pg(fusedRotRateData);
      std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
      fusedRotRateData->rotRateOrthogonal.setValid(false);
      std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
      fusedRotRateData->rotRateParallel.setValid(false);
      std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double));
      fusedRotRateData->rotRateTotal.setValid(true);
    }
  }
  VectorOperations<double>::normalize(quatDelta, rotVec, 3);
  VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotVec, 3);
  {
    PoolReadGuard pg(fusedRotRateData);
    std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
    fusedRotRateData->rotRateOrthogonal.setValid(false);
    std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
    fusedRotRateData->rotRateParallel.setValid(false);
    std::memcpy(fusedRotRateData->rotRateTotal.value, rotVec, 3 * sizeof(double));
    fusedRotRateData->rotRateTotal.setValid(true);
  }
}

void FusedRotationEstimation::estimateFusedRotationRateSafe(
    acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
    acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) {
  if ((not mgmDataProcessed->mgmVecTot.isValid() and not susDataProcessed->susVecTot.isValid() and
       not fusedRotRateData->rotRateTotal.isValid()) or
      (not susDataProcessed->susVecTotDerivative.isValid() and
       not mgmDataProcessed->mgmVecTotDerivative.isValid())) {
    {
      PoolReadGuard pg(fusedRotRateData);
      std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
      std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
      std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double));
      fusedRotRateData->setValidity(false, true);
    }
    // store for calculation of angular acceleration
    if (gyrDataProcessed->gyrVecTot.isValid()) {
      std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double));
    }
    return;
  }
  if (not susDataProcessed->susVecTot.isValid()) {
    estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData);
    // store for calculation of angular acceleration
    if (gyrDataProcessed->gyrVecTot.isValid()) {
      std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double));
    }
    return;
  }

  // calculate rotation around the sun
  double magSunCross[3] = {0, 0, 0};

  VectorOperations<double>::cross(mgmDataProcessed->mgmVecTot.value,
                                  susDataProcessed->susVecTot.value, magSunCross);
  double magSunCrossNorm = VectorOperations<double>::norm(magSunCross, 3);
  double magNorm = VectorOperations<double>::norm(mgmDataProcessed->mgmVecTot.value, 3);
  double fusedRotRateParallel[3] = {0, 0, 0};
  if (magSunCrossNorm >
      (acsParameters->safeModeControllerParameters.sineLimitSunRotRate * magNorm)) {
    double omegaParallel =
        VectorOperations<double>::dot(mgmDataProcessed->mgmVecTotDerivative.value, magSunCross) *
        pow(magSunCrossNorm, -2);
    VectorOperations<double>::mulScalar(susDataProcessed->susVecTot.value, omegaParallel,
                                        fusedRotRateParallel, 3);
  } else {
    estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData);
    // store for calculation of angular acceleration
    if (gyrDataProcessed->gyrVecTot.isValid()) {
      std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double));
    }
    return;
  }

  // calculate rotation orthogonal to the sun
  double fusedRotRateOrthogonal[3] = {0, 0, 0};
  VectorOperations<double>::cross(susDataProcessed->susVecTotDerivative.value,
                                  susDataProcessed->susVecTot.value, fusedRotRateOrthogonal);
  VectorOperations<double>::mulScalar(
      fusedRotRateOrthogonal,
      pow(VectorOperations<double>::norm(susDataProcessed->susVecTot.value, 3), -2),
      fusedRotRateOrthogonal, 3);

  // calculate total rotation rate
  double fusedRotRateTotal[3] = {0, 0, 0};
  VectorOperations<double>::add(fusedRotRateParallel, fusedRotRateOrthogonal, fusedRotRateTotal);

  {
    PoolReadGuard pg(fusedRotRateData);
    std::memcpy(fusedRotRateData->rotRateOrthogonal.value, fusedRotRateOrthogonal,
                3 * sizeof(double));
    std::memcpy(fusedRotRateData->rotRateParallel.value, fusedRotRateParallel, 3 * sizeof(double));
    std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double));
    fusedRotRateData->setValidity(true, true);
  }

  // store for calculation of angular acceleration
  if (gyrDataProcessed->gyrVecTot.isValid()) {
    std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double));
  }
}

void FusedRotationEstimation::estimateFusedRotationRateEclipse(
    acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) {
  if (not acsParameters->onBoardParams.fusedRateSafeDuringEclipse or
      not gyrDataProcessed->gyrVecTot.isValid() or
      VectorOperations<double>::norm(fusedRotRateData->rotRateTotal.value, 3) == 0) {
    {
      PoolReadGuard pg(fusedRotRateData);
      std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
      std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
      std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double));
      fusedRotRateData->setValidity(false, true);
    }
    return;
  }
  double angAccelB[3] = {0, 0, 0};
  VectorOperations<double>::subtract(gyrDataProcessed->gyrVecTot.value, rotRateOldB, angAccelB, 3);
  double fusedRotRateTotal[3] = {0, 0, 0};
  VectorOperations<double>::add(fusedRotRateData->rotRateTotal.value, angAccelB, fusedRotRateTotal,
                                3);
  {
    PoolReadGuard pg(fusedRotRateData);
    std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
    fusedRotRateData->rotRateOrthogonal.setValid(false);
    std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
    fusedRotRateData->rotRateParallel.setValid(false);
    std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double));
    fusedRotRateData->rotRateTotal.setValid(true);
  }
}