#include <Wire.h>
#include "WiFlyUDP.h"

#define ADDRESS 0x61                   // The address of the SD21
#define SERVO 0x3F                     // Address of first servo

char *ssid = "hangar_nau3";
char *pass = "m1cr0fug4s";

uint16_t remote_Port=8000; 
uint16_t local_Port=9000; 
char* ip_host = "172.26.0.46";

WiFlyUDP udp(&Serial);

int count = 0;
int inByte;

#define MESSAGE_SIZE 12

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



void setup(){
  Wire.begin();
  pinMode(2, OUTPUT);
  digitalWrite(2, HIGH);
  Serial.begin(9600);

  udp.begin(ssid, pass, ip_host, remote_Port, local_Port);
  if (udp.join()) 
  { 
    delay(4000);
    Serial.print(udp.ip());
  }

}

void loop(){

  //Serial.print("Hola mundo!");
  //Serial.println(count++);
  checkIncomming();
  //delay(1000);

}


// Check Incomming Messages
void checkIncomming() {

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

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

    // Check OSC Message
    if(incomming_message[0] != '/') {
      //Serial.println("cabecera no valida");
      return;
    }
    // Check namespace
    if( incomming_message[1] != 's' ) {
      //Serial.println("mensaje no valido");
      return;
    }
    else
    {

      switch (incomming_message[2])
      {
      case '1':
        Serial.print("servo1: ");
        Serial.println(incomming_message[11], DEC);
        break;
      case '2':
        Serial.print("servo2: ");
        Serial.println(incomming_message[11], DEC);
        break;
      case '3':
        Serial.print("servo3: ");
        Serial.println(incomming_message[11], DEC);
        break;
      case '4':
        Serial.print("servo4: ");
        Serial.println(incomming_message[11], DEC);
        break;

      }

    }


  }// If not enough bytes in buffer, then do nothing
}


/*
void serialEvent() {
  while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read(); 
    // add it to the inputString:
    inputString += inChar;
    // if the incoming character is a newline, set a flag
    // so the main loop can do something about it:
    //if (inChar == '\n') {
    if (inChar == '.') {
      stringComplete = true;
    } 
  }
}
*/