// FALTA ENVIAR EN FORMATO OSC EL ARRANQUE DEL WIFLY
// ARDUINO PRO - 5V - 16MHz - ATmega 328
// http://arduino.cc/en/Guide/ArduinoPro
// FTDI drivers http://www.ftdichip.com/Drivers/VCP.htm

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


const char mySSID[] = "hangar_nau3";  
const char myPassword[] = "m1cr0fug4s";
const char *IP = "172.26.0.255";

//const char mySSID[] = "gira_li";  
//sll -- const char myPassword[] = "wifpanspermia";
//const char myPassword[] = "";
//const char *IP = "192.168.2.10"; // receiber computer ip
//const char *IP = "192.168.2.14"; // receiber computer ip
/*
const char mySSID[] = "xarxalab";  
const char myPassword[] = "la$connexio$a$xarxalab";
const char *IP = "192.168.2.102";
*/
// const char *IP = "192.168.2.255"; // broadcast per a tots...


const uint16_t outPort = 8000;
const uint16_t localPort = 9000;                

#define ADDRESS 0x61                   // The address of the SD21
#define debug  0

#define   bat            A1
#define   ledBat         18 //A0
#define   ledDatos       4
#define   releServos     7
#define   batThreshold   136

#define SERVO1   0x3F                     // Address of first servo
#define SERVO2   0x40                    
#define SERVO3   0x41                     
#define SERVO4   0x42                    
#define SERVO5   0x43                    
#define SERVO6   0x44                     
#define SERVO7   0x45                    
#define SERVO8   0x46                    
#define SERVO9   0x47                     
#define SERVO10  0x48     

#define VELSERVO1   0x00                     // Address of first servo speed
#define VELSERVO2   0x03                    
#define VELSERVO3   0x06                     
#define VELSERVO4   0x09                    
#define VELSERVO5   0x0C                    
#define VELSERVO6   0x0F                     
#define VELSERVO7   0x12                    
#define VELSERVO8   0x15                    
#define VELSERVO9   0x18                     
#define VELSERVO10  0x1B     


#define limit1Low    30      
#define limit1High   100  
#define center1      55
#define limit2Low    10      
#define limit2High   75  
#define center2      45
#define limit3Low    15      
#define limit3High   80  
#define center3      50
#define limit4Low    25      
#define limit4High   95  
#define center4      63

#define limit5Low    5      
#define limit5High   65  
#define center5      50
#define limit6Low    15      
#define limit6High   80  
#define center6      25
#define limit7Low    4     
#define limit7High   100  
#define center7      15
#define limit8Low    1      
#define limit8High   100  
#define center8      15

#define limit9Low    1      
#define limit9High   90  
#define center9      50
#define limit10Low   12      
#define limit10High  100  
#define center10     50


uint32_t count=0;
uint32_t error=0;
uint8_t tick=0;
uint8_t tempByte = 0;
uint8_t serialCount=0;
uint32_t lastSend = 0; 	
uint32_t lastEvent = 0;
uint32_t intValue1, intValue2, intValue3, intValue4; 	


#define MESSAGE_SIZE 12


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

char pingBat[MESSAGE_SIZE] = { // Message template
  '/', 'b', 't', B0 ,
  ',', 'i', B0, B0, 
  B0 , B0 , B0, B0
};

char err[MESSAGE_SIZE] = { // Message template
  '/', 'e', 'r', B0,
  ',', 'i', B0, B0, 
  B0 , B0 , B0, B0
};


WiFly wifly;

void setup()
{

  char buf[32];

  Wire.begin();
  pinMode(ledDatos, OUTPUT);      // LED DATOS
  pinMode(releServos, OUTPUT);    // RELE
  pinMode(ledBat, OUTPUT);        // LED BATERIA 

  Serial.begin(9600);
  Serial1.begin(9600);
  wifly.begin(&Serial1);

  /* Join wifi network if not already associated */
  if (!wifly.isAssociated()) {
      /* Setup for UDP packets, sent automatically */
    wifly.setIpProtocol(WIFLY_PROTOCOL_UDP);
    wifly.setHost(IP, outPort);	  // Send UDP packet to this server and port
    wifly.setPort(localPort);     // listen in this local port
    /* Setup the WiFly to connect to a wifi network */
    wifly.setSSID(mySSID);
    wifly.setPassphrase(myPassword);
    //wifly.setKey(myKey); // modo WEP
    wifly.enableDHCP();
    wifly.join();
  } 


  //wifly.setIP(const __FlashStringHelper *buf)  // EN CASO DE USAR IP FIJA
  //wifly.setNetmask(const char *buf)
  
  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.print("LocalPort: ");
  Serial.println(wifly.getPort());
  Serial.print("HostPort: ");
  Serial.println(wifly.getHostPort());

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

  Serial.println("CAP ready");

  digitalWrite(releServos, HIGH); // ARRANCA LA PLACA DE SERVOS (RELE ON)
  delay(100);
  servoReset(); // RESETEAMOS SERVOS A POSICION CENTRAL

}


