#include "CspComIF.h"

#include <csp/drivers/can_socketcan.h>
#include <fsfw/serialize/SerializeAdapter.h>
#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
#include <mission/devices/devicedefinitions/GomspaceDefinitions.h>
#include <p60acu.h>
#include <p60dock.h>
#include <p60pdu.h>
#include <param/param_string.h>
#include <param/rparam_client.h>

#include "mission/csp/CspCookie.h"

using namespace GOMSPACE;

CspComIF::CspComIF(object_id_t objectId, const char* routeTaskName, uint32_t routerRealTimePriority)
    : SystemObject(objectId),
      routerRealTimePriority(routerRealTimePriority),
      routerTaskName(routeTaskName) {}

CspComIF::~CspComIF() {}

ReturnValue_t CspComIF::initializeInterface(CookieIF* cookie) {
  if (cookie == nullptr) {
    return NULLPOINTER;
  }

  CspCookie* cspCookie = dynamic_cast<CspCookie*>(cookie);
  if (cspCookie == nullptr) {
    return NULLPOINTER;
  }

  /* Perform CAN and CSP initialization only once */
  if (cspDeviceMap.empty()) {
    sif::info << "Performing " << canInterface << " initialization.." << std::endl;

    /* Define the memory to allocate for the CSP stack */
    int buf_count = 10;
    int buf_size = 300;
    /* Init CSP and CSP buffer system */
    if (csp_init(cspOwnAddress) != CSP_ERR_NONE ||
        csp_buffer_init(buf_count, buf_size) != CSP_ERR_NONE) {
      sif::error << "Failed to init CSP\r\n" << std::endl;
      return returnvalue::FAILED;
    }

    int promisc = 0;  // Set filter mode on
    csp_iface_t* csp_if_ptr = &csp_if;
    csp_if_ptr = csp_can_socketcan_init(canInterface, bitrate, promisc);

    /* Set default route and start router */
    uint8_t address = CSP_DEFAULT_ROUTE;
    uint8_t netmask = 0;
    uint8_t mac = CSP_NODE_MAC;
    int result = csp_rtable_set(address, netmask, csp_if_ptr, mac);
    if (result != CSP_ERR_NONE) {
      sif::error << "Failed to add can interface to router table" << std::endl;
      return returnvalue::FAILED;
    }

    /* Start the route task */
    result = startRouterTask();
    if (result != returnvalue::OK) {
      sif::error << "Failed to start csp route task" << std::endl;
      return returnvalue::FAILED;
    }
    sif::info << canInterface << " initialized successfully" << std::endl;
  }

  uint8_t cspAddress = cspCookie->getCspAddress();
  uint16_t maxReplyLength = cspCookie->getMaxReplyLength();
  if (cspDeviceMap.find(cspAddress) == cspDeviceMap.end()) {
    /* Insert device information in CSP map */
    cspDeviceMap.emplace(cspAddress, ReplyInfo(maxReplyLength));
  }
  return returnvalue::OK;
}

