// ******************************************************************************
//
//
//  Version/Date    Comment
//   2026.05.28     - First release
// ******************************************************************************

#include <stdbool.h>
#include <ustdlib.h>

#include "FreeRTOS.h"
#include "task.h"
#include "projdefs.h"

// For resetting the device
#include "rom.h"
#include "rom_map.h"

#ifdef FREERTOS_PLUS_TCP
  #include "FreeRTOS_IP.h"
  #include "FreeRTOS_Sockets.h"
#endif

#ifdef FREERTOS_PLUS_LWIP
  #include "include/lwiplib.h"
#endif

#include "TaskLocatorServer.h"


#ifdef FREERTOS_PLUS_TCP
  #ifndef configLOCATORSERVER_TX_WINDOW_SIZE
    #define configLOCATORSERVER_TX_WINDOW_SIZE    2
  #endif
  #ifndef configLOCATORSERVER_RX_WINDOW_SIZE
    #define configLOCATORSERVER_RX_WINDOW_SIZE    2
  #endif
#endif


// Stores the stack size
static uint16_t TaskLocatorServer_UsedStackSize = 0;


// ------------------------------------------------------------------------------
// These defines are used to describe the device locator protocol
// ------------------------------------------------------------------------------
#define TAG_CMD                 0xff
#define TAG_STATUS              0xfe
#define CMD_DISCOVER_TARGET     0x02
#define CMD_DISCOVER_RESET      0x03    // Global reset

// ------------------------------------------------------------------------------
// An array that contains the device locator response data.  The format of the
// data is as follows:
//
//     Byte        Description
//     --------    ------------------------
//      0          TAG_STATUS
//      1          packet length
//      2          CMD_DISCOVER_TARGET
//      3          board type
//      4          board ID
//      5..8       client IP address
//      9..14      MAC address
//     15..18      firmware version
//     19..82      application title
//     83          checksum
// ------------------------------------------------------------------------------
static uint8_t GLocatorData[84];


// The maximum time to wait for a closing socket to close
#define TASKLOCATORSERVER_SHUTDOWN_DELAY    ( pdMS_TO_TICKS( 5000 ) )

// The port number
#define TASKLOCATORSERVER_PORT_NUMBER     23

#define TASKLOCATORSERVER_DEVICENAME      "DeviceName"


// Forwards
static void     TaskLocatorServer_ConnectionListeningTask( void * pvParameters );
static void     TaskLocatorServer_ConnectionInstanceTask ( void * pvParameters );
       uint32_t TaskLocatorServer_ProcessDataReceived    ( uint8_t *ABuffer, uint32_t ADataLength );


// ------------------------------------------------------------------------------
//  Descript: Start the locator server (listener) task
//  Params  : <AStackSize>  Stack size
//  Params  : <APriority>   Priority
//  Returns : -
// ------------------------------------------------------------------------------
void TaskLocatorServer_StartLocatorTasks( size_t AStackSize, UBaseType_t APriority )
{
  configPRINTF( ( "LocatorServer task init\n" ) );

  if (xTaskCreate(
        TaskLocatorServer_ConnectionListeningTask,  // The function that implements the task
        "LocatorServerListener",                    // Just a text name for the task to aid debugging
        AStackSize,                                 // The stack size
        NULL,                                       // The task parameter, not used in this case
        APriority,                                  // The priority assigned to the task
        NULL ) != pdPASS)
  {
    configPRINTF( ( "LocatorServer task init task failure\n" ) );
    while (1) {}
  }

  // Remember the requested stack size so it can be re-used by the server listening task when it creates tasks to handle connections
  TaskLocatorServer_UsedStackSize = AStackSize;
}


