void Con::handleWiFiConnect(){
  this->printf("handleWiFiConnect %d\n",this->curType&CON_TCP);
  if (this->curType&CON_TCP){
     ConSrv.begin();
     ConSrv.setNoDelay(true);   
  }
}

void loop() {

	static boolean packetActive = false;
	static uint8_t index = 0;
	static boolean readingReturn = false;
	static uint8_t rindex = 0;

	if (!client.connected()) {
		// try to connect to a new client
		client = server.available();
	} else {
		// read data from the connected client
		if (client.available() > 0) {
			char c = client.read();

			if (packetActive) {
				commandBuffer[index] = c;
				commandBuffer[++index] = 0;
				if (c == EOP) {
					comHandler();
					packetActive = false;
				}
			}

			else if (c == SOP) {
				index = 0;
				commandBuffer[index] = c;
				commandBuffer[++index] = 0;
				packetActive = true;
			}

			if (returnReady) {
				client.println(returnBuffer);
				server.println(returnBuffer);
				Serial.println(returnBuffer);
				returnReady = false;
			}
		}
	}

	if (Serial.available() && !returnReady) {
		char s = Serial.read();

		if (s == SOP) {
			readingReturn = true;
			rindex = 0;
		}
		if (readingReturn) {
			returnBuffer[rindex] = s;
			returnBuffer[++rindex] = 0;

			if (s == EOP) {
				returnReady = true;
			}
		}
	}
}

void Con::TCPhandler(){
   if (this->curType&CON_TCP){
      if (ConSrv.hasClient()){
         if (ConTcp.connected()){
            ConTcp.stop();
         }
         ConTcp=ConSrv.available();
         ConTcp.write("Hello\n",6);
      }  
   }
}

void cTcpTrace::task(void)
{
  if (tcpTraceServer.hasClient())
  {
    if (!tcpTraceClient || !tcpTraceClient.connected())
    {
      if(tcpTraceClient) tcpTraceClient.stop();
      tcpTraceClient = tcpTraceServer.available();
      println("Welcom to trace.");
    }
  }
}

void WiFiManager::startWebConfig() {
  DEBUG_PRINT("");
  DEBUG_PRINT("WiFi connected");
  DEBUG_PRINT(WiFi.localIP());
  DEBUG_PRINT(WiFi.softAPIP());
  if (!mdns.begin(_apName, WiFi.localIP())) {
    DEBUG_PRINT("Error setting up MDNS responder!");
    while (1) {
      delay(1000);
    }
  }
  DEBUG_PRINT("mDNS responder started");
  // Start the server
  server.begin();
  DEBUG_PRINT("Server started");

  while (serverLoop() == WM_WAIT) {
    //looping
    if(timeout > 0 && start + timeout < millis()) {
      //we passed timeout value, release
      DEBUG_PRINT("timeout reached");
      return;
    }
  }

  DEBUG_PRINT("Setup done");
  delay(5000);
  ESP.reset();

}

void setup() {

	pinMode(enA, OUTPUT);
	pinMode(enB, OUTPUT);
	pinMode(dirA, OUTPUT);
	pinMode(dirB, OUTPUT);

	setupWiFi();
	server.begin();
#ifdef DEBUG
	delay(2000);
#endif
	Serial.begin(115200);

#ifdef DEBUG

	delay(250);
	Serial.println();
	Serial.println("Hello");
	Serial.flush();
	delay(750);
	Serial.print("IP address");
	Serial.println(WiFi.localIP());
	delay(500);
#endif
}

void WiFiManager::startWebConfig() {
    DEBUG_PRINT("");
    display.print("WiFi connected");
    DEBUG_PRINT("WiFi connected");
    DEBUG_PRINT(WiFi.localIP());
    DEBUG_PRINT(WiFi.softAPIP());
    if (!mdns.begin(_apName, WiFi.localIP())) {
        DEBUG_PRINT("Error setting up MDNS responder!");
        display.print("mDNS error");
        while(1) {
            delay(1000);
        }
    }
    DEBUG_PRINT("mDNS responder started");
    // Start the server
    server_s.begin();
    display.print("Server started");
    DEBUG_PRINT("Server started");

    while(serverLoop() == WM_WAIT) {
      //looping
    }

    display.print("All done.  Bye.");
    DEBUG_PRINT("Setup done");
    delay(10000);
    ESP.reset();
}

