/*
 * 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::updateGyr() {
  ReturnValue_t result;
  PoolReadGuard pgGyr0(&gyr0AdisSet), pgGyr1(&gyr1L3gSet), pgGyr2(&gyr2AdisSet),
      pgGyr3(&gyr3L3gSet);

  result = (pgGyr0.getReadResult() || pgGyr1.getReadResult() || pgGyr2.getReadResult() ||
            pgGyr3.getReadResult());
  return result;
}

ReturnValue_t SensorValues::updateStr() {
  ReturnValue_t result;
  PoolReadGuard pgStr(&strSet);

  result = pgStr.getReadResult();
  return result;
}

ReturnValue_t SensorValues::updateGps() {
  ReturnValue_t result;
  PoolReadGuard pgGps(&gpsSet);

  result = pgGps.getReadResult();
  return result;
}

ReturnValue_t SensorValues::updateRw() {
  ReturnValue_t result;
  PoolReadGuard pgRw1(&rw1Set), pgRw2(&rw2Set), pgRw3(&rw3Set), pgRw4(&rw4Set);

  result = (pgRw1.getReadResult() || pgRw2.getReadResult() || pgRw3.getReadResult() ||
            pgRw4.getReadResult());
  return result;
}

ReturnValue_t SensorValues::update() {
  ReturnValue_t mgmUpdate = updateMgm();
  ReturnValue_t susUpdate = updateSus();
  ReturnValue_t gyrUpdate = updateGyr();
  ReturnValue_t strUpdate = updateStr();
  ReturnValue_t gpsUpdate = updateGps();
  ReturnValue_t rwUpdate = updateRw();

  if ((mgmUpdate && susUpdate && gyrUpdate && strUpdate && gpsUpdate && rwUpdate) ==
      returnvalue::FAILED) {
    return returnvalue::FAILED;
  };
  return returnvalue::OK;
}

}  // namespace ACS