/*
 * Copyright (C) 2021 Satlab A/S (https://www.satlab.com)
 *
 * This file is part of CSP. See COPYING for details.
 */

#include <csp/interfaces/csp_if_slgnd.h>

#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <errno.h>
#include <limits.h>
#include <inttypes.h>
#include <unistd.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netdb.h>

#include <pthread.h>

#include <csp/csp.h>
#include <csp/csp_comms.h>
#include <csp/csp_endian.h>
#include <csp/csp_interface.h>
#include <csp/arch/csp_malloc.h>
#include <csp/arch/csp_thread.h>

/* SLGND default port */
#define CSP_IF_SLGND_PORT	"52002"

/* The MTU is actually defined by the radio configuration */
#define CSP_IF_SLGND_MTU	1400

/* The MTU is actually defined by the radio configuration */
#define CSP_IF_SLGND_MTU_UHF	251

//#define UHF_SIM
//#define SBAND_SIM

struct csp_slgnd_ifdata_tcp {
	char ifname[CSP_IFLIST_NAME_MAX + 1];
	csp_iface_t iface;

	int radio_socket;
	struct addrinfo *radio_addr;
	pthread_t rx_thread;
};

struct sockaddr_in dest;

static const uint32_t crc32c_table[256] = {
	0x00000000l, 0xf26b8303l, 0xe13b70f7l, 0x1350f3f4l,
	0xc79a971fl, 0x35f1141cl, 0x26a1e7e8l, 0xd4ca64ebl,
	0x8ad958cfl, 0x78b2dbccl, 0x6be22838l, 0x9989ab3bl,
	0x4d43cfd0l, 0xbf284cd3l, 0xac78bf27l, 0x5e133c24l,
	0x105ec76fl, 0xe235446cl, 0xf165b798l, 0x030e349bl,
	0xd7c45070l, 0x25afd373l, 0x36ff2087l, 0xc494a384l,
	0x9a879fa0l, 0x68ec1ca3l, 0x7bbcef57l, 0x89d76c54l,
	0x5d1d08bfl, 0xaf768bbcl, 0xbc267848l, 0x4e4dfb4bl,
	0x20bd8edel, 0xd2d60dddl, 0xc186fe29l, 0x33ed7d2al,
	0xe72719c1l, 0x154c9ac2l, 0x061c6936l, 0xf477ea35l,
	0xaa64d611l, 0x580f5512l, 0x4b5fa6e6l, 0xb93425e5l,
	0x6dfe410el, 0x9f95c20dl, 0x8cc531f9l, 0x7eaeb2fal,
	0x30e349b1l, 0xc288cab2l, 0xd1d83946l, 0x23b3ba45l,
	0xf779deael, 0x05125dadl, 0x1642ae59l, 0xe4292d5al,
	0xba3a117el, 0x4851927dl, 0x5b016189l, 0xa96ae28al,
	0x7da08661l, 0x8fcb0562l, 0x9c9bf696l, 0x6ef07595l,
	0x417b1dbcl, 0xb3109ebfl, 0xa0406d4bl, 0x522bee48l,
	0x86e18aa3l, 0x748a09a0l, 0x67dafa54l, 0x95b17957l,
	0xcba24573l, 0x39c9c670l, 0x2a993584l, 0xd8f2b687l,
	0x0c38d26cl, 0xfe53516fl, 0xed03a29bl, 0x1f682198l,
	0x5125dad3l, 0xa34e59d0l, 0xb01eaa24l, 0x42752927l,
	0x96bf4dccl, 0x64d4cecfl, 0x77843d3bl, 0x85efbe38l,
	0xdbfc821cl, 0x2997011fl, 0x3ac7f2ebl, 0xc8ac71e8l,
	0x1c661503l, 0xee0d9600l, 0xfd5d65f4l, 0x0f36e6f7l,
	0x61c69362l, 0x93ad1061l, 0x80fde395l, 0x72966096l,
	0xa65c047dl, 0x5437877el, 0x4767748al, 0xb50cf789l,
	0xeb1fcbadl, 0x197448ael, 0x0a24bb5al, 0xf84f3859l,
	0x2c855cb2l, 0xdeeedfb1l, 0xcdbe2c45l, 0x3fd5af46l,
	0x7198540dl, 0x83f3d70el, 0x90a324fal, 0x62c8a7f9l,
	0xb602c312l, 0x44694011l, 0x5739b3e5l, 0xa55230e6l,
	0xfb410cc2l, 0x092a8fc1l, 0x1a7a7c35l, 0xe811ff36l,
	0x3cdb9bddl, 0xceb018del, 0xdde0eb2al, 0x2f8b6829l,
	0x82f63b78l, 0x709db87bl, 0x63cd4b8fl, 0x91a6c88cl,
	0x456cac67l, 0xb7072f64l, 0xa457dc90l, 0x563c5f93l,
	0x082f63b7l, 0xfa44e0b4l, 0xe9141340l, 0x1b7f9043l,
	0xcfb5f4a8l, 0x3dde77abl, 0x2e8e845fl, 0xdce5075cl,
	0x92a8fc17l, 0x60c37f14l, 0x73938ce0l, 0x81f80fe3l,
	0x55326b08l, 0xa759e80bl, 0xb4091bffl, 0x466298fcl,
	0x1871a4d8l, 0xea1a27dbl, 0xf94ad42fl, 0x0b21572cl,
	0xdfeb33c7l, 0x2d80b0c4l, 0x3ed04330l, 0xccbbc033l,
	0xa24bb5a6l, 0x502036a5l, 0x4370c551l, 0xb11b4652l,
	0x65d122b9l, 0x97baa1bal, 0x84ea524el, 0x7681d14dl,
	0x2892ed69l, 0xdaf96e6al, 0xc9a99d9el, 0x3bc21e9dl,
	0xef087a76l, 0x1d63f975l, 0x0e330a81l, 0xfc588982l,
	0xb21572c9l, 0x407ef1cal, 0x532e023el, 0xa145813dl,
	0x758fe5d6l, 0x87e466d5l, 0x94b49521l, 0x66df1622l,
	0x38cc2a06l, 0xcaa7a905l, 0xd9f75af1l, 0x2b9cd9f2l,
	0xff56bd19l, 0x0d3d3e1al, 0x1e6dcdeel, 0xec064eedl,
	0xc38d26c4l, 0x31e6a5c7l, 0x22b65633l, 0xd0ddd530l,
	0x0417b1dbl, 0xf67c32d8l, 0xe52cc12cl, 0x1747422fl,
	0x49547e0bl, 0xbb3ffd08l, 0xa86f0efcl, 0x5a048dffl,
	0x8ecee914l, 0x7ca56a17l, 0x6ff599e3l, 0x9d9e1ae0l,
	0xd3d3e1abl, 0x21b862a8l, 0x32e8915cl, 0xc083125fl,
	0x144976b4l, 0xe622f5b7l, 0xf5720643l, 0x07198540l,
	0x590ab964l, 0xab613a67l, 0xb831c993l, 0x4a5a4a90l,
	0x9e902e7bl, 0x6cfbad78l, 0x7fab5e8cl, 0x8dc0dd8fl,
	0xe330a81al, 0x115b2b19l, 0x020bd8edl, 0xf0605beel,
	0x24aa3f05l, 0xd6c1bc06l, 0xc5914ff2l, 0x37faccf1l,
	0x69e9f0d5l, 0x9b8273d6l, 0x88d28022l, 0x7ab90321l,
	0xae7367cal, 0x5c18e4c9l, 0x4f48173dl, 0xbd23943el,
	0xf36e6f75l, 0x0105ec76l, 0x12551f82l, 0xe03e9c81l,
	0x34f4f86al, 0xc69f7b69l, 0xd5cf889dl, 0x27a40b9el,
	0x79b737bal, 0x8bdcb4b9l, 0x988c474dl, 0x6ae7c44el,
	0xbe2da0a5l, 0x4c4623a6l, 0x5f16d052l, 0xad7d5351l
};


