#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() {
  std::vector<ReturnValue_t> results;

  {
    PoolReadGuard pgMgm(&mgm0Lis3Set);
    results.push_back(pgMgm.getReadResult());
  }
  {
    PoolReadGuard pgMgm(&mgm1Rm3100Set);
    results.push_back(pgMgm.getReadResult());
  }
  {
    PoolReadGuard pgMgm(&mgm2Lis3Set);
    results.push_back(pgMgm.getReadResult());
  }
  {
    PoolReadGuard pgMgm(&mgm3Rm3100Set);
    results.push_back(pgMgm.getReadResult());
  }
  {
    PoolReadGuard pgMgm(&imtqMgmSet);
    results.push_back(pgMgm.getReadResult());
  }
  for (const auto& result : results) {
    if (result != returnvalue::OK) {
      return result;
    }
  }
  return returnvalue::OK;
}

ReturnValue_t SensorValues::updateSus() {
  std::vector<ReturnValue_t> results;
  for (auto& susSet : susSets) {
    {
      PoolReadGuard pgSus(&susSet);
      results.push_back(pgSus.getReadResult());
    }
  }
  for (const auto& result : results) {
    if (result != returnvalue::OK) {
      return result;
    }
  }
  return returnvalue::OK;
}

ReturnValue_t SensorValues::updateGyr() {
  std::vector<ReturnValue_t> results;
  {
    PoolReadGuard pgGyr(&gyr0AdisSet);
    results.push_back(pgGyr.getReadResult());
  }
  {
    PoolReadGuard pgGyr(&gyr1L3gSet);
    results.push_back(pgGyr.getReadResult());
  }
  {
    PoolReadGuard pgGyr(&gyr2AdisSet);
    results.push_back(pgGyr.getReadResult());
  }
  {
    PoolReadGuard pgGyr(&gyr3L3gSet);
    results.push_back(pgGyr.getReadResult());
  }
  for (const auto& result : results) {
    if (result != returnvalue::OK) {
      return result;
    }
  }
  return returnvalue::OK;
}

ReturnValue_t SensorValues::updateStr() {
  PoolReadGuard pgStr(&strSet);
  return pgStr.getReadResult();
}

ReturnValue_t SensorValues::updateGps() {
  PoolReadGuard pgGps(&gpsSet);
  return pgGps.getReadResult();
}

ReturnValue_t SensorValues::updateRw() {
  std::vector<ReturnValue_t> results;
  {
    PoolReadGuard pgRw(&rw1Set);
    results.push_back(pgRw.getReadResult());
  }
  {
    PoolReadGuard pgRw(&rw2Set);
    results.push_back(pgRw.getReadResult());
  }
  {
    PoolReadGuard pgRw(&rw3Set);
    results.push_back(pgRw.getReadResult());
  }
  {
    PoolReadGuard pgRw(&rw4Set);
    results.push_back(pgRw.getReadResult());
  }
  for (const auto& result : results) {
    if (result != returnvalue::OK) {
      return result;
    }
  }
  return returnvalue::OK;
}

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