#include <ESP8266WiFi.h>
#include <ESP8266WiFiMulti.h>
#include <ArduinoOTA.h>
#include <ESP8266mDNS.h>
#include <WiFiUdp.h>
#include <ArduinoOTA.h>
ESP8266WiFiMulti wifiMulti;
//variabls for blinking an LED with Millis
#define LED 2
 // ESP32 Pin to which onboard LED is connected
unsigned long previousMillis = 0;  // will store last time LED was updated
const long interval = 3000;  // interval at which to blink (milliseconds)
int ledState = LOW;  // ledState used to set the LED
//how many clients should be able to telnet to this ESP32
#define MAX_SRV_CLIENTS 1
const char* ssid = "xxxxxxxxxxxx";
const char* password = "xxxxxxxx";

// Set your Static IP address
IPAddress local_IP(192, 168, xxx, xxx);
// Set your Gateway IP address
IPAddress gateway(192, 168, xxx, xxx);

IPAddress subnet(255, 255,255, 0);
IPAddress primaryDNS(192, 168, xxx, xxx);   //optional
IPAddress secondaryDNS(192, 168, xxx, xxx); //optional



WiFiServer server(23);
WiFiClient serverClients[MAX_SRV_CLIENTS];

void wifisetup(){
    Serial1.println("Connecting Wifi ");
  for (int loops = 10; loops > 0; loops--) {
    if (wifiMulti.run() == WL_CONNECTED) {
      Serial1.println("");
      Serial1.print("WiFi connected ");
      Serial1.print("IP address: ");
      Serial1.println(WiFi.localIP());
      break;
    }
    else {
      Serial1.println(loops);
      delay(1000);
    }
  }
  if (wifiMulti.run() != WL_CONNECTED) {
    Serial1.println("WiFi connect failed");
    delay(1000);
    ESP.restart();
}
}

void setup() {
  
  Serial1.begin(115200);
  Serial1.println("\nConnecting");
  pinMode(LED,OUTPUT);
  digitalWrite(LED,  LOW);

   // Configures static IP address
  if (!WiFi.config(local_IP, gateway, subnet, primaryDNS, secondaryDNS)) {
    Serial1.println("STA Failed to configure");
  }

  wifiMulti.addAP(ssid, password);
  //wifiMulti.addAP("x1", "x1");
  wifisetup();

  // }

  //start UART and the server
  Serial.begin(9600);
  server.begin();
  server.setNoDelay(true);




  Serial1.print("Ready! Use 'telnet ");
  Serial1.print(WiFi.localIP());
  Serial1.println(" 23' to connect");
}

int count;
void loop() {
  uint8_t i;

  
  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
  // save the last time you blinked the LED
  previousMillis = currentMillis;
  // if the LED is off turn it on and vice-versa:
  ledState = not(ledState);
  // set the LED with the ledState of the variable:
  digitalWrite(LED,  ledState);
  }


  if (wifiMulti.run() == WL_CONNECTED) {
    //check if there are any new clients
 
    count = 0;
    if (server.hasClient()){
      for(i = 0; i < MAX_SRV_CLIENTS; i++){
        //find free/disconnected spot
        if (!serverClients[i] || !serverClients[i].connected()){
          if(serverClients[i]) serverClients[i].stop();
          serverClients[i] = server.available();
          if (!serverClients[i]) Serial1.println("available broken");
          Serial1.print("New client: ");
          Serial1.print(i); Serial1.print(' ');
          Serial1.println(serverClients[i].remoteIP());
          break;
        }
      }
      if (i >= MAX_SRV_CLIENTS) {
        //no free/disconnected spot so reject
        server.available().stop();
      }
    }
    //check clients for data
    for(i = 0; i < MAX_SRV_CLIENTS; i++){
      if (serverClients[i] && serverClients[i].connected()){
        if(serverClients[i].available()){
          //get data from the telnet client and push it to the UART
          while(serverClients[i].available()) {Serial.write(serverClients[i].read());
            // if the LED is off turn it on and vice-versa:
  ledState = not(ledState);
  // set the LED with the ledState of the variable:
  digitalWrite(LED,  ledState);}
        }
      }
      else {
        if (serverClients[i]) {
          serverClients[i].stop();
        }
      }
    }
    //check UART for data
    if(Serial.available()){
      size_t len = Serial.available();
      uint8_t sbuf[len];
      Serial.readBytes(sbuf, len);
      //push UART data to all connected telnet clients
      for(i = 0; i < MAX_SRV_CLIENTS; i++){
        if (serverClients[i] && serverClients[i].connected()){
          serverClients[i].write(sbuf, len);
          delay(1);
            ledState = not(ledState);
            // set the LED with the ledState of the variable:
            digitalWrite(LED,  ledState);
        }
      }
    }
  }
  else {
    Serial1.println("WiFi not connected!");
    for(i = 0; i < MAX_SRV_CLIENTS; i++) {
      if (serverClients[i]) serverClients[i].stop();
    }
    wifisetup();
    }

  }