#include <fcntl.h>
#include <linux/obc/Ptme.h>
#include <sys/mman.h>

CCSDSIPCoreBridge::CCSDSIPCoreBridge(object_id_t objectId, object_id_t tcDestination,
                                     object_id_t tmStoreId, object_id_t tcStoreId,
                                     LinuxLibgpioIF* gpioComIF, std::string uioPtme,
                                     gpioId_t papbBusyId, gpioId_t papbEmptyId)
    : TmTcBridge(objectId, tcDestination, tmStoreId, tcStoreId),
      gpioComIF(gpioComIF),
      uioPtme(uioPtme),
      papbBusyId(papbBusyId),
      papbEmptyId(papbEmptyId) {}

CCSDSIPCoreBridge::~CCSDSIPCoreBridge() {}

ReturnValue_t CCSDSIPCoreBridge::initialize() {
  ReturnValue_t result = TmTcBridge::initialize();

  fd = open("/dev/uio0", O_RDWR);
  if (fd < 1) {
    sif::debug << "CCSDSIPCoreBridge::initialize: Invalid UIO device file" << std::endl;
    return RETURN_FAILED;
  }

  /**
   * Map uio device in virtual address space
   * PROT_WRITE: Map uio device in writable only mode
   */
  ptmeBaseAddress =
      static_cast<uint32_t*>(mmap(NULL, MAP_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0));

  if (ptmeBaseAddress == MAP_FAILED) {
    sif::error << "CCSDSIPCoreBridge::initialize: Failed to map uio address" << std::endl;
    return RETURN_FAILED;
  }

  return result;
}

ReturnValue_t CCSDSIPCoreBridge::handleTm() {
#if OBSW_TEST_CCSDS_PTME == 1
  return sendTestFrame();
#else
  return TmTcBridge::handleTm();
#endif
}

ReturnValue_t CCSDSIPCoreBridge::sendTm(const uint8_t* data, size_t dataLen) {
  if (pollPapbBusySignal() == RETURN_OK) {
    startPacketTransfer();
  }

  for (size_t idx = 0; idx < dataLen; idx++) {
    if (pollPapbBusySignal() == RETURN_OK) {
      *(ptmeBaseAddress + PTME_DATA_REG_OFFSET) = static_cast<uint32_t>(*(data + idx));
    } else {
      sif::debug << "CCSDSIPCoreBridge::sendTm: Only written " << idx - 1 << " of " << dataLen
                 << " data" << std::endl;
      return RETURN_FAILED;
    }
  }

  if (pollPapbBusySignal() == RETURN_OK) {
    endPacketTransfer();
  }
  return RETURN_OK;
}

void CCSDSIPCoreBridge::startPacketTransfer() { *ptmeBaseAddress = PTME_CONFIG_START; }

void CCSDSIPCoreBridge::endPacketTransfer() { *ptmeBaseAddress = PTME_CONFIG_END; }

ReturnValue_t CCSDSIPCoreBridge::pollPapbBusySignal() {
  int papbBusyState = 0;
  ReturnValue_t result = RETURN_OK;

  /** Check if PAPB interface is ready to receive data */
  result = gpioComIF->readGpio(papbBusyId, &papbBusyState);
  if (result != RETURN_OK) {
    sif::debug << "CCSDSIPCoreBridge::pollPapbBusySignal: Failed to read papb busy signal"
               << std::endl;
    return RETURN_FAILED;
  }
  if (!papbBusyState) {
    sif::debug << "CCSDSIPCoreBridge::pollPapbBusySignal: PAPB busy" << std::endl;
    return PAPB_BUSY;
  }

  return RETURN_OK;
}

void CCSDSIPCoreBridge::isPtmeBufferEmpty() {
  ReturnValue_t result = RETURN_OK;
  int papbEmptyState = 1;

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

  if (result != RETURN_OK) {
    sif::debug << "CCSDSIPCoreBridge::isPtmeBufferEmpty: Failed to read papb empty signal"
               << std::endl;
    return;
  }

  if (papbEmptyState == 1) {
    sif::debug << "CCSDSIPCoreBridge::isPtmeBufferEmpty: Buffer is empty" << std::endl;
  } else {
    sif::debug << "CCSDSIPCoreBridge::isPtmeBufferEmpty: Buffer is not empty" << std::endl;
  }
  return;
}

ReturnValue_t CCSDSIPCoreBridge::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 = sendTm(testPacket, 1105);
  if (result != RETURN_OK) {
    return result;
  }

  return RETURN_OK;
}