#include "Guidance.h"

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

Guidance::~Guidance() {}

void Guidance::targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta,
                                 const double sunDirI[3], const double posSatF[4],
                                 double targetQuat[4], double targetSatRotRate[3]) {
  // positive z-Axis of EIVE in direction of sun
  double zAxisIX[3] = {0, 0, 0};
  VectorOperations<double>::normalize(sunDirI, zAxisIX, 3);

  // determine helper vector to point x-Axis and therefore the STR away from Earth
  double helperXI[3] = {0, 0, 0}, posSatI[3] = {0, 0, 0};
  CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute);
  VectorOperations<double>::normalize(posSatI, helperXI, 3);

  // construct y-axis from helper vector and z-axis
  double yAxisIX[3] = {0, 0, 0};
  VectorOperations<double>::cross(zAxisIX, helperXI, yAxisIX);
  VectorOperations<double>::normalize(yAxisIX, yAxisIX, 3);

  // x-axis completes RHS
  double xAxisIX[3] = {0, 0, 0};
  VectorOperations<double>::cross(yAxisIX, zAxisIX, xAxisIX);
  VectorOperations<double>::normalize(xAxisIX, xAxisIX, 3);

  // join transformation matrix
  double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]},
                        {xAxisIX[1], yAxisIX[1], zAxisIX[1]},
                        {xAxisIX[2], yAxisIX[2], zAxisIX[2]}};
  QuaternionOperations::fromDcm(dcmIX, targetQuat);

  // calculate of reference rotation rate
  targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
}

void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3],
                                   double velSatF[3], double targetQuat[4],
                                   double targetSatRotRate[3]) {
  //-------------------------------------------------------------------------------------
  //	Calculation of target quaternion for target pointing
  //-------------------------------------------------------------------------------------
  // transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
  double targetF[3] = {0, 0, 0};
  CoordinateTransformations::cartesianFromLatLongAlt(
      acsParameters->targetModeControllerParameters.latitudeTgt,
      acsParameters->targetModeControllerParameters.longitudeTgt,
      acsParameters->targetModeControllerParameters.altitudeTgt, targetF);
  double targetDirF[3] = {0, 0, 0};
  VectorOperations<double>::subtract(targetF, posSatF, targetDirF, 3);

  // target direction in the ECI frame
  double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI[3] = {0, 0, 0};
  CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute);
  CoordinateTransformations::positionEcfToEci(targetF, targetI, &timeAbsolute);
  VectorOperations<double>::subtract(targetI, posSatI, targetDirI, 3);

  // x-axis aligned with target direction
  // this aligns with the camera, E- and S-band antennas
  double xAxisIX[3] = {0, 0, 0};
  VectorOperations<double>::normalize(targetDirI, xAxisIX, 3);

  // transform velocity into inertial frame
  double velSatI[3] = {0, 0, 0};
  CoordinateTransformations::velocityEcfToEci(velSatF, posSatF, velSatI, &timeAbsolute);

  // orbital normal vector of target and velocity vector
  double orbitalNormalI[3] = {0, 0, 0};
  VectorOperations<double>::cross(posSatI, velSatI, orbitalNormalI);
  VectorOperations<double>::normalize(orbitalNormalI, orbitalNormalI, 3);

  // y-axis of satellite in orbit plane so that z-axis is parallel to long side of picture
  // resolution
  double yAxisIX[3] = {0, 0, 0};
  VectorOperations<double>::cross(orbitalNormalI, xAxisIX, yAxisIX);
  VectorOperations<double>::normalize(yAxisIX, yAxisIX, 3);

  // z-axis completes RHS
  double zAxisIX[3] = {0, 0, 0};
  VectorOperations<double>::cross(xAxisIX, yAxisIX, zAxisIX);

  // join transformation matrix
  double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]},
                        {xAxisIX[1], yAxisIX[1], zAxisIX[1]},
                        {xAxisIX[2], yAxisIX[2], zAxisIX[2]}};
  QuaternionOperations::fromDcm(dcmIX, targetQuat);

  targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
}

