// hotbox server to run on pico w and drive an old silicone heatbed to
// warm up a sealed bin holding filament. Very handy to have the wifi
// interface to monitor and/or set the temperature.

#include <stdio.h>

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

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

#include "hardware/adc.h"
#include "hardware/gpio.h"
#include "hardware/pwm.h"

#include "wifi_private.h"

struct client_state {
   struct tcp_pcb * client_pcb; // the pcb created during accept
   char client_ip[50]; // ascii rep of client IP addr for debug output
   u16_t client_port;  // port of client
   char inbuf[1024];
   int inbuf_len;
};

struct val_struct {
   int curval;
   int nxtval;
   float vals[3];
};

static err_t accept_hotbox_connection(void *,struct tcp_pcb *,err_t);
static err_t hotbox_sent(void *, struct tcp_pcb *, u16_t);
static err_t hotbox_recv(void *, struct tcp_pcb *, struct pbuf *, err_t);
static void hotbox_err(void *, err_t);
static void hotbox_close(void *);
static void hotbox_command(void *);
static void update_temps();
static void record_val(struct val_struct *, float);
static void set_fan_speed(int);
static bool limit_callback(struct repeating_timer *);
static void set_limit(struct tcp_pcb *, char *);
static uint32_t pwm_set_freq_duty(uint, uint, uint32_t, int);
static float lookup_temp(float);
static void set_heater(bool);

static struct tcp_pcb *server_pcb;

static struct val_struct cputemp = {0,1,{0.0,0.0,0.0}};
static struct val_struct bedtemp = {0,1,{0.0,0.0,0.0}};
static struct val_struct target = {0,1,{0.0,0.0,0.0}};

const char welcome[] = "Welcome to hotbox\n";
const char saywhat[] = "That does not compute (try typing \"help\")\n";

static unsigned long countdown = 0;
struct repeating_timer timer;
bool timer_expired = false;

static bool fan_pwm_running = false;

static bool heatbed_is_on = false;

int
main() {
    err_t err;
    struct tcp_pcb *pcb;

    // Initialize stdio to usb serial
    stdio_init_all();

    // initialize the fan to off
    gpio_init(FAN_GPIO);
    gpio_set_dir(FAN_GPIO, GPIO_OUT);
    gpio_put(FAN_GPIO, 1);

    // initialize the heatbed to off
    gpio_init(HEATBED_GPIO);
    gpio_set_dir(HEATBED_GPIO, GPIO_OUT);
    gpio_put(HEATBED_GPIO, 0);
    record_val(&target, -500.0);

    // Initialize GPIO 26 for reading thermistor
    adc_gpio_init(26);

    // Give me 5 seconds to run a terminal emulator to talk to /dev/ttyACM0
    // so I can see any debug output
    sleep_ms(5000);

    // Initialize reading of cpu internal temp
    adc_init();
    adc_set_temp_sensor_enabled(true);

    // Fill in initial values of temps
    update_temps();

   // Can't use wifi without initializing it.
   err = cyw43_arch_init();
   if (err) {
      printf("cyw43_arch_init() failed, err = %d\n",err);
      return 2;
   }

   // Light up LED to say we got initialized
   cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, 1);

   // I'm not an access point, I'm a "station"
   cyw43_arch_enable_sta_mode();

   // Connect to the router
   printf("Connecting to %s...\n", WIFI_SSID);
   err = cyw43_arch_wifi_connect_timeout_ms(WIFI_SSID, WIFI_PASSWORD,
                                            CYW43_AUTH_WPA2_AES_PSK, 30000);
   if (err) {
      printf("Connection failed, err = %d\n",err);
      return 2;
   } else {
      printf("Connected.\n");
   }
   printf("IP: %s\n",
   ip4addr_ntoa(netif_ip_addr4(netif_default)));
   printf("Mask: %s\n", 
   ip4addr_ntoa(netif_ip_netmask4(netif_default)));
   printf("Gateway: %s\n",
   ip4addr_ntoa(netif_ip_gw4(netif_default)));

   // Do the work to listen for connections and talk to each connectio
   // that comes in.

   pcb = tcp_new_ip_type(IPADDR_TYPE_ANY);
   if (!pcb) {
      printf("tcp_new_ip_type failed.\n");
      return 2;
   }
   err = tcp_bind(pcb, NULL, HOTBOX_PORT);
   if (err) {
      printf("failed to bind to port %u, err %d\n", HOTBOX_PORT, err);
      return 2;
   }

   // This listen call uses the port from pcb to listen on, then frees
   // the pcb, returning a new pcb (not sure why, but that's the way it is).
   server_pcb = tcp_listen_with_backlog(pcb, 1);
   if (!server_pcb) {
       printf("failed to listen\n");
       return 2;
   } else {
      // The accept_hotbox_connection callback is invoked when a new client
      // connection is made.
      tcp_accept(server_pcb, accept_hotbox_connection); 
      printf("Listening on port %d\n",HOTBOX_PORT);
   }

   for ( ; ; ) {
      // Do all the periodic work in this infinite loop

      if (timer_expired) {

         // If the timer for how long to run this has expired,
         // the turn everything off

         cancel_repeating_timer(&timer);
         timer_expired = false;
         printf("The time is up.\n");
         // Turn off fans
         set_fan_speed(0);
         // Set target bed temp to absurdly low value
         record_val(&target, -500.0);
      }

      // Check all thermistor temps

      update_temps();

      // Do "bang-bang" operation of headbed

      float thermC = bedtemp.vals[bedtemp.curval];
      float targC = target.vals[target.curval];
      // Do I want to add some kind of hysteresis here? 
      if (thermC > targC) {
         set_heater(false);
      } else if (thermC < targC) {
         set_heater(true);
      }

      // Do it all again in a tenth of a second

      sleep_ms(100);
   }

   // We ain't using wifi any longer.
   cyw43_arch_deinit();

   return 0;
}

