#include <fsfw_hal/linux/uio/UioMapper.h>
#include <linux/ipcore/PapbVcInterface.h>

#include "fsfw/serviceinterface/ServiceInterface.h"

PapbVcInterface::PapbVcInterface(LinuxLibgpioIF* gpioComIF, gpioId_t papbBusyId,
                                 gpioId_t papbEmptyId, std::string uioFile, int mapNum)
    : gpioComIF(gpioComIF),
      papbBusyId(papbBusyId),
      papbEmptyId(papbEmptyId),
      uioFile(std::move(uioFile)),
      mapNum(mapNum) {}

PapbVcInterface::~PapbVcInterface() {}

ReturnValue_t PapbVcInterface::initialize() {
  UioMapper uioMapper(uioFile, mapNum);
  return uioMapper.getMappedAdress(&vcBaseReg, UioMapper::Permissions::WRITE_ONLY);
}

ReturnValue_t PapbVcInterface::write(const uint8_t* data, size_t size) {
  if (pollPapbBusySignal() == returnvalue::OK) {
    startPacketTransfer();
  }
  for (size_t idx = 0; idx < size; idx++) {
    if (pollPapbBusySignal() == returnvalue::OK) {
      *(vcBaseReg + DATA_REG_OFFSET) = static_cast<uint32_t>(*(data + idx));
    } else {
      sif::warning << "PapbVcInterface::write: Only written " << idx << " of " << size << " data"
                   << std::endl;
      return returnvalue::FAILED;
    }
  }
  if (pollPapbBusySignal() == returnvalue::OK) {
    endPacketTransfer();
  }
  return returnvalue::OK;
}

void PapbVcInterface::startPacketTransfer() { *vcBaseReg = CONFIG_START; }

void PapbVcInterface::endPacketTransfer() { *vcBaseReg = CONFIG_END; }

ReturnValue_t PapbVcInterface::pollPapbBusySignal() const {
  gpio::Levels papbBusyState = gpio::Levels::LOW;
  ReturnValue_t result = returnvalue::OK;
  uint32_t busyIdx = 0;

  while (true) {
    /** Check if PAPB interface is ready to receive data */
    result = gpioComIF->readGpio(papbBusyId, papbBusyState);
    if (result != returnvalue::OK) {
      sif::warning << "PapbVcInterface::pollPapbBusySignal: Failed to read papb busy signal"
                   << std::endl;
      return returnvalue::FAILED;
    }
    if (papbBusyState == gpio::Levels::HIGH) {
      return returnvalue::OK;
    }
    if (busyIdx == 100) {
      return PAPB_BUSY;
    }
    busyIdx++;
  }

  return returnvalue::OK;
}

void PapbVcInterface::isVcInterfaceBufferEmpty() {
  ReturnValue_t result = returnvalue::OK;
  gpio::Levels papbEmptyState = gpio::Levels::HIGH;

  result = gpioComIF->readGpio(papbEmptyId, papbEmptyState);

  if (result != returnvalue::OK) {
    sif::warning << "PapbVcInterface::isVcInterfaceBufferEmpty: Failed to read papb empty signal"
                 << std::endl;
    return;
  }

  if (papbEmptyState == gpio::Levels::HIGH) {
    sif::debug << "PapbVcInterface::isVcInterfaceBufferEmpty: Buffer is empty" << std::endl;
  } else {
    sif::debug << "PapbVcInterface::isVcInterfaceBufferEmpty: Buffer is not empty" << std::endl;
  }
  return;
}

bool PapbVcInterface::isBusy() const { return pollPapbBusySignal() == PAPB_BUSY; }

ReturnValue_t PapbVcInterface::sendTestFrame() {
  /** Size of one complete transfer frame data field amounts to 1105 bytes */
  uint8_t testPacket[1105];

  /** Fill one test packet */
  for (int idx = 0; idx < 1105; idx++) {
    testPacket[idx] = static_cast<uint8_t>(idx & 0xFF);
  }

  ReturnValue_t result = write(testPacket, 1105);
  if (result != returnvalue::OK) {
    return result;
  }

  return returnvalue::OK;
}