/**
 * @file adcs_bsk_sim_intf.c
 *
 * @brief This file contains functions for sending ADCS data through socket
 *
 * @copyright Copyright 2023 Antaris, Inc.
 */

#include "adcs_va.h"
#include "exo_common.h"
#include "exo_osal.h"
#include "exo_osal_mem_management.h"
#include <arpa/inet.h>
#include <sys/socket.h>
#include <errno.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include "obc_main.h"

#include "adcs_vapi.h"
#include "api_req_resp_lib_common.h"
#include "api_req_common.h"
#include "api_resp_common.h"
#include "adcs_comm_interface.h"
#include "adcs_bsk_sim.h"

#define ADCS_BSK_PORT               (2364)   ///< Socket port number
#define ADCS_MAX_BUFF_SIZE          (300)    ///< ADCS_BSK_SERVER

#define ADCS_ETH_IP_ADDR0 (ETH_IP_ADDR0)       ///< Simulator IP0
#define ADCS_ETH_IP_ADDR1 (ETH_IP_ADDR1)       ///< Simulator IP1
#define ADCS_ETH_IP_ADDR2 (ETH_IP_ADDR2)       ///< Simulator IP2
#define ADCS_ETH_IP_ADDR3 (ETH_IP_ADDR3 + 1)   ///< Simulator IP3

/**
 * @brief  ADCS BSK socket related global variables
 */
volatile int32 adcs_bsk_server_fd = -1;
volatile int32 adcs_bsk_client_fd = -1;
volatile uint8 adcs_conn_bsk_status = -1;
s_obc_sock_info adcs_soc_inf;
s_api_req_frame adcs_api_req_orig;
s_api_resp_frame adcs_api_resp_orig;
uint8_t adcs_api_req_resp_data[ADCS_MAX_BUFF_SIZE] = {0};
uint8_t adcs_read_data[ADCS_MAX_BUFF_SIZE] = {0};
uint8_t timo_flag = 0;

#ifdef LINUX_TEMP_PORT
// ADCS interface error
uint8 adcs_interface_err = 0;
#endif

/**
 * @brief ADCS TCP server init api
 */
void adcs_tcp_server(void *param)
{
    // Variable declaration
    int server_socket, client_addr_len;
    struct sockaddr_in server_addr, client_addr;

    // Create a TCP socket
    if((server_socket = socket(AF_INET, SOCK_STREAM, 0)) == -1)
    {
        perror("socket");
        // exit(EXIT_FAILURE);
    }

    // assigning elements of server_addr to zero
    memset(&server_addr, 0, sizeof(server_addr));
    server_addr.sin_family = AF_INET;

    // IP address and port assignment
#ifndef LINUX_TEMP_PORT
    ip4_addr_t adcs_ip_address;
    server_addr.sin_port = htons(ADCS_BSK_PORT);
    IP4_ADDR(&adcs_ip_address,ADCS_ETH_IP_ADDR0,ADCS_ETH_IP_ADDR1,\
            ADCS_ETH_IP_ADDR2,ADCS_ETH_IP_ADDR3);
    server_addr.sin_addr.s_addr = adcs_ip_address.addr;
#else
    server_addr.sin_addr.s_addr = inet_addr(adcs_soc_inf.ip_addr);
    server_addr.sin_port = (adcs_soc_inf.port);
#endif

    // Bind the socket to a port
    if(bind(server_socket, (struct sockaddr *)&server_addr,
                sizeof(server_addr)) == -1)
    {
        perror("bind");
        // exit(EXIT_FAILURE);
    }

    // Listen for incoming connections
    if(listen(server_socket, 10) == -1)
    {
        perror("listen");
        // exit(EXIT_FAILURE);
    }

    DEBUG_CPRINT(("Server listening on port %d...\n", server_addr.sin_port));

    client_addr_len = sizeof(client_addr);
    while(1)
    {
        // Accept a new client connection
        if((adcs_bsk_server_fd =
                    accept(server_socket, (struct sockaddr *)&client_addr,
                        (socklen_t *)&client_addr_len)) == -1)
        {
            perror("accept");
        }

        DEBUG_CPRINT(("Accepted new connection from %s:%d\n",
                inet_ntoa(client_addr.sin_addr), ntohs(client_addr.sin_port)));
    }
}

/**
 * @brief ADCS socket init interface api
 */
