From 83d71548ec52e5fdbeda55660de066a40c8bcb43 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Fri, 29 Jul 2022 16:32:50 +0200
Subject: [PATCH] CCSDS unpacker implementation

---
 src/fsfw/tcdistribution/CcsdsUnpacker.cpp | 59 +++++++++++++++++++++--
 src/fsfw/tcdistribution/CcsdsUnpacker.h   |  9 +++-
 2 files changed, 64 insertions(+), 4 deletions(-)

diff --git a/src/fsfw/tcdistribution/CcsdsUnpacker.cpp b/src/fsfw/tcdistribution/CcsdsUnpacker.cpp
index 2f8e9f27..226b857b 100644
--- a/src/fsfw/tcdistribution/CcsdsUnpacker.cpp
+++ b/src/fsfw/tcdistribution/CcsdsUnpacker.cpp
@@ -1,7 +1,60 @@
 #include "CcsdsUnpacker.h"
 
-CcsdsUnpacker::CcsdsUnpacker() {}
+#include "fsfw/tmtcservices/TmTcMessage.h"
+
+CcsdsUnpacker::CcsdsUnpacker(MessageQueueIF& msgQueue, AcceptsTelecommandsIF& receiver,
+                             StorageManagerIF& sourceStore)
+  : sourceStore(sourceStore), msgQueue(msgQueue), receiver(receiver) {
+  msgQueue.setDefaultDestination(receiver.getRequestQueue());
+}
+
+ReturnValue_t CcsdsUnpacker::performOperation(uint8_t operationCode) {
+  TmTcMessage msg;
+  ReturnValue_t result;
+  for (result =  msgQueue.receiveMessage(&msg); result == HasReturnvaluesIF::RETURN_OK;
+       result = msgQueue.receiveMessage(&msg)) {
+    auto resultPair = sourceStore.getData(msg.getStorageId());
+    if(resultPair.first != HasReturnvaluesIF::RETURN_OK) {
+      continue;
+    }
+    if(resultPair.second.size() < 6) {
+      // TODO: This is a config error. Does it make sense to forward the message?
+      result = msgQueue.sendToDefault(&msg);
+      if(result != HasReturnvaluesIF::RETURN_OK) {
+
+      }
+      continue;
+    }
+    StorageManagerIF* tgtStore;
+    if(targetStore != nullptr) {
+      tgtStore = targetStore;
+    } else {
+      tgtStore = &sourceStore;
+    }
+    store_address_t newId;
+    uint8_t* ptr;
+    result = tgtStore->getFreeElement(&newId, resultPair.second.size(), &ptr);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+      // TODO: Implement error handling
+    }
+    std::memcpy(ptr, resultPair.second.data() + 6, resultPair.second.size() - 6);
+    result = sourceStore.deleteData(msg.getStorageId());
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+      // TODO: Implement error handling (though this really should not happen)
+    }
+    TmTcMessage newMsg(newId);
+    result = msgQueue.sendToDefault(&newMsg);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+
+    }
+  }
+  return result;
+}
 
-ReturnValue_t CcsdsUnpacker::performOperation(uint8_t operationCode) { return 0; }
 uint32_t CcsdsUnpacker::getIdentifier() { return 0; }
-MessageQueueId_t CcsdsUnpacker::getRequestQueue() { return 0; }
+
+MessageQueueId_t CcsdsUnpacker::getRequestQueue() { return msgQueue.getId(); }
+
+void CcsdsUnpacker::setDifferentTargetStore(StorageManagerIF& otherTargetStore) {
+  targetStore = &otherTargetStore;
+}
diff --git a/src/fsfw/tcdistribution/CcsdsUnpacker.h b/src/fsfw/tcdistribution/CcsdsUnpacker.h
index a134de33..c4eb7da4 100644
--- a/src/fsfw/tcdistribution/CcsdsUnpacker.h
+++ b/src/fsfw/tcdistribution/CcsdsUnpacker.h
@@ -1,16 +1,23 @@
 #ifndef FSFW_TCDISTRIBUTION_CCSDSUNPACKER_H
 #define FSFW_TCDISTRIBUTION_CCSDSUNPACKER_H
 
+#include "fsfw/storagemanager/StorageManagerIF.h"
 #include "fsfw/tasks/ExecutableObjectIF.h"
 #include "fsfw/tmtcservices/AcceptsTelecommandsIF.h"
 
 class CcsdsUnpacker : public ExecutableObjectIF, public AcceptsTelecommandsIF {
  public:
-  CcsdsUnpacker();
+  CcsdsUnpacker(MessageQueueIF& msgQueue, AcceptsTelecommandsIF& receiver, StorageManagerIF& sourceStore);
+
+  void setDifferentTargetStore(StorageManagerIF& otherTargetStore);
   ReturnValue_t performOperation(uint8_t operationCode) override;
   uint32_t getIdentifier() override;
   MessageQueueId_t getRequestQueue() override;
 
  private:
+  StorageManagerIF& sourceStore;
+  StorageManagerIF* targetStore = nullptr;
+  MessageQueueIF& msgQueue;
+  AcceptsTelecommandsIF& receiver;
 };
 #endif  // FSFW_TCDISTRIBUTION_CCSDSUNPACKER_H