#include <WiFi.h>
#include <AsyncTCP.h>

const char* ssid = "your-ssid";
const char* password = "your-pass";

AsyncServer server(23);

typedef struct {
  struct pbuf* pb;
  size_t index;
} packet_buf_t;

size_t sendMax(void *obj, AsyncClient* c){
  if(!obj || !c || !c->space()){
    return 0;
  }
  packet_buf_t * buf = (packet_buf_t*)obj;
  if(!buf->pb){
    return 0;
  }
  size_t space = c->space();
  size_t available = buf->pb->tot_len - buf->index;
  size_t maxBytes = space;
  if(space > available){
    maxBytes = available;
  }
  size_t written = 0;
  size_t toRead = 0;
  static uint8_t buffer[8192];
  /*uint8_t * buffer = (uint8_t *)malloc(maxBytes);
  if(!buffer){
    return 0;
  }*/
  while(written < maxBytes){
    toRead = buf->pb->len - buf->index;
    if((maxBytes - written) < toRead){
      toRead = maxBytes - written;
    }
    memcpy(buffer + written, reinterpret_cast<char*>(buf->pb->payload) + buf->index, toRead);
    written += toRead;
    buf->index += toRead;
    if(buf->index == buf->pb->len){
      buf->index = 0;
      if(!buf->pb->next) {
        c->ackPacket(buf->pb);
        buf->pb = NULL;
      } else {
        struct pbuf * head = buf->pb;
        buf->pb = buf->pb->next;
        head->next = NULL;
        c->ackPacket(head);
      }
    }
  }
  c->write((const char*)buffer, written, 1);
  //free(buffer);
  //Serial.printf("tx:%u\n", written);
  return written;
}

void onPacket(void *obj, AsyncClient* c, struct pbuf *pb){
  //Serial.printf("rx:%u\n", pb->len);
  packet_buf_t * buf = (packet_buf_t*)obj;
  if(buf->pb) {
    pbuf_cat(buf->pb, pb);
  } else {
    buf->pb = pb;
    buf->index = 0;
    sendMax(obj, c);
  }
}

void onAck(void *obj, AsyncClient* c, size_t len, uint32_t time){
  //Serial.printf("ack:%u:%u\n", len, time);
  sendMax(obj, c);
}

void onDisconnect(void *obj, AsyncClient* c){
  Serial.printf("disconnect\n");
  c->free();
  delete c;
  packet_buf_t * buf = (packet_buf_t*)obj;
  pbuf_free(buf->pb);
  free(obj);
}

void onTimeout(void *obj, AsyncClient* c, uint32_t time){
  Serial.printf("timeout:%u\n", time);
  c->close();
}

void onClient(void *obj, AsyncClient* c){
  packet_buf_t * buf = (packet_buf_t*)malloc(sizeof(packet_buf_t));
  if(!buf){
    c->close();
    c->free();
    return;
  }
  Serial.printf("connect\n");
  buf->pb = NULL;
  buf->index = 0;
  c->onDisconnect(onDisconnect, buf);
  c->onTimeout(onTimeout, buf);
  c->onAck(onAck, buf);
  c->onPacket(onPacket, buf);
}

void setup() {
  Serial.begin(115200);

  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);
  while (WiFi.waitForConnectResult() != WL_CONNECTED) {
    Serial.println("Connection Failed! Rebooting...");
    delay(1000);
    ESP.restart();
  }
  Serial.println(WiFi.localIP());
  server.onClient(onClient, 0);
  server.begin();
}

void loop() {}