boolean receivePacket(UDPpacket& packet, WiFly& wifly)
{
	boolean ret;
	char buf[100];
	ret = wifly.gets(buf, sizeof(buf));
	packet.data += buf;
// 	do {
// 		rByte = wifly.read();
// 		//Serial.println(rByte);
// 		packet.data = packet.data + (char) rByte;
// 		if((char)rByte == '\n') {
// 			break;
// 		}
// 	}while(rByte != -1);
	packet.dataLength = packet.data.length();
	wifly.getHostIP(packet.ip, sizeof(packet.ip));
	packet.port = wifly.getHostPort();

	return true;
	
// 	if(!packet.data.compareTo("-1"))
// 		return false;
// 	else
// 		return true;
}

boolean sendPacket(UDPpacket& packet, WiFly& wifly)
{
	boolean ret;
	char buf[100];
	
	packet.data.toCharArray(buf, sizeof(buf));

	ret = wifly.sendto((const char *)buf, packet.ip, packet.port);
	wifly.flush();
	return ret;
}

/* Connect the WiFly serial to the serial monitor. */
void terminal()
{
  while (1) {
    if (wifly.available() > 0) {
     Serial.write(wifly.read());
    }

    if (Serial.available() > 0) {
      wifly.write(Serial.read());
    }
  }
}

bool wifiSetup(WiFly myWiFly) {
	// Scan, nouveau mode nécessite un firmware >= 2.22
	if (myWiFly.version() < 2.22)
	{
		return false;
	}
	
	if (myWiFly.sendCommand(wifiCompactModeStr))
	{
		return true;
	}
	else
	{
		return false;
	}
}

bool wifiScan(WiFly myWiFly) {
	// un scan wifi via le RN 171 met environ 2500ms par défaut:
	// - 200ms par canal
	// - 13 canaux
	if (myWiFly.sendCommand(wifiScanStr))
	{
		return true;
	}
	else
	{
		return false;
	}
}

int wifiScanReadLn(WiFly myWiFly, char *ptrLine) {
	bool endOfLine = false;
	char c=' ';
	int i=0;

	while (!endOfLine && i<(MAX_LENGTH_SCAN_LINE-1)) 	{
		if (myWiFly.receive((uint8_t *)&c, 1, 300) > 0) {
			if (c != '\r')
				ptrLine[i++] = c;
			else
				endOfLine = true;
		}
		else endOfLine = true; // Plus rien de dispo en entrée...
	}

	ptrLine[i] = '\0';

	return i;
}

void setup()
{

    Serial.begin(115200);

#ifdef DEBUG
    Serial.println(F("Starting"));
    Serial.print(F("Free memory: "));
    Serial.println(wifly.getFreeMemory(),DEC);
#endif

    wifiSerial.begin(9600);
    if (!wifly.begin(&wifiSerial, &Serial)) {
#ifdef DEBUG
        Serial.println(F("Failed to start wifly"));
#endif
	wifly.terminal();
    }

	wifly.leave();

	delay(1000);

	wifly.scan(scanbuf, sizeof(scanbuf));


#ifdef DEBUG
	Serial.println(scanbuf);
#endif
	
	snprintf_P(scanbuf, sizeof(scanbuf), PSTR("%s b 13 -71 c 00:1c:58:10:1c:90 | a b 10 -93 c 00:12:43:8a:ea:22 | a b 11 -92 c d8:30:62:5f:f2:b1"), "a");

    /* Join wifi network if not already associated */
    if (!wifly.isAssociated()) {
	/* Setup the WiFly to connect to a wifi network */
#ifdef DEBUG
		Serial.println(F("Joining network"));
#endif
		wifly.setSSID(mySSID);
		wifly.setPassphrase(myPassword);
		wifly.enableDHCP();
		wifly.setDHCP(1);
		wifly.save();

		if (wifly.join()) {
#ifdef DEBUG
		    Serial.println(F("Joined wifi network"));
#endif
		} else {
#ifdef DEBUG
		    Serial.println(F("Failed to join wifi network"));
#endif
		    wifly.terminal();
		}
    } else {
#ifdef DEBUG
        Serial.println(F("Already joined network"));
#endif
    }

    wifly.setBroadcastInterval(0);	// Turn off UPD broadcast

#ifdef DEBUG
    Serial.print(F("MAC: "));
    Serial.println(wifly.getMAC(macaddr, sizeof(macaddr)));
    Serial.print(F("IP: "));
    Serial.println(wifly.getIP(buf, sizeof(buf)));
#endif

    wifly.setDeviceID("Wifly-WebServer");

    if (wifly.isConnected()) {
#ifdef DEBUG
        Serial.println(F("Old connection active. Closing"));
#endif
	    wifly.close();
    }

    wifly.setProtocol(WIFLY_PROTOCOL_TCP | WIFLY_PROTOCOL_UDP);
    if (wifly.getPort() != 80) {
        wifly.setPort(80);
	    wifly.save();
#ifdef DEBUG
	    Serial.println(F("Set port to 80, rebooting to make it work"));
#endif
	    wifly.reboot();
	    delay(3000);
    }
    Serial.println(F("Ready"));

}

