#include <WiFi.h>

#define APMODE

#ifdef APMODE
#define STASSID "pico_w_ap"
#define STAPSK "0123456789abc"
#else
#define STASSID "my_wifi_router_ssid"
#define STAPSK "*************"
#endif

#define CONNECT_TIMEOUT_MS 10000
#define MAX_SRV_CLIENTS 1
WiFiClient tcpServerClients[MAX_SRV_CLIENTS];
const char* ssid = STASSID;
const char* password = STAPSK;

IPAddress MyipAdr = IPAddress(192, 168, 1, 1);
IPAddress MyipSubnet = IPAddress(255, 255, 255, 0);
int port = 23;
bool pushinfo = false, pushreboot = false, pushboot = false;

#define _RS485_TX (4)
#define _RS485_RX (5)

WiFiServer server(port);

void setup() {
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, 0);

  Serial.begin(115200);
  Serial.ignoreFlowControl();

  gpio_pull_up(_RS485_RX);
  gpio_pull_up(_RS485_TX);
  Serial2.setRX(_RS485_RX);
  Serial2.setTX(_RS485_TX);
  Serial2.setFIFOSize(4096);
  Serial2.begin(115200);

#ifdef APMODE
  WiFi.mode(WIFI_AP);
  WiFi.softAPConfig(MyipAdr, MyipAdr, MyipSubnet);
  WiFi.softAP(ssid, password, 1, 0, 1);
  Serial.printf("My AP is '%s' with '%s'\n", ssid, password);
  Serial.printf("My IP is %s:%d", WiFi.softAPIP().toString(), port);
#else
  Serial.printf("Connect to '%s' with '%s'\n", ssid, password);
  WiFi.mode(WIFI_STA);
  WiFi.setHostname("PicoW2");
  WiFi.begin(ssid, password);
  uint32_t t = millis() + CONNECT_TIMEOUT_MS;
  uint8_t cled = 0;
  while (WiFi.status() != WL_CONNECTED) {
    if (millis() > t) {
      WiFi.disconnect();
      WiFi.end();
      WiFi.begin(ssid, password);
      t = millis() + CONNECT_TIMEOUT_MS;
    }
    delay(100);
    digitalWrite(LED_BUILTIN, cled);
    cled ^= 1;
  }
  Serial.printf("Connected to %s\n", ssid);
#endif
  server.begin();
  server.setNoDelay(true);
  Serial.printf("Listening at %s:%d\n", WiFi.localIP().toString().c_str(), port);
}

// **Not supported if packets are split.
bool PUSR_portconfig_check (uint8_t *p, uint32_t *baud, uint16_t *config, String *s) {
  if ((p[0] == 0x55) && (p[1] == 0xaa) && (p[2] == 0x55 && ((uint8_t)(p[3] + p[4] + p[5] + p[6]) == p[7]))) {
    *baud = (p[3] << 16) | (p[4] << 8) | p[5];
    union {
      struct {
        uint8_t databits : 2;    // 0:5bit 1:6bit 2:7bit 3:8bit
        uint8_t stopbits : 1;    // 0:1bit 1:2bit
        uint8_t parityen : 1;    // 0:di 1:en
        uint8_t paritytype : 2;  // 0:ODD 1:EVEN 2:MASK 3:SPACE
      };
      uint8_t byte;
    } bitset;
    bitset.byte = p[6];
    *config = 0;

    switch (bitset.databits) {
      case 0: *config |= SERIAL_DATA_5; *s = '5'; break;
      case 1: *config |= SERIAL_DATA_6; *s = '6'; break;
      case 2: *config |= SERIAL_DATA_7; *s = '7'; break;
      case 3: *config |= SERIAL_DATA_8; *s = '8'; break;
    }
    if (bitset.parityen == 1) {
      switch (bitset.stopbits) {
        case 0: *config |= SERIAL_PARITY_EVEN; *s += 'E'; break;
        case 1: *config |= SERIAL_PARITY_ODD; *s += 'O'; break;
        case 2: *config |= SERIAL_PARITY_MARK; *s += 'M'; break;
        case 3: *config |= SERIAL_PARITY_SPACE; *s += 'S'; break;
      }
    } else {
      *config |= SERIAL_PARITY_NONE;
      *s += 'N';
    }
    switch (bitset.stopbits) {
      case 0: *config |= SERIAL_STOP_BIT_1; *s += '1'; break;
      case 1: *config |= SERIAL_STOP_BIT_2; *s += '2';break;
    }
    return true;
  }
  return false;
}

