#include "PicoW_UDP.h"

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

inline int udp_server_init(struct udp_server_t *udp_server)
{
    udp_server->pcb = udp_new();
    if(udp_server->pcb == NULL)
    {
        puts("Error creating UDP server pcb.");
        return PICO_ERROR_GENERIC;
    }

    err_t err = udp_bind(udp_server->pcb, IP_ADDR_ANY, UDP_SERVER_PORT);
    if(err)
    {
        printf("Error binding UDP server: %d\n", err);
        return err;
    }

    return PICO_OK;
}

inline void udp_server_exit(struct udp_server_t *udp_server)
{
    if(udp_server->pcb)
    {
        udp_remove(udp_server->pcb);
        udp_server->pcb = NULL;
    }
}

int udp_server_send(struct udp_server_t *udp_server, void *data, uint16_t len)
{
    struct pbuf *p = pbuf_alloc(PBUF_TRANSPORT, len, PBUF_RAM);
    if(p == NULL)
    {
        puts("Pbuf memory allocation error");
        return PICO_ERROR_INSUFFICIENT_RESOURCES;
    }

    memcpy((uint8_t *)p->payload, data, len);

    err_t err = udp_sendto(udp_server->pcb, p, IP_ADDR_ANY, UDP_CLIENT_PORT);

    pbuf_free(p);

    if(err)
    {
        printf("UDP send error : %d\n", err);
        return err;
    }

    return PICO_OK;
}

static inline void builtin_udp_client_callback(void *arg, struct udp_pcb *pcb, struct pbuf *p, const ip_addr_t *addr, u16_t port)
{
    struct udp_client_t *udp_client = arg;

    if(p->len != UDP_CLIENT_DATA_LEN)
        return;

    memcpy(udp_client->data, p->payload, UDP_CLIENT_DATA_LEN);

    pbuf_free(p);
}

inline int udp_client_init(struct udp_client_t *udp_client, udp_recv_fn callback)
{
    udp_client->pcb = udp_new();
    if(udp_client->pcb == NULL)
    {
        puts("Error creating UDP server pcb.");
        return PICO_ERROR_GENERIC;
    }

    err_t err = udp_bind(udp_client->pcb, IP_ADDR_ANY, UDP_CLIENT_PORT);
    if(err)
    {
        printf("Error binding UDP server: %d\n", err);
        return err;
    }

    udp_recv(udp_client->pcb, callback ? callback : builtin_udp_client_callback, &udp_client);

    return PICO_OK;
}

inline void udp_client_exit(struct udp_client_t *udp_client)
{
    if(udp_client->pcb)
    {
        udp_remove(udp_client->pcb);
        udp_client->pcb = NULL;
    }
}