/*
 * sockettasks.h
 *
 *  Created on: 3 maj 2024
 *      Author: tobjo
 */

#ifndef SRC_SOCKETTASKS_H_
#define SRC_SOCKETTASKS_H_

#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"

/* lwIP core includes */
#include "lwip/opt.h"
#include "lwip/sys.h"
#include "lwip/tcpip.h"
#include "lwip/dhcp.h"
#include "lwip/sockets.h" // Include lwIP sockets API

#include <kernel/dpl/TaskP.h>
#include <kernel/dpl/ClockP.h>
#include <kernel/dpl/ClockP.h>
#include <networking/enet/utils/include/enet_apputils.h>
#include <networking/enet/utils/include/enet_board.h>
#include "ti_board_config.h"
#include "ti_board_open_close.h"
#include "ti_drivers_open_close.h"
#include "ti_enet_config.h"
#include "ti_enet_open_close.h"
#include "app_cpswconfighandler.h"
#include "app_socket.h"
#include "ti_enet_lwipif.h"

#include "event_groups.h"

#include "can_message.h"

EventGroupHandle_t xSocketSetupComplete;

#define BIT_0 ( 1 << 0 )

/* ========================================================================== */
/*                           Macros & Typedefs                                */
/* ========================================================================== */

static const uint8_t BROADCAST_MAC_ADDRESS[ENET_MAC_ADDR_LEN] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };

#define SOCK_HOST_SERVER_PORT  (8888)
#define APP_SOCKET_MAX_RX_DATA_LEN (1024U)
#define APP_SOCKET_NUM_ITERATIONS (5U)
#define MAX_IPV4_STRING_LEN (16U)

/* ========================================================================== */
/*                         Structure Declarations                             */
/* ========================================================================== */

struct App_hostInfo_t
{
    struct sockaddr_in socketAddr;
};

/* ========================================================================== */
/*                            Global Variables                                */
/* ========================================================================== */

static uint8_t gRxDataBuff[APP_SOCKET_MAX_RX_DATA_LEN];

static struct App_hostInfo_t gHostInfo;

//static char gHostServerIp4[MAX_IPV4_STRING_LEN] = "192.168.68.68";
static char gHostServerIp4[MAX_IPV4_STRING_LEN] = "172.16.27.130";

/* dhcp struct for the ethernet netif */
static struct dhcp g_netifDhcp[ENET_SYSCFG_NETIF_COUNT];
struct netif *g_pNetif[ENET_SYSCFG_NETIF_COUNT];

/* Handle to the Application interface for the LwIPIf Layer
 */
LwipifEnetApp_Handle hlwipIfApp = NULL;

QueueHandle_t *xDataSendQueueCanFramesReceived;
QueueHandle_t *xDataSendQueueCanFramesToSend;

int server_socket;

static int32_t App_isNetworkUp(struct netif* netif_)
{
    return (netif_is_up(netif_) && netif_is_link_up(netif_) && !ip4_addr_isany_val(*netif_ip4_addr(netif_)));
}

static void App_netifLinkChangeCb(struct netif *pNetif)
{
    if (netif_is_link_up(pNetif))
    {
        DebugP_logInfo("Network Link UP Event\r\n");
    }
    else
    {
        DebugP_logWarn("Network Link DOWN Event\r\n");
    }
    return;
}

static void App_netifStatusChangeCb(struct netif *pNetif)
{
    if (netif_is_up(pNetif))
    {
        DebugP_logInfo("Enet IF UP Event. Local interface IP:%s\r\n",
                    ip4addr_ntoa(netif_ip4_addr(pNetif)));
    }
    else
    {
        DebugP_logWarn("Enet IF DOWN Event\r\n");
    }
    return;
}

