#include "AcsParameters.h"

#include <fsfw/src/fsfw/globalfunctions/constants.h>
#include <stddef.h>

AcsParameters::AcsParameters() {}

AcsParameters::~AcsParameters() {}

ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
                                          ParameterWrapper* parameterWrapper,
                                          const ParameterWrapper* newValues,
                                          uint16_t startAtIndex) {
  switch (domainId) {
    case 0x0:  // direct members
      switch (parameterId) {
        default:
          return INVALID_IDENTIFIER_ID;
      }
      break;
    case 0x1:  // OnBoardParams
      switch (parameterId) {
        case 0x0:
          parameterWrapper->set(onBoardParams.sampleTime);
          break;
        case 0x1:
          parameterWrapper->set(onBoardParams.mekfViolationTimer);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
      }
      break;
    case 0x2:  // InertiaEIVE
      switch (parameterId) {
        case 0x0:
          parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixDeployed);
          break;
        case 0x1:
          parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixUndeployed);
          break;
        case 0x2:
          parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixPanel1);
          break;
        case 0x3:
          parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixPanel3);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
      }
      break;
    case 0x3:  // MgmHandlingParameters
      switch (parameterId) {
        case 0x0:
          parameterWrapper->setMatrix(mgmHandlingParameters.mgm0orientationMatrix);
          break;
        case 0x1:
          parameterWrapper->setMatrix(mgmHandlingParameters.mgm1orientationMatrix);
          break;
        case 0x2:
          parameterWrapper->setMatrix(mgmHandlingParameters.mgm2orientationMatrix);
          break;
        case 0x3:
          parameterWrapper->setMatrix(mgmHandlingParameters.mgm3orientationMatrix);
          break;
        case 0x4:
          parameterWrapper->setMatrix(mgmHandlingParameters.mgm4orientationMatrix);
          break;
        case 0x5:
          parameterWrapper->setVector(mgmHandlingParameters.mgm0hardIronOffset);
          break;
        case 0x6:
          parameterWrapper->setVector(mgmHandlingParameters.mgm1hardIronOffset);
          break;
        case 0x7:
          parameterWrapper->setVector(mgmHandlingParameters.mgm2hardIronOffset);
          break;
        case 0x8:
          parameterWrapper->setVector(mgmHandlingParameters.mgm3hardIronOffset);
          break;
        case 0x9:
          parameterWrapper->setVector(mgmHandlingParameters.mgm4hardIronOffset);
          break;
        case 0xA:
          parameterWrapper->setMatrix(mgmHandlingParameters.mgm0softIronInverse);
          break;
        case 0xB:
          parameterWrapper->setMatrix(mgmHandlingParameters.mgm1softIronInverse);
          break;
        case 0xC:
          parameterWrapper->setMatrix(mgmHandlingParameters.mgm2softIronInverse);
          break;
        case 0xD:
          parameterWrapper->setMatrix(mgmHandlingParameters.mgm3softIronInverse);
          break;
        case 0xE:
          parameterWrapper->setMatrix(mgmHandlingParameters.mgm4softIronInverse);
          break;
        case 0xF:
          parameterWrapper->setVector(mgmHandlingParameters.mgm02variance);
          break;
        case 0x10:
          parameterWrapper->setVector(mgmHandlingParameters.mgm13variance);
          break;
        case 0x11:
          parameterWrapper->setVector(mgmHandlingParameters.mgm4variance);
          break;
        case 0x12:
          parameterWrapper->set(mgmHandlingParameters.mgmDerivativeFilterWeight);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
      }
      break;
    case 0x4:  // SusHandlingParameters
      switch (parameterId) {
        case 0x0:
          parameterWrapper->setMatrix(susHandlingParameters.sus0orientationMatrix);
          break;
        case 0x1:
          parameterWrapper->setMatrix(susHandlingParameters.sus1orientationMatrix);
          break;
        case 0x2:
          parameterWrapper->setMatrix(susHandlingParameters.sus2orientationMatrix);
          break;
        case 0x3:
          parameterWrapper->setMatrix(susHandlingParameters.sus3orientationMatrix);
          break;
        case 0x4:
          parameterWrapper->setMatrix(susHandlingParameters.sus4orientationMatrix);
          break;
        case 0x5:
          parameterWrapper->setMatrix(susHandlingParameters.sus5orientationMatrix);
          break;
        case 0x6:
          parameterWrapper->setMatrix(susHandlingParameters.sus6orientationMatrix);
          break;
        case 0x7:
          parameterWrapper->setMatrix(susHandlingParameters.sus7orientationMatrix);
          break;
        case 0x8:
          parameterWrapper->setMatrix(susHandlingParameters.sus8orientationMatrix);
          break;
        case 0x9:
          parameterWrapper->setMatrix(susHandlingParameters.sus9orientationMatrix);
          break;
        case 0xA:
          parameterWrapper->setMatrix(susHandlingParameters.sus10orientationMatrix);
          break;
        case 0xB:
          parameterWrapper->setMatrix(susHandlingParameters.sus11orientationMatrix);
          break;
        case 0xC:
          parameterWrapper->setMatrix(susHandlingParameters.sus0coeffAlpha);
          break;
        case 0xD:
          parameterWrapper->setMatrix(susHandlingParameters.sus0coeffBeta);
          break;
        case 0xE:
          parameterWrapper->setMatrix(susHandlingParameters.sus1coeffAlpha);
          break;
        case 0xF:
          parameterWrapper->setMatrix(susHandlingParameters.sus1coeffBeta);
          break;
        case 0x10:
          parameterWrapper->setMatrix(susHandlingParameters.sus2coeffAlpha);
          break;
        case 0x11:
          parameterWrapper->setMatrix(susHandlingParameters.sus2coeffBeta);
          break;
        case 0x12:
          parameterWrapper->setMatrix(susHandlingParameters.sus3coeffAlpha);
          break;
        case 0x13:
          parameterWrapper->setMatrix(susHandlingParameters.sus3coeffBeta);
          break;
        case 0x14:
          parameterWrapper->setMatrix(susHandlingParameters.sus4coeffAlpha);
          break;
        case 0x15:
          parameterWrapper->setMatrix(susHandlingParameters.sus4coeffBeta);
          break;
        case 0x16:
          parameterWrapper->setMatrix(susHandlingParameters.sus5coeffAlpha);
          break;
        case 0x17:
          parameterWrapper->setMatrix(susHandlingParameters.sus5coeffBeta);
          break;
        case 0x18:
          parameterWrapper->setMatrix(susHandlingParameters.sus6coeffAlpha);
          break;
        case 0x19:
          parameterWrapper->setMatrix(susHandlingParameters.sus6coeffBeta);
          break;
        case 0x1A:
          parameterWrapper->setMatrix(susHandlingParameters.sus7coeffAlpha);
          break;
        case 0x1B:
          parameterWrapper->setMatrix(susHandlingParameters.sus7coeffBeta);
          break;
        case 0x1C:
          parameterWrapper->setMatrix(susHandlingParameters.sus8coeffAlpha);
          break;
        case 0x1D:
          parameterWrapper->setMatrix(susHandlingParameters.sus8coeffBeta);
          break;
        case 0x1E:
          parameterWrapper->setMatrix(susHandlingParameters.sus9coeffAlpha);
          break;
        case 0x1F:
          parameterWrapper->setMatrix(susHandlingParameters.sus9coeffBeta);
          break;
        case 0x20:
          parameterWrapper->setMatrix(susHandlingParameters.sus10coeffAlpha);
          break;
        case 0x21:
          parameterWrapper->setMatrix(susHandlingParameters.sus10coeffBeta);
          break;
        case 0x22:
          parameterWrapper->setMatrix(susHandlingParameters.sus11coeffAlpha);
          break;
        case 0x23:
          parameterWrapper->setMatrix(susHandlingParameters.sus11coeffBeta);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
      }
      break;
    case (0x5):  // GyrHandlingParameters
      switch (parameterId) {
        case 0x0:
          parameterWrapper->setMatrix(gyrHandlingParameters.gyr0orientationMatrix);
          break;
        case 0x1:
          parameterWrapper->setMatrix(gyrHandlingParameters.gyr1orientationMatrix);
          break;
        case 0x2:
          parameterWrapper->setMatrix(gyrHandlingParameters.gyr2orientationMatrix);
          break;
        case 0x3:
          parameterWrapper->setMatrix(gyrHandlingParameters.gyr3orientationMatrix);
          break;
        case 0x4:
          parameterWrapper->setVector(gyrHandlingParameters.gyr0bias);
          break;
        case 0x5:
          parameterWrapper->setVector(gyrHandlingParameters.gyr1bias);
          break;
        case 0x6:
          parameterWrapper->setVector(gyrHandlingParameters.gyr2bias);
          break;
        case 0x7:
          parameterWrapper->setVector(gyrHandlingParameters.gyr3bias);
          break;
        case 0x8:
          parameterWrapper->setVector(gyrHandlingParameters.gyr02variance);
          break;
        case 0x9:
          parameterWrapper->setVector(gyrHandlingParameters.gyr13variance);
          break;
        case 0xA:
          parameterWrapper->set(gyrHandlingParameters.preferAdis);
          break;
        case 0xB:
          parameterWrapper->set(gyrHandlingParameters.gyrFilterWeight);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
      }
      break;
    case (0x6):  // RwHandlingParameters
      switch (parameterId) {
        case 0x0:
          parameterWrapper->set(rwHandlingParameters.inertiaWheel);
          break;
        case 0x1:
          parameterWrapper->set(rwHandlingParameters.maxTrq);
          break;
        case 0x2:
          parameterWrapper->set(rwHandlingParameters.maxRwSpeed);
          break;
        case 0x3:
          parameterWrapper->set(rwHandlingParameters.stictionSpeed);
          break;
        case 0x4:
          parameterWrapper->set(rwHandlingParameters.stictionReleaseSpeed);
          break;
        case 0x5:
          parameterWrapper->set(rwHandlingParameters.stictionTorque);
          break;
        case 0x6:
          parameterWrapper->set(rwHandlingParameters.rampTime);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
      }
      break;
    case (0x7):  // RwMatrices
      switch (parameterId) {
        case 0x0:
          parameterWrapper->setMatrix(rwMatrices.alignmentMatrix);
          break;
        case 0x1:
          parameterWrapper->setMatrix(rwMatrices.pseudoInverse);
          break;
        case 0x2:
          parameterWrapper->setMatrix(rwMatrices.without1);
          break;
        case 0x3:
          parameterWrapper->setMatrix(rwMatrices.without2);
          break;
        case 0x4:
          parameterWrapper->setMatrix(rwMatrices.without3);
          break;
        case 0x5:
          parameterWrapper->setMatrix(rwMatrices.without4);
          break;
        case 0x6:
          parameterWrapper->setVector(rwMatrices.nullspace);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
      }
      break;
    case (0x8):  // SafeModeControllerParameters
      switch (parameterId) {
        case 0x0:
          parameterWrapper->set(safeModeControllerParameters.k_orthoMekf);
          break;
        case 0x1:
          parameterWrapper->set(safeModeControllerParameters.k_alignMekf);
          break;
        case 0x2:
          parameterWrapper->set(safeModeControllerParameters.k_parallelMekf);
          break;
        case 0x3:
          parameterWrapper->set(safeModeControllerParameters.k_orthoNonMekf);
          break;
        case 0x4:
          parameterWrapper->set(safeModeControllerParameters.k_alignNonMekf);
          break;
        case 0x5:
          parameterWrapper->set(safeModeControllerParameters.k_parallelNonMekf);
          break;
        case 0x6:
          parameterWrapper->setVector(safeModeControllerParameters.sunTargetDirLeop);
          break;
        case 0x7:
          parameterWrapper->setVector(safeModeControllerParameters.sunTargetDir);
          break;
        case 0x8:
          parameterWrapper->set(safeModeControllerParameters.useMekf);
          break;
        case 0x9:
          parameterWrapper->set(safeModeControllerParameters.dampingDuringEclipse);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
      }
      break;
    case (0x9):  // IdleModeControllerParameters
      switch (parameterId) {
        case 0x0:
          parameterWrapper->set(idleModeControllerParameters.zeta);
          break;
        case 0x1:
          parameterWrapper->set(idleModeControllerParameters.om);
          break;
        case 0x2:
          parameterWrapper->set(idleModeControllerParameters.omMax);
          break;
        case 0x3:
          parameterWrapper->set(idleModeControllerParameters.qiMin);
          break;
        case 0x4:
          parameterWrapper->set(idleModeControllerParameters.gainNullspace);
          break;
        case 0x5:
          parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef);
          break;
        case 0x6:
          parameterWrapper->set(idleModeControllerParameters.deSatGainFactor);
          break;
        case 0x7:
          parameterWrapper->set(idleModeControllerParameters.desatOn);
          break;
        case 0x8:
          parameterWrapper->set(idleModeControllerParameters.enableAntiStiction);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
      }
      break;
    case (0xA):  // TargetModeControllerParameters
      switch (parameterId) {
        case 0x0:
          parameterWrapper->set(targetModeControllerParameters.zeta);
          break;
        case 0x1:
          parameterWrapper->set(targetModeControllerParameters.om);
          break;
        case 0x2:
          parameterWrapper->set(targetModeControllerParameters.omMax);
          break;
        case 0x3:
          parameterWrapper->set(targetModeControllerParameters.qiMin);
          break;
        case 0x4:
          parameterWrapper->set(targetModeControllerParameters.gainNullspace);
          break;
        case 0x5:
          parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef);
          break;
        case 0x6:
          parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
          break;
        case 0x7:
          parameterWrapper->set(targetModeControllerParameters.desatOn);
          break;
        case 0x8:
          parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
          break;
        case 0x9:
          parameterWrapper->setVector(targetModeControllerParameters.refDirection);
          break;
        case 0xA:
          parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
          break;
        case 0xB:
          parameterWrapper->setVector(targetModeControllerParameters.quatRef);
          break;
        case 0xC:
          parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
          break;
        case 0xD:
          parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
          break;
        case 0xE:
          parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
          break;
        case 0xF:
          parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
          break;
        case 0x10:
          parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
          break;
        case 0x11:
          parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
          break;
        case 0x12:
          parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
          break;
        case 0x13:
          parameterWrapper->set(targetModeControllerParameters.blindRotRate);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
      }
      break;
    case (0xB):  // GsTargetModeControllerParameters
      switch (parameterId) {
        case 0x0:
          parameterWrapper->set(gsTargetModeControllerParameters.zeta);
          break;
        case 0x1:
          parameterWrapper->set(gsTargetModeControllerParameters.om);
          break;
        case 0x2:
          parameterWrapper->set(gsTargetModeControllerParameters.omMax);
          break;
        case 0x3:
          parameterWrapper->set(gsTargetModeControllerParameters.qiMin);
          break;
        case 0x4:
          parameterWrapper->set(gsTargetModeControllerParameters.gainNullspace);
          break;
        case 0x5:
          parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef);
          break;
        case 0x6:
          parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor);
          break;
        case 0x7:
          parameterWrapper->set(gsTargetModeControllerParameters.desatOn);
          break;
        case 0x8:
          parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction);
          break;
        case 0x9:
          parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
          break;
        case 0xA:
          parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
          break;
        case 0xB:
          parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
          break;
        case 0xC:
          parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
          break;
        case 0xD:
          parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
      }
      break;
    case (0xC):  // NadirModeControllerParameters
      switch (parameterId) {
        case 0x0:
          parameterWrapper->set(nadirModeControllerParameters.zeta);
          break;
        case 0x1:
          parameterWrapper->set(nadirModeControllerParameters.om);
          break;
        case 0x2:
          parameterWrapper->set(nadirModeControllerParameters.omMax);
          break;
        case 0x3:
          parameterWrapper->set(nadirModeControllerParameters.qiMin);
          break;
        case 0x4:
          parameterWrapper->set(nadirModeControllerParameters.gainNullspace);
          break;
        case 0x5:
          parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef);
          break;
        case 0x6:
          parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor);
          break;
        case 0x7:
          parameterWrapper->set(nadirModeControllerParameters.desatOn);
          break;
        case 0x8:
          parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
          break;
        case 0x9:
          parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
          break;
        case 0xA:
          parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
          break;
        case 0xC:
          parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
      }
      break;
    case (0xD):  // InertialModeControllerParameters
      switch (parameterId) {
        case 0x0:
          parameterWrapper->set(inertialModeControllerParameters.zeta);
          break;
        case 0x1:
          parameterWrapper->set(inertialModeControllerParameters.om);
          break;
        case 0x2:
          parameterWrapper->set(inertialModeControllerParameters.omMax);
          break;
        case 0x3:
          parameterWrapper->set(inertialModeControllerParameters.qiMin);
          break;
        case 0x4:
          parameterWrapper->set(inertialModeControllerParameters.gainNullspace);
          break;
        case 0x5:
          parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef);
          break;
        case 0x6:
          parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor);
          break;
        case 0x7:
          parameterWrapper->set(inertialModeControllerParameters.desatOn);
          break;
        case 0x8:
          parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
          break;
        case 0x9:
          parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
          break;
        case 0xA:
          parameterWrapper->setVector(inertialModeControllerParameters.refRotRate);
          break;
        case 0xC:
          parameterWrapper->setVector(inertialModeControllerParameters.quatRef);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
      }
      break;
    case (0xE):  // StrParameters
      switch (parameterId) {
        case 0x0:
          parameterWrapper->set(strParameters.exclusionAngle);
          break;
        case 0x1:
          parameterWrapper->setVector(strParameters.boresightAxis);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
      }
      break;
    case (0xF):  // GpsParameters
      switch (parameterId) {
        case 0x0:
          parameterWrapper->set(gpsParameters.timeDiffVelocityMax);
          break;
        case 0x1:
          parameterWrapper->set(gpsParameters.minimumFdirAltitude);
          break;
        case 0x2:
          parameterWrapper->set(gpsParameters.maximumFdirAltitude);
          break;
        case 0x3:
          parameterWrapper->set(gpsParameters.fdirAltitude);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
      }
      break;
    case (0x10):  // SunModelParameters
      switch (parameterId) {
        case 0x0:
          parameterWrapper->set(sunModelParameters.domega);
          break;
        case 0x1:
          parameterWrapper->set(sunModelParameters.omega_0);
          break;
        case 0x2:
          parameterWrapper->set(sunModelParameters.m_0);
          break;
        case 0x3:
          parameterWrapper->set(sunModelParameters.dm);
          break;
        case 0x4:
          parameterWrapper->set(sunModelParameters.e);
          break;
        case 0x5:
          parameterWrapper->set(sunModelParameters.e1);
          break;
        case 0x6:
          parameterWrapper->set(sunModelParameters.p1);
          break;
        case 0x7:
          parameterWrapper->set(sunModelParameters.p2);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
      }
      break;
    case (0x11):  // KalmanFilterParameters
      switch (parameterId) {
        case 0x0:
          parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR);
          break;
        case 0x1:
          parameterWrapper->set(kalmanFilterParameters.sensorNoiseSS);
          break;
        case 0x2:
          parameterWrapper->set(kalmanFilterParameters.sensorNoiseMAG);
          break;
        case 0x3:
          parameterWrapper->set(kalmanFilterParameters.sensorNoiseGYR);
          break;
        case 0x4:
          parameterWrapper->set(kalmanFilterParameters.sensorNoiseArwGYR);
          break;
        case 0x5:
          parameterWrapper->set(kalmanFilterParameters.sensorNoiseBsGYR);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
      }
      break;
    case (0x12):  // MagnetorquesParameter
      switch (parameterId) {
        case 0x0:
          parameterWrapper->setMatrix(magnetorquerParameter.mtq0orientationMatrix);
          break;
        case 0x1:
          parameterWrapper->setMatrix(magnetorquerParameter.mtq1orientationMatrix);
          break;
        case 0x2:
          parameterWrapper->setMatrix(magnetorquerParameter.mtq2orientationMatrix);
          break;
        case 0x3:
          parameterWrapper->setMatrix(magnetorquerParameter.alignmentMatrixMtq);
          break;
        case 0x4:
          parameterWrapper->setMatrix(magnetorquerParameter.inverseAlignment);
          break;
        case 0x5:
          parameterWrapper->set(magnetorquerParameter.dipolMax);
          break;
        case 0x6:
          parameterWrapper->set(magnetorquerParameter.torqueDuration);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
      }
      break;
    case (0x13):  // DetumbleParameter
      switch (parameterId) {
        case 0x0:
          parameterWrapper->set(detumbleParameter.detumblecounter);
          break;
        case 0x1:
          parameterWrapper->set(detumbleParameter.omegaDetumbleStart);
          break;
        case 0x2:
          parameterWrapper->set(detumbleParameter.omegaDetumbleEnd);
          break;
        case 0x3:
          parameterWrapper->set(detumbleParameter.gainBdot);
          break;
        case 0x4:
          parameterWrapper->set(detumbleParameter.gainFull);
          break;
        case 0x5:
          parameterWrapper->set(detumbleParameter.useFullDetumbleLaw);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
      }
      break;
    default:
      return INVALID_DOMAIN_ID;
  }
  return returnvalue::OK;
}