#include "AcsParameters.h"

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

AcsParameters::AcsParameters(){};  //(uint8_t parameterModuleId) :
                                   // parameterModuleId(parameterModuleId) {}

AcsParameters::~AcsParameters() {}

/*ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint16_t parameterId,
                                          ParameterWrapper* parameterWrapper,
                                          const ParameterWrapper* newValues,
                                          uint16_t startAtIndex) {
  if (domainId == parameterModuleId) {
    switch (parameterId >> 8) {
      case 0x0:  // direct members
        switch (parameterId & 0xFF) {
          default:
            return INVALID_IDENTIFIER_ID;
        }
        break;
      case 0x1:  // OnBoardParams
        switch (parameterId & 0xFF) {
          case 0x0:
            parameterWrapper->set(onBoardParams.sampleTime);
            break;
          default:
            return INVALID_IDENTIFIER_ID;
        }
        break;
      case 0x2:  // InertiaEIVE
        switch (parameterId & 0xFF) {
          case 0x0:
            parameterWrapper->set(inertiaEIVE.inertiaMatrix);
            break;
          case 0x1:
            parameterWrapper->set(inertiaEIVE.inertiaMatrixInverse);
            break;
          default:
            return INVALID_IDENTIFIER_ID;
        }
        break;
      case 0x3:  // MgmHandlingParameters
        switch (parameterId & 0xFF) {
          case 0x0:
            parameterWrapper->set(mgmHandlingParameters.mgm0orientationMatrix);
            break;
          case 0x1:
            parameterWrapper->set(mgmHandlingParameters.mgm1orientationMatrix);
            break;
          case 0x2:
            parameterWrapper->set(mgmHandlingParameters.mgm2orientationMatrix);
            break;
          case 0x3:
            parameterWrapper->set(mgmHandlingParameters.mgm3orientationMatrix);
            break;
          case 0x4:
            parameterWrapper->set(mgmHandlingParameters.mgm4orientationMatrix);
            break;
          case 0x5:
            parameterWrapper->set(mgmHandlingParameters.mgm0hardIronOffset);
            break;
          case 0x6:
            parameterWrapper->set(mgmHandlingParameters.mgm1hardIronOffset);
            break;
          case 0x7:
            parameterWrapper->set(mgmHandlingParameters.mgm2hardIronOffset);
            break;
          case 0x8:
            parameterWrapper->set(mgmHandlingParameters.mgm3hardIronOffset);
            break;
          case 0x9:
            parameterWrapper->set(mgmHandlingParameters.mgm4hardIronOffset);
            break;
          case 0xA:
            parameterWrapper->set(mgmHandlingParameters.mgm0softIronInverse);
            break;
          case 0xB:
            parameterWrapper->set(mgmHandlingParameters.mgm1softIronInverse);
            break;
          case 0xC:
            parameterWrapper->set(mgmHandlingParameters.mgm2softIronInverse);
            break;
          case 0xD:
            parameterWrapper->set(mgmHandlingParameters.mgm3softIronInverse);
            break;
          case 0xE:
            parameterWrapper->set(mgmHandlingParameters.mgm4softIronInverse);
            break;
          default:
            return INVALID_IDENTIFIER_ID;
        }
        break;
      case 0x4:  // SusHandlingParameters
        switch (parameterId & 0xFF) {
          case 0x0:
            parameterWrapper->set(susHandlingParameters.sus0orientationMatrix);
            break;
          case 0x1:
            parameterWrapper->set(susHandlingParameters.sus1orientationMatrix);
            break;
          case 0x2:
            parameterWrapper->set(susHandlingParameters.sus2orientationMatrix);
            break;
          case 0x3:
            parameterWrapper->set(susHandlingParameters.sus3orientationMatrix);
            break;
          case 0x4:
            parameterWrapper->set(susHandlingParameters.sus4orientationMatrix);
            break;
          case 0x5:
            parameterWrapper->set(susHandlingParameters.sus5orientationMatrix);
            break;
          case 0x6:
            parameterWrapper->set(susHandlingParameters.sus6orientationMatrix);
            break;
          case 0x7:
            parameterWrapper->set(susHandlingParameters.sus7orientationMatrix);
            break;
          case 0x8:
            parameterWrapper->set(susHandlingParameters.sus8orientationMatrix);
            break;
          case 0x9:
            parameterWrapper->set(susHandlingParameters.sus9orientationMatrix);
            break;
          case 0xA:
            parameterWrapper->set(susHandlingParameters.sus10orientationMatrix);
            break;
          case 0xB:
            parameterWrapper->set(susHandlingParameters.sus11orientationMatrix);
            break;
          case 0xC:
            parameterWrapper->set(susHandlingParameters.sus0coeffAlpha);
            break;
          case 0xD:
            parameterWrapper->set(susHandlingParameters.sus0coeffBeta);
            break;
          case 0xE:
            parameterWrapper->set(susHandlingParameters.sus1coeffAlpha);
            break;
          case 0xF:
            parameterWrapper->set(susHandlingParameters.sus1coeffBeta);
            break;
          case 0x10:
            parameterWrapper->set(susHandlingParameters.sus2coeffAlpha);
            break;
          case 0x11:
            parameterWrapper->set(susHandlingParameters.sus2coeffBeta);
            break;
          case 0x12:
            parameterWrapper->set(susHandlingParameters.sus3coeffAlpha);
            break;
          case 0x13:
            parameterWrapper->set(susHandlingParameters.sus3coeffBeta);
            break;
          case 0x14:
            parameterWrapper->set(susHandlingParameters.sus4coeffAlpha);
            break;
          case 0x15:
            parameterWrapper->set(susHandlingParameters.sus4coeffBeta);
            break;
          case 0x16:
            parameterWrapper->set(susHandlingParameters.sus5coeffAlpha);
            break;
          case 0x17:
            parameterWrapper->set(susHandlingParameters.sus5coeffBeta);
            break;
          case 0x18:
            parameterWrapper->set(susHandlingParameters.sus6coeffAlpha);
            break;
          case 0x19:
            parameterWrapper->set(susHandlingParameters.sus6coeffBeta);
            break;
          case 0x1A:
            parameterWrapper->set(susHandlingParameters.sus7coeffAlpha);
            break;
          case 0x1B:
            parameterWrapper->set(susHandlingParameters.sus7coeffBeta);
            break;
          case 0x1C:
            parameterWrapper->set(susHandlingParameters.sus8coeffAlpha);
            break;
          case 0x1D:
            parameterWrapper->set(susHandlingParameters.sus8coeffBeta);
            break;
          case 0x1E:
            parameterWrapper->set(susHandlingParameters.sus9coeffAlpha);
            break;
          case 0x1F:
            parameterWrapper->set(susHandlingParameters.sus9coeffBeta);
            break;
          case 0x20:
            parameterWrapper->set(susHandlingParameters.sus10coeffAlpha);
            break;
          case 0x21:
            parameterWrapper->set(susHandlingParameters.sus10coeffBeta);
            break;
          case 0x22:
            parameterWrapper->set(susHandlingParameters.sus11coeffAlpha);
            break;
          case 0x23:
            parameterWrapper->set(susHandlingParameters.sus11coeffBeta);
            break;
          case 0x24:
            parameterWrapper->set(susHandlingParameters.filterAlpha);
            break;
          case 0x25:
            parameterWrapper->set(susHandlingParameters.sunThresh);
            break;
          default:
            return INVALID_IDENTIFIER_ID;
        }
        break;
      case (0x5):  // GyrHandlingParameters
        switch (parameterId & 0xFF) {
          case 0x0:
            parameterWrapper->set(gyrHandlingParameters.gyr0orientationMatrix);
            break;
          case 0x1:
            parameterWrapper->set(gyrHandlingParameters.gyr1orientationMatrix);
            break;
          case 0x2:
            parameterWrapper->set(gyrHandlingParameters.gyr2orientationMatrix);
            break;
          case 0x3:
            parameterWrapper->set(gyrHandlingParameters.gyr3orientationMatrix);
            break;
          case 0x4:
            parameterWrapper->set(gyrHandlingParameters.gyrFusionWeight);
            break;
          default:
            return INVALID_IDENTIFIER_ID;
        }
        break;
      case (0x6):  // RwHandlingParameters
        switch (parameterId & 0xFF) {
          case 0x0:
            parameterWrapper->set(rwHandlingParameters.rw0orientationMatrix);
            break;
          case 0x1:
            parameterWrapper->set(rwHandlingParameters.rw1orientationMatrix);
            break;
          case 0x2:
            parameterWrapper->set(rwHandlingParameters.rw2orientationMatrix);
            break;
          case 0x3:
            parameterWrapper->set(rwHandlingParameters.rw3orientationMatrix);
            break;
          case 0x4:
            parameterWrapper->set(rwHandlingParameters.inertiaWheel);
            break;
          case 0x5:
            parameterWrapper->set(rwHandlingParameters.maxTrq);
            break;
          default:
            return INVALID_IDENTIFIER_ID;
        }
        break;
      case (0x7):  // RwMatrices
        switch (parameterId & 0xFF) {
          case 0x0:
            parameterWrapper->set(rwMatrices.alignmentMatrix);
            break;
          case 0x1:
            parameterWrapper->set(rwMatrices.pseudoInverse);
            break;
          case 0x2:
            parameterWrapper->set(rwMatrices.without0);
            break;
          case 0x3:
            parameterWrapper->set(rwMatrices.without1);
            break;
          case 0x4:
            parameterWrapper->set(rwMatrices.without2);
            break;
          case 0x5:
            parameterWrapper->set(rwMatrices.without3);
            break;
          default:
            return INVALID_IDENTIFIER_ID;
        }
        break;
      case (0x8):  // SafeModeControllerParameters
        switch (parameterId & 0xFF) {
          case 0x0:
            parameterWrapper->set(safeModeControllerParameters.k_rate_mekf);
            break;
          case 0x1:
            parameterWrapper->set(safeModeControllerParameters.k_align_mekf);
            break;
          case 0x2:
            parameterWrapper->set(safeModeControllerParameters.k_rate_no_mekf);
            break;
          case 0x3:
            parameterWrapper->set(safeModeControllerParameters.k_align_no_mekf);
            break;
          case 0x4:
            parameterWrapper->set(safeModeControllerParameters.sunMagAngleMin);
            break;
          case 0x5:
            parameterWrapper->set(safeModeControllerParameters.sunTargetDir);
            break;
          case 0x6:
            parameterWrapper->set(safeModeControllerParameters.satRateRef);
            break;
          default:
            return INVALID_IDENTIFIER_ID;
        }
        break;
      case (0x9):  // DetumbleCtrlParameters
        switch (parameterId & 0xFF) {
          case 0x0:
            parameterWrapper->set(detumbleCtrlParameters.gainD);
            break;
          default:
            return INVALID_IDENTIFIER_ID;
        }
        break;
      case (0xA):  // PointingModeControllerParameters
        switch (parameterId & 0xFF) {
          case 0x0:
            parameterWrapper->set(targetModeControllerParameters.updtFlag);
            break;
          case 0x1:
            parameterWrapper->set(targetModeControllerParameters.A_rw);
            break;
          case 0x2:
            parameterWrapper->set(targetModeControllerParameters.refDirection);
            break;
          case 0x3:
            parameterWrapper->set(targetModeControllerParameters.refRotRate);
            break;
          case 0x4:
            parameterWrapper->set(targetModeControllerParameters.quatRef);
            break;
          case 0x5:
            parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
            break;
          case 0x6:
            parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
            break;
          case 0x7:
            parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
            break;
          case 0x8:
            parameterWrapper->set(targetModeControllerParameters.blindRotRate);
            break;
          case 0x9:
            parameterWrapper->set(targetModeControllerParameters.zeta);
            break;
          case 0xA:
            parameterWrapper->set(targetModeControllerParameters.zetaLow);
            break;
          case 0xB:
            parameterWrapper->set(targetModeControllerParameters.om);
            break;
          case 0xC:
            parameterWrapper->set(targetModeControllerParameters.omLow);
            break;
          case 0xD:
            parameterWrapper->set(targetModeControllerParameters.omMax);
            break;
          case 0xE:
            parameterWrapper->set(targetModeControllerParameters.qiMin);
            break;
          case 0xF:
            parameterWrapper->set(targetModeControllerParameters.gainNullspace);
            break;
          case 0x10:
            parameterWrapper->set(targetModeControllerParameters.desatMomentumRef);
            break;
          case 0x11:
            parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
            break;
          case 0x12:
            parameterWrapper->set(targetModeControllerParameters.desatOn);
            break;
          case 0x13:
            parameterWrapper->set(targetModeControllerParameters.omegaEarth);
            break;
          default:
            return INVALID_IDENTIFIER_ID;
        }
        break;
      case (0xB):  // StrParameters
        switch (parameterId & 0xFF) {
          case 0x0:
            parameterWrapper->set(strParameters.exclusionAngle);
            break;
          case 0x1:
            parameterWrapper->set(strParameters.boresightAxis);
            break;
          default:
            return INVALID_IDENTIFIER_ID;
        }
        break;
      case (0xC):  // GpsParameters
        switch (parameterId & 0xFF) {
          default:
            return INVALID_IDENTIFIER_ID;
        }
        break;
      case (0xD):  // GroundStationParameters
        switch (parameterId & 0xFF) {
          case 0x0:
            parameterWrapper->set(groundStationParameters.latitudeGs);
            break;
          case 0x1:
            parameterWrapper->set(groundStationParameters.longitudeGs);
            break;
          case 0x2:
            parameterWrapper->set(groundStationParameters.altitudeGs);
            break;
          case 0x3:
            parameterWrapper->set(groundStationParameters.earthRadiusEquat);
            break;
          case 0x4:
            parameterWrapper->set(groundStationParameters.earthRadiusPolar);
            break;
          default:
            return INVALID_IDENTIFIER_ID;
        }
        break;
      case (0xE):  // SunModelParameters
        switch (parameterId & 0xFF) {
          case 0x0:
            parameterWrapper->set(sunModelParameters.useSunModel);
            break;
          case 0x1:
            parameterWrapper->set(sunModelParameters.domega);
            break;
          case 0x2:
            parameterWrapper->set(sunModelParameters.omega_0);
            break;
          case 0x3:
            parameterWrapper->set(sunModelParameters.m_0);
            break;
          case 0x4:
            parameterWrapper->set(sunModelParameters.dm);
            break;
          case 0x5:
            parameterWrapper->set(sunModelParameters.e);
            break;
          case 0x6:
            parameterWrapper->set(sunModelParameters.e1);
            break;
          case 0x7:
            parameterWrapper->set(sunModelParameters.p1);
            break;
          case 0x8:
            parameterWrapper->set(sunModelParameters.p2);
            break;
          default:
            return INVALID_IDENTIFIER_ID;
        }
        break;
      case (0xF):  // KalmanFilterParameters
        switch (parameterId & 0xFF) {
          case 0x0:
            parameterWrapper->set(kalmanFilterParameters.activateKalmanFilter);
            break;
          case 0x1:
            parameterWrapper->set(kalmanFilterParameters.requestResetFlag);
            break;
          case 0x2:
            parameterWrapper->set(
                kalmanFilterParameters.maxToleratedTimeBetweenKalmanFilterExecutionSteps);
            break;
          case 0x3:
            parameterWrapper->set(kalmanFilterParameters.processNoiseOmega);
            break;
          case 0x4:
            parameterWrapper->set(kalmanFilterParameters.processNoiseQuaternion);
            break;
          case 0x5:
            parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR);
            break;
          case 0x6:
            parameterWrapper->set(kalmanFilterParameters.sensorNoiseSS);
            break;
          case 0x7:
            parameterWrapper->set(kalmanFilterParameters.sensorNoiseMAG);
            break;
          case 0x8:
            parameterWrapper->set(kalmanFilterParameters.sensorNoiseGYR);
            break;
          case 0x9:
            parameterWrapper->set(kalmanFilterParameters.sensorNoiseArwGYR);
            break;
          case 0xA:
            parameterWrapper->set(kalmanFilterParameters.sensorNoiseBsGYR);
            break;
          default:
            return INVALID_IDENTIFIER_ID;
        }
        break;
      case (0x10):  // MagnetorquesParameter
        switch (parameterId & 0xFF) {
          case 0x0:
            parameterWrapper->set(magnetorquesParameter.mtq0orientationMatrix);
            break;
          case 0x1:
            parameterWrapper->set(magnetorquesParameter.mtq1orientationMatrix);
            break;
          case 0x2:
            parameterWrapper->set(magnetorquesParameter.mtq2orientationMatrix);
            break;
          case 0x3:
            parameterWrapper->set(magnetorquesParameter.alignmentMatrixMtq);
            break;
          case 0x4:
            parameterWrapper->set(magnetorquesParameter.inverseAlignment);
            break;
          case 0x5:
            parameterWrapper->set(magnetorquesParameter.DipolMax);
            break;
          default:
            return INVALID_IDENTIFIER_ID;
        }
        break;
      case (0x11):  // DetumbleParameter
        switch (parameterId & 0xFF) {
          case 0x0:
            parameterWrapper->set(detumbleParameter.detumblecounter);
            break;
          case 0x1:
            parameterWrapper->set(detumbleParameter.omegaDetumbleStart);
            break;
          case 0x2:
            parameterWrapper->set(detumbleParameter.omegaDetumbleEnd);
            break;
          default:
            return INVALID_IDENTIFIER_ID;
        }
        break;
      default:
        return INVALID_IDENTIFIER_ID;
    }
    return returnvalue::OK;
  } else {
    return INVALID_DOMAIN_ID;
  }
}*/