static void App_setupNetif()
{
    ip4_addr_t ipaddr, netmask, gw;

    ip4_addr_set_zero(&gw);
    ip4_addr_set_zero(&ipaddr);
    ip4_addr_set_zero(&netmask);

    DebugP_logInfo("Starting lwIP, local interface IP is dhcp-enabled\r\n");
    hlwipIfApp = LwipifEnetApp_getHandle();
    for (uint32_t i = 0U; i < ENET_SYSCFG_NETIF_COUNT; i++)
    {
        /* Open the netif and get it populated*/
        g_pNetif[i] = LwipifEnetApp_netifOpen(hlwipIfApp, NETIF_INST_ID0 + i, &ipaddr, &netmask, &gw);
        netif_set_status_callback(g_pNetif[i], App_netifStatusChangeCb);
        netif_set_link_callback(g_pNetif[i], App_netifLinkChangeCb);
        netif_set_up(g_pNetif[NETIF_INST_ID0 + i]);
    }
    LwipifEnetApp_startSchedule(hlwipIfApp, g_pNetif[ENET_SYSCFG_DEFAULT_NETIF_IDX]);
}

static void App_allocateIPAddress()
{
    sys_lock_tcpip_core();
    for (uint32_t  i = 0U; i < ENET_SYSCFG_NETIF_COUNT; i++)
    {
        dhcp_set_struct(g_pNetif[NETIF_INST_ID0 + i], &g_netifDhcp[NETIF_INST_ID0 + i]);

        const err_t err = dhcp_start(g_pNetif[NETIF_INST_ID0 + i]);
        EnetAppUtils_assert(err == ERR_OK);
    }
    sys_unlock_tcpip_core();
    return;
}

static void App_tcpipInitCompleteCb(void *pArg)
{
    sys_sem_t *pSem = (sys_sem_t*)pArg;
    EnetAppUtils_assert(pArg != NULL);

    /* init randomizer again (seed per thread) */
    srand((unsigned int)sys_now()/1000);

    App_setupNetif();

    App_allocateIPAddress();

    sys_sem_signal(pSem);
}

void Socket_SendMessage(can_message_t *frame) {
    // Send data
    // Assume client_socket is the socket connected to the client
    char pcData[CAN_PACKET_SIZE_8_DATA];

    // Header
    pcData[0] = CAN_HEADER_1;
    pcData[1] = CAN_HEADER_2;

    // Id - needs to be shifted 18 bits to the left
    DebugP_logInfo("[vSocketTask] ID: 0x%x\r\n", frame->id >> 18);
    pcData[2] = (frame->id >> (18 + 8)) & 0x07;
    pcData[3] = (frame->id >> 18) & 0xFF;

    // DLC
    pcData[4] = frame->dlc & 0xFF;

    // Data
    for(int i = 0; i < frame->dlc; i++)
    {
        pcData[5 + i] = frame->data[i];
    }

    DebugP_logInfo("[vSocketTask] Sending packet\r\n");
    lwip_send(server_socket, pcData, sizeof(char) * (CAN_PACKET_SIZE_8_DATA), 0);
}

bool Socket_CheckForNewMessage(can_message_t *frame) {
    int32_t ret = 0;
    bool new_message_received = false;

    if (frame == NULL) {
        return false; // Indicate failure if the provided pointer is NULL
    }

    ret = lwip_read(server_socket, gRxDataBuff, CAN_PACKET_SIZE_8_DATA);

    if(ret > 0) {
        DebugP_logInfo("[vSocketTask] Received packet\r\n");
        gRxDataBuff[ret] = '\0';

        // Handle input
        if(gRxDataBuff[0] == CAN_HEADER_1 && gRxDataBuff[1] == CAN_HEADER_2)
        {
            int id = (gRxDataBuff[2] << 4) + gRxDataBuff[3];
            frame->id = id;

            int dlc = gRxDataBuff[4];
            frame->dlc = dlc;

            for(int i = 0; i < dlc; i++)
            {
                frame->data[i] = gRxDataBuff[5 + i];
            }

            new_message_received = true;
        } else {
            DebugP_log("NO HEADER FOUND: ");
            for(int j = 0; j < 15; j++)
            {
                DebugP_log("%x:", gRxDataBuff[j]);
            }
            DebugP_log(" (bytes rec: %d)\r\n", ret);
        }
    } else if(ret == -3) { // ETIMEDOUT
        DebugP_logWarn("[vSocketTask] ETIMEDOUT\r\n");
        // Nothing received - ignore and start over
    }
    else if(ret == -1) { // EAGAIN
        //DebugP_logWarn("[vSocketTask] EAGAIN\r\n");
        // Nothing received - ignore and start over
    }
    else if (ret < 0) {
        // Error
        //ret = lwip_close(server_socket);
        DebugP_logError("[vSocketTask] ERR: socket read failed\r\n");
    }

    for(int i = 0; i < APP_SOCKET_MAX_RX_DATA_LEN; i++) {
        gRxDataBuff[i] = 0;
    }

    return new_message_received;
}