void setup() {
  Serial.begin(115200);
  // Initialize the output variables as outputs
  pinMode(output5, OUTPUT);
  pinMode(output4, OUTPUT);
  // Set outputs to LOW
  digitalWrite(output5, LOW);
  digitalWrite(output4, LOW);

  // Connect to Wi-Fi network with SSID and password
  Serial.print("Connecting to ");
  Serial.println(ssid);
  //WiFi.begin(ssid, password);
  setupWiFi();
  server.begin();


//  while (WiFi.status() != WL_CONNECTED) {
//    delay(500);
//    Serial.print(".");
//  }

//  WiFiClient client = server.available();
//  if (!client) {
//    return;
//  }

}

void transmitTele()
{
  Serial.println("tele"); 
  
  while(!alreadyConnectedTele)
  {
    //attempt to save a client connecting to the server
    teleClient = teleServer.available();
    
    //if a client is connected, begin communication
    if (teleClient) {
      if (!alreadyConnectedTele) {
        // clead out the input buffer:
        teleClient.flush();
        Serial.println("We have a new client");
        alreadyConnectedTele = true;
        
      }
    }
  }
  
    for(int i = 0; i < 20; i++)
    {
      if (teleClient.available() > 0) {
        // read the bytes incoming from the client:
        char thisChar = teleClient.read();
        // echo the bytes back to the client:
        
        if(thisChar == '1')
          teleServer.println(curSpeedKn * KNOTS_TO_MPS);
          
        if(thisChar == '2')  
          teleServer.println(pollPing());
          
        if(thisChar == '3')
          teleServer.println(distToTar());
          
        if(thisChar == '4')
          teleServer.println(curLat);
        
        if(thisChar == '5')
          teleServer.println(curLon);
        
        if(thisChar == '6')
          teleServer.println(curHead);
        
        
        // echo the bytes to the server as well:
        Serial.println(thisChar);
      }
    }
    
    
    
  
}

void start_server() {
  Serial.print("\nMAC: "); Serial.println(WiFi.macAddress());
  Serial.print("\nSSID: "); Serial.println(ssid);
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  server.begin();
  delay(10000);
  Serial.print("\nIP: "); Serial.println(WiFi.localIP());
}

void APServer(){
    WiFiClient client = server.available();  // Check if a client has connected
    if (!client) {
        return;
    }
    Serial.println("");
    Serial.println("New client");

    // Wait for data from client to become available
    while (client.connected() && !client.available()) {
      delay(1);
    }
    
    // Read the first line of HTTP request
    String req = client.readStringUntil('\r');
    Serial.println(req);
    String s;
    String gsid;
    String gpwd;
    //if the form has been submitted
    if(req.indexOf("ssid")!=-1){
      //TODO Make this a POST request
      //I'm basically hoping they're being nice
      int x = req.indexOf("ssid")+5;
      gsid = req.substring(x,req.indexOf("&",x));
      x = req.indexOf("pwd")+4;
      gpwd = req.substring(x,req.indexOf(" ",x));
      s = gsid;
      saveWifiConfig(gsid,gpwd);
      Serial.print("Restarting");
      client.print("Thanks! Restarting to new configuration");
      client.flush();
      ESP.restart();
    }
    else{
    s =  "<h1>Welcome to your ESP</h1><form action='/' method='get'><table><tr><td>SSID</td><td><input type='text' name='ssid'></td></tr><tr><td>Password</td><td><input type='text' name='pwd'></td><tr><td /><td><input type='submit' value='Submit'></td></tr></form>";
    client.print(s);
    }
    client.flush();
    delay(1);
    //Serial.println("Client disonnected");
}

