#include "LocalPoolOwnerBase.h"

#include <fsfw/datapool/PoolReadGuard.h>

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

LocalPoolOwnerBase::~LocalPoolOwnerBase() = default;

ReturnValue_t LocalPoolOwnerBase::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();
}

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

ReturnValue_t LocalPoolOwnerBase::serializeHkDataset(sid_t structureId, uint8_t *buf,
                                                     size_t maxSize) {
  return returnvalue::OK;
}

ReturnValue_t LocalPoolOwnerBase::specifyHkDatasets(
    std::vector<periodicHk::SetSpecification> &setList) {
  return returnvalue::OK;
}

/*
ReturnValue_t LocalPoolOwnerBase::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                          PeriodicHkGenerationHelper &poolManager) {
  // Default initialization empty for now.
  localDataPoolMap.emplace(lpool::uint8VarId, &u8PoolEntry);
  localDataPoolMap.emplace(lpool::floatVarId, &floatPoolEntry);
  localDataPoolMap.emplace(lpool::uint32VarId, &u32PoolEntry);

  localDataPoolMap.emplace(lpool::uint16Vec3Id, &u16VecPoolEntry);
  localDataPoolMap.emplace(lpool::int64Vec2Id, &i64VecPoolEntry);
  return returnvalue::OK;
}
 */

LocalPoolObjectBase *LocalPoolOwnerBase::getPoolObjectHandle(lp_id_t localPoolId) {
  if (localPoolId == lpool::uint8VarId) {
    return &testUint8;
  } else if (localPoolId == lpool::uint16Vec3Id) {
    return &testUint16Vec;
  } else if (localPoolId == lpool::floatVarId) {
    return &testFloat;
  } else if (localPoolId == lpool::int64Vec2Id) {
    return &testInt64Vec;
  } else if (localPoolId == lpool::uint32VarId) {
    return &testUint32;
  } else {
    return &testUint8;
  }
}

ReturnValue_t LocalPoolOwnerBase::reset() {
  // resetSubscriptionList();
  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;
    // testUint32.setValid(false);
  }

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

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