/**
 * Copyright (c) 2022 Andrew McDonnell
 *
 * SPDX-License-Identifier: BSD-3-Clause
 * 
 *
 */
/*
 * UDP sned/receive Adapted from:
 ** Copyright (c) 2016 Stephan Linz <linz@li-pro.net>, Li-Pro.Net
 * All rights reserved.
 *
 * Based on examples provided by
 * Iwan Budi Kusnanto <ibk@labhijau.net> (https://gist.github.com/iwanbk/1399729)
 * Juri Haberland <juri@sapienti-sat.org> (https://lists.gnu.org/archive/html/lwip-users/2007-06/msg00078.html)
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 * 3. The name of the author may not be used to endorse or promote products
 *    derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
 * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
 * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
 * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
 * OF SUCH DAMAGE.
 *
 * This file is part of and a contribution to the lwIP TCP/IP stack.
 *
 * Credits go to Adam Dunkels (and the current maintainers) of this software.
 *
 * Stephan Linz rewrote this file to get a basic echo example.
 * 
 * =============================================================
 * UDP send/recv code is from :
 * Pico examples  
 * https://github.com/raspberrypi/pico-examples/tree/master/pico_w/wifi/udp_beacon
 * lwip contrib apps 
 * https://github.com/lwip-tcpip/lwip/tree/master/contrib/apps
 * UDP send/recv on Windows is from:
 * Microsoft 
 * https://apps.microsoft.com/store/detail/udp-senderreciever/9NBLGGH52BT0?hl=en-us&gl=us
 * a bare-bones packet whacker
 * =============================================================
 * Threads:
 * -- udp send
 * -- udp recv
 * -- blink cyw43 LED
 * -- serial for debug, set the mode to 'send, echo' and set blink time in 'send' mode
 * -- pico discovery:
 *      in send mode: broadcast sender's IP address. format IP xxx.xxx.xxx.xxx
 *      pico in echo mode: sees braodcast and sends it's IP address back to sender's IP addr
 */

#include <string.h>
#include <stdlib.h>
#include <pico/multicore.h>
#include "hardware/sync.h"
#include "hardware/gpio.h"
#include "hardware/timer.h"
#include "hardware/uart.h"
#include "stdio.h"

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

#include "lwip/pbuf.h"
#include "lwip/udp.h"
#include "lwip/opt.h"
#include "lwip/debug.h"
#include "lwip/stats.h"
#include "lwip/dns.h"
#include "lwip/netif.h"

// ======================================
// udp constants
#define UDP_PORT 4444
#define UDP_MSG_LEN_MAX 32
#define UDP_TARGET_DESK "192.168.1.9" //desktop
//#define UDP_TARGET_PICO "192.168.1.54"
#define UDP_TARGET_BROADCAST "255.255.255.255"
#define UDP_INTERVAL_MS 10
// should resolve to a actual addr after pairing
char udp_target_pico[20] =  "255.255.255.255" ;

// =======================================
// necessary to connect to wireless
// !!! Do NOT post this info !!!
#define WIFI_SSID "your_ssid"
#define WIFI_PASSWORD "password"

// =======================================
// protothreads and thread communication
#include "pt_cornell_rp2040_v1_1_2.h"
char recv_data[UDP_MSG_LEN_MAX];
char send_data[UDP_MSG_LEN_MAX];

// payload to led blink
// or send to remote system
int blink_time, remote_blink_time ;
// interthread communicaitqoin
struct pt_sem new_udp_recv_s, new_udp_send_s ;
// mode: send/echo
#define echo 0
#define send 1
int mode = echo ;
// did the addresses get set up?
int paired = false ;

//==================================================
// UDP async receive callback setup
// NOTE that udpecho_raw_recv is triggered by a signal
// directly from the LWIP package -- not from your code
// this callback juswt copies out the packet string
// and sets a "new data" flag
// This runs in an ISR -- KEEP IT SHORT!!!

#if LWIP_UDP

static struct udp_pcb *udpecho_raw_pcb;
struct pbuf *p ;

static void
udpecho_raw_recv(void *arg, struct udp_pcb *upcb, struct pbuf *p,
                 const ip_addr_t *addr, u16_t port)
{
  LWIP_UNUSED_ARG(arg);

  if (p != NULL) {
    //printf("p payload in call back: = %s\n", p->payload);
    memcpy(recv_data, p->payload, UDP_MSG_LEN_MAX);
    // can signal from an ISR -- BUT NEVER wait in an ISR
    PT_SEM_SIGNAL(pt, &new_udp_recv_s) ;
    
    /* free the pbuf */
    pbuf_free(p);
  }
  else printf("NULL pt in callback");
}