void mainsetup()
{
  // setup globals
  ulReqcount = 0;
  ulReconncount = 0;
  WiFiStart();
  delay(1);

  server.begin();

  // allocate ram for data storage
  uint32_t free = system_get_free_heap_size() - KEEP_MEM_FREE;
  ulNoMeasValues = free / (sizeof(float) * 5 + sizeof(unsigned long)); // humidity & temp --> 2 + time
  pulTime = new unsigned long[ulNoMeasValues];
  pfTemp1 = new float[ulNoMeasValues];
  pfTemp2 = new float[ulNoMeasValues];
  pfTemp3 = new float[ulNoMeasValues];
  pfTemp4 = new float[ulNoMeasValues];

  if (pulTime == NULL || pfTemp1 == NULL || pfTemp2 == NULL || pfTemp3 == NULL || pfTemp4 == NULL)
  {
    ulNoMeasValues = 0;
    Serial.println("Error in memory allocation!");
  }
  else
  {
    Serial.print("Allocated storage for ");
    Serial.print(ulNoMeasValues);
    Serial.println(" data points.");

    float fMeasDelta_sec = MEAS_SPAN_H * 3600. / ulNoMeasValues;
    ulMeasDelta_ms = ( (unsigned long)(fMeasDelta_sec + 0.5) ) * 1000; // round to full sec
    Serial.print("Measurements will happen each ");
    Serial.print(ulMeasDelta_ms);
    Serial.println(" ms.");

    ulNextMeas_ms = millis() + ulMeasDelta_ms;
  }
}

void WiFiStart()
{
  ulReconncount++;



  while (WiFi.status() != WL_CONNECTED)
  {
    delay(500);
    Serial.print(".");
  }
  Serial.println("");
  Serial.println("WiFi connected");

  // Start the server
  server.begin();
  Serial.println("Server started");

  // Print the IP address
  Serial.println(WiFi.localIP());

}

void setup() {
  Serial.begin(115200);
  pixels.begin();
  pixels.setBrightness(BASE_BRIGHTNESS);

  reset_pixels();

  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(ssid);

  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);
  WiFi.hostname(HOSTNAME);

  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }

  Serial.println("WiFi connected");
  Serial.println(WiFi.localIP());

  if (!MDNS.begin(HOSTNAME)) {
    update_pixel(0, 200, 0, 0);
    Serial.println("Error setting up MDNS responder!");
    while (1) {
      delay(1000);
    }
  }
  Serial.println("mDNS responder started");

  update_pixel(0, 0, 200, 0);
  pixels.show();

  server.begin();
  MDNS.addService("http", "tcp", 80);
}

void next_client() {
  // Listenning for new clients
  client = server.available();
  if (client) {
    Serial.println("New client");
    // bolean to locate when the http request ends
    boolean blank_line = true;
    while (client.connected()) {
      if (client.available()) {
        char c = client.read();
        if (c == '\n' && blank_line) {
            client.println("HTTP/1.1 200 OK");
            client.println("Content-Type: application/json");
            client.println("Access-Control-Allow-Origin: *");
            client.println("Connection: close");
            client.println();
            print_json();
            break;
        }
        if (c == '\n') {
          // when starts reading a new line
          Serial.println();
          blank_line = true;
        }
        else if (c != '\r') {
          // when finds a character on the current line
          Serial.write(c);
          blank_line = false;
        }
      }
    }  
    // closing the client connection
    delay(1);
    client.stop();
    Serial.println("Client disconnected.");
  }
}

void setup() {
  Serial.begin(115200);
  // Initialize the output variables as outputs
  pinMode(output5, OUTPUT);
  pinMode(output4, OUTPUT);
  // Set outputs to LOW
  digitalWrite(output5, LOW);
  digitalWrite(output4, LOW);

  // Connect to Wi-Fi network with SSID and password
  Serial.print("Connecting to ");
  Serial.println(ssid);
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  // Print local IP address and start web server
  Serial.println("");
  Serial.println("WiFi connected.");
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());
  server.begin();
}

void cTcpTrace::begin()
{
  tcpTraceServer.begin();
}