void setup() {
  char buf[64];

  Serial.begin(115200);
  Serial.println("Starting");
  Serial.print("Free memory: ");
  Serial.println(wifly.getFreeMemory(),DEC);

  wifiSerial.begin(9600);
  if (!wifly.begin(&wifiSerial, &Serial)) {
    Serial.println("Failed to start wifly");
  }

  if (!wifly.isAssociated()) {
    Serial.println("Joining network");
    wifly.setSSID(mySSID);
    wifly.setPassphrase(myPassword);
    wifly.enableDHCP();

    if (wifly.join()) {
      Serial.println("Joined wifi network");
    } else {
      Serial.println("Failed to join wifi network");
    }
  } else {
    Serial.println("Already joined network");
  }

  Serial.println("WiFly ready");

  Serial.print("MAC: ");
  Serial.println(wifly.getMAC(buf, sizeof(buf)));
  Serial.print("IP: ");
  Serial.println(wifly.getIP(buf, sizeof(buf)));
  Serial.print("Netmask: ");
  Serial.println(wifly.getNetmask(buf, sizeof(buf)));
  Serial.print("Gateway: ");
  Serial.println(wifly.getGateway(buf, sizeof(buf)));
  Serial.println("Set DeviceID");
  wifly.setDeviceID("Wifly-TCP");
  Serial.print("DeviceID: ");
  Serial.println(wifly.getDeviceID(buf, sizeof(buf)));

  wifly.setIpProtocol(WIFLY_PROTOCOL_TCP);

  if (wifly.isConnected()) {
    Serial.println("Old connection active. Closing");
    wifly.close();
  }
}

void readResponse (char *buf, int max_len, int content_len) {
  // Read content.
  int len = wifly.readBytes(buf, max_len > content_len ? content_len : max_len);
  buf[len] = '\0'; // insurance
  
  // Flush buffer.
  while (wifly.available() > 0) {
    wifly.read();
  }
}

void debugWifiState () {
  char buf[32];
  Serial.print(F("MAC: "));
  Serial.println(wifly.getMAC(buf, sizeof(buf)));
  Serial.print(F("IP: "));
  Serial.println(wifly.getIP(buf, sizeof(buf)));
  Serial.print(F("Netmask: "));
  Serial.println(wifly.getNetmask(buf, sizeof(buf)));
  Serial.print(F("Gateway: "));
  Serial.println(wifly.getGateway(buf, sizeof(buf)));
  Serial.print(F("DeviceID: "));
  Serial.println(wifly.getDeviceID(buf, sizeof(buf)));
}

void JSONAPI::_headerStart (const char *method) {
  // If an old connection is active, close.
  if (wifly.isConnected()) {
    wifly.close();
  }

  if (!wifly.open(this->host, 80)) {
    Serial.println(F("Failed to connect to host."));
  }
  
  wifly.print(method);
  wifly.print(" ");
  // wifly.print("http://");
  // wifly.print(this->host);
}

int JSONAPI::request ( js0n_user_cb_t cb ) {
  if (this->hasBody) {
    wifly.println("0");
    wifly.println();
  }
  
  js0n_parser_t parser;
  parser.buffer = this->buffer;
  parser.stream = &wifly;
  parser.user_cb = cb;
  
  int status_code = 0;
  readResponseHeaders(&status_code, (int *) &parser.length);
  if (status_code != 0 && status_code < 500) {
    int parse_status = js0n_parse ( &parser );
  }
  return status_code;
}

void loop() {
  int available;

  if (wifly.isConnected() == false) {
    Serial.println("Connecting");

    if (wifly.open("192.168.1.100", 4001)) {
      Serial.println("Connected");
    } else {
      Serial.println("Failed to open");
    }
  } else {
    available = wifly.available();

    if (available < 0) {
      Serial.println("Disconnected");
    } else if (available > 0) {
      Serial.write(wifly.read());
    }
  }
}

void JSONAPI::_headerEnd () {
  wifly.println(" HTTP/1.1"); // paste your number here
  wifly.print("Host: ");
  wifly.println(this->host);
  wifly.println("User-Agent: lifegraph/0.0.1");
  if (this->hasBody) {
    wifly.println("Content-Type: application/x-www-form-urlencoded");
    wifly.println("Transfer-Encoding: chunked");
  }
  wifly.println();
}