// ===================================
// Define the recv callback 
void 
udpecho_raw_init(void)
{
  udpecho_raw_pcb = udp_new_ip_type(IPADDR_TYPE_ANY);
  p = pbuf_alloc(PBUF_TRANSPORT, UDP_MSG_LEN_MAX+1, PBUF_RAM);

  if (udpecho_raw_pcb != NULL) {
    err_t err;
    // netif_ip4_addr returns the picow ip address
    err = udp_bind(udpecho_raw_pcb, netif_ip4_addr(netif_list), UDP_PORT); //DHCP addr

    if (err == ERR_OK) {
      udp_recv(udpecho_raw_pcb, udpecho_raw_recv, NULL);
      //printf("Set up recv callback\n");
    } else {
      printf("bind error");
    }
  } else {
    printf("udpecho_raw_pcb error");
  }
}

#endif /* LWIP_UDP */
// end recv setup

// =======================================
// UDP send thead
// for now just bounces back the recv packet

static PT_THREAD (protothread_udp_send(struct pt *pt))
 { PT_BEGIN(pt);
    static struct udp_pcb* pcb;
    pcb = udp_new();
    pcb->remote_port = UDP_PORT ;
    pcb->local_port = UDP_PORT ;

    static ip_addr_t addr;
    //ipaddr_aton(UDP_TARGET, &addr);

    static int counter = 0;
    
    while (true) {
        
        PT_SEM_WAIT(pt, &new_udp_send_s) ;

        // in paired mode, the two picos talk just to each other
        // before pairing, the echo unit talks to the laptop
        if(mode == echo) {
          if(paired == true){
                ipaddr_aton(udp_target_pico, &addr);
            }
            else{
                ipaddr_aton(UDP_TARGET_DESK, &addr);
            }         
        }
        // broadcast mode makes sure that another pico sees the packet
        // to sent an address and for testing
        else if(mode == send) {
            if(paired == true){
                ipaddr_aton(udp_target_pico, &addr);
            }
            else{
                ipaddr_aton(UDP_TARGET_BROADCAST, &addr);
            }
        }

        struct pbuf *p = pbuf_alloc(PBUF_TRANSPORT, UDP_MSG_LEN_MAX+1, PBUF_RAM);
        char *req = (char *)p->payload;
        memset(req, 0, UDP_MSG_LEN_MAX+1);
        //
        memcpy(req, send_data, UDP_MSG_LEN_MAX) ;
        err_t er = udp_sendto(pcb, p, &addr, UDP_PORT); //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();
        sleep_ms(BEACON_INTERVAL_MS);
#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.

        PT_YIELD_usec(UDP_INTERVAL_MS*1000);
#endif
    }

    PT_END(pt);
}

// ==================================================
// udp recv processing
// ==================================================
static PT_THREAD (protothread_udp_recv(struct pt *pt))
{
    PT_BEGIN(pt);
        static char  arg1[32], arg2[32], arg3[32] , arg4[32]  ;
        static char* token ;
    
     // data structure for interval timer
     PT_INTERVAL_INIT() ;

      while(1) {
        // wait for new packet
        // MUST be an integer format number!!
        PT_SEM_WAIT(pt, &new_udp_recv_s) ;

        token = strtok(recv_data, "  ");
        strcpy(arg1, token) ;
        token = strtok(NULL, "  ");
        strcpy(arg2, token) ;
        token = strtok(NULL, "  ");
        strcpy(arg3, token) ;
        token = strtok(NULL, "  ");
        strcpy(arg4, token) ;

        // is this a pairing packet (starts with IP)
        // if so, parse address otherwise parse blink time
        // process packet to get time
        if(strcmp(arg1,"IP")==0){
          if(mode == echo){
              strcpy(udp_target_pico, arg2) ;
              //printf("%s\n", udp_target_pico);         
              paired = true ;
              // send back echo unit address to send pico
              memset(send_data, 0, UDP_MSG_LEN_MAX) ;
              sprintf(send_data,"IP %s" ,ip4addr_ntoa(netif_ip4_addr(netif_list))) ;
            // tell send threead 
            PT_SEM_SIGNAL(pt, &new_udp_send_s) ;
          }
          else{
            // if I m the send unit, then just save for future transmit
            strcpy(udp_target_pico, arg2) ;
            //printf("echo addr: %s\n", udp_target_pico ); 
          }
        }
        else{
            sscanf(arg1, "%d", &blink_time) ; 
        } 

        // if in echo mode fornmat data and signal udp send thread
        if ((strcmp(arg1,"IP")!=0) && (mode == echo)){
            memset(send_data, 0, UDP_MSG_LEN_MAX) ;
            sprintf(send_data, "Blink time =%d", blink_time) ;
            // tell send threead 
            PT_SEM_SIGNAL(pt, &new_udp_send_s) ;
        }
        // the sender mode should get the message just print it
        else if ((strcmp(arg1,"IP")!=0) && (mode == send)){
           printf("Blink time %s\n", arg3);
        }


        PT_YIELD_INTERVAL(1) ;
        //
        // NEVER exit while
      } // END WHILE(1)
    PT_END(pt);
} // blink thread

