/*
 * nmea.c
 *
 *  Created on: 16/nov/2011
 *      Author: operatore
 */

#include "ip_nmea.h"

#include "lwip/opt.h"
#include "lwip/tcp.h"

#include <string.h>

#define N_DEBUG LWIP_DBG_ON

volatile int debug = N_DEBUG;

static struct tcp_pcb *nmea_pcb, *nmea_ntcb = NULL;

static err_t ip_nmea_accept(void *arg, struct tcp_pcb *newpcb, err_t err);
static err_t ip_nmea_recv(void *arg, struct tcp_pcb *tpcb, struct pbuf *p, err_t err);
static void ip_nmea_error(void *arg, err_t err);
static err_t ip_nmea_poll(void *arg, struct tcp_pcb *tpcb);
static err_t ip_nmea_sent(void *arg, struct tcp_pcb *tpcb, u16_t len);
static void ip_nmea_close(struct tcp_pcb *tpcb);
static void ip_nmea_trysend(struct tcp_pcb *tpcb);

void ip_nmea_init(void)
{
   LWIP_DEBUGF(N_DEBUG, ("ip_nmea_init: start\r\n"));

   nmea_pcb = tcp_new();
   if (nmea_pcb != NULL)
   {
      err_t err;

      err = tcp_bind(nmea_pcb, IP_ADDR_ANY, 1000);
      if (err == ERR_OK)
      {
         LWIP_DEBUGF(N_DEBUG, ("ip_nmea_init: bound\r\n"));

         nmea_pcb = tcp_listen(nmea_pcb);
         tcp_accept(nmea_pcb, ip_nmea_accept);
         LWIP_DEBUGF(N_DEBUG, ("ip_nmea_init: waiting for connection\r\n"));
      }
      else
      {
         /* abort? output diagnostic? */
         LWIP_DEBUGF(N_DEBUG, ("ip_nmea_init: unable to bind\r\n"));
      }
   }
   else
   {
      /* abort? output diagnostic? */
      LWIP_DEBUGF(N_DEBUG, ("ip_nmea_init: unable to allocate pcb\r\n"));
   }
}

err_t ip_nmea_accept(void *arg, struct tcp_pcb *newpcb, err_t err)
{
   LWIP_UNUSED_ARG(arg);
   LWIP_UNUSED_ARG(err);

   LWIP_DEBUGF(N_DEBUG, ("ip_nmea_accept: start\r\n"));

   /* commonly observed practice to call tcp_setprio(), why? */
   tcp_setprio(newpcb, TCP_PRIO_MIN);

   tcp_arg(newpcb, NULL);
   tcp_recv(newpcb, ip_nmea_recv);
   tcp_err(newpcb, ip_nmea_error);
   tcp_poll(newpcb, ip_nmea_poll, 0);

   nmea_ntcb = newpcb;

   return ERR_OK;
}

#define MAXLINE 133
static char line[MAXLINE];
static int nchars;

err_t ip_nmea_recv(void *arg, struct tcp_pcb *tpcb, struct pbuf *p, err_t err)
{
   err_t ret_err;
   LWIP_UNUSED_ARG(arg);

   if (p == NULL)
   {
      /* remote host closed connection */
      LWIP_DEBUGF(N_DEBUG, ("ip_nmea_recv: remote host closed connection\r\n"));
      /* we're done sending, close it */
      ip_nmea_close(tpcb);
      ret_err = ERR_OK;
   }
   else if (err != ERR_OK)
   {
      LWIP_DEBUGF(N_DEBUG, ("ip_nmea_recv: ERROR! discardng pbuf\r\n"));
      /* cleanup, for unkown reason */
      if (p != NULL)
      {
         pbuf_free(p);
      }
      ret_err = err;
   }
   else
   {
      if (p != NULL)
      {
         int n;
         /* data chunk in p->payload, copy data to line */
         n = pbuf_copy_partial(p, line + nchars, MAXLINE - nchars, 0);
         if (n < p->tot_len)
         {
            LWIP_DEBUGF(N_DEBUG,
                  ("ip_nmea_recv: ERROR! discarding %d bytes\r\n", p->tot_len-n));
         }
         pbuf_free(p);
         nchars += n;
         tcp_recved(tpcb, n);

         ip_nmea_trysend(tpcb);
      }
      ret_err = ERR_OK;
   }
   return ret_err;
}

void ip_nmea_error(void *arg, err_t err)
{
   LWIP_UNUSED_ARG(arg);

   LWIP_DEBUGF(N_DEBUG, ("ip_nmea_error: something went wrong (%d)\r\n", err));
   //TODO: handle error
}

err_t ip_nmea_poll(void *arg, struct tcp_pcb *tpcb)
{
   LWIP_UNUSED_ARG(arg);
   ip_nmea_trysend(tpcb);
   return ERR_OK;
}

err_t ip_nmea_sent(void *arg, struct tcp_pcb *tpcb, u16_t len)
{
   LWIP_UNUSED_ARG(arg);
   LWIP_DEBUGF(N_DEBUG, ("ip_nmea_sent: %d chars acknowledged\r\n", len));

   if (len > nchars)
   {
      LWIP_DEBUGF(N_DEBUG, ("ip_nmea_sent: ERROR! len > nchars (%d > %d)\r\n", len, nchars));
      len = nchars;
   }

   if (len > 0) {
      strcpy(line, line+len);
      nchars -= len;
   }
   ip_nmea_trysend(tpcb);

   return ERR_OK;
}

void ip_nmea_trysend(struct tcp_pcb *tpcb)
{
   LWIP_DEBUGF(N_DEBUG, ("ip_nmea_trysend: %d chars in line", nchars));
   if (nchars > 0) {
      char *end = strchr(line, '\n');
      if (end != NULL)
      {
         err_t wr_err;
         tcp_sent(tpcb, ip_nmea_sent);
         wr_err = tcp_write(tpcb, line, end-line+1, 0);
         if (wr_err == ERR_OK)
         {
            LWIP_DEBUGF(N_DEBUG, ("... %d chars sent", end-line+1));
            tcp_output(tpcb);
         }
         else
         {
            LWIP_DEBUGF(N_DEBUG, ("... ERROR! (%d)", wr_err));
         }
      }
   }
   LWIP_DEBUGF(N_DEBUG, (".\r\n"));
}


void ip_nmea_close(struct tcp_pcb *tpcb)
{
   LWIP_DEBUGF(N_DEBUG, ("ip_nmea_close:\r\n"));

   nmea_ntcb = NULL;

   tcp_arg(tpcb, NULL);
   tcp_sent(tpcb, NULL);
   tcp_recv(tpcb, NULL);
   tcp_err(tpcb, NULL);
   tcp_poll(tpcb, NULL, 0);

   tcp_close(tpcb);

   //FIXME: should I call listen again?
}

void ip_nmea_write(char *buf, int len)
{
   if (nmea_ntcb != NULL)
   {
      err_t err;
      u16_t max = tcp_sndbuf(nmea_ntcb);
      while (len > max)
      {
         err = tcp_write(nmea_ntcb, buf, max,
               TCP_WRITE_FLAG_MORE | TCP_WRITE_FLAG_COPY);
         if (err != ERR_OK)
         {
            //FIXME: do something
         }
         buf += max;
         len -= max;
      }
      err = tcp_write(nmea_ntcb, buf, len, TCP_WRITE_FLAG_COPY);
      if (err != ERR_OK)
      {
         //FIXME: do something
      }
   }
}