void update_uart (uint32_t baud, uint16_t config, String s) {
  Serial2.flush();
#if 0
  uart_set_baudrate(uart1, baud);
  uart_set_format(uart1, 8, 1, UART_PARITY_NONE);
#else
  Serial2.end();
  Serial2.begin(baud, config);
#endif
  Serial.printf("Update UART %ubps %s\n",baud, s.c_str());
}

void loop() {
  bool led = false;
  static uint32_t blink_t = 0;
  static uint8_t buf1[2048], buf2[2048];
  int l;

  if (server.hasClient()) {
    for (int i = 0; i < MAX_SRV_CLIENTS; i++) {
      if (!tcpServerClients[i] || !tcpServerClients[i].connected()) {
        if (tcpServerClients[i]) tcpServerClients[i].stop();
        tcpServerClients[i] = server.available();
        Serial.printf("New client: %d\n", i);
        continue;
      }
    }
    WiFiClient severClient = server.available();
    severClient.stop();
  }

  // Send data from the socket to UART
  for (int i = 0; i < MAX_SRV_CLIENTS; i++) {
    if (tcpServerClients[i] && tcpServerClients[i].connected()) {
      while ((l = tcpServerClients[i].available()) > 0) {
        if ((l = tcpServerClients[i].readBytes(buf1, min(sizeof(buf1), l))) > 0) {
          int ll = 0;
          uint8_t *p = buf1;
          String s;
          for (int j = 0; j < l;) {
            uint32_t baud;
            uint16_t conf;
            if (j + 8 <= l) {
              if (PUSR_portconfig_check (&buf1[j], &baud, &conf, &s)) {
                if (ll - 8 > 0) {
                  Serial2.write(p, ll - 8);
                  led = true;
                  p = &buf1[j] + 8;
                }
                update_uart(baud, conf, s);
                ll = 0;
                j+=8;
              } else {
                ll++;
                j++;
              }
            } else {
              ll++;
              j++;
            }
          }
          if (ll > 0) {
            Serial2.write(p, ll);
            led = true;
          }
        }
      }
    }
  }

  // Send data from UART to socket
  while ((l = Serial2.available()) > 0) {
    if ((l = Serial2.readBytes(buf2, min(sizeof(buf2), l))) > 0) {
      for (int i = 0; i < MAX_SRV_CLIENTS; i++) {
        if (tcpServerClients[i] && tcpServerClients[i].connected()) {
          tcpServerClients[i].write((uint8_t*)buf2, l);
          led = true;
        }
      }
    }
  }

  if (led) {
    blink_t = millis() + 20;
    digitalWrite(LED_BUILTIN, 1);
    led = false;
  }

  if (millis() > blink_t) digitalWrite(LED_BUILTIN, 0);

  if (pushreboot) {
    pushreboot = false;
    WiFi.disconnect();
    Serial.end();
    delay(200);
    watchdog_enable(1, 1);
    while (1) ;
  }
  if (pushboot){
    WiFi.disconnect();
    Serial.end();
    delay(200);
    reset_usb_boot(0, 0);
    while (1) ;
  }
  if (pushinfo) {
    pushinfo = false;
#ifdef APMODE
    Serial.printf("My AP is '%s' with '%s'\n", ssid, password);
    Serial.printf("Listening at %s:%d\n", WiFi.softAPIP().toString().c_str(), port);
    Serial.printf("RSSI=%d\n", WiFi.RSSI());
#else
    Serial.printf("Listening at %s:%d\n", WiFi.localIP().toString().c_str(), port);
#endif
  }
  __WFI();  // wait for Interrupt
}

void setup1(){
  delay (1000);
}

void loop1(){
  while (Serial.available()) {
    char c = Serial.read();
    switch (c) {
      case '\33': Serial.printf("\x1b[2J"); break;
      case '#':
        pushreboot = true;
        break;
      case '!':
        pushboot = true;
        break;
      case 'i':
        pushinfo = true;
        break;
    }
  }
  delay (50);
}