void loop()
{

  if ((millis() - lastSend) > 30000) {  // interrupccion cada 30s para enviar estado de bateria y actualizacion de led

    digitalWrite(ledDatos, LOW);

    long bateria = analogRead(bat)/4;

    if( bateria < batThreshold)      // Bat MAX 16V = 203
      digitalWrite(ledBat, HIGH);    // Bat 11V = 136
    else                             // Bat min 10V = 125
      digitalWrite(ledBat, LOW);

    bateria = map(bateria, 130, 210, 0, 100);
    if( bateria > 100 ) bateria = 100;
    else if( bateria < 0 )  bateria = 0;
    
    //pingBat[11] = (int)bateria;
    for(int b=0 ; b<12 ; b++)
      wifly.write(pingBat[b]);
      
    lastSend = millis();

    digitalWrite(ledDatos, HIGH);
    
  }


  if ((millis() - lastEvent) > 60000)  // INTERRUPCION PARA PONER EN REPOSO LOS SERVOS TRAS 60 SEG DE INACTIVIDAD
  {
    digitalWrite(releServos, LOW);
  }
  
  checkIncomming();  // chequeamos los mensajes por wifi!!!

}





void SD21( byte CMD, byte inByte )
{

  digitalWrite(ledDatos, LOW);
  Wire.beginTransmission(ADDRESS);
  Wire.write(CMD);
  Wire.write(inByte);                   // Send a value of 0-255 to servo 1
  Wire.endTransmission();
  digitalWrite(ledDatos, HIGH);  

}




void servoReset()
{
  
  // SERVOS A POSICION CENTRAL EXCEPTO LA BOCA
  /*
  SD21(SERVO1, 85*255/180);   // min:50 max:120 med:85
  SD21(SERVO2, 96*255/180);   // min:80 max:112 med:96  
  SD21(SERVO3, 90*255/180);   // min:55 max:125 med:90
  SD21(SERVO4, 80*255/180);   // min:60 max:100 med:80
  SD21(SERVO5, 31*255/180);   // min:12 max:50 med:31
  SD21(SERVO6, 101*255/180);  // min:80 max:122 med:101
  SD21(SERVO7, 65*255/180);   // min:0 max:130 med:65
  SD21(SERVO8, 117*255/180);  // min:55 max:180 med:117
  SD21(SERVO9, 50*255/180);   // min:50 max:120 med:85  ->  BOCA ARRANCA EN POSICION BAJA
  SD21(SERVO10, 80*255/180);  // min:45 max:115 med:80  
  */
  SD21(SERVO1, center1);
  SD21(SERVO2, center2);
  SD21(SERVO3, center3);
  SD21(SERVO4, center4);
  SD21(SERVO5, center5);
  SD21(SERVO6, center6);
  SD21(SERVO7, center7);
  SD21(SERVO8, center8);
  SD21(SERVO9, center9);
  SD21(SERVO10, limit10High);

}




void moveServoOriginal(long servo, long pos, long limitDown, long limitHigh)
{

  if(pos>100) pos = 100;
  else if(pos<0)  pos = 0;
  if(pos > 0) // si es igual a 0 no se actualiza el motor!!!
    SD21( servo, map(pos, 1, 100, limitDown*255/180, limitHigh*255/180) ); 
}

void moveServo(long servo, long pos, long limitDown, long limitHigh)
{

  if(pos != 0) { // si es igual a 0 no se actualiza el motor!!!
    if(pos > limitHigh) pos = limitHigh;
    else if(pos < limitDown)  pos = limitDown;
    SD21( servo, pos );
  }
}




void velServo(long servoVel, long vel)
{

  if(vel>100) vel = 100;
  else if(vel<0)  vel = 0;
  if(vel > 0) // si es igual a 0 no se actualiza el motor!!!
    SD21( servoVel , map(vel, 1, 100, 1, 128 ) ); 

}