void waitForTarget()
{
  //define a client object to connect to the server
  WiFiClient mainClient;
  
  //wait for a client to connect to the server
  while(!alreadyConnectedMain)
  {
    //attempt to save a client connecting to the server
    mainClient = mainServer.available();
    
    //if a client is connected, begin communication
    if (mainClient) {
      if (!alreadyConnectedMain) {
        // clead out the input buffer:
        mainClient.flush();
        Serial.println("We have a new client");
        debugServer.println("We have a new client");
        mainClient.println("Hello, client!");
        alreadyConnectedMain = true;
        
      }
    }
    
    delay(100);
    
    delay(100);
  }
  Serial.println("writing");
  debugClient.println("writing");
  
  //mainServer.println("ready");
  delay(1000);
  
  //Strings to read in latitude and longitude from the client
  char lat[50] = "";
  char lon[50] = "";
  
  int ind = 0;
  
  //Wait for input to be on the buffer
  while(!mainClient.available());
  
  char destNum = '0';
  
  while(!(destNum == '1' || destNum == '2' || destNum == '3'))
  {
    destNum = mainClient.read();
    Serial.println(destNum);
  }
  
  if(destNum == '1')
  {
    tarLat = LAT1;
    tarLon = LON1;
  }
  if(destNum == '2')
  {
    tarLat = LAT2;
    tarLon = LON2;
  }
  if(destNum == '3')
  {
    tarLat = LAT3;
    tarLon = LON3;
  }
  
  /*
  //Read in characters from the input buffer until a new line character is reached
  //this will be the latitude
  while(mainClient.available())
    {
      char c = mainClient.read();
      lat[ind] = c;
      
      if(c == '\n')
      {
        lat[ind] = NULL;
        break;
      }
      
      ind++; 
    }
  
  ind = 0;
  
  //Read in characters from the input buffer until a new line character is reached
  //this will be the longitude
  while(mainClient.available())
    {
      char c = mainClient.read();
      lon[ind] = c;
      
      if(c == '\n')
      {
        lon[ind] = NULL;
        break;
      }
      
      ind++; 
//.........这里部分代码省略.........

int WiFiManager::serverLoop()
{
    // Check for any mDNS queries and send responses
    mdns.update();
    String s;

    WiFiClient client = server_s.available();
    if (!client) {
        return(WM_WAIT);
    }

    DEBUG_PRINT("New client");
    
    // Wait for data from client to become available
    while(client.connected() && !client.available()){
        delay(1);
    }
    
    // Read the first line of HTTP request
    String req = client.readStringUntil('\r');
    
    // First line of HTTP request looks like "GET /path HTTP/1.1"
    // Retrieve the "/path" part by finding the spaces
    int addr_start = req.indexOf(' ');
    int addr_end = req.indexOf(' ', addr_start + 1);
    if (addr_start == -1 || addr_end == -1) {
        DEBUG_PRINT("Invalid request: ");
        DEBUG_PRINT(req);
        return(WM_WAIT);
    }
    req = req.substring(addr_start + 1, addr_end);
    DEBUG_PRINT("Request: ");
    DEBUG_PRINT(req);
    client.flush();

    if (req == "/")
    {
        s = HTTP_200;
        String head = HTTP_HEAD;
        head.replace("{v}", "Config ESP");
        s += head;
        s += HTTP_SCRIPT;
        s += HTTP_STYLE;
        s += HTTP_HEAD_END;

        int n = WiFi.scanNetworks();
        DEBUG_PRINT("scan done");
        if (n == 0) {
            DEBUG_PRINT("no networks found");
            s += "<div>No networks found. Refresh to scan again.</div>";
        }
        else {
            for (int i = 0; i < n; ++i)
            {
                DEBUG_PRINT(WiFi.SSID(i));
                DEBUG_PRINT(WiFi.RSSI(i));
                String item = HTTP_ITEM;
                item.replace("{v}", WiFi.SSID(i));
                s += item;
                delay(10);
            }
        }
        
        s += HTTP_FORM;
        s += HTTP_END;
        
        DEBUG_PRINT("Sending config page");
    }
    else if ( req.startsWith("/s") ) {
        String qssid;
        qssid = urldecode(req.substring(8,req.indexOf('&')).c_str());
        DEBUG_PRINT(qssid);
        DEBUG_PRINT("");
        req = req.substring( req.indexOf('&') + 1);
        String qpass;
        qpass = urldecode(req.substring(req.lastIndexOf('=')+1).c_str());

        setEEPROMString(0, 32, qssid);
        setEEPROMString(32, 64, qpass);

        EEPROM.commit();

        s = HTTP_200;
        String head = HTTP_HEAD;
        head.replace("{v}", "Saved config");
        s += HTTP_STYLE;
        s += HTTP_HEAD_END;
        s += "saved to eeprom...<br/>resetting in 10 seconds";
        s += HTTP_END;
        client.print(s);
        client.flush();

        DEBUG_PRINT("Saved WiFiConfig...restarting.");
        return WM_DONE;
    }
    else
    {
        s = HTTP_404;
        DEBUG_PRINT("Sending 404");
    }
//.........这里部分代码省略.........

void loop(){
  WiFiClient client = server.available();   // Listen for incoming clients

  if (client) {
      // Print local IP address and start web server
      Serial.println("");
      Serial.println("WiFi connected.");
      Serial.println("IP address: ");
      Serial.println(WiFi.localIP());
      //server.begin();

      // If a new client connects,
    Serial.println("New Client.");          // print a message out in the serial port
    String currentLine = "";                // make a String to hold incoming data from the client
    while (client.connected()) {            // loop while the client's connected
      if (client.available()) {             // if there's bytes to read from the client,
        char c = client.read();             // read a byte, then
        Serial.write(c);                    // print it out the serial monitor
        header += c;
        if (c == '\n') {                    // if the byte is a newline character
          // if the current line is blank, you got two newline characters in a row.
          // that's the end of the client HTTP request, so send a response:
          if (currentLine.length() == 0) {
            // HTTP headers always start with a response code (e.g. HTTP/1.1 200 OK)
            // and a content-type so the client knows what's coming, then a blank line:
            client.println("HTTP/1.1 200 OK");
            client.println("Content-type:text/html");
            client.println("Connection: close");
            client.println();

            // turns the GPIOs on and off
            if (header.indexOf("GET /5/on") >= 0) {
              Serial.println("GPIO 5 on");
              output5State = "on";
              digitalWrite(output5, HIGH);
            } else if (header.indexOf("GET /5/off") >= 0) {
              Serial.println("GPIO 5 off");
              output5State = "off";
              digitalWrite(output5, LOW);
            } else if (header.indexOf("GET /4/on") >= 0) {
              Serial.println("GPIO 4 on");
              output4State = "on";
              digitalWrite(output4, HIGH);
            } else if (header.indexOf("GET /4/off") >= 0) {
              Serial.println("GPIO 4 off");
              output4State = "off";
              digitalWrite(output4, LOW);
            }

            // Display the HTML web page
            client.println("<!DOCTYPE html><html>");
            client.println("<head><meta name=\"viewport\" content=\"width=device-width, initial-scale=1\">");
            client.println("<link rel=\"icon\" href=\"data:,\">");
            // CSS to style the on/off buttons
            // Feel free to change the background-color and font-size attributes to fit your preferences
            client.println("<style>html { font-family: Helvetica; display: inline-block; margin: 0px auto; text-align: center;}");
            client.println(".button { background-color: #195B6A; border: none; color: white; padding: 16px 40px;");
            client.println("text-decoration: none; font-size: 30px; margin: 2px; cursor: pointer;}");
            client.println(".button2 {background-color: #77878A;}</style></head>");

            // Web Page Heading
            client.println("<body><h1>ESP8266 Web Server</h1>");

            // Display current state, and ON/OFF buttons for GPIO 5
            client.println("<p>GPIO 5 - State " + output5State + "</p>");
            // If the output5State is off, it displays the ON button
            if (output5State=="off") {
              client.println("<p><a href=\"/5/on\"><button class=\"button\">ON</button></a></p>");
            } else {
              client.println("<p><a href=\"/5/off\"><button class=\"button button2\">OFF</button></a></p>");
            }

            // Display current state, and ON/OFF buttons for GPIO 4
            client.println("<p>GPIO 4 - State " + output4State + "</p>");
            // If the output4State is off, it displays the ON button
            if (output4State=="off") {
              client.println("<p><a href=\"/4/on\"><button class=\"button\">ON</button></a></p>");
            } else {
              client.println("<p><a href=\"/4/off\"><button class=\"button button2\">OFF</button></a></p>");
            }
            client.println("</body></html>");

            // The HTTP response ends with another blank line
            client.println();
            // Break out of the while loop
            break;
          } else { // if you got a newline, then clear currentLine
            currentLine = "";
          }
        } else if (c != '\r') {  // if you got anything else but a carriage return character,
          currentLine += c;      // add it to the end of the currentLine
        }
      }
    }
    // Clear the header variable
    header = "";
    // Close the connection
    client.stop();
    Serial.println("Client disconnected.");
    Serial.println("");
//.........这里部分代码省略.........