/**
 * Copyright (c) 2022 Andrew McDonnell
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include <string.h>
#include <stdlib.h>
#include <math.h>

#include "pico/stdlib.h"
#include "pico/cyw43_arch.h"

#include "lwip/pbuf.h"
#include "lwip/udp.h"

// Si le fichier n'existe pas, créez-le à partir du modèle "wifi_settings.h.default"
#include "wifi_settings.h"

#define UDP_PORT 47269
#define BEACON_MSG_LEN_MAX 500
#define BEACON_TARGET "192.168.1.58"
#define BEACON_INTERVAL_MS 25

struct udp_pcb* pcb;
ip_addr_t addr;

int teleplot_compteur_envoie = 0;
int teleplot_compteur_cache = 0;
int time_snprintf = 0;
int time_strlen = 0;
int time_strcat = 0;
int time_udp = 0;

char teleplot_tampon[BEACON_MSG_LEN_MAX]="";

void teleplot_udp_send_string(char * message);

void Teleplot_ajout_ou_envoie_tampon(char * message){
    // Si le tampon ne peut pas accueillir le prochain message
    // On envoie et on vide le tampon
    int time = time_us_32();
    if(strlen(message) + strlen(teleplot_tampon) > BEACON_MSG_LEN_MAX){
        int m_time = time_us_32();
        teleplot_udp_send_string(teleplot_tampon);
        time_udp = time_us_32() - m_time;

        teleplot_tampon[0]='\0'; // On "vide" le tampon
        teleplot_compteur_envoie++;
    }else{
        time_strlen += time_us_32() - time;
    }
    // On ajoute le message au tampon
    time = time_us_32();
    strcat(teleplot_tampon, message);
    time_strcat += time_us_32() - time;
    teleplot_compteur_cache++;
    
}

void Teleplot_add_variable_float_2decimal(char * nom_variable, float valeur){
    char tampon[100];
    int time = time_us_32();
    sprintf(tampon, "%s:%lu:%.2f\n", nom_variable, (long)time_us_64()/1000, valeur);
    time_snprintf += time_us_32() - time;
    Teleplot_ajout_ou_envoie_tampon(tampon);
}

void Teleplot_add_variable_int(char * nom_variable, int valeur){
    char tampon[100];
    int time = time_us_32();
    sprintf(tampon, "%s:%lu:%d\n", nom_variable, (long)time_us_64()/1000, valeur);
    time_snprintf += time_us_32() - time;
    Teleplot_ajout_ou_envoie_tampon(tampon);
}

void teleplot_udp_send_string(char * message){
    static int counter = 0;

    struct pbuf *p = pbuf_alloc(PBUF_TRANSPORT, BEACON_MSG_LEN_MAX+1, PBUF_RAM);
    char *req = (char *)p->payload;
    memset(req, 0, BEACON_MSG_LEN_MAX+1);

    snprintf(req, BEACON_MSG_LEN_MAX, "%s", message);

    err_t er = udp_sendto(pcb, p, &addr, UDP_PORT);
    
    pbuf_free(p);
    if (er != ERR_OK) {
        printf("Failed to send UDP packet! error=%d", er);
    } else {
        printf("Sent packet %d\n", counter);
        counter++;
    }

    // Note in practice for this simple UDP transmitter,
    // the end result for both background and poll is the same

#if PICO_CYW43_ARCH_POLL
    // if you are using pico_cyw43_arch_poll, then you must poll periodically from your
    // main loop (not from a timer) to check for Wi-Fi driver or lwIP work that needs to be done.
    cyw43_arch_poll();
#else
    // if you are not using pico_cyw43_arch_poll, then WiFI driver and lwIP work
    // is done via interrupt in the background. This sleep is just an example of some (blocking)
    // work you might be doing.
    // sleep_ms(BEACON_INTERVAL_MS);
#endif
}

void run_udp_beacon() {
    static int counter = 0;
    
    uint32_t old_time = time_us_32();
    uint32_t time, driver_time;
    uint32_t delta_time = 0, delta_driver = 0, teleplot_time=0, delta_teleplot=0;
    int teleplot_counter_old = teleplot_compteur_envoie;
    
    while (true) {
        char tampon[100];

        snprintf(tampon, BEACON_MSG_LEN_MAX, "temps_total:%d\ntemps_driver:%d\nsin_b:%.2f\n",
                delta_time, delta_driver, sinf(counter/10.));

        time = time_us_32();
        teleplot_udp_send_string(tampon);
        delta_time = time_us_32() - time;



        teleplot_time = time_us_32();
        Teleplot_add_variable_float_2decimal("cos", cosf(counter/10.));
        Teleplot_add_variable_float_2decimal("sin", sinf(counter/10.));
        if(teleplot_counter_old != teleplot_compteur_envoie){
            Teleplot_add_variable_int("t_time", delta_teleplot);
            Teleplot_add_variable_int("m_time", delta_teleplot / (teleplot_compteur_cache));
            Teleplot_add_variable_int("snprintf_time", time_snprintf / (teleplot_compteur_cache));
            Teleplot_add_variable_int("strlen_time", time_strlen / (teleplot_compteur_cache));
            Teleplot_add_variable_int("strcat_time", time_strcat / (teleplot_compteur_cache));
            Teleplot_add_variable_int("time_udp", time_udp / (teleplot_compteur_cache));
            Teleplot_add_variable_float_2decimal("nb_cache", teleplot_compteur_cache);
            delta_teleplot = 0;
            time_snprintf= 0;
            time_strlen = 0;
            time_strcat = 0;
            time_udp = 0;
            teleplot_compteur_cache = 0;
            teleplot_counter_old = teleplot_compteur_envoie;
        }
        delta_teleplot += time_us_32() - teleplot_time;
        
        counter++;

        sleep_ms(BEACON_INTERVAL_MS);
    }
}

int init_wifi(void){
    if (cyw43_arch_init()) {
        printf("failed to initialise\n");
        return 1;
    }

    cyw43_arch_enable_sta_mode();

    printf("Connecting to Wi-Fi...\n");
    if (cyw43_arch_wifi_connect_timeout_ms(MY_WIFI_SSID, MY_WIFI_PASSWORD, CYW43_AUTH_WPA2_AES_PSK, 30000)) {
        printf("failed to connect.\n");
        return 1;
    } else {
        printf("Connected.\n");
    }

    pcb = udp_new();
    ipaddr_aton(BEACON_TARGET, &addr);
    return 0;
}

int main() {
    int erreur;
    stdio_init_all();

    erreur = init_wifi();
    if(!erreur){
        run_udp_beacon();
    }
    cyw43_arch_deinit();
    return 0;
}