/*
 * SensorValues.cpp
 *
 *  Created on: 30 Mar 2022
 *      Author: rooob
 */
#include "SensorValues.h"

#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/datapoollocal/LocalPoolVariable.h>
#include <fsfw/datapoollocal/LocalPoolVector.h>
#include <stddef.h>

#include <cmath>

namespace ACS {

SensorValues::SensorValues() {}

SensorValues::~SensorValues() {}

ReturnValue_t SensorValues::updateMgm() {
  ReturnValue_t result;
  PoolReadGuard pgMgm0(&mgm0Lis3Set), pgMgm1(&mgm1Rm3100Set), pgMgm2(&mgm2Lis3Set),
      pgMgm3(&mgm3Rm3100Set), pgImtq(&imtqMgmSet);

  result = (pgMgm0.getReadResult() || pgMgm1.getReadResult() || pgMgm2.getReadResult() ||
            pgMgm3.getReadResult() || pgImtq.getReadResult());
  return result;
}

ReturnValue_t SensorValues::updateSus() {
  ReturnValue_t result;
  PoolReadGuard pgSus0(&susSets[0]), pgSus1(&susSets[1]), pgSus2(&susSets[2]), pgSus3(&susSets[3]),
      pgSus4(&susSets[4]), pgSus5(&susSets[5]), pgSus6(&susSets[6]), pgSus7(&susSets[7]),
      pgSus8(&susSets[8]), pgSus9(&susSets[9]), pgSus10(&susSets[10]), pgSus11(&susSets[11]);

  result = (pgSus0.getReadResult() || pgSus1.getReadResult() || pgSus2.getReadResult() ||
            pgSus3.getReadResult() || pgSus4.getReadResult() || pgSus5.getReadResult() ||
            pgSus6.getReadResult() || pgSus7.getReadResult() || pgSus8.getReadResult() ||
            pgSus9.getReadResult() || pgSus10.getReadResult() || pgSus11.getReadResult());
  return result;
}

ReturnValue_t SensorValues::update() {
  updateSus();
  updateMgm();

  //  lp_var_t<float> quaternion(objects::STAR_TRACKER, PoolIds::CALI_QW, nullptr,
  //                             pool_rwm_t::VAR_READ);
  //  ReturnValue_t result = quaternion.read();
  //
  //  if (result != RETURN_OK) {
  //    return RETURN_FAILED;
  //  }
  //	quatJB[3] = static_cast<double>(quaternion.value);
  //	quatJBValid = quaternion.isValid();

  return returnvalue::OK;
}
}  // namespace ACS