void vSocketTask(void *pvParamters) {
    can_message_t receivedFrame;

    xEventGroupWaitBits(xSocketSetupComplete, BIT_0, pdTRUE, pdFALSE, portMAX_DELAY);

    while(1) {
        // 1. Check if xDataSendQueueCanFramesToSend has any packets to send
        //      a. If it does, send packets/clear queue
        // 2. Check if any new packets are available
        //      a. If there are, parse and add to xDataSendQueueCanFramesReceived

        // 1.
        while (xQueueReceive(*xDataSendQueueCanFramesToSend, &receivedFrame, (TickType_t)0) == pdTRUE) {
            DebugP_logInfo("[vSocketTask] Received from Queue\r\n");

            // a.
            Socket_SendMessage(&receivedFrame);
        }

        // 2.
        if(Socket_CheckForNewMessage(&receivedFrame)) {
            DebugP_logInfo("[vSocketTask] Sending to Queue\r\n");
            xQueueSend(*xDataSendQueueCanFramesReceived, &receivedFrame, (TickType_t) 0);
        }
    }
}

void initNetworkInterface()
{
    Enet_Type enetType;
    uint32_t instId;
    sys_sem_t pInitSem;
    const err_t err = sys_sem_new(&pInitSem, 0);
    EnetAppUtils_assert(err == ERR_OK);

    DebugP_logInfo("[SocketInit] Begin\r\n");

    EnetApp_getEnetInstInfo(CONFIG_ENET_CPSW0, &enetType, &instId);
    EnetAppUtils_enableClocks(enetType, instId);
    EnetApp_driverInit();

    const int32_t status = EnetApp_driverOpen(enetType, instId);
    if (ENET_SOK != status)
    {
        DebugP_logError("[SocketInit] Failed to open ENET: %d\r\n", status);
        EnetAppUtils_assert(false);
        return;
    }

    EnetApp_addMCastEntry(enetType,
                          instId,
                          EnetSoc_getCoreId(),
                          BROADCAST_MAC_ADDRESS,
                          CPSW_ALE_ALL_PORTS_MASK);

    tcpip_init(App_tcpipInitCompleteCb, &pInitSem);

    /* wait for TCP/IP initialization to complete */
    sys_sem_wait(&pInitSem);
    sys_sem_free(&pInitSem);

    while (!App_isNetworkUp(netif_default))
    {
        DebugP_logInfo("[SocketInit] Waiting for network UP ...\r\n");
        ClockP_sleep(2);
    }

    DebugP_logInfo("[SocketInit] Network is UP ...\r\n");
    ClockP_sleep(1);
}

int connect_socket_with_retry(int *socket_fd, const struct sockaddr *address, socklen_t address_len)
{
    int retries = 0;
    const int max_retries = 100;
    int result;
    const int retry_delay_ms = 2000;
    struct timeval opt = {0};

    do {
        // Close the socket if it's already open
        if (*socket_fd >= 0) {
            DebugP_logInfo("[SocketInit] Socket already opened, closing...\r\n");
            lwip_close(*socket_fd);
        }

        // Create a new socket
        DebugP_logInfo("[SocketInit] Creating new socket...\r\n");
        *socket_fd = lwip_socket(AF_INET, SOCK_STREAM, 0);
        if (*socket_fd < 0) {
            // Handle socket creation error
            return -1;
        }

        /* set recv timeout (1s) */
        opt.tv_sec = 1;
        opt.tv_usec = 0;
        result = lwip_setsockopt(server_socket, SOL_SOCKET, SO_RCVTIMEO, &opt, sizeof(opt));
        if(result != 0) {
            DebugP_logWarn("[SocketInit] Set sockopt failed\r\n");
        }

        // Attempt to connect
        result = lwip_connect(*socket_fd, address, address_len);

        // Check if the connection was successful
        if (result == 0) {
            DebugP_logInfo("[SocketInit] Connection successful...\r\n");
            return 0; // Connection successful
        }

        // If not successful, wait for a short time before retrying

        DebugP_logWarn("[SocketInit] Connection failed, retrying in %d ms...\r\n", retry_delay_ms);
        vTaskDelay(pdMS_TO_TICKS(retry_delay_ms));

        retries++;
    } while (retries < max_retries);

    // Return an error if all retries failed
    return -1;
}

