/*
 * Detumble.cpp
 *
 *  Created on: 17 Aug 2022
 *      Author: Robin Marquardt
 */

#include "Detumble.h"

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

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

Detumble::Detumble(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); }

Detumble::~Detumble() {}

void Detumble::loadAcsParameters(AcsParameters *acsParameters_) {
  detumbleParameter = &(acsParameters_->detumbleParameter);
  magnetorquesParameter = &(acsParameters_->magnetorquerParameter);
}

ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid,
                                const double *magField, const bool magFieldValid, double *magMom) {
  if (!magRateValid || !magFieldValid) {
    return DETUMBLE_NO_SENSORDATA;
  }
  double gain = detumbleParameter->gainD;
  double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2);
  VectorOperations<double>::mulScalar(magRate, factor, magMom, 3);
  return returnvalue::OK;
}

ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateValid,
                                    double *magMom) {
  if (!magRateValid) {
    return DETUMBLE_NO_SENSORDATA;
  }

  double dipolMax = magnetorquesParameter->dipolMax;
  for (int i = 0; i < 3; i++) {
    magMom[i] = -dipolMax * sign(magRate[i]);
  }

  return returnvalue::OK;
}

ReturnValue_t Detumble::bDotLawGyro(const double *satRate, const bool *satRateValid,
                                    const double *magField, const bool *magFieldValid,
                                    double *magMom) {
  if (!satRateValid || !magFieldValid) {
    return DETUMBLE_NO_SENSORDATA;
  }
  double gain = detumbleParameter->gainD;
  double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2);
  VectorOperations<double>::mulScalar(satRate, factor, magMom, 3);
  return returnvalue::OK;
}