int adcs_sock_init(void)
{

#ifdef ADCS_SERVER
    int status;

    status = os_thread_create(ADCS_SERVER_THREAD, T_ADCS_STACK_SIZE,
            P_ADCS_SERVER, "adcs_server_thread",
            adcs_tcp_server, NULL, NULL, NULL);

    return status;
#else
    // Variable declaration
    int status;
    struct sockaddr_in serv_addr;
    // Set timeout for receiving operations
    struct timeval timeout;
    /*
     * Timeout changed to 5s as per customer team request
     */
    timeout.tv_sec = 3;  // Timeout in seconds
    timeout.tv_usec = 0;  // Additional microseconds


    // Create a TCP socket
    if((adcs_bsk_client_fd = socket(AF_INET, SOCK_STREAM, 0)) < 0)
    {
        DEBUG_CPRINT(("ADCS_BSK_SOCK: Socket creation error \n"));
        return -1;
    }

    if (setsockopt(adcs_bsk_client_fd, SOL_SOCKET, SO_RCVTIMEO, \
                (char *)&timeout, sizeof(timeout)) < 0)
    {
        DEBUG_CPRINT(("ADCS_BSK_SOCK: Socket timeout configuration error\n"));
        return -1;
    }

    serv_addr.sin_family = AF_INET;

    // IP address and port assignment
#ifndef LINUX_TEMP_PORT
    ip4_addr_t adcs_ip_address;
    serv_addr.sin_port = htons(ADCS_BSK_PORT);
    IP4_ADDR(&adcs_ip_address,ADCS_ETH_IP_ADDR0,ADCS_ETH_IP_ADDR1,\
            ADCS_ETH_IP_ADDR2,ADCS_ETH_IP_ADDR3);
    serv_addr.sin_addr.s_addr = adcs_ip_address.addr;
#else
    serv_addr.sin_port = htons((adcs_soc_inf.port));
    if(inet_pton(AF_INET, adcs_soc_inf.ip_addr, &serv_addr.sin_addr) <= 0)
    {
        DEBUG_CPRINT(("ADCS_BSK_SOCK: Invalid address/" 
                    "Address not supported \n"));
        return -1;
    }
#endif

    // Connects to the client socket
    if((status = connect(adcs_bsk_client_fd, (struct sockaddr *)&serv_addr,
                    sizeof(serv_addr))) < 0)
    {
        DEBUG_CPRINT(("ADCS_BSK_SOCK: Connection Failed \n"));
        return -1;
    }
    else
    {
        adcs_conn_bsk_status = 1;
    }

    return status;
#endif
}

/**
 * @brief Function to request Telemetry from ADCS
 */