// When this routine is called, it is passed a new client_pcb for the
// new client connection.
//
static err_t accept_hotbox_connection(
   void *arg,
   struct tcp_pcb *client_pcb,
   err_t err) {

   if (err != ERR_OK || client_pcb == NULL) {
      printf("Failure in accept\n");
      return err;
   }
   printf("Client connected\n");

   // Create the struct holding any information we need for each client,
   // set that as the "arg" to be passed to each callback as data comes in,
   // sent data is completed, etc.

   struct client_state * cs =
      (struct client_state*)calloc(sizeof(struct client_state),1);
   cs->client_pcb = client_pcb;

   tcp_arg(client_pcb, cs);
   tcp_sent(client_pcb, hotbox_sent);
   tcp_recv(client_pcb, hotbox_recv);
   tcp_err(client_pcb, hotbox_err);

   ip_addr_t clip;
   if (tcp_tcp_get_tcp_addrinfo(client_pcb, 0, &clip, &cs->client_port)) {
      strcpy(cs->client_ip, "unknown");
      cs->client_port = 0;
   } else {
      ipaddr_ntoa_r(&clip, cs->client_ip, sizeof(cs->client_ip));
   }
   printf("Client IP: %s, port %d\n",cs->client_ip, cs->client_port);

   err = tcp_write(client_pcb, welcome, sizeof(welcome), TCP_WRITE_FLAG_COPY);
   if (err) {
      printf("Failed to write welcome message, err = %d\n",err);
   }
   return err;
}

// This routine is called when a chunk of data has completed sending
//
static err_t hotbox_sent(void *arg, struct tcp_pcb *tpcb, u16_t len) {
//    struct client_state * cs = (struct client_state *)arg;
    // PUT CODE HERE
    printf("Completed sending %d bytes of data\n", len);
    return ERR_OK;
}

// This routine is called when a chunk of data comes in from client
//
err_t hotbox_recv(void *arg, struct tcp_pcb *tpcb, struct pbuf *p, err_t err) {
   struct client_state * cs = (struct client_state *)arg;
   if (p == NULL) {
      printf("received NULL pbuf\n");
      hotbox_close(arg);
   } else {
      // All the commands and responses should be short strings, so I'm
      // going to simplify this code by assuming I never need to collect
      // multiple chunks of pbuf (I'll fix it if experience proves me wrong).

      int len = p->tot_len;
      if (len > 0) {
         if (len >= sizeof(cs->inbuf)) {
            len = sizeof(cs->inbuf)-1;
         }
         cs->inbuf_len = pbuf_copy_partial(p, cs->inbuf, len, 0);
         cs->inbuf[len] = '\0';
      }
      // Tell library we got the data and free the pbuf we no longer need.
      tcp_recved(tpcb, p->tot_len);
      pbuf_free(p);
      hotbox_command(arg);
   }
   return ERR_OK;
}