uint32_t crc32c_update(uint32_t crc, const uint8_t *input, size_t bytes)
{
	while (bytes--)
		crc = crc32c_table[(crc ^ *input++) & 0xff] ^ (crc >> 8);

	return crc;
}

uint32_t crc32c(const uint8_t *input, size_t bytes)
{
	uint32_t crc = ~(uint32_t)0;

	crc = crc32c_update(crc, input, bytes);

	return crc ^ ~0UL;
}

static void *csp_slgnd_rx_tcp(void *arg)
{
	struct csp_slgnd_ifdata_tcp *data = arg;

	int nbytes;
	csp_packet_t *packet;
#ifdef BACKDOOR_SOCK_SIM
        uint32_t id;
        uint16_t length;
        uint8_t frame[sizeof(id) + CSP_IF_SLGND_MTU];
#elif IQS_S_SIM
        uint8_t frame[1432];
#elif SBAND_SIM
	uint32_t id;
	uint16_t length;

	uint32_t crc=0, rcv_crc = 0;
	uint8_t frame[sizeof(length) + sizeof(id) + CSP_IF_SLGND_MTU +  sizeof(crc)];
#elif UHF_SIM
	int nbytes;
	uint32_t id;
	uint16_t length;

	uint32_t crc=0, rcv_crc = 0;

#endif
	while (1) {
            memset(frame,0,sizeof(frame));

            /* Wait for frames from radio */
            nbytes  = recv(data->radio_socket, frame, sizeof(frame) ,MSG_DONTWAIT);
            //nbytes = read(data->radio_socket, frame, sizeof(frame));

            if(nbytes<0)
                continue;
#ifdef BACKDOOR_SOCK_SIM
            printf("\nBackdoor Socket SIM: Received length %d\n",nbytes);
            if (nbytes < 0)
                continue;

            length = nbytes - sizeof(id);

            /* Allocate buffer for frame */
            packet = csp_buffer_get(length);
            if (!packet)
                continue;
            packet->length = length;

            /* Set packet ID */
            memcpy(&id, &frame[0], sizeof(id));
            packet->id.ext = csp_ntoh32(id);

            /* Copy received data into CSP buffer */
            memcpy(packet->data, &frame[sizeof(id)], packet->length);
            //printf("\n%ld\n", (sizeof(id)));
            for(int i = 0; i < 50; i++)
            {
                printf("%x ",packet->data[i]);
            }
            printf("\n");

#elif IQS_S_SIM
            printf("\nS-Band IQS SIM: Received length %d\n",nbytes);
            packet = csp_buffer_get(0);
            packet->length = 1432;
            memcpy(packet->data, &frame[12], sizeof(frame));
            packet->id.ext = 0x96c57c00;
            nbytes = 50;

#elif SBAND_SIM
            if (nbytes < 0 || (unsigned int)nbytes < sizeof(frame))
                continue;
            /* Extract CSP length */
            memcpy(&length, &frame[0], sizeof(length));

	    crc = crc32c(frame,(nbytes-4));
	    memcpy(&rcv_crc,&frame[nbytes-4],4);

	    if(crc == rcv_crc)
	    {
		    printf("\nCRC Matched");
	    }
	    else
	    {
		    printf("\nCRC mismatched; Packet dropped, %x - %x",crc,rcv_crc);
		    continue;
	    }

            /* Allocate buffer for frame */
            packet = csp_buffer_get(length);
            if (!packet)
                continue;
            packet->length = length;

            /* Set packet ID */
            memcpy(&id, &frame[sizeof(length)], sizeof(id));
            packet->id.ext = csp_ntoh32(id);

            /* Copy received data into CSP buffer */
            memcpy(packet->data, &frame[sizeof(length)+ sizeof(id)], packet->length);
            
            /* Pass frame to CSP stack */
            csp_qfifo_write(packet, &data->iface, NULL);
#elif UHF_SIM

            /* Allocate buffer for frame */
            packet = csp_buffer_get(length);

            uhf_unpack_uart_frame(packet, frame);
            
#endif
            /* Pass frame to CSP stack */
            csp_qfifo_write(packet, &data->iface, NULL);

	}

	return NULL;
}

