#include <SoftwareSerial.h>
SoftwareSerial wifiSerial(8,9);
#include <WiFlyHQ.h>

//const char mySSID[] = "knalito";
const char mySSID[] = "mid";
const char myPassword[] = "";

WiFly wifly;

void setup()
{
    char buf[32];

    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.getFlushTimeout() != 10) {
        Serial.println("Restoring flush timeout to 10msecs");
        wifly.setFlushTimeout(10);
	wifly.save();
	wifly.reboot();
    }
    
    if (wifly.getBaud() != 9600) {
        Serial.println("Restoring baudRate to 9600");
        wifly.setBaud(9600);
	wifly.save();
	wifly.reboot();
    }
    
    if (wifly.getPort() != 9000) {
        wifly.setPort(9000);
	/* local port does not take effect until the WiFly has rebooted (2.32) */
	wifly.save();
	Serial.println(F("Set port to 9000, rebooting to make it work"));
	wifly.reboot();
    }
    

    /* Join wifi network if not already associated */
    if (!wifly.isAssociated()) {
	/* Setup the WiFly to connect to a wifi network */
	Serial.println("Joining network");
	wifly.setSSID(mySSID);
	wifly.setPassphrase(myPassword);
        //wifly.setKey(myKey);
	wifly.enableDHCP();

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

    /* Setup for UDP packets, sent automatically */
    wifly.setIpProtocol(WIFLY_PROTOCOL_UDP);
    wifly.setHost("192.168.1.38", 8000);	// Send UDP packet to this server and port
    
    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)));

    wifly.setDeviceID("Wifly-UDP");
    Serial.print("DeviceID: ");
    Serial.println(wifly.getDeviceID(buf, sizeof(buf)));

    Serial.println("WiFly ready");
}

uint32_t count=0;
uint8_t tick=0;
uint32_t lastSend = 0;	/* Last time message was sent */
uint32_t intValue = 0;	/* Last time message was sent */

#define MESSAGE_SIZE 12

unsigned char incomming_message[MESSAGE_SIZE] = { // Message template
  '/', 's', '1', B0,
  ',', 'i', B0, B0, 
  B0 , B0 , B0, '/0'
};


void loop()
{

    if ((millis() - lastSend) > 10000) {
        count++;
	Serial.print("Sending message ");
	Serial.println(count);

        //wifly.println(wifly.getPort());
        wifly.print(wifly.getRSSI());
	wifly.print("dB, count=");
	wifly.println(count);
	lastSend = millis();
    }

    checkIncomming();

}


// Check Incomming Messages
void checkIncomming() {

  if(wifly.available() >= MESSAGE_SIZE) {

    for(byte i = 0; i < MESSAGE_SIZE; i++) {
      incomming_message[i] = wifly.read();
      //Serial.print(i);
      //Serial.print("  ");
      //Serial.println(incomming_message[i],DEC);
    }

    // Check OSC Message
    if(incomming_message[0] != '/') {
      Serial.println("cabecera no valida");
      wifly.flush();
      return;
    }
    // Check namespace
    if( incomming_message[1] != 's' ) {
      Serial.println("mensaje no valido");
      wifly.flush();
      return;
    }
    else
    {
      
      // Check int
      
      //intValue = (incomming_message[11] | incomming_message[10]<<8 | incomming_message[9]<<16 | incomming_message[8]<<24 );
      intValue = ( incomming_message[11] | incomming_message[10]<<8 );
      //Serial.print("Valor INTEGER: ");
      //Serial.println(intValue);
      
      
      //wifly.print(incomming_message); 
              
      switch (incomming_message[2])
      {
      case '1':
        Serial.print("servo1: ");
        Serial.println(intValue);
        break;
      case '2':
        Serial.print("servo2: ");
        Serial.println(intValue, DEC);
        break;
      case '3':
        Serial.print("servo3: ");
        Serial.println(intValue, DEC);
        break;
      case '4':
        Serial.print("servo4: ");
        Serial.println(intValue, DEC);
        break;

      }

    }


  }
}