int nCliente = -1;
WebServer server (80);
WebSocketsServer webSocket = WebSocketsServer(81);



void webSocketEvent(uint8_t num, WStype_t type, uint8_t * payload, size_t length) {

  switch (type) {
    case WStype_DISCONNECTED:
      Serial.printf("[%u] Disconnected!\n", num);
      misMotores.Stop();
      nCliente =-1;
      break;

    case WStype_CONNECTED: {
        IPAddress ip = webSocket.remoteIP(num);
        Serial.printf("[%u] Connected from %d.%d.%d.%d url: %s\n", num, ip[0], ip[1], ip[2], ip[3], payload);
        webSocket.sendTXT(num, "Connected"); 
        nCliente = num;         // send message to client
      }
      break;

    case WStype_TEXT:
      //Serial.printf("Número de conexión: %u - Texto recibido: %s\n", num, payload);
      if (num == 0) {                                           //Únicamente va a reconocer la primera conexión (softAP).
        String str = (char*)payload;                            //Ejemplo de texto recibido: Velocidad:100 Angulo:-1.88
        int velocidad = str.substring(str.indexOf(':', str.indexOf("Velocidad:")) + 1, str.indexOf(' ', str.indexOf("Velocidad:"))).toInt();
        float angulo = str.substring(str.indexOf(':', str.indexOf("Angulo:")) + 1).toFloat();

        //uint8_t velocidadMax = velocidad*255/100;     //La velocidad se envía con valores entre 0 y 100. Se debe mapear a 0-255
        uint8_t velocidadMax = 85 + velocidad * 170 / 100; //Se corrige el mapeo porque el motor entre los valores 0-84 no se mueve, se mueve ente los valores 85-255

        uint8_t velocidadSin = abs(velocidadMax * sin(angulo));                   //Giro coche rápido
        //uint8_t velocidadSin = abs(velocidadMax*sin(abs(2*angulo/3)+PI/6));         //Giro coche intermedio
        //uint8_t velocidadSin = abs(velocidadMax*sin(abs(angulo/2)+PI/4));         //Giro coche lento
        float anguloCos = cos(angulo);

        //Sentido de giro de los motores
        if (angulo > 0) {                                       //Primer y segundo cuadrante
          misMotores.Forward();
        } else if (angulo < 0) {                                 //Tercer y cuarto cuadrante
          misMotores.Backward();
        } else {                                                 //Parado
          misMotores.Stop();
        }
        //Velocidad de los motores
        uint8_t izq, der;
        if (anguloCos > 0) {                                    //Primer y cuarto cuadrante
          izq = velocidadMax;
          der = velocidadSin;
        } else {                                                //Segundo y tercer cuadrante
          izq = velocidadSin;
          der = velocidadMax;
        }
        misMotores.Motores(izq,der);
        
        Serial.print(anguloCos); Serial.print("    "); Serial.print(izq); Serial.print("    "); Serial.println(der);
      }
      break;
  }
}

