#include <SoftwareSerial.h>
SoftwareSerial ESP(10, 11); // RX (cable Amarillo) | TX (cable Verde)

String W,W1,W2,W3,W4,W5;
char w;

int Condicion = 1;
int Salir = 0;
int Contador = 0;

String msg;
int OK;

int led = 13;
int cont = 0;

int xpin = 0;
int x;

char buf[40];

int increase_i(int i) {
    return (i + 1) % 4;   
}

void setup() {
  
  Serial.println("Listo");  
  pinMode(led,OUTPUT);
  digitalWrite(led,LOW);
  pinMode(xpin,INPUT);
  Serial.begin(9600);
  ESP.begin(9600);
  Serial.setTimeout(1000);
  Serial.println("Esperando");
  delay(1000);
  
///////////////// Comando AT  ///////////////////

  ESP.print("AT\r\n");
  W = ESP.readString();
  Serial.println(W);
  delay(1000);

///////////////// Comando CWMODE  ///////////////////
 
  ESP.print("AT+CWMODE=3\r\n");
  W = ESP.readString();
  Serial.print(W);
  delay(500);
  
////////////////// Comando CWJAP ////////////

  //ESP.print("AT+CWJAP=\"Richard\",\"qrtasj49\"\r\n");
  //ESP.print("AT+CWJAP=\"Fernandez.Pino1\",\"VALPO2014\"\r\n");
  ESP.print("AT+CWJAP=\"IRSS Deluxe\",\"robocup2015\"\r\n");
  W = ESP.readString();
  Serial.print(W);
  delay(10000);
  
////////////////// Comando CIPMUX ////////////   

  ESP.print("AT+CIPMUX=1\r\n");  
  W = ESP.readString();
  Serial.print(W);  
  delay(500);
  
////////////////// Comando CIPSERVER ////////////

  ESP.print("AT+CIPSERVER=1,1234\r\n");
  W = ESP.readString();
  Serial.print(W);
  delay(500);
  
  //////////////// Comando CIFSR //////////////
  
  ESP.print("AT+CIFSR\r\n");
  W = ESP.readString();
  Serial.print(W);
  delay(1000);
  ESP.setTimeout(400);
  digitalWrite(led,HIGH);
  Serial.println("Esperando Conexion Entrantes");
}

void loop(){


                  // Lo que entra por WIFI à Serial
  if (Condicion){
      if (ESP.available()){
         W = ESP.readString();
         Serial.print(W);
         Condicion = 0;
         Salir = 1;
      }
  }
  while(Salir){
          
           W1 = "AT+CIPSEND=0,";
           W2 = "12";           
           W3 = "\r\n"; // 17 hasta aca           
           String msg = W1+W2+W3;
           ESP.print(msg);

           int ID;
           int Packet_Number;
           int x,y,z;
           char data[4];
          
           char Paquete[12];
           
           ID = 0;
           Packet_Number = Contador;
           
           x = analogRead(xpin);    // read the input pin
           Serial.print(Contador);
           Serial.print(". Valor de x: ");
           Serial.println(x);             // debug value

           y = 0;
           z = 0;
           
           
           
           Paquete[0] = 0x00FF & (ID);
           Paquete[1] = 0x00FF & (ID >> 8);
           Paquete[2] = 0x00FF & (ID >> 16);
           Paquete[3] = 0x00FF & (ID >> 24);
           
           Paquete[4] = 0x00FF & (Packet_Number);
           Paquete[5] = 0x00FF & (Packet_Number >> 8);
           Paquete[6] = 0x00FF & (Packet_Number >> 16);
           Paquete[7] = 0x00FF & (Packet_Number >> 24);

           Paquete[8] = 0x00FF & x;
           Paquete[9] = 0x00FF & (((0x0300 & x) >> 8) | (y << 2));
           Paquete[10] = 0x00FF & (((0x03C0 & y) >> 6) | (z << 4));
           Paquete[11] = 0x00FF & (z >> 4);

           for (int i = 0; i < 12; i++) {
               ESP.write(Paquete[i]);
           }
           ESP.flush();
           
            int i = 0;
            
            while(ESP.available() == 0);
            
            while (1) {
                buf[i] = ESP.read();
                if (buf[i] == 'K'){
                  if (buf[i-1] == 'O'){
                    break;
                    }
                  }
                  if(buf[i] == 'k'){
                    if (buf[i-1] == 'n'){
                      if (buf[i-2] == 'i'){
                        Salir = 0;
                        Condicion = 1;
                        Serial.println("Cerrando Conexiones...");
                        delay(5000);
                        Serial.println("Listo");
                        break;
                      }
                    }
                  }
                else {
                  i++;
                  Serial.print(i);
                  Serial.print(" buf ");
                  Serial.println(buf[i]);
                }
              }

           Contador++;
           
         }
      }