// ------------------------------------------------------------------------------
//  Descript: Listener task (starts instance if a client connects)
//  Params  : <AParameters>  Parameters
//  Returns : -
// ------------------------------------------------------------------------------
static void TaskLocatorServer_ConnectionListeningTask( void * AParameters )
{
#ifdef FREERTOS_PLUS_TCP
  struct freertos_sockaddr LBindAddress;
         Socket_t          LListeningSocket;
#endif
#ifdef FREERTOS_PLUS_LWIP
  struct sockaddr_in LBindAddress;
         int         LListeningSocket;
  struct timeval     LTimeout;
#endif
  static const TickType_t LReceiveTimeOut = portMAX_DELAY;

  configPRINTF( ( "LocatorServer listener task started\n" ) );

#ifdef FREERTOS_PLUS_TCP
  #if ( ipconfigUSE_TCP_WIN == 1 )
    WinProperties_t xWinProps;

    // Fill in the buffer and window sizes that will be used by the socket
    xWinProps.lTxBufSize = ipconfigTCP_TX_BUFFER_LENGTH;
    xWinProps.lTxWinSize = configLOCATORSERVER_TX_WINDOW_SIZE;
    xWinProps.lRxBufSize = ipconfigTCP_RX_BUFFER_LENGTH;
    xWinProps.lRxWinSize = configLOCATORSERVER_RX_WINDOW_SIZE;
  #endif
#endif

  // Just to prevent compiler warnings
  ( void ) AParameters;

  // Attempt to open the socket (UDP)
#ifdef FREERTOS_PLUS_TCP
  LListeningSocket = FreeRTOS_socket( FREERTOS_AF_INET, FREERTOS_SOCK_DGRAM, FREERTOS_IPPROTO_UDP );
  configASSERT( LListeningSocket != FREERTOS_INVALID_SOCKET );
  // Set a time out so accept() will just wait for a connection
  FreeRTOS_setsockopt( LListeningSocket, 0, FREERTOS_SO_RCVTIMEO, &LReceiveTimeOut, sizeof( LReceiveTimeOut ) );
  // Set the window and buffer sizes
  #if ( ipconfigUSE_TCP_WIN == 1 )
    FreeRTOS_setsockopt( LListeningSocket, 0, FREERTOS_SO_WIN_PROPERTIES, ( void * ) &xWinProps, sizeof( xWinProps ) );
  #endif
#endif

#ifdef FREERTOS_PLUS_LWIP
  LListeningSocket = lwip_socket(AF_INET, SOCK_DGRAM, 0);
  // Set a time out so accept() will just wait for a connection
  LTimeout.tv_sec = LReceiveTimeOut;
  LTimeout.tv_usec = 0;
  lwip_setsockopt( LListeningSocket, SOL_SOCKET, SO_RCVTIMEO, &LTimeout, sizeof( LTimeout ) );
#endif

  // Bind the socket to the port that the client task will send to, then listen for incoming connections
  LBindAddress.sin_port = TASKLOCATORSERVER_PORT_NUMBER;
#ifdef FREERTOS_PLUS_TCP
  LBindAddress.sin_port = FreeRTOS_htons( LBindAddress.sin_port );
  LBindAddress.sin_family = FREERTOS_AF_INET;
  FreeRTOS_bind  ( LListeningSocket, &LBindAddress, sizeof( LBindAddress ) );
#endif
#ifdef FREERTOS_PLUS_LWIP
  LBindAddress.sin_addr.s_addr = INADDR_ANY;
  LBindAddress.sin_port = htons( LBindAddress.sin_port );
  LBindAddress.sin_family = AF_INET;
  lwip_bind  (LListeningSocket, (struct sockaddr *)&LBindAddress, sizeof(LBindAddress));
#endif

  // Since UDP is connectionless, emulate as if we listened and accepted a connection (as would be with TCP)
  if (xTaskCreate(
    TaskLocatorServer_ConnectionInstanceTask, // The function that implements the task
    "LocatorServer",                          // Just a text name for the task to aid debugging
    TaskLocatorServer_UsedStackSize,          // The stack size
    (void *) LListeningSocket,                // The task parameter
    tskIDLE_PRIORITY,                         // The priority assigned to the task
    NULL ) != pdPASS)
  {
    configPRINTF( ( "LocatorServer listener instance task failure\n" ) );
    while (1) {}
  }
  // This task is done (TaskLocatorServer_ConnectionInstanceTask handles it)
  vTaskDelete(NULL);
}


