#include "FsfwTestController.h"

#include <fsfw/datapool/PoolReadGuard.h>

FsfwTestController::FsfwTestController(object_id_t objectId,
                                       object_id_t device0, object_id_t device1,
                                       uint8_t verboseLevel)
    : TestController(objectId, objects::NO_OBJECT, 5), device0Id(device0),
      device1Id(device1), deviceDataset0(device0), deviceDataset1(device1) {}

FsfwTestController::~FsfwTestController() {}

ReturnValue_t
FsfwTestController::handleCommandMessage(CommandMessage *message) {
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t FsfwTestController::initializeLocalDataPool(
    localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) {
  return HasReturnvaluesIF::RETURN_OK;
}

void FsfwTestController::performControlOperation() {
  // We will trace variables if we received an update notification or snapshots
  if (verboseLevel >= 1) {
    if (not traceVariable) {
      return;
    }
    switch (currentTraceType) {
      case (NONE): {
        break;
      }
      case (TRACE_DEV_0_UINT8): {
        if (traceCounter == 0) {
          FSFW_LOGI("Tracing finished\n");
          traceVariable = false;
          traceCounter = traceCycles;
          currentTraceType = TraceTypes::NONE;
          break;
        }
        PoolReadGuard readHelper(&deviceDataset0.testUint8Var);
        FSFW_LOGI("Tracing device 0 variable 0 (UINT8), current value: {}",
                  static_cast<int>(deviceDataset0.testUint8Var.value));
        traceCounter--;
        break;
      }
      case (TRACE_DEV_0_VECTOR): {
        break;
      }
    }
  }
}

ReturnValue_t FsfwTestController::initializeAfterTaskCreation() {
  namespace td = testdevice;
  ReturnValue_t result = TestController::initializeAfterTaskCreation();
  if (result != HasReturnvaluesIF::RETURN_OK) {
    return result;
  }
  auto* device0 =
      ObjectManager::instance()->get<HasLocalDataPoolIF>(deviceDataset0.getCreatorObjectId());
  if (device0 == nullptr) {
    FSFW_LOGW("initializeAfterTaskCreation: Test device handler 0 handle invalid\n");
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }
  ProvidesDataPoolSubscriptionIF *subscriptionIF =
      device0->getSubscriptionInterface();
  if (subscriptionIF != nullptr) {
    /* For DEVICE_0, we only subscribe for notifications */
    subscriptionIF->subscribeForSetUpdateMessage(td::TEST_SET_ID, getObjectId(),
                                                 getCommandQueue(), false);
    subscriptionIF->subscribeForVariableUpdateMessage(
        td::PoolIds::TEST_UINT8_ID, getObjectId(), getCommandQueue(), false);
  }

  auto* device1 =
      ObjectManager::instance()->get<HasLocalDataPoolIF>(deviceDataset0.getCreatorObjectId());
  if (device1 == nullptr) {
    FSFW_LOGW("initializeAfterTaskCreation: Test device handler 1 handle invalid\n");
  }

  subscriptionIF = device1->getSubscriptionInterface();
  if (subscriptionIF != nullptr) {
    /* For DEVICE_1, we will subscribe for snapshots */
    subscriptionIF->subscribeForSetUpdateMessage(td::TEST_SET_ID, getObjectId(),
                                                 getCommandQueue(), true);
    subscriptionIF->subscribeForVariableUpdateMessage(
        td::PoolIds::TEST_UINT8_ID, getObjectId(), getCommandQueue(), true);
  }
  return result;
}

LocalPoolDataSetBase *FsfwTestController::getDataSetHandle(sid_t sid) {
  return nullptr;
}

ReturnValue_t FsfwTestController::checkModeCommand(Mode_t mode,
                                                   Submode_t submode,
                                                   uint32_t *msToReachTheMode) {
  return HasReturnvaluesIF::RETURN_OK;
}

void FsfwTestController::handleChangedDataset(sid_t sid,
                                              store_address_t storeId,
                                              bool *clearMessage) {
  using namespace std;

  if (verboseLevel >= 1) {
    char const *printout = nullptr;
    if (storeId == storeId::INVALID_STORE_ADDRESS) {
      printout = "Notification";
    } else {
      printout = "Snapshot";
    }
    FSFW_LOGI("handleChangedDataset: {} update from object ID {:#010x} and set ID {}\n", printout,
              sid.objectId, sid.ownerSetId);

    if (storeId == storeId::INVALID_STORE_ADDRESS) {
      if (sid.objectId == device0Id) {
        PoolReadGuard readHelper(&deviceDataset0.testFloat3Vec);
        float floatVec[3];
        floatVec[0] = deviceDataset0.testFloat3Vec.value[0];
        floatVec[1] = deviceDataset0.testFloat3Vec.value[1];
        floatVec[2] = deviceDataset0.testFloat3Vec.value[2];
        FSFW_LOGI("Current float vector (3) values: [{},{},{}]\n", floatVec[0], floatVec[1],
                  floatVec[2]);
      }
    }
  }

  /* We will trace the variables for snapshots and update notifications */
  if (not traceVariable) {
    traceVariable = true;
    traceCounter = traceCycles;
    currentTraceType = TraceTypes::TRACE_DEV_0_VECTOR;
  }
}

void FsfwTestController::handleChangedPoolVariable(gp_id_t globPoolId,
                                                   store_address_t storeId,
                                                   bool *clearMessage) {
  using namespace std;

  if (verboseLevel >= 1) {
    char const *printout = nullptr;
    if (storeId == storeId::INVALID_STORE_ADDRESS) {
      printout = "Notification";
    } else {
      printout = "Snapshot";
    }

    FSFW_LOGI(
        "TestController::handleChangedPoolVariable: {} update from object "
        "ID {:#010x} and LPID {}\n",
        printout, globPoolId.objectId, globPoolId.localPoolId);

    if (storeId == storeId::INVALID_STORE_ADDRESS) {
      if (globPoolId.objectId == device0Id) {
        PoolReadGuard readHelper(&deviceDataset0.testUint8Var);
        FSFW_LOGI("Current test variable 0 (UINT8) value: {}",
                  static_cast<int>(deviceDataset0.testUint8Var.value));
      }
    }
  }

  /* We will trace the variables for snapshots and update notifications */
  if (not traceVariable) {
    traceVariable = true;
    traceCounter = traceCycles;
    currentTraceType = TraceTypes::TRACE_DEV_0_UINT8;
  }
}