#include "udp_server.h"

#include <stdio.h>
#include "pico/cyw43_arch.h"
#include "lwip/def.h"

udp_server_t udp_server;

void udp_server_init(int port)
{
    udp_server.pcb = udp_new();
    if(!udp_server.pcb)
    {
        puts("Error creating UDP server");
        return;
    }

    err_t err = udp_bind(udp_server.pcb, IP_ADDR_ANY, port);
    if(err != ERR_OK)
    {
        printf("Error binding UDP server: %d\n", err);
        return;
    }
}

void udp_server_exit(void)
{
    if(udp_server.pcb)
    {
        udp_remove(udp_server.pcb);
        udp_server.pcb = NULL;
    }
}

void udp_server_send(const uint8_t *data, size_t length, const char *ip, uint16_t port)
{
    ip4addr_aton(ip, &udp_server.remote_addr);
    udp_server.remote_port = port;

    // Nombre maximum de tentatives
    const int MAX_RETRIES = 3;
    uint tries = 0;

    while(tries < MAX_RETRIES)
    {
        struct pbuf *p = pbuf_alloc(PBUF_TRANSPORT, length, PBUF_RAM);
        if(p == NULL)
        {
            printf("Try %d - Error allocating pbuf\n", ++tries);
            sleep_ms(100);  // Wait before retry
            continue;
        }

        memcpy(p->payload, (char *)data, length);
        err_t err = udp_sendto(udp_server.pcb, p, &udp_server.remote_addr, udp_server.remote_port);
        pbuf_free(p);

        if (err == ERR_OK)
        {
            return;  // Succès
        }
        else
        {
            const char* error_msg;

            switch(err)
            {
              case ERR_MEM:
                error_msg = "Insuficient memory";
                break;

              case ERR_BUF:
                error_msg = "Buffer full";
                break;

              default:
                error_msg = "Unknown error";
            }

            printf("Try %d - Error send UDP: %d (%s)\n", ++tries, err, error_msg);

            sleep_ms(100);  // Wait before retry
        }
    }

    printf("Error sending data after %d tries...\n", tries);
}