#include "SusPolling.h"

#include <fsfw/tasks/SemaphoreFactory.h>
#include <fsfw_hal/linux/spi/SpiCookie.h>

using namespace returnvalue;

SusPolling::SusPolling(object_id_t objectId, SpiComIF& spiComIF, GpioIF& gpioIF)
    : SystemObject(objectId), spiComIF(spiComIF), gpioIF(gpioIF) {
  semaphore = SemaphoreFactory::instance()->createBinarySemaphore();
  semaphore->acquire();
  ipcLock = MutexFactory::instance()->createMutex();
}

ReturnValue_t SusPolling::performOperation(uint8_t operationCode) {
  while (true) {
    ipcLock->lockMutex();
    state = InternalState::IDLE;
    ipcLock->unlockMutex();
    semaphore->acquire();
    for(auto& susDev: susDevs) {
      acs::SimpleSensorMode mode;
      bool doPerformStartup = false;
      {
        MutexGuard mg(ipcLock);
        mode = susDev.mode;
        doPerformStartup = susDev.performStartup;
      }
      if (mode == acs::SimpleSensorMode::NORMAL) {
        if(doPerformStartup) {
          // Startup handling.
        }
        // Regular sensor polling.
      }
    }
  }
  return OK;
}

ReturnValue_t SusPolling::initialize() { return OK; }

ReturnValue_t SusPolling::initializeInterface(CookieIF* cookie) {
  auto* spiCookie = dynamic_cast<SpiCookie*>(cookie);
  if (spiCookie == nullptr) {
    return FAILED;
  }
  int susIdx = addressToIndex(spiCookie->getSpiAddress());
  if (susIdx < 0) {
    return FAILED;
  }
  susDevs[susIdx].cookie = spiCookie;
  return spiComIF.initializeInterface(cookie);
}

ReturnValue_t SusPolling::sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) {
  auto* spiCookie = dynamic_cast<SpiCookie*>(cookie);
  if (spiCookie == nullptr) {
    return FAILED;
  }
  int susIdx = addressToIndex(spiCookie->getSpiAddress());
  if (susIdx < 0) {
    return FAILED;
  }
  if (sendLen != sizeof(acs::SusRequest)) {
    return FAILED;
  }
  const auto* susReq = reinterpret_cast<const acs::SusRequest*>(sendData);
  MutexGuard mg(ipcLock);
  if (susDevs[susIdx].mode != susReq->mode) {
    if(susReq->mode == acs::SimpleSensorMode::NORMAL) {
     susDevs[susIdx].performStartup = true;
    } else {
      susDevs[susIdx].ownReply.dataWasSet = false;
    }
    susDevs[susIdx].mode = susReq->mode;
  }
  return OK;
}

ReturnValue_t SusPolling::getSendSuccess(CookieIF* cookie) { return OK; }

ReturnValue_t SusPolling::requestReceiveMessage(CookieIF* cookie, size_t requestLen) { return OK; }

ReturnValue_t SusPolling::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) {
  auto* spiCookie = dynamic_cast<SpiCookie*>(cookie);
  if (spiCookie == nullptr) {
    return FAILED;
  }
  int susIdx = addressToIndex(spiCookie->getSpiAddress());
  if (susIdx < 0) {
    return FAILED;
  }
  MutexGuard mg(ipcLock);
  std::memcpy(&susDevs[susIdx].readerReply, &susDevs[susIdx].ownReply, sizeof(acs::SusReply));
  *buffer = reinterpret_cast<uint8_t*>(&susDevs[susIdx].readerReply);
  *size = sizeof(acs::SusReply);
  return OK;
}

int SusPolling::addressToIndex(address_t addr) {
  switch (addr) {
    case (addresses::SUS_0):
      return 0;
      break;
    case (addresses::SUS_1):
      return 1;
      break;
    case (addresses::SUS_2):
      return 2;
      break;
    case (addresses::SUS_3):
      return 3;
      break;
    case (addresses::SUS_4):
      return 4;
      break;
    case (addresses::SUS_5):
      return 5;
      break;
    case (addresses::SUS_6):
      return 6;
      break;
    case (addresses::SUS_7):
      return 7;
      break;
    case (addresses::SUS_8):
      return 8;
      break;
    case (addresses::SUS_9):
      return 9;
      break;
    case (addresses::SUS_10):
      return 10;
      break;
    case (addresses::SUS_11):
      return 11;
      break;
    default: {
      return -1;
    }
  }
}