ReturnValue_t CspComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) {
  int result;
  if (cookie == nullptr) {
    return returnvalue::FAILED;
  }
  CspCookie* cspCookie = dynamic_cast<CspCookie*>(cookie);
  if (cspCookie == nullptr) {
    return returnvalue::FAILED;
  }

  uint8_t cspPort;
  uint16_t querySize = 0;
  if (cspCookie->getRequest() == GOMSPACE::SpecialRequestTypes::DEFAULT_COM_IF) {
    /* Extract csp port and bytes to query from command buffer */
    result = getPortAndQuerySize(&sendData, &sendLen, &cspPort, &querySize);
    if (result != returnvalue::OK) {
      return result;
    }
  } else {
    cspPort = cspCookie->getCspPort();
    querySize = cspCookie->getReplyLen();
  }
  if (querySize > cspCookie->getMaxReplyLength()) {
    sif::error << "Query size " << querySize << " is larger than maximum allowed "
               << cspCookie->getMaxReplyLength() << std::endl;
    return returnvalue::FAILED;
  }
  uint8_t cspAddress = cspCookie->getCspAddress();
  auto iter = cspDeviceMap.find(cspAddress);
  if (iter == cspDeviceMap.end()) {
    return returnvalue::FAILED;
  }
  switch (cspPort) {
    case (CspPorts::CSP_PING): {
      initiatePingRequest(cspAddress, querySize);
      break;
    }
    case (CspPorts::CSP_REBOOT): {
      csp_reboot(cspAddress);
      break;
    }
    case (CspPorts::P60_PORT_GNDWDT_RESET_ENUM):
    case (CspPorts::P60_PORT_RPARAM_ENUM): {
      if (cspCookie->getRequest() != SpecialRequestTypes::DEFAULT_COM_IF) {
        param_index_t requestStruct{};
        requestStruct.physaddr = iter->second.replyBuf.data();
        auto req = cspCookie->getRequest();
        if (req == GOMSPACE::SpecialRequestTypes::GET_PDU_HK) {
          if (!p60pdu_get_hk(&requestStruct, cspAddress, cspCookie->getTimeout())) {
            return returnvalue::FAILED;
          }

        } else if (req == GOMSPACE::SpecialRequestTypes::GET_ACU_HK) {
          if (!p60acu_get_hk(&requestStruct, cspAddress, cspCookie->getTimeout())) {
            return returnvalue::FAILED;
          }
        } else if (req == GOMSPACE::SpecialRequestTypes::GET_P60DOCK_HK) {
          if (!p60dock_get_hk(&requestStruct, cspAddress, cspCookie->getTimeout())) {
            return returnvalue::FAILED;
          }
        } else if (req == GOMSPACE::SpecialRequestTypes::GET_PDU_CONFIG) {
          requestStruct.table = p60pdu_config;
          requestStruct.mem_id = P60PDU_PARAM;
          requestStruct.count = p60pdu_config_count;
          requestStruct.size = P60PDU_PARAM_SIZE;
          result = rparam_get_full_table(&requestStruct, cspAddress, P60_PORT_RPARAM,
                                         requestStruct.mem_id, cspCookie->getTimeout());
          if (result != 0) {
            return returnvalue::FAILED;
          }
        } else if (req == GOMSPACE::SpecialRequestTypes::GET_ACU_CONFIG) {
          requestStruct.table = p60acu_config;
          requestStruct.mem_id = P60ACU_PARAM;
          requestStruct.count = p60acu_config_count;
          requestStruct.size = P60ACU_PARAM_SIZE;
          result = rparam_get_full_table(&requestStruct, cspAddress, P60_PORT_RPARAM,
                                         requestStruct.mem_id, cspCookie->getTimeout());
          if (result != 0) {
            return returnvalue::FAILED;
          }
        } else if (req == GOMSPACE::SpecialRequestTypes::GET_P60DOCK_CONFIG) {
          requestStruct.table = p60dock_config;
          requestStruct.mem_id = P60DOCK_PARAM;
          requestStruct.count = p60dock_config_count;
          requestStruct.size = P60DOCK_PARAM_SIZE;
          result = rparam_get_full_table(&requestStruct, cspAddress, P60_PORT_RPARAM,
                                         requestStruct.mem_id, cspCookie->getTimeout());
          if (result != 0) {
            return returnvalue::FAILED;
          }
        } else if (req == GOMSPACE::SpecialRequestTypes::SAVE_TABLE) {
          if (sendLen < 2) {
            return returnvalue::FAILED;
          }
          const TableInfo* tableInfo = reinterpret_cast<const TableInfo*>(sendData);
          result = gs_rparam_save(cspAddress, cspCookie->getTimeout(), tableInfo->sourceTable,
                                  tableInfo->targetTable);
          if (result != 0) {
            return returnvalue::FAILED;
          }
        } else if (req == GOMSPACE::SpecialRequestTypes::LOAD_TABLE) {
          if (sendLen < 2) {
            return returnvalue::FAILED;
          }
          const TableInfo* tableInfo = reinterpret_cast<const TableInfo*>(sendData);
          result = gs_rparam_load(cspAddress, cspCookie->getTimeout(), tableInfo->sourceTable,
                                  tableInfo->targetTable);
          if (result != 0) {
            return returnvalue::FAILED;
          }
        }
      } else {
        /* No CSP fixed port was selected. Send data to the specified port and
         * wait for querySize number of bytes */
        result = cspTransfer(cspAddress, cspPort, sendData, sendLen, querySize);
        if (result != returnvalue::OK) {
          return returnvalue::FAILED;
        }
      }
      iter->second.replyLen = querySize;
      break;
    }
    default:
      sif::error << "CspComIF: Invalid port specified" << std::endl;
      break;
  }
  return returnvalue::OK;
}

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

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