// Called when there is an error (though I can't figure out if it
// is ever actually called, close the connection if it is called).
//
static void hotbox_err(void *arg, err_t err) {
   printf("hotbox_err %d\n", err);
   hotbox_close(arg);
}

// Close and clean up a connection
//
static void hotbox_close(void *arg) {
   err_t err;
   struct client_state * cs = (struct client_state *)arg;
   struct tcp_pcb *tpcb = cs->client_pcb;
   printf("Closing connection to client IP %s port %d\n",
          cs->client_ip, cs->client_port);
   if (tpcb != NULL) {
      tcp_arg(tpcb, NULL);
      tcp_poll(tpcb, NULL, 0);
      tcp_sent(tpcb, NULL);
      tcp_recv(tpcb, NULL);
      tcp_err(tpcb, NULL);
      err = tcp_shutdown(tpcb, 1, 1);
      if (err) {
         printf("tcp_shutdown returns err %d\n",err);
      }
      err = tcp_close(tpcb);
      if (err) {
         printf("tcp_close returns err %d\n",err);
      }
   }
   if (cs != NULL) {
      free(cs);
   }
}

// Parse the input command and execute it...
//
static void hotbox_command(void *arg) {
   err_t err = ERR_OK;
   struct client_state * cs = (struct client_state *)arg;
   struct tcp_pcb * client_pcb = cs->client_pcb;
   if (cs->inbuf_len <= 0) {
      return;
   }

   // remove line end and trailing spaces from command

   char * eol = strpbrk(cs->inbuf, "\r\n");
   if (eol != NULL) {
      *eol = '\0';
      cs->inbuf_len = eol - cs->inbuf;
   }
   while ((cs->inbuf_len > 0) && (cs->inbuf[cs->inbuf_len - 1] == ' ')) {
      cs->inbuf_len--;
   }
   cs->inbuf[cs->inbuf_len] = '\0';
   printf("Saw command: %s\n", cs->inbuf);
   if (strcmp(cs->inbuf, "bye") == 0) {
      // Simplest command terminates connection. Let's see if this works
      // before getting more complex.
      hotbox_close(arg);
   } else if (strcmp(cs->inbuf, "cputemp") == 0) {
      char answer[150];
      float tempC = cputemp.vals[cputemp.curval];
      float tempF = tempC * 9 / 5 + 32;
      sprintf(answer,"Cpu temp: %.02f C %.02f F\n",tempC,tempF);
      err = tcp_write(client_pcb, answer, strlen(answer), TCP_WRITE_FLAG_COPY);
   } else if (strcmp(cs->inbuf, "bedtemp") == 0) {
      char answer[150];
      float thermC = bedtemp.vals[bedtemp.curval];
      float thermF = thermC * 9 / 5 + 32;
      sprintf(answer,"Bed temp: %.02f C %.02f F\n", thermC, thermF);
      err = tcp_write(client_pcb, answer, strlen(answer), TCP_WRITE_FLAG_COPY);
   } else if ((strncmp(cs->inbuf, "fan", 3) == 0) && (cs->inbuf[3] == ' ')) {
      char msg[100];
      int num = atoi(&cs->inbuf[4]);
      if ((num < 0) || (num > 100)) {
         sprintf(msg, "Fan speed %d outside of range [0,100]\n", num);
         err = tcp_write(client_pcb, msg, strlen(msg), TCP_WRITE_FLAG_COPY);
      } else {
         set_fan_speed(num);
      }
   } else if ((strncmp(cs->inbuf, "limit", 5) == 0) && (cs->inbuf[5] == ' ')) {
      // limit how long the heatbed and fans will remain on
      set_limit(client_pcb, &cs->inbuf[6]);
   } else if ((strncmp(cs->inbuf, "heatbed", 7) == 0) && (cs->inbuf[7] == ' ')) {
      char answer[150];
      char * endp;
      double val = strtod(&cs->inbuf[8], &endp);
      if ((endp != NULL) && ((*endp == 'f') || (*endp == 'F'))) {
         // convert fahrenheit to celsius
         val = (val - 32.0) / (9.0/5.0);
      }
      record_val(&target, val);
      float valf = val * 9 / 5 + 32;
      sprintf(answer,"Target temp: %.02f C %.02f F\n", val, valf);
      err = tcp_write(client_pcb, answer, strlen(answer), TCP_WRITE_FLAG_COPY);
   } else if ((strncmp(cs->inbuf, "led", 3) == 0) && (cs->inbuf[3] == ' ')) {
      if (strcmp(&cs->inbuf[4], "on") == 0) {
         cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, 1);
      } else if (strcmp(&cs->inbuf[4], "off") == 0) {
         cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, 0);
      } else {
         char msg[100];
         sprintf(msg, "Unrecognized \"%s\", arg should be \"on\" or \"off\"\n",
                 &cs->inbuf[4]);
         err = tcp_write(client_pcb, msg, strlen(msg), TCP_WRITE_FLAG_COPY);
      }
   } else if (strcmp(cs->inbuf, "help") == 0) {
      const char helpmsg[] = "\
bye       close connection\n\
cputemp   print internal cpu temp of pico\n\
bedtemp   print heatbed temp\n\
fan       set fan speed [0,100]\n\
limit     set how long heater and fan will run (numbers with h m or s suffix)\n\
heatbed   set heatbed target temp (numbers with f or c suffix)\n\
led       turn led on or off\n\
help      print this help\n";
      err = tcp_write(client_pcb, helpmsg, strlen(helpmsg), TCP_WRITE_FLAG_COPY);
   } else {
      // Unrecognized command
      err = tcp_write(client_pcb, saywhat, sizeof(saywhat), TCP_WRITE_FLAG_COPY);
   }
   if (err) {
      printf("Command %s err %d\n",cs->inbuf,err);
   }
}

