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

#include <cstring>
#include <ctime>

#include "fsfw/serviceinterface/ServiceInterface.h"

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

PapbVcInterface::~PapbVcInterface() {}

ReturnValue_t PapbVcInterface::initialize() {
  UioMapper uioMapper(uioFile, mapNum);
  ReturnValue_t result = uioMapper.getMappedAdress(const_cast<uint32_t**>(&vcBaseReg),
                                                   UioMapper::Permissions::READ_WRITE);
  if (result != returnvalue::OK) {
    return result;
  }
  return returnvalue::OK;
}

ReturnValue_t PapbVcInterface::write(const uint8_t* data, size_t size, size_t& writtenSize) {
  // There are no packets smaller than 4, this is considered a configuration error.
  if (size < 4) {
    sif::warning << "PapbVcInterface::write: Passed packet smaller than 4 bytes" << std::endl;
    return returnvalue::FAILED;
  }
  // The user must call advance until completion before starting a new packet transfer.
  if (writeActiveStatus) {
    return IS_BUSY;
  }
  if (size > packetBuf.capacity()) {
    sif::error << "PapbVcInterface: Packet with size " << size << " larger than maximum configured"
               << " byte size " << packetBuf.capacity() << std::endl;
    return returnvalue::FAILED;
  }
  std::memcpy(packetBuf.data(), data, size);
  currentPacketSize = size;
  currentPacketIndex = 0;
  if (pollReadyForPacket()) {
    startPacketTransfer(ByteWidthCfg::ONE);
  } else {
    return DirectTmSinkIF::IS_BUSY;
  }
  return advanceWrite(writtenSize);
}

void PapbVcInterface::startPacketTransfer(ByteWidthCfg initWidth) {
  *vcBaseReg = CONFIG_DATA_INPUT | initWidth;
  writeActiveStatus = true;
}

bool PapbVcInterface::pollReadyForPacket() const {
  //  Check if PAPB interface is ready to receive data. Use the configuration register for this.
  //  Bit 5, see PTME ptme_001_01-0-7-r2 Table 31.
  uint32_t reg = *vcBaseReg;
  return (reg >> 6) & 0b1;
}

ReturnValue_t PapbVcInterface::advanceWrite(size_t& writtenSize) {
  if (!writeActiveStatus) {
    return NO_WRITE_ACTIVE;
  }
  if (not pollReadyForPacket()) {
    return IS_BUSY;
  }
  while (currentPacketIndex < currentPacketSize) {
    if (not pollReadyForOctet(MAX_BUSY_POLLS)) {
      if (not pollReadyForPacket()) {
        return PARTIALLY_WRITTEN;
      }
      abortPacketTransfer();
      return returnvalue::FAILED;
    }
    *(vcBaseReg + DATA_REG_OFFSET) = static_cast<uint32_t>(packetBuf[currentPacketIndex++]);
    writtenSize++;
  }
  if (not pollReadyForOctet(MAX_BUSY_POLLS)) {
    if (not pollReadyForPacket()) {
      return PARTIALLY_WRITTEN;
    }
    abortPacketTransfer();
    return returnvalue::FAILED;
  }
  completePacketTransfer();
  return returnvalue::OK;
}

bool PapbVcInterface::writeActive() const { return writeActiveStatus; }

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

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

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

  if (papbEmptyState == gpio::Levels::HIGH) {
    return true;
  }
  return false;
}

bool PapbVcInterface::isBusy() const { return not pollReadyForPacket(); }

void PapbVcInterface::cancelTransfer() { abortPacketTransfer(); }

inline bool PapbVcInterface::pollReadyForOctet(uint32_t maxCycles) const {
  uint32_t reg;
  uint32_t idx = 0;
  while (idx < maxCycles) {
    reg = *vcBaseReg;
    // Busy bit.
    if (not((reg >> 5) & 0b1)) {
      return true;
    }
    idx++;
  }
  return false;
}

void PapbVcInterface::abortPacketTransfer() {
  *vcBaseReg = CONFIG_ABORT;
  writeActiveStatus = false;
  currentPacketIndex = 0;
  currentPacketSize = 0;
}

void PapbVcInterface::completePacketTransfer() {
  *vcBaseReg = CONFIG_END;
  writeActiveStatus = false;
  currentPacketIndex = 0;
  currentPacketSize = 0;
}