void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta,
                               const double posSatF[3], const double sunDirI[3],
                               double targetQuat[4], double targetSatRotRate[3]) {
  //-------------------------------------------------------------------------------------
  //	Calculation of target quaternion for ground station pointing
  //-------------------------------------------------------------------------------------
  // transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
  double posGroundStationF[3] = {0, 0, 0};
  CoordinateTransformations::cartesianFromLatLongAlt(
      acsParameters->gsTargetModeControllerParameters.latitudeTgt,
      acsParameters->gsTargetModeControllerParameters.longitudeTgt,
      acsParameters->gsTargetModeControllerParameters.altitudeTgt, posGroundStationF);

  // target direction in the ECI frame
  double posSatI[3] = {0, 0, 0}, posGroundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0};
  CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute);
  CoordinateTransformations::positionEcfToEci(posGroundStationI, posGroundStationI, &timeAbsolute);
  VectorOperations<double>::subtract(posGroundStationI, posSatI, groundStationDirI, 3);

  // negative x-axis aligned with target direction
  // this aligns with the camera, E- and S-band antennas
  double xAxisIX[3] = {0, 0, 0};
  VectorOperations<double>::normalize(groundStationDirI, xAxisIX, 3);
  VectorOperations<double>::mulScalar(xAxisIX, -1, xAxisIX, 3);

  // normalize sun vector in ECI
  double sunDirNormalizedI[3] = {0, 0, 0};
  // get earth vector in ECI
  double earthDirNormalizedI[3] = {0, 0, 0};
  VectorOperations<double>::normalize(posSatI, earthDirNormalizedI, 3);
  VectorOperations<double>::mulScalar(earthDirNormalizedI, -1, earthDirNormalizedI, 3);

  // sun avoidance calculations
  double sunPerpendicularX[3] = {0, 0, 0}, sunFloorYZ[3] = {0, 0, 0};
  VectorOperations<double>::mulScalar(
      xAxisIX, VectorOperations<double>::dot(xAxisIX, sunDirNormalizedI), sunPerpendicularX, 3);
  VectorOperations<double>::subtract(sunDirNormalizedI, sunPerpendicularX, sunFloorYZ, 3);
  VectorOperations<double>::normalize(sunFloorYZ, sunFloorYZ, 3);
  double sunWeight = 0, strVecSun[3] = {0, 0, 0}, strVecSunX[3] = {0, 0, 0},
         strVecSunZ[3] = {0, 0, 0};
  VectorOperations<double>::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0],
                                      strVecSunX, 3);
  VectorOperations<double>::mulScalar(sunFloorYZ, acsParameters->strParameters.boresightAxis[2],
                                      strVecSunZ, 3);
  VectorOperations<double>::add(strVecSunX, strVecSunZ, strVecSun, 3);
  VectorOperations<double>::normalize(strVecSun, strVecSun, 3);
  sunWeight = VectorOperations<double>::dot(strVecSun, sunDirNormalizedI);

  // earth avoidance calculations
  double earthPerpendicularX[3] = {0, 0, 0}, earthFloorYZ[3] = {0, 0, 0};
  VectorOperations<double>::mulScalar(
      xAxisIX, VectorOperations<double>::dot(xAxisIX, earthDirNormalizedI), earthPerpendicularX, 3);
  VectorOperations<double>::subtract(earthDirNormalizedI, earthPerpendicularX, earthFloorYZ, 3);
  VectorOperations<double>::normalize(earthFloorYZ, earthFloorYZ, 3);
  double earthWeight = 0, strVecEarth[3] = {0, 0, 0}, strVecEarthX[3] = {0, 0, 0},
         strVecEarthZ[3] = {0, 0, 0};
  VectorOperations<double>::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0],
                                      strVecEarthX, 3);
  VectorOperations<double>::mulScalar(earthFloorYZ, acsParameters->strParameters.boresightAxis[2],
                                      strVecEarthZ, 3);
  VectorOperations<double>::add(strVecEarthX, strVecEarthZ, strVecEarth, 3);
  VectorOperations<double>::normalize(strVecEarth, strVecEarth, 3);
  earthWeight = VectorOperations<double>::dot(strVecEarth, earthDirNormalizedI);

  if ((sunWeight == 0.0) and (earthWeight == 0.0)) {
    // if this actually ever happens i will eat a broom
    sunWeight = 0.5;
    earthWeight = 0.5;
  }

  // normalize weights for convenience
  double normFactor = 1. / (std::abs(sunWeight) + std::abs(earthWeight));
  sunWeight *= normFactor;
  earthWeight *= normFactor;

  // calculate z-axis for str blinding avoidance
  double zAxisSun[3] = {0, 0, 0}, zAxisEarth[3] = {0, 0, 0}, zAxisIX[3] = {0, 0, 0};
  VectorOperations<double>::mulScalar(sunFloorYZ, sunWeight, zAxisSun, 3);
  VectorOperations<double>::mulScalar(earthFloorYZ, earthWeight, zAxisEarth, 3);
  VectorOperations<double>::add(zAxisSun, zAxisEarth, zAxisIX, 3);
  VectorOperations<double>::mulScalar(zAxisIX, -1, zAxisIX, 3);
  VectorOperations<double>::normalize(zAxisIX, zAxisIX, 3);

  // calculate y-axis
  double yAxisIX[3] = {0, 0, 0};
  VectorOperations<double>::cross(zAxisIX, xAxisIX, yAxisIX);
  VectorOperations<double>::normalize(yAxisIX, yAxisIX, 3);

  // join transformation matrix
  double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]},
                        {xAxisIX[1], yAxisIX[1], zAxisIX[1]},
                        {xAxisIX[2], yAxisIX[2], zAxisIX[2]}};
  QuaternionOperations::fromDcm(dcmIX, targetQuat);

  targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
}