static void set_limit(struct tcp_pcb * client_pcb, char * args) {
   // Parse the arguments to the limit command and start (or stop) the timer
   if (strncmp(args, "off", 3) == 0) {
      // Turn off the timer if it is running
      static const char msg[] = "Heatbed and fans will never be turned off.\n";
      if (countdown > 0) {
         cancel_repeating_timer(&timer);
         countdown = 0;
         timer_expired = false;
      }
      tcp_write(client_pcb, msg, strlen(msg), TCP_WRITE_FLAG_COPY);
   } else {
      // Any collection of numbers with h m or s suffix chars are added
      // up to create the value for total seconds to set the limit
      bool garbage = false;
      double tot_sec = 0.0;
      while ((! garbage) && (*args != '\0')) {
         char * endp;
         double val = strtod(args, &endp);
         if (args == endp) {
            // Woa! Didn't advance, must be garbage on line.
            char msg[100];
            sprintf(msg,"Do not recognize %s\n",args);
            tcp_write(client_pcb, msg, strlen(msg), TCP_WRITE_FLAG_COPY);
            garbage = true;
            break;
         }
         double mult = 1.0;
         args = endp;
         switch(*endp) {
            case 'H': case 'h':
               mult = 60.0 * 60.0;
               args++;
               break;
            case 'M': case 'm':
               mult = 60.0;
               args++;
               break;
            case 'S': case 's':
               mult = 1.0;
               args++;
               break;
         }
         tot_sec += (val * mult);
      }
      if (! garbage) {
         if (countdown > 0) {
            // If a previous limit timer is running, cancel it
            cancel_repeating_timer(&timer);
         }
         countdown = (unsigned long)tot_sec;
         char msg[100];
         sprintf(msg,"Heatbed and fans will be turned off in %lu seconds.\n",
                 countdown);
         add_repeating_timer_ms(1000, limit_callback, NULL, &timer);
         tcp_write(client_pcb, msg, strlen(msg), TCP_WRITE_FLAG_COPY);
      }
   }
}

static bool limit_callback(struct repeating_timer *t) {
   if (countdown > 0) {
      countdown--;
      if (countdown == 0) {
         timer_expired = true;
      }
   }
   return true;
}