static int csp_slgnd_tx_tcp(const csp_route_t *route, csp_packet_t *packet)
{
    struct csp_slgnd_ifdata_tcp *data = route->iface->driver_data;
    int nbytes;
    uint32_t id;

    uint16_t frame_length = 0;

#ifdef BACKDOOR_SOCK_SIM
    uint8_t frame[sizeof(id) + CSP_IF_SLGND_MTU];
    memset(frame,0,sizeof(frame));

    id = csp_hton32(packet->id.ext);

    memcpy(&frame[frame_length], &id, sizeof(id));
    frame_length += sizeof(id);

    memcpy(&frame[frame_length], packet->data, packet->length);
    frame_length += packet->length;

    printf("\n BACK Door Socket length %d, frame size:%ld \n",packet->length,sizeof(frame));

    //nbytes = write(data->radio_socket, frame, sizeof(frame));
    //nbytes = sendto(data->radio_socket, frame, sizeof(frame),0,(struct sockaddr *) &dest, sizeof(dest));
    //csp_buffer_free(packet);
#elif IQS_S_SIM
    uint16_t frame_size=0;
    uint8_t frame[sizeof(frame_length) + sizeof(id) + CSP_IF_SLGND_MTU];// +  sizeof(crc_val)];

    memset(frame,0,sizeof(frame));

    frame_size = packet->length;
    memcpy(&frame[frame_length], &frame_size, sizeof(frame_size));
    frame_length += sizeof(frame_size);
    memcpy(&frame[frame_length], &id, sizeof(id));
    frame_length += sizeof(id);
    memcpy(&frame[frame_length], packet->data, packet->length);
    frame_length += packet->length;


    printf("\n IQS SIM length %d, frame size:%ld \n",frame_size,sizeof(frame));

#elif SBAND_SIM
    uint32_t id;
    uint32_t crc_val = 0;
    uint16_t frame_length = 0, frame_size=0;

    uint8_t frame[sizeof(frame_length) + sizeof(id) + CSP_IF_SLGND_MTU +  sizeof(crc_val)];

    memset(frame,0,sizeof(frame));

    id = csp_hton32(packet->id.ext);
    //memcpy(&frame[frame_length], &id, sizeof(id));
    //frame_length += sizeof(id);
    //memcpy(&frame[frame_length], packet->data, packet->length);
    //frame_length += packet->length;

    frame_size = packet->length;
    memcpy(&frame[frame_length], &frame_size, sizeof(frame_size));
    frame_length += sizeof(frame_size);
    memcpy(&frame[frame_length], &id, sizeof(id));
    frame_length += sizeof(id);
    memcpy(&frame[frame_length], packet->data, packet->length);
    frame_length += packet->length;

    crc_val = crc32c(frame,(sizeof(frame)-sizeof(uint32_t)));

    memcpy(&frame[(sizeof(frame)-sizeof(uint32_t))],&crc_val,sizeof(uint32_t));

    frame_length = frame_length + sizeof(uint32_t);

    printf("\n CRC attached %x \n",crc_val);
    printf("\n length %d \n",frame_size);
#elif UHF_SIM
    uint32_t id;
    uint32_t crc_val = 0;
    uint16_t frame_length = 0, frame_size=0;

    uint8_t frame[CSP_IF_SLGND_MTU_UHF];
    uhf_pack_uart_frame(packet, frame);
#endif

	//nbytes = write(data->radio_socket, frame, sizeof(frame));
	nbytes = sendto(data->radio_socket, frame, sizeof(frame), 0, data->radio_addr->ai_addr, data->radio_addr->ai_addrlen);
//	nbytes = sendto(data->radio_socket, frame, sizeof(frame),0,(struct sockaddr *) &dest, sizeof(dest));

	if (nbytes != (int)(sizeof(frame)))
        {
            printf("[SLGND] Packet send timeout %d, %d",nbytes,(int)(sizeof(frame)));
            return CSP_ERR_TIMEDOUT;
        }

        printf("[SLGND] Packet sent %d, %d",nbytes,(int)(sizeof(frame)));
	csp_buffer_free(packet);
	return CSP_ERR_NONE;
}