void Guidance::targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, double posSatE[3],
                                  double velSatE[3], double targetQuat[4], double refSatRate[3]) {
  //-------------------------------------------------------------------------------------
  //	Calculation of target quaternion for Nadir pointing
  //-------------------------------------------------------------------------------------

  // satellite position in inertial reference frame
  double posSatI[3] = {0, 0, 0};
  CoordinateTransformations::positionEcfToEci(posSatE, posSatI, &timeAbsolute);

  // negative x-axis aligned with position vector
  // this aligns with the camera, E- and S-band antennas
  double xAxisIX[3] = {0, 0, 0};
  VectorOperations<double>::normalize(posSatI, xAxisIX, 3);
  VectorOperations<double>::mulScalar(xAxisIX, -1, xAxisIX, 3);

  // make z-Axis parallel to major part of camera resolution
  double zAxisIX[3] = {0, 0, 0};
  double velSatI[3] = {0, 0, 0};
  CoordinateTransformations::velocityEcfToEci(velSatE, posSatE, velSatI, &timeAbsolute);
  VectorOperations<double>::cross(xAxisIX, velSatI, zAxisIX);
  VectorOperations<double>::normalize(zAxisIX, zAxisIX, 3);

  // y-Axis completes RHS
  double yAxisIX[3] = {0, 0, 0};
  VectorOperations<double>::cross(zAxisIX, xAxisIX, yAxisIX);

  // join transformation matrix
  double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]},
                        {xAxisIX[1], yAxisIX[1], zAxisIX[1]},
                        {xAxisIX[2], yAxisIX[2], zAxisIX[2]}};
  QuaternionOperations::fromDcm(dcmIX, targetQuat);

  targetRotationRate(timeDelta, targetQuat, refSatRate);
}

void Guidance::targetRotationRate(const double timeDelta, double quatIX[4], double *refSatRate) {
  if (VectorOperations<double>::norm(quatIXprev, 4) == 0) {
    std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev));
  }
  if (timeDelta != 0.0) {
    QuaternionOperations::rotationFromQuaternions(quatIX, quatIXprev, timeDelta, refSatRate);
  } else {
    std::memcpy(refSatRate, ZERO_VEC3, 3 * sizeof(double));
  }
  std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev));
}