void loop()
{

    if (wifly.available() > 0) {

    	/* See if there is a request */
		if (wifly.gets(buf, sizeof(buf))) {
		    if (strncmp_P(buf, PSTR("GET /ping"), 9) == 0) {

				/* GET request */
#ifdef DEBUG
				Serial.println(F("PONG XML requested"));
#endif
				while (wifly.gets(buf, sizeof(buf)) > 0) {
				    //Skip rest of request
				}

				sendPong();

	   	 	} else if (strncmp_P(buf, PSTR("GET /data"), 9) == 0) {

	        	/* POST request */
#ifdef DEBUG
	        	Serial.println(F("DATACOLLECTOR XML: sendind sensors data"));
#endif

				while (wifly.gets(buf, sizeof(buf)) > 0) {
				    //Skip rest of request
				}

                // discard rest of input
		    	// wifly.flushRx();		
				sendSensorsDataXML();

	    	} else {

	       		// Unexpected request
#ifdef DEBUG
				Serial.print(F("Unexpected: "));
				Serial.println(buf);
				Serial.println(F("Sending 404"));
#endif
				while (wifly.gets(buf, sizeof(buf)) > 0) {
				    //Skip rest of request
				}

                // discard rest of input
				wifly.flushRx();		
				send404();

	    	}
		}
    }
}

int LifegraphAPI::connect (uint8_t uid[], int uidLength) {
  this->hasBody = false;
  this->_headerStart("GET");
  this->_headerPath("tokens");
  wifly.print("/");
  char token[3];
  for (int i = 0; i < uidLength; i++) {
    snprintf(token, 3, "%02x", uid[i]);
    wifly.print(token);
  }
  wifly.print("?namespace=");
  wifly.print(this->ns);
  wifly.print("&key=");
  wifly.print(this->key);
  wifly.print("&secret=");
  wifly.print(this->secret);
  this->_headerEnd();
  return this->request( NULL );
}

/** Send an index HTML page with an input box for a username */
void sendPong()
{
    /* Send the header direclty with print */
    wifly.println(F("HTTP/1.1 200 OK"));
    wifly.println(F("Content-Type: text/xml"));
    wifly.println(F("Transfer-Encoding: chunked"));
    wifly.println();

    /* Send the body using the chunked protocol so the client knows when
     * the message is finished.
     * Note: we're not simply doing a close() because in version 2.32
     * firmware the close() does not work for client TCP streams.
     */
    wifly.sendChunkln(F("<?xml version=\"1.0\" encoding=\"UTF-8\"?>"));
	snprintf_P(wbuf, sizeof(wbuf), PSTR("<pong>%s</pong>"), macaddr);
    wifly.sendChunkln(wbuf);

    wifly.sendChunkln();
}

boolean connectWifi (SoftwareSerial *wifiSerial, const char *ssid, const char *pass) {
  if (!wifly.begin(wifiSerial, &Serial)) {
    Serial.println(F("Could not start Wifly module."));
    terminal();
    return false;
  }
 
  // Join wifi network if not already associated
  if (!wifly.isAssociated()) {
    wifly.setSSID(ssid);
    wifly.setPassphrase(pass);
    wifly.enableDHCP();
    wifly.setDeviceID("Wifly-WebClient");
 
    if (!wifly.join()) {
      Serial.println(F("Failed to join wifi network"));
      terminal();
      return false;
    }
  }
  return true;
}

void readResponseHeaders (int *status_code, int *content_len) {
  boolean line_start = true;
  char toss[1], buf[64];
  int len;
  *content_len = *status_code = 0;

  // HTTP/1.1 xxx
  if (wifly.readBytes(buf, 9) == 0) {
    return;
  }
  if (wifly.readBytes(buf, 3) == 0) {
    return;
  }
  buf[3] = '\0';
  *status_code = strtol(buf, NULL, 10);
  if (wifly.readBytesUntil('\n', buf, sizeof(buf)) == 0) {
    return;
  }

  while (true) {
    // Read first character of line.
    if (wifly.readBytes(buf, 1) == 0) {
      return;
    }
    char ch = buf[0];
 
    if (line_start) {
      // Beginning content.
      if (ch == '\r') {
        // read ...\n\r\n
        if (wifly.readBytes(buf, 1) == 0) { 
          return;
        }
        break;
      }
    }
     
    // Read header name. 
    len = wifly.readBytesUntil(':', &buf[1], sizeof(buf) - 2);
    if (len == 0) {
      return;
    }
    buf[len + 1] = '\0';
    // space
    if (wifly.readBytes(toss, 1) == 0) { 
      return;
    }
    
    if (strncmp(buf, "Content-Length", len) == 0) {
      len = wifly.readBytesUntil('\n', buf, sizeof(buf));
      if (len == 0) {
        return;
      }
      buf[len] = '\0';
      *content_len = atoi(buf);
    } else {   
      if (wifly.readBytesUntil('\n', buf, sizeof(buf)) == 0) {
        return;
      }
    }
  }
}

void FacebookAPI::_headerPath (const char *path, const char *access_token) {
  wifly.print(this->base);
  wifly.print("/");
  wifly.print(path);
  if (access_token != 0) {
    wifly.print(strstr(path, "?") == 0 ? "?" : "&");
    wifly.print("access_token=");
    wifly.print(access_token);
  }
  wifly.print(strstr(path, "?") == 0 ? "?" : "&");
  wifly.print("namespace=");
  wifly.print(Lifegraph.ns);
  wifly.print("&key=");
  wifly.print(Lifegraph.key);
  wifly.print("&secret=");
  wifly.print(Lifegraph.secret);
}