#include "PusLiveDemux.h"

#include <fsfw/storagemanager/storeAddress.h>
#include <fsfw/tmtcservices/TmTcMessage.h>

PusLiveDemux::PusLiveDemux(MessageQueueIF& ownerQueue) : ownerQueue(ownerQueue) {}

ReturnValue_t PusLiveDemux::demultiplexPackets(StorageManagerIF& tmStore,
                                               store_address_t origStoreId, const uint8_t* tmData,
                                               size_t tmSize) {
  ReturnValue_t result = returnvalue::OK;
  // sif::debug << "tm size: " << tmSize  << " for " << destinations.size() << " destinations"  <<
  // std::endl;
  for (unsigned int idx = 0; idx < destinations.size(); idx++) {
    const auto& dest = destinations[idx];
    if ((destinations.size() > 1) and (idx < (destinations.size() - 1))) {
      // Create copy of data to ensure each TM recipient has its own copy. That way, we don't need
      // to bother with send order and where the data is deleted.
      store_address_t storeId;
      result = tmStore.addData(&storeId, tmData, tmSize);
      if (result == returnvalue::OK) {
        message.setStorageId(storeId);
      } else if (result == StorageManagerIF::DATA_STORAGE_FULL) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
        sif::error << "PusLiveDemux::handlePacket: Store too full to create data copy" << std::endl;
        uint8_t fillCounts[16];
        uint8_t written = 0;
        tmStore.getFillCount(fillCounts, &written);
        sif::error << "Fill counts: [";
        for (uint8_t fillIdx = 0; fillIdx < written; fillIdx++) {
          sif::error << fillCounts[fillIdx];
          if (fillIdx < written - 1) {
            sif::error << ", ";
          }
        }
        sif::error << "]" << std::endl;
#endif
      }
    } else {
      message.setStorageId(origStoreId);
    }
    result = ownerQueue.sendMessage(dest.queueId, &message);
    if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
      sif::error << "PusLiveDemux::handlePacket: Error sending TM to downlink handler " << dest.name
                 << ", failed with code 0x" << std::hex << std::setw(4) << result << std::dec
                 << std::endl;
#endif
      tmStore.deleteData(message.getStorageId());
    }
  }
  return result;
}

void PusLiveDemux::addDestination(const char* name, const AcceptsTelemetryIF& downlinkDestination,
                                  uint8_t vcid) {
  auto queueId = downlinkDestination.getReportReceptionQueue(vcid);
  destinations.emplace_back(name, queueId, vcid);
}