int8 adcs_requesttlm_bsk(uint8 tlmId, uint8* msgBuffer, uint16 len)
{
    // Variable declaration
    e_adcs_status status = OPERATION_FAILURE;
    int16_t send_status = -1;
    int16_t rcv_status = 0;
    int32_t frame_size = 0;
    uint8_t api_resp_id = 0;
    uint16_t api_resp_len = 0;

    // Copy socket fd to local variable
#ifdef ADCS_SERVER
    int adcs_sock_fd = adcs_bsk_server_fd;
#else
    int adcs_sock_fd = adcs_bsk_client_fd;
#endif

    if(timo_flag == 1) //check timeout occur or not
    {
        socket_flush(adcs_sock_fd);
    }
    /*
     * Function to Encode payload to data stream in basilisk
     * frame format
     */
    frame_size = api_req_encode(tlmId,NULL,frame_size,
            adcs_api_req_resp_data);
        /*
         * Verify connection status and frame packing status was true
         */
    if(1 == adcs_conn_bsk_status && 0 < frame_size)
    {
        if(check_socket_disconnected(adcs_sock_fd))
        {
            adcs_conn_bsk_status = 0;
            send_status = 0;
            close(adcs_sock_fd);
        }
        else
        {

            DEBUG_CPRINT(("\n\nAPI_Req Sent:- "));
            for(int i = 0;i<frame_size;i++)
                DEBUG_CPRINT(("%x ",adcs_api_req_resp_data[i]));
            DEBUG_CPRINT(("\n"));
            /*
             * Socket send function for transmit data from Controller
             * SW to basilisk Simulator
             */
            send_status = send(adcs_sock_fd, adcs_api_req_resp_data, frame_size,0);

            // Verifying transmitted length and actual frame length was same
            if(frame_size != send_status)
            {
                if(send_status == 0)    // Socket Disconnected
                {
                    adcs_conn_bsk_status = 0;
                    DEBUG_CPRINT(("ADCS_BSK_SOCK: Socket Disconnected\n"));
                }
                else                    // Socket Disconnected with error
                {
                    DEBUG_CPRINT(("ADCS_BSK_SOCK: socket send ERROR %d errno %d\n",
                                send_status, errno));
                }
            }
            else
            {
                // Socket Read function to receive data from Basilisk simulator
                rcv_status = read(adcs_sock_fd, adcs_read_data,ADCS_MAX_BUFF_SIZE);

                DEBUG_CPRINT(("\nAPI_Resp Received:- "));
                for(int i=0;i<rcv_status;i++)
                    DEBUG_CPRINT(("%x ",adcs_read_data[i]));
                DEBUG_CPRINT(("\n\n"));
                /*
                 * Verifying valid response received
                 */
                if(-1 == rcv_status)
                {
                    DEBUG_CPRINT(("ADCS_BSK_SOCK: socket Timeout"
                                "error %d errno %d \n",rcv_status, errno));
                    timo_flag = 1;
                    status = IO_INTERFACE_ERR;
                }
                else if(0 < rcv_status)
                {
                    /*
                     * Function to decode the basilisk format data stream to payload
                     */
                    frame_size = api_resp_decode(adcs_read_data,rcv_status,\
                            msgBuffer,&api_resp_id,&api_resp_len);

                    if(tlmId == api_resp_id)
                    {
                        timo_flag = 0;
                        status = OPERATION_SUCCESS;
                    }
                }
                else    // Socket Disconnected with error
                {
                    DEBUG_CPRINT(("ADCS_BSK_SOCK: socket read error %d errno %d\n",
                                rcv_status, errno));
                    adcs_conn_bsk_status = 0;
                }
            }
        }
    }
    else    // Connection Status or frame size invalid
    {
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wformat"
        DEBUG_CPRINT(("ADCS_BSK_SOCK: Socket Send Failed Connection Status:%hu"
                    " Frame Size : %ld\n",adcs_conn_bsk_status,frame_size));
#pragma GCC diagnostic pop
    }

    return status;
}

#ifdef LINUX_TEMP_PORT
#ifdef ADCS_HW_SIM
/**
 * @brief Function to send telecommand to ADCS
 */
int8 adcs_send_cmd(uint8 tcId, uint8* msgBuffer, uint16 len)
{
    int8_t status;
    status = adcs_sendtc_bsk(tcId, msgBuffer, len);
    return status;
}

/**
 * @brief Function to request Telemetry from ADCS
 */
int8 adcs_requesttlm(uint8 tlmId, uint8 *msgBuffer, uint16 len)
{
    int8 status;
    status = adcs_requesttlm_bsk(tlmId, msgBuffer, len);
    return status;
}
#endif
#endif

/**
 * @brief Function to send telecommand to ADCS through socket interface
 */