static void set_fan_speed(int num) {
   if ((num == 0) || (num == 100)) {
      if (fan_pwm_running) {
         // If I want full on or full off, get FAN_GPIO completely out
         // of pwm mode
         pwm_set_enabled(pwm_gpio_to_slice_num(FAN_GPIO), false);
         fan_pwm_running = false;
         gpio_init(FAN_GPIO);
         gpio_set_dir(FAN_GPIO, GPIO_OUT);
      }
      if (num == 0) {
         gpio_put(FAN_GPIO, 1);
      } else {
         gpio_put(FAN_GPIO, 0);
      }
   } else {
      // For a fractional fan speed, go back to PWM mode if we aren't
      // already.
      if (! fan_pwm_running) {
         gpio_set_function(FAN_GPIO, GPIO_FUNC_PWM);
      }
      pwm_set_freq_duty(pwm_gpio_to_slice_num(FAN_GPIO),
                        pwm_gpio_to_channel(FAN_GPIO),
                        1000,
                        100 - num);
      pwm_set_enabled(pwm_gpio_to_slice_num(FAN_GPIO), true);
      fan_pwm_running = true;
   }
}

// Handy utility function stolen from The Pico In C: Basic PWM by Harry Fairhead
//
static uint32_t pwm_set_freq_duty(uint slice_num,
                                  uint chan,
                                  uint32_t f,
                                  int d)
{
   uint32_t clock = 125000000;
   uint32_t divider16 = clock / f / 4096 + 
                             (clock % (f * 4096) != 0);
   if (divider16 / 16 == 0)
   divider16 = 16;
   uint32_t wrap = clock * 16 / divider16 / f - 1;
   pwm_set_clkdiv_int_frac(slice_num, divider16/16,
                                       divider16 & 0xF);
   pwm_set_wrap(slice_num, wrap);
   pwm_set_chan_level(slice_num, chan, wrap * d / 100);
   return wrap;
}

static void update_temps() {
   // Steal this code from pico-examples/adc/adc_console/adc_console.c
   // to get the internal CPU temperature.

   // 12-bit conversion, assume max value == ADC_VREF == 3.3 V
   const float conversionFactor = 3.3f / (1 << 12);

   // Read the internal chip temp
   adc_select_input(4);
   float adc = (float)adc_read() * conversionFactor;
   float tempC = 27.0f - (adc - 0.706f) / 0.001721f;

   // read the voltage from the thermistor voltage divider
   // PUT CODE HERE - figure out how to convert resistance to temperature...

   adc_select_input(0);
   float thermv = conversionFactor * adc_read();
   float thermr = (thermv*THERMISTOR_R0)/(3.3-thermv);
   float thermC = lookup_temp(thermr);

   record_val(&cputemp, tempC);
   record_val(&bedtemp, thermC);
}

static void set_heater(bool onoff) {
   if (onoff) {
      // I want to turn heatbed on if it isn't already on
      if (! heatbed_is_on) {
         printf("Turning heatbed on.\n");
         gpio_put(HEATBED_GPIO, 1);
         heatbed_is_on = true;
      }
   } else {
      // I want to turn heatbed off if it isn't already off
      if (heatbed_is_on) {
         printf("Turning heatbed off.\n");
         gpio_put(HEATBED_GPIO, 0);
         heatbed_is_on = false;
      }
   }
}

static void record_val(struct val_struct * vs, float newval) {
   int nxt = vs->nxtval++;
   if (nxt >= 3) {
      nxt = 0;
      vs->nxtval = nxt;
   }
   vs->vals[nxt] = newval;
   vs->curval = nxt;
}

#include "thermistor.h"

static float lookup_temp(float thermr) {
   // The pico has nothing better to do than spin most of the time
   // so don't go all insane and try to code a broken binary search
   // here, just linear search the thermistor table.

   int i;
   for (i = 0; i < THERM_LOOKUP_NUM-1; ++i) {
      if ((therm_lookup[i].ohms >= thermr) &&
          (therm_lookup[i+1].ohms < thermr)) {
         // OK, the resistance we have lies between i and i+1, figure
         // out the percentage between the entries and apply that to
         // the temp to get the return value.
         float ediff = therm_lookup[i].ohms - therm_lookup[i+1].ohms;
         float rdiff = therm_lookup[i].ohms - thermr;
         float tdiff = therm_lookup[i+1].tempC - therm_lookup[i].tempC;
         return therm_lookup[i].tempC + (tdiff * (rdiff / ediff));
      }
   }
   // Something is seriously wrong if I get here, so return a seriously
   // wrong value.
   return 0.0;
}