ReturnValue_t CspComIF::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) {
  if (cookie == NULL) {
    return returnvalue::FAILED;
  }
  CspCookie* cspCookie = dynamic_cast<CspCookie*>(cookie);
  if (cspCookie == NULL) {
    return returnvalue::FAILED;
  }

  uint8_t cspAddress = cspCookie->getCspAddress();
  auto iter = cspDeviceMap.find(cspAddress);
  if (iter == cspDeviceMap.end()) {
    return returnvalue::FAILED;
  }
  *buffer = iter->second.replyBuf.data();
  *size = iter->second.replyLen;

  return returnvalue::OK;
}

ReturnValue_t CspComIF::cspTransfer(uint8_t cspAddress, uint8_t cspPort, const uint8_t* cmdBuffer,
                                    int cmdLen, uint16_t querySize) {
  uint32_t timeout_ms = 1000;
  uint16_t bytesRead = 0;
  int32_t expectedSize = static_cast<int32_t>(querySize);
  auto iter = cspDeviceMap.find(cspAddress);
  if (iter == cspDeviceMap.end()) {
    sif::error << "CSP device with address " << cspAddress << " no found in"
               << " device map" << std::endl;
    return returnvalue::FAILED;
  }
  uint8_t* replyBuffer = iter->second.replyBuf.data();

  csp_conn_t* conn = csp_connect(CSP_PRIO_HIGH, cspAddress, cspPort, 0, CSP_O_NONE);

  csp_packet_t* commandPacket = (csp_packet_t*)csp_buffer_get(cmdLen);
  if (commandPacket == NULL) {
    sif::error << "CspComIF::cspTransfer: Failed to get memory for a csp packet from the csp "
               << "stack" << std::endl;
    csp_close(conn);
    return returnvalue::FAILED;
  }

  memcpy(commandPacket->data, cmdBuffer, cmdLen);
  commandPacket->length = cmdLen;

  if (!csp_send(conn, commandPacket, timeout_ms)) {
    csp_buffer_free(commandPacket);
    sif::error << "CspComIF::cspTransfer: Failed to send csp packet" << std::endl;
    csp_close(conn);
    return returnvalue::FAILED;
  }

  /* Return when no reply is expected */
  if (expectedSize == 0) {
    return returnvalue::OK;
  }

  csp_packet_t* reply;
  reply = csp_read(conn, timeout_ms);
  if (reply == NULL) {
    sif::error << "CspComIF::cspTransfer: Failed to read csp packet" << std::endl;
    csp_close(conn);
    return returnvalue::FAILED;
  }
  memcpy(replyBuffer, reply->data, reply->length);
  expectedSize = expectedSize - reply->length;
  bytesRead += reply->length;
  csp_buffer_free(reply);
  while (expectedSize > 0) {
    reply = csp_read(conn, timeout_ms);
    if (reply == NULL) {
      sif::error << "CspComIF::cspTransfer: Failed to read csp packet" << std::endl;
      csp_close(conn);
      return returnvalue::FAILED;
    }
    if ((reply->length + bytesRead) > iter->second.replyBuf.size()) {
      sif::error << "CspComIF::cspTransfer: Reply buffer to short" << std::endl;
      csp_buffer_free(reply);
      csp_close(conn);
      return returnvalue::FAILED;
    }
    memcpy(replyBuffer + bytesRead, reply->data, reply->length);
    expectedSize = expectedSize - reply->length;
    bytesRead += reply->length;
    csp_buffer_free(reply);
  }

  if (expectedSize != 0) {
    sif::error << "CspComIF::cspTransfer: Received more bytes than requested" << std::endl;
    sif::debug << "CspComIF::cspTransfer: Received bytes: " << bytesRead << std::endl;
    csp_close(conn);
    return returnvalue::FAILED;
  }

  csp_close(conn);

  return returnvalue::OK;
}