int8 adcs_sendtc_bsk(uint8 tc_id, uint8 *tx_buffer, uint16 tx_size)
{
    // Variable declaration
    int8_t status = -1;
    int16_t send_status = -1;
    int16_t rcv_status = 0;
    int32_t req_frame_size = 0;
    int32_t unpack_status = 0;
    uint8_t api_resp_id = 0;
    uint16_t api_resp_len = 0;

    // Copy socket fd to local variable
#ifdef ADCS_SERVER
    int adcs_sock_fd = adcs_bsk_server_fd;
#else
    int adcs_sock_fd = adcs_bsk_client_fd;
#endif

    if((NULL != tx_buffer)) // Verify valid payload
    {
        if(timo_flag == 1) //check timeout occur or not
        {
            socket_flush(adcs_sock_fd);
        }
        // Initialize to zero
        memset(adcs_api_req_resp_data,0,ADCS_MAX_BUFF_SIZE);

        /*
         * Packing payload data in basilisk frame format
         */
        req_frame_size = api_req_encode(tc_id,tx_buffer,tx_size,
                adcs_api_req_resp_data);
        if(1 == adcs_conn_bsk_status && 0 < req_frame_size)
        {
            if(check_socket_disconnected(adcs_sock_fd))
            {
                adcs_conn_bsk_status = 0;
                close(adcs_sock_fd);
            }
            else
            {
                DEBUG_CPRINT(("\n\nAPI_Req Sent:- "));
                for(int i = 0;i<req_frame_size;i++)
                    DEBUG_CPRINT(("%x ",adcs_api_req_resp_data[i]));
                DEBUG_CPRINT(("\n"));

                /*
                 * Socket send function for transmit data from Controller 
                 * SW to basilisk Simulator
                 */
                send_status = send(adcs_sock_fd, adcs_api_req_resp_data,
                        req_frame_size, 0);

                // Verifying transmitted length and actual frame length was same
                if(req_frame_size != send_status) 
                {
                    if(send_status == 0)    // Socket disconnected
                    {
                        adcs_conn_bsk_status = 0;
                        DEBUG_CPRINT(("ADCS_BSK_SOCK: Server Disconnected\n"));
                    }
                    else                    // Socket disconnected with error
                    {
                        DEBUG_CPRINT(("ADCS_BSK_SOCK: socket send "
                                    "ERROR %d errno %d \n", send_status,errno));
                    }
                }
                else
                {
                    // initialize to zero
                    os_memset(adcs_read_data,0,ADCS_MAX_BUFF_SIZE);

                    /*
                     * Socket read function for receive data from
                     * basilisk Simulator
                     */
                    rcv_status = read(adcs_sock_fd, adcs_read_data,
                            ADCS_MAX_BUFF_SIZE);

                    // Verifying valid packet received
                    if((0 < rcv_status))
                    {
                        DEBUG_CPRINT(("\nAPI_Resp Received:- "));
                        for(int i=0;i<rcv_status;i++)
                            DEBUG_CPRINT(("%x ",adcs_read_data[i]));
                        DEBUG_CPRINT(("\n\n"));

                        /*
                         * Function to decode the basilisk format data 
                         * stream to payload
                         */
                        unpack_status = api_resp_decode(adcs_read_data,rcv_status,
                                adcs_api_req_resp_data,&api_resp_id,&api_resp_len);
                        /*
                         * Verifying valid response received
                         */
                        if(unpack_status > 0 && api_resp_id == API_RESP_OK)
                        {
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wformat"
                            DEBUG_CPRINT(("RX UnPack Success:- %ld\n"
                                        ,unpack_status));
                            timo_flag = 0;
                            status = OPERATION_SUCCESS;
#pragma GCC diagnostic pop
                        }
                        else    // length or response id not valid
                        {
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wformat"
                            DEBUG_CPRINT(("API Resp UnPack Fail frame len:-"
                                        " %ld Resp ID:-"
                                        " %hu Resp error status %hu\n",unpack_status,\
                                        api_resp_id,adcs_api_req_resp_data[0]));
#pragma GCC diagnostic pop
                        }
                    }
                    else if(-1 == rcv_status)
                    {
                        DEBUG_CPRINT(("ADCS_BSK_SOCK: socket Timeout"
                                    "error %d errno %d \n",rcv_status, errno));
                        timo_flag = 1;
                        status = IO_INTERFACE_ERR;
                    }
                    else    // Socket disconnected
                    {
                        DEBUG_CPRINT(("ADCS_BSK_SOCK: socket read error"
                                    "%d errno %d \n",rcv_status, errno));
                        adcs_conn_bsk_status = 0;
                    }
                }
            }
        }
        else    // Connection Status or frame size invalid
        {
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wformat"
            DEBUG_CPRINT(("ADCS_BSK_SOCK: Socket Send Failed "
                        "Connection Status : %d "
                        "Frame Size : %ld\n",adcs_conn_bsk_status,req_frame_size));
#pragma GCC diagnostic pop
        }
    }
    return status;
}

/**
 * @brief This function handle the socket timeout
 */
void socket_flush(int adcs_sock_fd)
{
    int16_t rcv_status = 0;
    struct sockaddr addr;
    uint32_t len = 0;
    uint8_t i = 0;
    while(i<3)
    {
        rcv_status = recvfrom(adcs_sock_fd, adcs_read_data, ADCS_MAX_BUFF_SIZE,\
                MSG_DONTWAIT, (struct sockaddr *)&addr, &len);
#if 0
        DEBUG_CPRINT(("\nAfter timeout API_Resp Received:- "));
        for(int i=0;i<rcv_status;i++)
            DEBUG_CPRINT(("%x ",adcs_read_data[i]));
        DEBUG_CPRINT(("\n\n"));
        DEBUG_CPRINT(("\n#####socket flushed###### :- %d\n", rcv_status));
#endif
        if(-1 == rcv_status)
        {
            DEBUG_CPRINT(("ADCS_BSK_SOCK: After Recv socket Timeout"
                        "error %d errno %d \n",rcv_status, errno));
            os_delay(200);
        }
        else
        {
            timo_flag = 0;
            memset(&adcs_read_data,0,ADCS_MAX_BUFF_SIZE);
            break;
        }
        i++;
    }
}