// ------------------------------------------------------------------------------
//  Descript: Connection instance task
//  Params  : <AParameters>  Parameters
//  Returns : -
// ------------------------------------------------------------------------------
static void TaskLocatorServer_ConnectionInstanceTask( void * AParameters )
{
#ifdef FREERTOS_PLUS_TCP
  struct freertos_sockaddr LClientAddress;
         socklen_t         LAddressLength   = sizeof(LClientAddress);
         Socket_t          LConnectedSocket = ( Socket_t ) AParameters;
#endif
#ifdef FREERTOS_PLUS_LWIP
  struct sockaddr_in LClientAddress;
         socklen_t   LAddressLength   = sizeof(LClientAddress);
         int         LConnectedSocket = ( int ) AParameters;
#endif

  configPRINTF( ( "LocatorServer listener instance task started\n" ) );

  while (1) {
    // Wait for data (we use the same buffer for input and output
    // Note that this is blocking (until the set timeout expires, if no data is received)
#ifdef FREERTOS_PLUS_TCP
    int LDataLength = FreeRTOS_recvfrom(LConnectedSocket, GLocatorData, sizeof(GLocatorData) - 1, 0,
                                        &LClientAddress, &LAddressLength);
#endif
#ifdef FREERTOS_PLUS_LWIP
    int LDataLength = lwip_recvfrom(LConnectedSocket, GLocatorData, sizeof(GLocatorData) - 1, 0,
                                    (struct sockaddr *)&LClientAddress, &LAddressLength);
#endif

    if (LDataLength > 0) {
      // Process the data (in place)
      LDataLength =  TaskLocatorServer_ProcessDataReceived( &GLocatorData[0], LDataLength );
      // Return data if needed
      if (LDataLength > 0)
#ifdef FREERTOS_PLUS_TCP
        FreeRTOS_sendto(LConnectedSocket, &GLocatorData[0], LDataLength, 0,
                        &LClientAddress, LAddressLength);
#endif
#ifdef FREERTOS_PLUS_LWIP
        lwip_sendto(LConnectedSocket, &GLocatorData[0], LDataLength, 0,
                      (struct sockaddr *)&LClientAddress, LAddressLength);
#endif
    }
  }
  // We will NEVER get here
  configPRINTF( ( "LocatorServer listener instance task stopped\n" ) );

  vTaskDelete( NULL );
}