ReturnValue_t CspComIF::getPortAndQuerySize(const uint8_t** sendData, size_t* sendLen,
                                            uint8_t* cspPort, uint16_t* querySize) {
  ReturnValue_t result =
      SerializeAdapter::deSerialize(cspPort, sendData, sendLen, SerializeIF::Endianness::BIG);
  if (result != returnvalue::OK) {
    sif::error << "CspComIF: Failed to deserialize CSP port from command "
               << "buffer" << std::endl;
    return returnvalue::FAILED;
  }
  SerializeAdapter::deSerialize(querySize, sendData, sendLen, SerializeIF::Endianness::BIG);
  if (result != returnvalue::OK) {
    sif::error << "CspComIF: Failed to deserialize querySize from command "
               << "buffer" << std::endl;
    return returnvalue::FAILED;
  }
  return returnvalue::OK;
}

void CspComIF::initiatePingRequest(uint8_t cspAddress, uint16_t querySize) {
  uint32_t timeout_ms = 500;
  uint32_t replyTime = csp_ping(cspAddress, timeout_ms, querySize, CSP_O_NONE);
  sif::info << "Ping address: " << cspAddress << ", reply after " << replyTime << " ms"
            << std::endl;
  auto iter = cspDeviceMap.find(cspAddress);
  if (iter == cspDeviceMap.end()) {
    return;
  }
  /* Store reply time in reply buffer * */
  uint8_t* replyBuffer = iter->second.replyBuf.data();
  memcpy(replyBuffer, &replyTime, sizeof(replyTime));
  iter->second.replyLen = sizeof(replyTime);
}

ReturnValue_t CspComIF::startRouterTask() {
  pthread_attr_t attr;
  int res = pthread_attr_init(&attr);
  if (res) {
    return returnvalue::FAILED;
  }

  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);

  res = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
  if (res != 0) {
    return returnvalue::FAILED;
  }

  // Set scheduling policy to SCHED_RR
  res = pthread_attr_setschedpolicy(&attr, SCHED_RR);
  if (res) {
    pthread_attr_destroy(&attr);
    return returnvalue::FAILED;
  }

  struct sched_param sched_param;
  sched_param.sched_priority = routerRealTimePriority;
  res = pthread_attr_setschedparam(&attr, &sched_param);
  if (res) {
    pthread_attr_destroy(&attr);
    return returnvalue::FAILED;
  }

  res = pthread_create(&routerTaskHandle, &attr, routerWorkWrapper, NULL);
  if (res) {
    pthread_attr_destroy(&attr);
    return returnvalue::FAILED;
  }

  res = pthread_setname_np(routerTaskHandle, routerTaskName);
  if (res) {
    pthread_attr_destroy(&attr);
    return returnvalue::FAILED;
  }
  pthread_attr_destroy(&attr);
  return returnvalue::OK;
}

void* CspComIF::routerWorkWrapper(void* args) {
  /* Here there be routing */
  while (1) {
    csp_route_work(FIFO_TIMEOUT);
  }
  return nullptr;
}