#include "ServoCommInterface.h"

#include <arpa/inet.h>
#include <errno.h>
#include <fcntl.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.
#include <unistd.h>
#include <utility>

#include "SerialTCPCookie.h"

ServoCommInterface::ServoCommInterface(object_id_t setObjectId) : SystemObject(setObjectId) {}

ReturnValue_t ServoCommInterface::initializeInterface(CookieIF *cookie) {
  SerialTCPCookie *tcpCookie = dynamic_cast<SerialTCPCookie *>(cookie);
  if (tcpCookie != nullptr) {
    return initializeInterface(tcpCookie);
  }
  return DeviceCommunicationIF::INVALID_COOKIE_TYPE;
}

ReturnValue_t ServoCommInterface::sendMessage(CookieIF *cookie, const uint8_t *sendData,
                                              size_t sendLen) {
  SerialTCPCookie *tcpCookie = dynamic_cast<SerialTCPCookie *>(cookie);
  if (tcpCookie != nullptr) {
    return sendMessage(tcpCookie, sendData, sendLen);
  }
  return DeviceCommunicationIF::INVALID_COOKIE_TYPE;
}

ReturnValue_t ServoCommInterface::sendMessage(SerialTCPCookie *cookie, const uint8_t *sendData,
                                              size_t sendLen) {
  if (cookie->socket == -1) {
    return returnvalue::FAILED;
  }

  ssize_t result = send(cookie->socket, sendData, sendLen, 0);

  if (result < 0){
    return returnvalue::FAILED;
  }
  
  if (std::cmp_not_equal(result, sendLen)) {
    return returnvalue::FAILED;
  }

  return returnvalue::OK;
}

ReturnValue_t ServoCommInterface::getSendSuccess(CookieIF *cookie) { return returnvalue::OK; }

ReturnValue_t ServoCommInterface::requestReceiveMessage(CookieIF *cookie, size_t requestLen) {
  return returnvalue::OK;
}

ReturnValue_t ServoCommInterface::readReceivedMessage(CookieIF *cookie, uint8_t **buffer,
                                                      size_t *size) {
  SerialTCPCookie *tcpCookie = dynamic_cast<SerialTCPCookie *>(cookie);
  if (tcpCookie != nullptr) {
    return readReceivedMessage(tcpCookie, buffer, size);
  }
  return DeviceCommunicationIF::INVALID_COOKIE_TYPE;
}

ReturnValue_t ServoCommInterface::readReceivedMessage(SerialTCPCookie *cookie, uint8_t **buffer,
                                                      size_t *size) {
  if (cookie->socket == -1) {
    return returnvalue::FAILED;
  }

  ssize_t result = recv(cookie->socket, cookie->buffer, cookie->bufferSize, 0);

  if (result <= -1) {
    *size = 0;
    if (errno == EAGAIN or errno == EWOULDBLOCK) {
      //no data
      return returnvalue::OK;
    }
    // TODO wrap errno
    return returnvalue::FAILED;
  }

  if (result == 0) {
    // peer shut down
    close(cookie->socket);
    cookie->socket = -1;
    return returnvalue::FAILED;  // TODO connection closed
  }

  *size = result;
  *buffer = cookie->buffer;

  
  if (std::cmp_greater_equal(result, cookie->bufferSize) and cookie->flush) {
    // we do not know if there is more data in the socket, so we read until we get no more
    uint8_t ignore[10];
    while (recv(cookie->socket, ignore, sizeof(ignore), 0) > 0) {
    }
    //ignore any fault here as previous read was ok. next call will catch it
  }

  return returnvalue::OK;
}

ReturnValue_t ServoCommInterface::initializeInterface(SerialTCPCookie *cookie) {
  cookie->socket = socket(AF_INET6, SOCK_STREAM, 0);
  if (cookie->socket == -1) {
    return returnvalue::FAILED;
  }

  sockaddr_in6 serverAddr;
  serverAddr.sin6_family = AF_INET6;
  serverAddr.sin6_port = htons(cookie->port);

  int retval =inet_pton(AF_INET6, cookie->host, &serverAddr.sin6_addr);
  if (retval == 0) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::error << "IpCommInterface::initializeInterface: Invalid IP!" << std::endl;
#endif
    return returnvalue::FAILED;
  }

  retval = connect(cookie->socket, (struct sockaddr *)&serverAddr, sizeof(serverAddr));

  if (retval == -1) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::error << "IpCommInterface::initializeInterface: connection failed with " << errno
               << std::endl;
#endif
    return returnvalue::FAILED;
  }

  fcntl(cookie->socket, F_SETFL, O_NONBLOCK);

  return returnvalue::OK;
}