#include "UdpTmTcBridge.h"

extern "C" {
#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 <fsfw/tmtc/TmManager.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.

#include "FsfwProtocolHeader.h"
#include "TmMessage.h"

UdpTmTcBridgeNew::UdpTmTcBridgeNew(object_id_t objectId, uint16_t listeningPort,
                                   const sockaddr *defaultDestination_in,
                                   socklen_t defaultDestinationLen)
    : SystemObject(objectId),
      listeningPort(listeningPort),
      defaultDestinationLen(defaultDestinationLen) {
  messageQueue =
      QueueFactory::instance()->createMessageQueue(50, MessageQueueMessage::MAX_MESSAGE_SIZE);
  memcpy(&this->defaultDestination, defaultDestination_in, defaultDestinationLen);
}

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

  TmManager *tmManager = ObjectManager::instance()->get<TmManager>(objects::TM_MANAGER);
  if (tmManager == nullptr) {
    return returnvalue::FAILED;
  }

  // we do not need any space reserved for the header, as we can send with detached header
  // information
  tmManager->registerNetworkProtocolInterface(this, IP6, 0);

  int retval;

  serverSocket = socket(AF_INET6, SOCK_DGRAM, 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;
  }

  //TODO verify that lwip interprets this correctly
  int flags = fcntl(serverSocket, F_GETFL, 0);
  flags |= O_NONBLOCK;
  fcntl(serverSocket, F_SETFL, flags);

  sockaddr_in6 serverAddr;

  memset(&serverAddr, 0, sizeof(serverAddr));

  serverAddr.sin6_family = AF_INET6;
  serverAddr.sin6_port = htons(listeningPort);
  serverAddr.sin6_addr = IN6ADDR_ANY_INIT;

  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) {
  switch (operationCode) {
    case BOTH:
      handleTC();
      handleTM();
      break;
    case TM:
      handleTM();
      break;
    case TC:
      handleTC();
      break;
    default:
      break;
  }
  return returnvalue::OK;
}

MessageQueueId_t UdpTmTcBridgeNew::getReportReceptionQueue() const { return messageQueue->getId(); }

void UdpTmTcBridgeNew::handleTC() {
  sockaddr_storage sender;
  socklen_t senderlen = sizeof(sender);

  ssize_t peekLen =
      recvfrom(serverSocket, NULL, 0, MSG_PEEK | MSG_TRUNC, (struct sockaddr *)&sender, &senderlen);

  if (peekLen <= 0) {
    return;
  }

  if (peekLen < FsfwProtocolHeader::HEADER_SIZE) {
    recv(serverSocket, NULL, 0, MSG_TRUNC);
    return;
  }

  if (sender.ss_family != AF_INET6) {
    // TODO handle v4 if we allow setting a listening address
    // for now, we listen as AF_INET6 and this should not happen
    return;
  }

  store_address_t storageId;
  uint8_t *bufferPointer;
  ReturnValue_t result =
      IPCStore->getFreeElement(&storageId, peekLen + 1 + sizeof(sockaddr_in6), &bufferPointer);

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

  *bufferPointer = IP6;

  uint8_t *applicationData = bufferPointer + 1 + sizeof(sockaddr_in6);
  senderlen = sizeof(sockaddr_in6);

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

  if (receivedLen == -1) {
    return;
  }

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


  size_t bufferLen = receivedLen;
  const uint8_t *constApplicationData = applicationData;

  FsfwProtocolHeader header;

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

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

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

      ActionMessage::setCommand(&message, 1 + sizeof(sockaddr_in6), 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;
  }
}

void UdpTmTcBridgeNew::handleTM() {
  CommandMessage message;
  ReturnValue_t result = messageQueue->receiveMessage(&message);

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

  if (message.getCommand() != TmMessage::SEND_TM) {
    return;
  }

  store_address_t tc = TmMessage::getTc(&message);
  store_address_t tm = TmMessage::getTm(&message);

  const sockaddr *receiver;
  socklen_t receiverLen;

  const uint8_t *tcData;
  size_t tcDataSize;
  result = IPCStore->getData(tc, &tcData, &tcDataSize);
  if (result != returnvalue::OK) {
    receiver = (const sockaddr *)&defaultDestination;
    receiverLen = defaultDestinationLen;
  } else {
    if (*tcData != IP6) {
      // this should not have been routed here
      return;
    }
    receiver = (const sockaddr *)(tcData + 1);
    receiverLen = sizeof(sockaddr_in6);
  }

  const uint8_t *tmData;
  size_t tmDataSize;
  result = IPCStore->getData(tm, &tmData, &tmDataSize);
  if (result != returnvalue::OK) {
    // nothing to send
    if (IPCStore->hasDataAtId(tc)) {
      IPCStore->deleteData(tc);
    }
    return;
  }

  int res = sendto(serverSocket, tmData, tmDataSize, 0, receiver, receiverLen);

  if (res == -1) {
    sif::error << "UdpTmTcBridge::handleTM: sendto failed with " << errno << std::endl;
  }
  IPCStore->deleteData(tm);
  if (IPCStore->hasDataAtId(tc)) {
    IPCStore->deleteData(tc);
  }
}