int csp_slgnd_init_tcp(const char *radio_host, const char *radio_addrs,const char *ifname, csp_iface_t **ifc)
{
	int ret;
	struct addrinfo hints;
        struct sockaddr_in sa;
	char *port, host[HOST_NAME_MAX];

	/* Allocate interface data */
	struct csp_slgnd_ifdata_tcp *data = csp_calloc(1, sizeof(*data));
	if (!data)
		return CSP_ERR_NOMEM;

	if (!ifname)
		ifname = CSP_IF_SLGND_DEFAULT_NAME;

	strncpy(data->ifname, ifname, sizeof(data->ifname) - 1);
	data->iface.name = data->ifname;
	data->iface.driver_data = data;
	data->iface.nexthop = csp_slgnd_tx_tcp;
	data->iface.mtu = CSP_IF_SLGND_MTU + 20;

	/* Prepare host */
	strncpy(host, radio_host, sizeof(host) - 1);
	host[sizeof(host) - 1] = '\0';

	/* Use port if specified in host string */
	port = strchr(host, ':');
	if (port) {
		*port = '\0';
		port = port + 1;
	} else {
		port = (char *)CSP_IF_SLGND_PORT;
	}

        uint16_t src_port = atoi(port);

	memset(&hints, 0, sizeof(hints));
	hints.ai_family = AF_INET; /* GNU Radio Socket PDU only supports IPv4 */
	hints.ai_socktype = SOCK_STREAM;
	hints.ai_flags = AI_ADDRCONFIG;

	/* Look up host info */
	ret = getaddrinfo(host, port, &hints, &data->radio_addr);
	if (ret < 0) {
		free(data);
		csp_log_error("failed to look up host info for %s\n", radio_host);
		return CSP_ERR_DRIVER;
	}

	/* Create radio socket */
	data->radio_socket = socket(data->radio_addr->ai_family,
				    data->radio_addr->ai_socktype,
				    data->radio_addr->ai_protocol);

	if (data->radio_socket < 0) {
		free(data);
		csp_log_error("failed to allocate radio socket\n");
		return CSP_ERR_DRIVER;
        }

        /* Bind to incoming port on interface */
        memset(&sa, 0, sizeof(struct sockaddr_in));
        sa.sin_family = AF_INET;
        sa.sin_addr.s_addr = inet_addr(radio_addrs);
        sa.sin_port = htons(src_port);

        struct timeval tv;
        tv.tv_sec = 0;
        tv.tv_usec = 5000;
        setsockopt(data->radio_socket, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv, sizeof tv);

        printf(" connecting to server ..");
        while(connect(data->radio_socket, (const struct sockaddr *)&sa, sizeof(struct sockaddr_in)) <0)
        {
            usleep(1000);
        }

        printf("SOCKET: connected to the OBC CSP server \n");

            /* Create destination address */
        memset(&dest, 0, sizeof(dest));
        dest.sin_family = AF_INET;
        dest.sin_addr.s_addr = inet_addr(radio_addrs);
        dest.sin_port = htons(src_port);


	/* Start receiver thread */
	ret = pthread_create(&data->rx_thread, NULL, csp_slgnd_rx_tcp, data);
	if (ret < 0) {
		free(data);
		csp_log_error("failed to start receive thread\n");
		return CSP_ERR_DRIVER;
	}

	/* Register interface */
	csp_iflist_add(&data->iface);

	if (ifc)
		*ifc = &data->iface;

	return CSP_ERR_NONE;
}