// ------------------------------------------------------------------------------
//  Descript: Callback when an UDP event takes place
//  Params  : <ABuffer>      Pointer to input (and output) data buffer
//            <ADataLength>  Length of input data
//  Returns : <return>       Length of output data
// ------------------------------------------------------------------------------
uint32_t TaskLocatorServer_ProcessDataReceived( uint8_t *ABuffer, uint32_t ADataLength )
{
         uint32_t LIndex;

  if (ADataLength == 4)
    configPRINTF( ( "LocatorServer: Request %02X %02X %02X %02X\n", ABuffer[0], ABuffer[1], ABuffer[2], ABuffer[3] ) );
  else
    configPRINTF( ( "LocatorServer: Invalid request length\n" ) );
  if ((ADataLength == 4) &&
      (ABuffer[0] == TAG_CMD) &&
      (ABuffer[1] == 4) &&
      (ABuffer[2] == CMD_DISCOVER_RESET) &&
      // Checksum
      (ABuffer[3] == ((0 - ABuffer[0] - 4 - ABuffer[2]) & 0xff)))
  {
    // If global reset detected
    configPRINTF( ( "LocatorServer: Reset requested\n" ) );
    // Small delay (allow print to complete)
    for (LIndex=0; LIndex<100000; LIndex++) ;
    MAP_SysCtlReset();
    return 0;
  }
  if ((ADataLength == 4) &&
      (ABuffer[0] == TAG_CMD) &&
      (ABuffer[1] == 4) &&
      (ABuffer[2] == CMD_DISCOVER_TARGET) &&
      // Checksum
      (ABuffer[3] == ((0 - ABuffer[0] - 4 - ABuffer[2]) & 0xff)))
  {
    // Valid header detected, now reply
    // Fill in the header for the response data.
    ABuffer[ 0] = TAG_STATUS;
    ABuffer[ 1] = sizeof(GLocatorData);
    ABuffer[ 2] = CMD_DISCOVER_TARGET;
    // Board type
    ABuffer[ 3] = 0;
    // Board ID
    ABuffer[ 4] = 0;

#ifdef FREERTOS_PLUS_TCP
    uint32_t LIPAddress;
    uint32_t LNetMask;
    uint32_t LGatewayAddress;
    uint32_t LDNSServerAddress;
    FreeRTOS_GetEndPointConfiguration( &LIPAddress, &LNetMask, &LGatewayAddress, &LDNSServerAddress, pxNetworkEndPoints );
    // IP address
    ABuffer[ 5] = (LIPAddress >> 24) & 0xff;
    ABuffer[ 6] = (LIPAddress >> 16) & 0xff;
    ABuffer[ 7] = (LIPAddress >>  8) & 0xff;
    ABuffer[ 8] = (LIPAddress      ) & 0xff;
    // MAC address
    ABuffer[ 9] = pxNetworkEndPoints->xMACAddress.ucBytes[0];
    ABuffer[10] = pxNetworkEndPoints->xMACAddress.ucBytes[1];
    ABuffer[11] = pxNetworkEndPoints->xMACAddress.ucBytes[2];
    ABuffer[12] = pxNetworkEndPoints->xMACAddress.ucBytes[3];
    ABuffer[13] = pxNetworkEndPoints->xMACAddress.ucBytes[4];
    ABuffer[14] = pxNetworkEndPoints->xMACAddress.ucBytes[5];
#endif
#ifdef FREERTOS_PLUS_LWIP
    struct netif  *nif = netif_default;
    // IP address
    ABuffer[ 5] = (nif->ip_addr.addr >> 24) & 0xff;
    ABuffer[ 6] = (nif->ip_addr.addr >> 16) & 0xff;
    ABuffer[ 7] = (nif->ip_addr.addr >>  8) & 0xff;
    ABuffer[ 8] =  nif->ip_addr.addr        & 0xff;
    // MAC address
    ABuffer[ 9] = nif->hwaddr[0];
    ABuffer[10] = nif->hwaddr[1];
    ABuffer[11] = nif->hwaddr[2];
    ABuffer[12] = nif->hwaddr[3];
    ABuffer[13] = nif->hwaddr[4];
    ABuffer[14] = nif->hwaddr[5];
#endif
    // Firmware version
    ABuffer[15] = tskKERNEL_VERSION_MAJOR;
    ABuffer[16] = tskKERNEL_VERSION_MINOR;
#ifdef FREERTOS_PLUS_TCP
    ABuffer[17] = ipFR_TCP_VERSION_MAJOR;
    ABuffer[18] = ipFR_TCP_VERSION_MINOR;
#endif
#ifdef FREERTOS_PLUS_LWIP
    ABuffer[17] = LWIP_VERSION_MAJOR;
    ABuffer[18] = LWIP_VERSION_MINOR;
#endif
    // Title (null terminated string)
    for (LIndex = 0; LIndex < 64; LIndex++)
      ABuffer[LIndex + 19] = TASKLOCATORSERVER_DEVICENAME[LIndex];

    // Calculate and fill in the checksum on the response packet
    ABuffer[sizeof(GLocatorData) - 1] = 0;
    for(LIndex = 0; LIndex < (sizeof(GLocatorData) - 1); LIndex++)
      ABuffer[sizeof(GLocatorData) - 1] -= ABuffer[LIndex];
    return sizeof(GLocatorData);
  }
  return 0;
}
