#include "TestPoolOwner.h"

#include <fsfw/datapool/PoolReadGuard.h>

using namespace lpool;

TestPoolOwner::TestPoolOwner(MessageQueueIF &queue, MessageQueueId_t hkDestId, object_id_t objectId)
    : SystemObject(objectId),
      hkHelper(this, &queue, hkDestId),
      sharedPool(TestPoolOwner::getObjectId()),
      set1(sharedPool, lpool::testSetId1),
      set2(sharedPool, lpool::testSetId2),
      queue(queue) {}

TestPoolOwner::~TestPoolOwner() = default;

ReturnValue_t TestPoolOwner::initialize() {
  sharedPool.addPoolEntry(lpool::uint8VarId, &u8PoolEntry);
  sharedPool.addPoolEntry(lpool::floatVarId, &floatPoolEntry);
  sharedPool.addPoolEntry(lpool::uint32VarId, &u32PoolEntry);
  sharedPool.addPoolEntry(lpool::uint16Vec3Id, &u16VecPoolEntry);
  sharedPool.addPoolEntry(lpool::int64Vec2Id, &i64VecPoolEntry);
  ReturnValue_t result = hkHelper.initialize(&queue);
  if (result != returnvalue::OK) {
    return result;
  }
  return SystemObject::initialize();
}

SharedPool *TestPoolOwner::getOptionalSharedPool() { return &sharedPool; }

ReturnValue_t TestPoolOwner::serializeHkDataset(structure_id_t structureId, uint8_t *buf,
                                                size_t maxSize) {
  size_t dummy = 0;
  if (structureId == testSid0) {
    return set0.serialize(buf, dummy, maxSize, SerializeIF::Endianness::NETWORK);
  }
  if (structureId == testSid1) {
    return set1.serialize(buf, dummy, maxSize, SerializeIF::Endianness::NETWORK);
  }
  if (structureId == testSid2) {
    return set2.serialize(buf, dummy, maxSize, SerializeIF::Endianness::NETWORK);
  }
  return returnvalue::FAILED;
}

ReturnValue_t TestPoolOwner::specifyHkDatasets(std::vector<hk::SetSpecification> &setList) {
  // For the first set, we explicitely associate a set with an ID ourselves.
  setList.emplace_back(testSid0, set0.getSerializedSize(), 50);
  // For the other sets, we can use getter functions of the same structure.
  setList.emplace_back(set1.getStructureId(), set1.getSerializedSize(), 50);
  setList.emplace_back(set2.getStructureId(), set2.getSerializedSize(), 50);
  return returnvalue::OK;
}

ReturnValue_t TestPoolOwner::reset() {
  ReturnValue_t status = returnvalue::OK;
  {
    PoolReadGuard readHelper(&set1);
    if (readHelper.getReadResult() != returnvalue::OK) {
      status = readHelper.getReadResult();
    }
    set1.localPoolVarUint8.value = 0;
    set1.localPoolVarFloat.value = 0.0;
    set1.localPoolUint16Vec.value[0] = 0;
    set1.localPoolUint16Vec.value[1] = 0;
    set1.localPoolUint16Vec.value[2] = 0;
    // dataset.setValidity(false, true);
  }

  {
    PoolReadGuard readHelper(&testUint32);
    if (readHelper.getReadResult() != returnvalue::OK) {
      status = readHelper.getReadResult();
    }
    testUint32.value = 0;
  }

  {
    PoolReadGuard readHelper(&testInt64Vec);
    if (readHelper.getReadResult() != returnvalue::OK) {
      status = readHelper.getReadResult();
    }
    testInt64Vec.value[0] = 0;
    testInt64Vec.value[1] = 0;
  }
  return status;
}

void TestPoolOwner::setHkDestId(MessageQueueId_t id) { hkHelper.setHkDestinationId(id); }