#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 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);
  ReturnValue_t result = uioMapper.getMappedAdress(const_cast<uint32_t**>(&vcBaseReg),
                                                   UioMapper::Permissions::WRITE_ONLY);
  if (result != returnvalue::OK) {
    return result;
  }
  return returnvalue::OK;
}

ReturnValue_t PapbVcInterface::write(const uint8_t* data, size_t size) {
  // There are no packets smaller than 4, this is considered a configuration error.
  if (size < 4) {
    return returnvalue::FAILED;
  }
  if (pollInterfaceReadiness(0, true) == returnvalue::OK) {
    startPacketTransfer(ByteWidthCfg::ONE);
  } else {
    return DirectTmSinkIF::IS_BUSY;
  }
  // TODO: This should work but does not.. :(
  //  size_t idx = 0;
  //  while (idx < size) {
  //
  //    nanosleep(&BETWEEN_POLL_DELAY, &remDelay);
  //    if ((size - idx) < 4) {
  //      *vcBaseReg = CONFIG_DATA_INPUT | (size - idx - 1);
  //      usleep(1);
  //    }
  //    if (pollPapbBusySignal(2) == returnvalue::OK) {
  //      //      vcBaseReg + DATA_REG_OFFSET + 3 = static_cast<uint8_t>(data + idx);
  //      //      vcBaseReg + DATA_REG_OFFSET + 2 = static_cast<uint8_t>(data + idx + 1);
  //      //      vcBaseReg + DATA_REG_OFFSET + 1 = static_cast<uint8_t>(data + idx + 2);
  //      //      vcBaseReg + DATA_REG_OFFSET = static_cast<uint8_t>(data + idx + 3);
  //
  //      // std::memcpy((vcBaseReg + DATA_REG_OFFSET), data + idx , nextWriteSize);
  //      *(vcBaseReg + DATA_REG_OFFSET) = *reinterpret_cast<const uint32_t*>(data + idx);
  //      //uint8_t* byteReg = reinterpret_cast<uint8_t*>(vcBaseReg + DATA_REG_OFFSET);
  //
  //      //byteReg[0] = data[idx];
  //      //byteReg[1] = data[idx];
  //    } else {
  //      abortPacketTransfer();
  //      return returnvalue::FAILED;
  //    }
  //    // TODO: Change this after the bugfix. Right now, the PAPB ignores the content of the byte
  //    // width configuration.5
  //    // It's okay to increment by a larger amount for the last segment here, loop will be over
  //    // in any case.
  //    idx += 4;
  //  }
  for (size_t idx = 0; idx < size; idx++) {
    // This delay is super-important, DO NOT REMOVE!
    // Polling the GPIO or the config register too often messes up the scheduler.
    // TODO: Maybe this should not be done like this. It would be better if there was a custom
    // FPGA module which can accept packets and then takes care of dumping that packet into
    // the PTME. DMA would be an ideal solution for this.
    nanosleep(&BETWEEN_POLL_DELAY, &remDelay);
    if (pollInterfaceReadiness(2, false) == returnvalue::OK) {
      *(vcBaseReg + DATA_REG_OFFSET) = static_cast<uint32_t>(data[idx]);
    } else {
      abortPacketTransfer();
      return returnvalue::FAILED;
    }
  }
  nanosleep(&BETWEEN_POLL_DELAY, &remDelay);
  if (pollInterfaceReadiness(2, false) == returnvalue::OK) {
    completePacketTransfer();
  } else {
    abortPacketTransfer();
    return returnvalue::FAILED;
  }
  return returnvalue::OK;
}

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

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

ReturnValue_t PapbVcInterface::pollInterfaceReadiness(uint32_t maxPollRetries,
                                                      bool checkReadyState) const {
  uint32_t busyIdx = 0;
  nextDelay.tv_nsec = FIRST_DELAY_PAPB_POLLING_NS;

  while (true) {
    // 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;
    bool busy = (reg >> 5) & 0b1;
    bool ready = (reg >> 6) & 0b1;
    if (not busy) {
      return returnvalue::OK;
    }
    if (checkReadyState and not ready) {
      return PAPB_BUSY;
    }

    busyIdx++;
    if (busyIdx >= maxPollRetries) {
      return PAPB_BUSY;
    }

    // Ignore signal handling here for now.
    nanosleep(&nextDelay, &remDelay);
    // Adaptive delay.
    if (nextDelay.tv_nsec * 2 <= MAX_DELAY_PAPB_POLLING_NS) {
      nextDelay.tv_nsec *= 2;
    }
  }
  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 pollInterfaceReadiness(0, true) == PAPB_BUSY; }

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

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;
}

void PapbVcInterface::abortPacketTransfer() { *vcBaseReg = CONFIG_ABORT; }