void initSocket(void *pvParameters)
{
    struct sockaddr* pAddr = (struct sockaddr *)&gHostInfo.socketAddr;
    ip_addr_t ipAddr;
    int32_t addr_ok = 0;

    initNetworkInterface();
    // Network interface is up and active here

    // If you want the option to input server IP address, uncomment below
    //    DebugP_log(" UDP socket Menu: \r\n");
    //    do
    //    {
    //        DebugP_log("[SocketInit]  Enter server IPv4 address:(example: 192.168.101.100)\r\n");
    //        DebugP_scanf("%s", gHostServerIp4);
    //        addr_ok = ip4addr_aton(gHostServerIp4, ip_2_ip4(&ipAddr));
    //        TaskP_yield();
    //    } while (addr_ok != 1);

    addr_ok = ip4addr_aton(gHostServerIp4, ip_2_ip4(&ipAddr));

    memset(&gHostInfo.socketAddr, 0, sizeof(gHostInfo.socketAddr));

    struct sockaddr_in*  pAddrIn = &gHostInfo.socketAddr;
    IP_SET_TYPE_VAL(dstaddr, IPADDR_TYPE_V4);
    addr_ok = ip4addr_aton(gHostServerIp4, ip_2_ip4(&ipAddr));
    pAddrIn->sin_len = sizeof(gHostInfo.socketAddr);
    pAddrIn->sin_family = AF_INET;
    pAddrIn->sin_port = PP_HTONS(SOCK_HOST_SERVER_PORT);
    inet_addr_from_ip4addr(&pAddrIn->sin_addr, ip_2_ip4(&ipAddr));
    EnetAppUtils_assert(addr_ok);

    DebugP_logInfo("[SocketInit]  Connecting to: %s:%d \r\n", gHostServerIp4, SOCK_HOST_SERVER_PORT);
    if(connect_socket_with_retry(&server_socket, pAddr, pAddr->sa_len) == 0)
    {
        DebugP_logInfo("[SocketInit] - Connected\r\n");
    }
    else
    {
        DebugP_logError("[SocketInit] - Could not connect\r\n");
    }

    DebugP_logInfo("[SocketInit] - Socket Setup Complete\r\n");
    xEventGroupSetBits(xSocketSetupComplete, BIT_0);
    vTaskDelete(NULL);
}

#define SOCKET_TASK_SIZE (16384U/sizeof(configSTACK_DEPTH_TYPE))
StackType_t gSocketInitTaskStack[SOCKET_TASK_SIZE] __attribute__((aligned(32)));
StackType_t gSocketTaskStack[SOCKET_TASK_SIZE] __attribute__((aligned(32)));

void vSocketTasks(QueueHandle_t *frameQueueIn, QueueHandle_t *frameQueueOut) {

    static const char SOCKET_INIT_TASK_NAME[] = "SocketInit";
    static const char SOCKET_TASK_NAME[] = "SocketTask";

    static StaticTask_t SocketInitTaskHandle;
    static StaticTask_t SocketTaskHandle;

    xDataSendQueueCanFramesReceived = frameQueueOut;
    xDataSendQueueCanFramesToSend = frameQueueIn;

    xSocketSetupComplete = xEventGroupCreate();
    xEventGroupClearBits(xSocketSetupComplete, BIT_0);

    xTaskCreateStatic(
        initSocket,
        SOCKET_INIT_TASK_NAME,
        SOCKET_TASK_SIZE,
        NULL,
        tskIDLE_PRIORITY + 2,
        gSocketInitTaskStack,
        &SocketInitTaskHandle);

    xTaskCreateStatic(
        vSocketTask,
        SOCKET_TASK_NAME,
        SOCKET_TASK_SIZE,
        NULL,
        tskIDLE_PRIORITY + 1,
        gSocketTaskStack,
        &SocketTaskHandle);
}

#endif /* SRC_SOCKETTASKS_H_ */