/**
 * @brief Function to request Telemetry from ADCS
 */
int8 adcs_requesttlm_wt_data(uint8* tc_data, uint16 tc_len,\
       uint8* msgBuffer, uint16 len)
{
    // Variable declaration
    e_adcs_status status = OPERATION_FAILURE;
    int16_t send_status = -1;
    int16_t rcv_status = 0;
    int32_t frame_size = 0;
    uint8_t api_resp_id = 0;
    uint16_t api_resp_len = 0;

    // Copy socket fd to local variable
#ifdef ADCS_SERVER
    int adcs_sock_fd = adcs_bsk_server_fd;
#else
    int adcs_sock_fd = adcs_bsk_client_fd;
#endif

    if(timo_flag == 1) //check timeout occur or not
    {
        socket_flush(adcs_sock_fd);
    }
    /*
     * Function to Encode payload to data stream in basilisk
     * frame format
     */
    frame_size = api_req_encode(tc_data[0], &tc_data[1], tc_len - 1,
            adcs_api_req_resp_data);

    /*
     * Verify connection status and frame packing status was true
     */
    if(1 == adcs_conn_bsk_status && 0 < frame_size)
    {
        DEBUG_CPRINT(("\n\nAPI_Req Sent:- "));
        for(int i = 0;i<frame_size;i++)
            DEBUG_CPRINT(("%x ",adcs_api_req_resp_data[i]));
        DEBUG_CPRINT(("\n"));
        /*
         * Socket send function for transmit data from Controller
         * SW to basilisk Simulator
         */
        send_status = send(adcs_sock_fd, adcs_api_req_resp_data, frame_size,0);

        // Verifying transmitted length and actual frame length was same
        if(frame_size != send_status)
        {
            if(send_status == 0)    // Socket Disconnected
            {
                adcs_conn_bsk_status = 0;
                DEBUG_CPRINT(("ADCS_BSK_SOCK: Socket Disconnected\n"));
            }
            else                    // Socket Disconnected with error
            {
                DEBUG_CPRINT(("ADCS_BSK_SOCK: socket send ERROR %d errno %d\n",
                            send_status, errno));
            }
        }
        else
        {
            // Socket Read function to receive data from Basilisk simulator
            rcv_status = read(adcs_sock_fd, adcs_read_data,ADCS_MAX_BUFF_SIZE);

            DEBUG_CPRINT(("\nAPI_Resp Received:- "));
            for(int i=0;i<rcv_status;i++)
                DEBUG_CPRINT(("%x ",adcs_read_data[i]));
            DEBUG_CPRINT(("\n\n"));
            /*
             * Verifying valid response received
             */
            if(-1 == rcv_status)
            {
                DEBUG_CPRINT(("ADCS_BSK_SOCK: socket Timeout"
                            "error %d errno %d \n",rcv_status, errno));
                timo_flag = 1;
                status = IO_INTERFACE_ERR;
            }
            else if(0 < rcv_status)
            {
                /*
                 * Function to decode the basilisk format data stream to payload
                 */
                frame_size = api_resp_decode(adcs_read_data,rcv_status,\
                        adcs_api_req_resp_data,&api_resp_id,&api_resp_len);

                if(tc_data[0] == api_resp_id)
                {
                    timo_flag = 0;
                    status = OPERATION_SUCCESS;
                    os_memcpy(msgBuffer, adcs_api_req_resp_data, api_resp_len); 
                }
                else
                {
                    status = OPERATION_FAILURE;
                    timo_flag = 0;
                }
            }
            else    // Socket Disconnected with error
            {
                DEBUG_CPRINT(("ADCS_BSK_SOCK: socket read error %d errno %d\n",
                            rcv_status, errno));
                adcs_conn_bsk_status = 0;
            }
        }
    }
    else    // Connection Status or frame size invalid
    {
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wformat"
        DEBUG_CPRINT(("ADCS_BSK_SOCK: Socket Send Failed Connection Status:%hu"
                    " Frame Size : %ld\n",adcs_conn_bsk_status,frame_size));
#pragma GCC diagnostic pop
    }

    return status;
}