/**
 * * @brief This function is used for packing the UART data to send via UHF module
 * */
void uhf_pack_uart_frame(csp_packet_t *packet, uint8_t *frame_buff)
{
    uint16_t length = 0;
    uint32_t id;
    s_comms_uhf_data *uhf_data = (s_comms_uhf_data*)frame_buff;

    uhf_data->header.sync1 = UHF_START_BYTE_0;
    uhf_data->header.sync2 = UHF_START_BYTE_1;
    uhf_data->header.length = packet->length + UHF_HEADER_SIZE + CSP_HEADER_LENGTH;
    uhf_data->header.hwid = 15;
    uhf_data->header.seqnum = 0;
    uhf_data->header.system = 0;
    uhf_data->header.command = 0;

    id = csp_hton32(packet->id.ext);

    memcpy(&uhf_data->data[length], &id, sizeof(id));
    length += sizeof(id);

    memcpy(&uhf_data->data[length], packet->data, packet->length);
    length += packet->length;

    int i = 0;
    printf("\ntx data_length: %d\n",packet->length);
    printf("\npacket_len:%d, packet: ",uhf_data->header.length);
    for(i=0;i<uhf_data->header.length;i++)
    {
        printf("%x ",frame_buff[i]);
    }
}

/**
 * * @brief This function is used for packing the UART data to send via UHF module
 * */
void uhf_unpack_uart_frame(csp_packet_t *packet, uint8_t *frame_buff)
{
    uint16_t length = 0;
    uint32_t id;
    s_comms_uhf_data *uhf_data = (s_comms_uhf_data *)frame_buff;

    length = uhf_data->header.length - UHF_HEADER_SIZE - CSP_HEADER_LENGTH;

    packet->length = length;

    length = 0;
    //if (!packet)
    //        //continue;

    /* Set packet ID */
    memcpy(&id, &uhf_data->data[length], sizeof(id));
    length += sizeof(id);
    packet->id.ext = csp_ntoh32(id);

    /* Copy received data into CSP buffer */
    memcpy(packet->data, &uhf_data->data[length], packet->length);

    int i = 0;
    printf("\nrx data_length: %d\n",packet->length);
    printf("\npacket_len:%d, packet: ",uhf_data->header.length);
    for(i=0;i<uhf_data->header.length;i++)
    {
        printf("%x ",frame_buff[i]);
    }
}

