#include "SusPolling.h"

#include <fsfw/tasks/SemaphoreFactory.h>
#include <fsfw/tasks/TaskFactory.h>
#include <fsfw/timemanager/Stopwatch.h>
#include <fsfw_hal/linux/spi/SpiCookie.h>
#include <mission/acs/susMax1227Helpers.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/tcs/max1227.h>
#include <unistd.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();
    // Give SUS handlers a chance to submit all requests.
    TaskFactory::delayTask(2);
    {
      // Takes 4-5 ms in debug mode.
      // Stopwatch watch;
      handleSusPolling();
    }
    // Protection against tardy tasks unlocking the thread again immediately.
    TaskFactory::delayTask(20);
  }
  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.cfgWasSet = false;
      susDevs[susIdx].ownReply.dataWasSet = false;
    }
    susDevs[susIdx].mode = susReq->mode;
  }
  if (state == InternalState::IDLE) {
    state = InternalState::IS_BUSY;
    semaphore->release();
  }
  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;
  }
  if(susDevs[susIdx].replyResult != returnvalue::OK) {
    return susDevs[susIdx].replyResult;
  }
  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 susDevs[susIdx].replyResult;
}

ReturnValue_t SusPolling::handleSusPolling() {
  ReturnValue_t result;
  acs::SimpleSensorMode modes[12];
  bool performStartups[12]{};
  bool cfgsWereSet[12]{};
  uint8_t idx = 0;
  {
    MutexGuard mg(ipcLock);
    for (idx = 0; idx < 12; idx++) {
      modes[idx] = susDevs[idx].mode;
      performStartups[idx] = susDevs[idx].performStartup;
    }
  }
  for (idx = 0; idx < 12; idx++) {
    if (modes[idx] == acs::SimpleSensorMode::NORMAL) {
      if (performStartups[idx]) {
        // Startup handling.
        cmdBuf[0] = susMax1227::SETUP_INT_CLOKED;
        result = spiComIF.sendMessage(susDevs[idx].cookie, cmdBuf.data(), 1);
        if (result != OK) {
          susDevs[idx].replyResult = result;
          continue;
        }
        MutexGuard mg(ipcLock);
        susDevs[idx].ownReply.cfgWasSet = true;
        cfgsWereSet[idx] = true;
        susDevs[idx].performStartup = true;
      }
    }
  }
  for (idx = 0; idx < 12; idx++) {
    if (modes[idx] == acs::SimpleSensorMode::NORMAL and cfgsWereSet[idx]) {
      // Regular sensor polling.
      cmdBuf[0] = max1227::buildResetByte(true);
      cmdBuf[1] = susMax1227::CONVERSION;
      result = spiComIF.sendMessage(susDevs[idx].cookie, cmdBuf.data(), 2);
      if (result != OK) {
        susDevs[idx].replyResult = result;
        continue;
      }
    }
  }

  // Internal conversion time is 3.5 us
  usleep(4);

  for (idx = 0; idx < 12; idx++) {
    if (modes[idx] == acs::SimpleSensorMode::NORMAL and cfgsWereSet[idx]) {
      std::memset(cmdBuf.data(), 0, susMax1227::SIZE_READ_INT_CONVERSIONS);
      result = spiComIF.sendMessage(susDevs[idx].cookie, cmdBuf.data(),
                                    susMax1227::SIZE_READ_INT_CONVERSIONS);
      if (result != OK) {
        susDevs[idx].replyResult = result;
        continue;
      }
      result = spiComIF.readReceivedMessage(susDevs[idx].cookie, &rawReply, &dummy);
      if (result != OK) {
        susDevs[idx].replyResult = result;
        continue;
      }
      MutexGuard mg(ipcLock);
      susDevs[idx].ownReply.tempRaw = ((rawReply[0] & 0x0f) << 8) | rawReply[1];
      // Reply is all ones. Sensor is probably off or faulty when
      // it should not be.
      if(susDevs[idx].ownReply.tempRaw == 0x0fff) {
        susDevs[idx].replyResult = returnvalue::FAILED;
      } else {
        susDevs[idx].replyResult = returnvalue::OK;
        for (unsigned chIdx = 0; chIdx < 6; chIdx++) {
          susDevs[idx].ownReply.channelsRaw[chIdx] =
              (rawReply[chIdx * 2 + 2] << 8) | rawReply[chIdx * 2 + 3];
        }
        susDevs[idx].ownReply.dataWasSet = true;
      }
    }
  }
  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;
    }
  }
}