void checkIncomming() {

  if(wifly.available() ) 
  {

    lastEvent = millis(); // inicializo contador de actividad!
    digitalWrite(releServos, HIGH); // Activamos la placa de motores
    
    digitalWrite(ledDatos, LOW); // led de datos ON

    tempByte = wifly.read();
    Serial.write(tempByte);
    if (tempByte == '/')
    {
      serialCount = 0;
      for(int j=0; j<MESSAGE_SIZE ; j++)
        incomming_message[j] = '/0';
    }

    incomming_message[serialCount] = tempByte;

    serialCount++;

    if(serialCount>=12)
    {
      // Check OSC Message

      if( (incomming_message[0] == '/') && ( incomming_message[1] == 'm' ) )  // MENSAJE DE CONTROL DE MOTOR
      {

        intValue4 = incomming_message[8];
        intValue3 = incomming_message[9];
        intValue2 = incomming_message[10];
        intValue1 = incomming_message[11];

        switch (incomming_message[2]) // COMANDO
        {

        case '1': // servos 1, 2, 3 y 4

          moveServo(SERVO1, intValue1, limit1Low, limit1High);
          moveServo(SERVO2, intValue2, limit2Low, limit2High);
          moveServo(SERVO3, intValue3, limit3Low, limit3High);
          moveServo(SERVO4, intValue4, limit4Low, limit4High);

          if(debug)
          {
            wifly.print("s1: ");
            wifly.println(intValue1);
            wifly.print("s2: ");
            wifly.println(intValue2);
            wifly.print("s3: ");
            wifly.println(intValue3);
            wifly.print("s4: ");
            wifly.println(intValue4);
          }
          break;

        case '2':

          moveServo(SERVO5, intValue1, limit5Low, limit5High);
          moveServo(SERVO6, intValue2, limit6Low, limit6High);
          moveServo(SERVO7, intValue3, limit7Low, limit7High);
          moveServo(SERVO8, intValue4, limit8Low, limit8High);

          if(debug)
          {
            wifly.print("s5: ");
            wifly.println(intValue1);
            wifly.print("s6: ");
            wifly.println(intValue2);
            wifly.print("s7: ");
            wifly.println(intValue3);
            wifly.print("s8: ");
            wifly.println(intValue4);
          }     
          break;

        case '3':

          moveServo(SERVO9, intValue1, limit9Low, limit9High);
          moveServo(SERVO10, intValue2, limit10Low, limit10High);

          if(debug)
          {
            wifly.print("s9: ");
            wifly.println(intValue1);
            wifly.print("s10: ");
            wifly.println(intValue2);
          }
          break;

        }

        //Reseteamos contador y array despues de dato correcto
        serialCount = 0;
        for(int i=0; i<MESSAGE_SIZE ; i++)
          incomming_message[i] = '/0';

      }
      else if( (incomming_message[0] == '/') && ( incomming_message[1] == 'v' ) )  // MENSAJE DE CONTROL DE VELOCIDAD
      {

        //intValue = (incomming_message[11] | incomming_message[10]<<8 | incomming_message[9]<<16 | incomming_message[8]<<24 );
        intValue4 = incomming_message[8];
        intValue3 = incomming_message[9];
        intValue2 = incomming_message[10];
        intValue1 = incomming_message[11];

        switch (incomming_message[2])
        {

        case '1': // servos 1, 2, 3 y 4

          velServo(VELSERVO1, intValue1);
          velServo(VELSERVO2, intValue2);
          velServo(VELSERVO3, intValue3);
          velServo(VELSERVO4, intValue4);

          if(debug)
          {
            wifly.print("v1: ");
            wifly.println(intValue1);
            wifly.print("v2: ");
            wifly.println(intValue2);
            wifly.print("v3: ");
            wifly.println(intValue3);
            wifly.print("v4: ");
            wifly.println(intValue4);
          }
          break;

        case '2':

          velServo(VELSERVO5, intValue1);
          velServo(VELSERVO6, intValue2);
          velServo(VELSERVO7, intValue3);
          velServo(VELSERVO8, intValue4);

          if(debug)
          {
            wifly.print("v5: ");
            wifly.println(intValue1);
            wifly.print("v6: ");
            wifly.println(intValue2);
            wifly.print("v7: ");
            wifly.println(intValue3);
            wifly.print("v8: ");
            wifly.println(intValue4);
          }     
          break;

        case '3':

          velServo(VELSERVO9, intValue1);
          velServo(VELSERVO10, intValue2);

          if(debug)
          {
            wifly.print("v9: ");
            wifly.println(intValue1);
            wifly.print("v10: ");
            wifly.println(intValue2);
          }
          break;

        }

        //Reseteamos contador y array despues de dato correcto
        serialCount = 0;
        for(int i=0; i<MESSAGE_SIZE ; i++)
          incomming_message[i] = '/0';

      }
      else if( (incomming_message[0] == '/') && ( incomming_message[1] == 's' ) )  // MENSAJE DE STOP
      {
        intValue1 = incomming_message[11];
        if (intValue1)
          digitalWrite(releServos, HIGH);
        else
          digitalWrite(releServos, LOW);
      }
      else
      {
        error++; 

        if(debug){
          err[11] = error;

          for(int b=0 ; b<12 ; b++)
            wifly.write(err[b]);
          //wifly.print("Error = ");
          //wifly.print(error);
          //wifly.print("  ");
        }  

        //Reseteamos contador y array
        serialCount = 0;
        for(int i=0; i<MESSAGE_SIZE ; i++)
          incomming_message[i] = '/0';

      }

    }

    digitalWrite(ledDatos, HIGH);  //APAGAMOS LED DE DATOS

  }

}