#include "Detumble.h"

#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <math.h>

Detumble::Detumble() {}

Detumble::~Detumble() {}

acs::SafeModeStrategy Detumble::detumbleStrategy(const bool magFieldValid,
                                                 const bool satRotRateValid,
                                                 const bool magFieldRateValid,
                                                 const bool useFullDetumbleLaw) {
  if (not magFieldValid) {
    return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
  } else if (satRotRateValid and useFullDetumbleLaw) {
    return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL;
  } else if (magFieldRateValid) {
    return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED;
  } else {
    return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
  }
}

void Detumble::bDotLawFull(const double *satRotRateB, const double *magFieldB, double *magMomB,
                           double gain) {
  // convert uT to T
  double magFieldBT[3];
  VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
  // control law
  double factor = gain / pow(VectorOperations<double>::norm(magFieldBT, 3), 2);
  double magFieldNormed[3] = {0, 0, 0}, crossProduct[3] = {0, 0, 0};
  VectorOperations<double>::normalize(magFieldBT, magFieldNormed, 3);
  VectorOperations<double>::cross(satRotRateB, magFieldNormed, crossProduct);
  VectorOperations<double>::mulScalar(crossProduct, factor, magMomB, 3);
}

void Detumble::bDotLaw(const double *magRateB, const double *magFieldB, double *magMomB,
                       double gain) {
  // convert uT to T
  double magFieldBT[3], magRateBT[3];
  VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
  VectorOperations<double>::mulScalar(magRateB, 1e-6, magRateBT, 3);
  // control law
  double factor = -gain / pow(VectorOperations<double>::norm(magFieldBT, 3), 2);
  VectorOperations<double>::mulScalar(magRateBT, factor, magMomB, 3);
}