// ==================================================
// toggle cyw43 LED  
// this is really just a test of multitasking
// compatability with LWIP
// ==================================================
static PT_THREAD (protothread_toggle_cyw43(struct pt *pt))
{
    PT_BEGIN(pt);
    static bool LED_state = false ;
    //
     // data structure for interval timer
     PT_INTERVAL_INIT() ;
     // set some default blink time
     blink_time = 100 ;
     // echo the default time to udp connection
      PT_SEM_SIGNAL(pt, &new_udp_send_s) ;

      while(1) {
        //
        LED_state = !LED_state ;
        // the onboard LED is attached to the wifi module
        cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, LED_state);
        // blink time is modifed by the udp recv thread
        PT_YIELD_INTERVAL(blink_time*1000) ;
        //
        // NEVER exit while
      } // END WHILE(1)
    PT_END(pt);
} // blink thread

// =================================================
static PT_THREAD (protothread_serial(struct pt *pt))
{
    PT_BEGIN(pt);
        static char cmd[16], arg1[16], arg2[16], arg3[16], arg4[16], arg5[16], arg6[16] ;
        static char* token ;
      //
      printf("Type 'help' for commands\n\r") ;

      while(1) {
        PT_YIELD_usec(100000);

        // print prompt
        sprintf(pt_serial_out_buffer, "cmd> ");
        // spawn a thread to do the non-blocking write
        serial_write ;

        // spawn a thread to do the non-blocking serial read
         serial_read ;
        //sscanf(pt_serial_in_buffer, "%s %f %f %f %f", arg0, &arg1, arg2, arg3, arg4) ;
        // tokenize
        token = strtok(pt_serial_in_buffer, "  ");
        strcpy(cmd, token) ;
        token = strtok(NULL, "  ");
        strcpy(arg1, token) ;
        token = strtok(NULL, "  ");
        strcpy(arg2, token) ;
        token = strtok(NULL, "  ");
        strcpy(arg3, token) ;
        token = strtok(NULL, "  ");
        strcpy(arg4, token) ;
        token = strtok(NULL, "  ");
        strcpy(arg5, token) ;
        token = strtok(NULL, "  ");
        strcpy(arg6, token) ;


        // parse by command
        if(strcmp(cmd,"help")==0){
            // commands
            printf("***\n\rget mode \n\r"); 
            printf("set mode [send, echo]\n\r");  
            printf("send blink_time \n\r");  
            printf("pair \n\r");
        }
        
        else if(strcmp(cmd,"set")==0){
            if(strcmp(arg1,"echo")==0) mode = echo ;
            else if(strcmp(arg1,"send")==0) mode = send ;
            else printf("bad mode");
            //printf("%d\n", mode);
        }

        else if(strcmp(cmd,"pair")==0){
            if(mode == send) {
                memset(send_data, 0, UDP_MSG_LEN_MAX) ;
                sprintf(send_data,"IP %s" ,ip4addr_ntoa(netif_ip4_addr(netif_list))) ;
                PT_SEM_SIGNAL(pt, &new_udp_send_s) ;
                //
                printf("send IP %s\n" ,ip4addr_ntoa(netif_ip4_addr(netif_list))) ;
                printf("sendto IP %s\n" , udp_target_pico) ;
                
                paired = true ;
            }           
        }

        else if(strcmp(cmd,"send")==0){
          if(mode == send){
              sscanf(arg1, "%d", &remote_blink_time);
              memset(send_data, 0, UDP_MSG_LEN_MAX) ;
              sprintf(send_data, "%d", remote_blink_time) ;
              // test pairing
              printf("sendto IP %s paired=%d\n", udp_target_pico, paired) ;
              // trigger send threead 
            PT_SEM_SIGNAL(pt, &new_udp_send_s) ;
           }
           else printf("No send in echo mode\n");
        }

        // no valid command
        else printf("Huh?\n\r") ;

        // NEVER exit while
      } // END WHILE(1)

  PT_END(pt);
} // serial thread


// ====================================================
int main() {
  // =======================
  // init the serial
    stdio_init_all();

  // =======================
  // init the wifi network
    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(WIFI_SSID, WIFI_PASSWORD, CYW43_AUTH_WPA2_AES_PSK, 30000)) {
        printf("failed to connect.\n");
        return 1;
    } else {
      // optional print addr
        printf("Connected: picoW IP addr: %s\n", ip4addr_ntoa(netif_ip4_addr(netif_list)));
    }

    //============================
    // UDP recenve ISR routines
    udpecho_raw_init();

      //========================================
  // start core 1 threads -- none here
  //multicore_reset_core1();
  //multicore_launch_core1(&core1_main);

  // === config threads ========================
  // for core 0

  // init the thread control semaphores
  PT_SEM_INIT(&new_udp_send_s, 0) ;
  PT_SEM_INIT(&new_udp_recv_s, 0) ;

  //printf("Starting threads\n") ;
  pt_add_thread(protothread_udp_send);
  pt_add_thread(protothread_udp_recv);
  pt_add_thread(protothread_toggle_cyw43) ;
  pt_add_thread(protothread_serial) ;
  //
  // === initalize the scheduler ===============
  pt_schedule_start ;

    cyw43_arch_deinit();
    return 0;
}