#include "UdpTmTcBridge.h"

#include "FsfwProtocolHeader.h"

#include <arpa/inet.h>
#include <errno.h>
#include <fsfw/action.h>
#include <fsfw/ipc/QueueFactory.h>
#include <fsfw/objectmanager.h>
#include <fsfw/serviceinterface/ServiceInterface.h>
#include <netinet/in.h>
#include <sys/socket.h>
#include <sys/types.h>  // POSIX.1-2001 does not require the inclusion of <sys/types.h>, and this header file is not required on Linux. However, some historical (BSD) implementations required this header file, and portable applications are probably wise to include it.

UdpTmTcBridgeNew::UdpTmTcBridgeNew(object_id_t objectId, object_id_t tmStoreId,
                                   object_id_t tcStoreId)
    : SystemObject(objectId) {
  messageQueue =
      QueueFactory::instance()->createMessageQueue(50, MessageQueueMessage::MAX_MESSAGE_SIZE);
}

UdpTmTcBridgeNew::~UdpTmTcBridgeNew() {}

ReturnValue_t UdpTmTcBridgeNew::initialize() {
  ReturnValue_t result = SystemObject::initialize();
  if (result != returnvalue::OK) {
    return result;
  }

  IPCStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
  if (IPCStore == nullptr) {
    return returnvalue::FAILED;
  }

  int retval;

  serverSocket = socket(AF_INET, SOCK_DGRAM | SOCK_NONBLOCK, 0);
  if (serverSocket == -1) {
// TODO resolve errno
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::error << "UdpTmTcBridge::initialize: Socket initialization failed!" << std::endl;
#endif
    return returnvalue::FAILED;
  }

  sockaddr_in serverAddr;
  serverAddr.sin_family = AF_INET;
  serverAddr.sin_port = htons(6667);

  retval = inet_aton("10.13.90.2", &serverAddr.sin_addr);
  if (retval == 0) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::error << "UdpTmTcBridge::initialize: Invalid IP!" << std::endl;
#endif
    return returnvalue::FAILED;
  }

  retval = bind(serverSocket, (struct sockaddr *)&serverAddr, sizeof(serverAddr));
  if (retval == -1) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::error << "UdpTmTcBridge::initialize: bind failed with " << errno << std::endl;
#endif
    return returnvalue::FAILED;
  }

  return returnvalue::OK;
}

ReturnValue_t UdpTmTcBridgeNew::performOperation(uint8_t operationCode) {
  ssize_t peekLen = recv(serverSocket, NULL, 0, MSG_PEEK | MSG_TRUNC);

  if (peekLen <= 0) {
    return returnvalue::OK;
  }

  if (peekLen < FsfwProtocolHeader::HEADER_SIZE) {
    recv(serverSocket, NULL, 0, MSG_TRUNC);
    return returnvalue::OK;
  }
  store_address_t storageId;
  uint8_t *bufferPointer;
  ReturnValue_t result = IPCStore->getFreeElement(&storageId, peekLen, &bufferPointer);

  if (result != returnvalue::OK) {
    return returnvalue::OK;
  }

  sockaddr_storage sender;
  socklen_t senderlen = sizeof(sender);

  ssize_t receivedLen =
      recvfrom(serverSocket, bufferPointer, peekLen, 0, (struct sockaddr *)&sender, &senderlen);

  if (receivedLen == -1) {
    return returnvalue::OK;
  }

  if (receivedLen != peekLen) {
    // should not happen, if it does throw away
    return returnvalue::OK;
  }



  size_t bufferLen = receivedLen;
  const uint8_t *constBufferPointer = bufferPointer;

  FsfwProtocolHeader header;

  result = header.deSerialize(&constBufferPointer, &bufferLen,
                                           SerializeIF::Endianness::NETWORK);

  if (result != returnvalue::OK) {
    return returnvalue::OK;
  }

  sif::debug << "Received msg for 0x" << std::hex << header.getObjectId() << std::dec << " interface "
             << (int) header.getInterface() << std::endl;

  CommandMessage message;

  switch (header.getInterface()) {
    case HasActionsIF::INTERFACE_ID: {
      HasActionsIF *object = ObjectManager::instance()->get<HasActionsIF>(header.getObjectId());
      if (object == nullptr) {
        return returnvalue::OK;
      }
      
      ActionMessage::setCommand(&message, header.HEADER_SIZE, storageId);
      result = messageQueue->sendMessage(object->getCommandQueue(), &message);
      // sif::debug << "UdpTmTcBridge::performOperation: sent " << (int)storageId.raw << std::endl;
    } break;
    default:
#if FSFW_CPP_OSTREAM_ENABLED == 1
      // sif::debug << "UdpTmTcBridge::performOperation: illegal interface"
      //            << (int) header.getInterface() << std::endl;
#endif
      break;
  }

  return returnvalue::OK;
}