void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
                          double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
                          double errorQuat[4], double errorSatRotRate[3], double &errorAngle) {
  // First calculate error quaternion between current and target orientation without reference
  // quaternion
  double errorQuatWoRef[4] = {0, 0, 0, 0};
  QuaternionOperations::multiply(currentQuat, targetQuat, errorQuatWoRef);
  // Then add rotation from reference quaternion
  QuaternionOperations::multiply(refQuat, errorQuatWoRef, errorQuat);
  // Keep scalar part of quaternion positive
  if (errorQuat[3] < 0) {
    VectorOperations<double>::mulScalar(errorQuat, -1, errorQuat, 4);
  }
  // Calculate error angle
  errorAngle = QuaternionOperations::getAngle(errorQuat, true);

  // Calculate error satellite rotational rate
  // Convert target rotational rate into body RF
  double errorQuatInv[4] = {0, 0, 0, 0}, targetSatRotRateB[3] = {0, 0, 0};
  QuaternionOperations::inverse(errorQuat, errorQuatInv);
  QuaternionOperations::multiplyVector(errorQuatInv, targetSatRotRate, targetSatRotRateB);
  // Combine the target and reference satellite rotational rates
  double combinedRefSatRotRate[3] = {0, 0, 0};
  VectorOperations<double>::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3);
  // Then subtract the combined required satellite rotational rates from the actual rate
  VectorOperations<double>::subtract(currentSatRotRate, combinedRefSatRotRate, errorSatRotRate, 3);
}

void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
                          double targetSatRotRate[3], double errorQuat[4],
                          double errorSatRotRate[3], double &errorAngle) {
  double refQuat[4] = {0, 0, 0, 1}, refSatRotRate[3] = {0, 0, 0};
  comparePtg(currentQuat, currentSatRotRate, targetQuat, targetSatRotRate, refQuat, refSatRotRate,
             errorQuat, errorSatRotRate, errorAngle);
}

ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
                                                double *rwPseudoInv) {
  bool rw1valid = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid());
  bool rw2valid = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid());
  bool rw3valid = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid());
  bool rw4valid = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.state.isValid());

  if (rw1valid and rw2valid and rw3valid and rw4valid) {
    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double));
    return returnvalue::OK;
  } else if (not rw1valid and rw2valid and rw3valid and rw4valid) {
    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW1,
                12 * sizeof(double));
    return acsctrl::SINGLE_RW_UNAVAILABLE;
  } else if (rw1valid and not rw2valid and rw3valid and rw4valid) {
    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW2,
                12 * sizeof(double));
    return acsctrl::SINGLE_RW_UNAVAILABLE;
  } else if (rw1valid and rw2valid and not rw3valid and rw4valid) {
    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW3,
                12 * sizeof(double));
    return acsctrl::SINGLE_RW_UNAVAILABLE;
  } else if (rw1valid and rw2valid and rw3valid and not rw4valid) {
    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW4,
                12 * sizeof(double));
    return acsctrl::SINGLE_RW_UNAVAILABLE;
  }
  return acsctrl::MULTIPLE_RW_UNAVAILABLE;
}

void Guidance::resetValues() { std::memcpy(quatIXprev, ZERO_VEC4, sizeof(quatIXprev)); }

void Guidance::getTargetParamsSafe(double sunTargetSafe[3]) {
  std::error_code e;
  if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e) or
      not std::filesystem::exists(SD_1_SKEWED_PTG_FILE, e)) {
    std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDir,
                3 * sizeof(double));
  } else {
    std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDirLeop,
                3 * sizeof(double));
  }
}

ReturnValue_t Guidance::solarArrayDeploymentComplete() {
  std::error_code e;
  if (std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e)) {
    std::remove(SD_0_SKEWED_PTG_FILE);
    if (std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e)) {
      return returnvalue::FAILED;
    }
  }
  if (std::filesystem::exists(SD_1_SKEWED_PTG_FILE, e)) {
    std::remove(SD_1_SKEWED_PTG_FILE);
    if (std::filesystem::exists(SD_1_SKEWED_PTG_FILE, e)) {
      return returnvalue::FAILED;
    }
  }
  return returnvalue::OK;
}