// FALTA ENVIAR EN FORMATO OSC EL ARRANQUE DEL WIFLY
// ARDUINO LEONARDO - 5V - 16MHz - ATmega32u4
// http://arduino.cc/en/Guide/ArduinoLeonardo

#include "WiFlyHQ.h"
#include "ArdOSCForWiFlyHQ.h"
#include <Wire.h>

#define wiflyEnabled   true

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

//const char mySSID[] = "Mi$Red";  
//const char myPassword[] = "FINALFANTASY";
//const char *IP = "192.168.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   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     

//Ojos
#define limit1Low    0x10      
#define limit1High   0x60  
#define center1      0x35
#define limit2Low    0x17      
#define limit2High   0x60  
#define center2      0x39
#define limit3Low    0x10      
#define limit3High   0x60  
#define center3      0x30
#define limit4Low    0x10      
#define limit4High   0x50  
#define center4      0x35


//Orejas y parpados
#define limit5Low    0x07      
#define limit5High   0x40  
#define center5      0x48
#define limit6Low    0x10      
#define limit6High   0x49  
#define center6      0x53
#define limit7Low    4     
#define limit7High   100  
#define center7      15
#define limit8Low    1      
#define limit8High   100  
#define center8      15

//Lengua y boca
#define limit9Low    0x01    //Boca Abajo
#define limit9High   0x65   //Boca Arriba
#define center9      50   //Boca Centro
#define limit10Low   0x00   //Lengua Abajo 
#define limit10High  0x68  //Lengua Arriba
#define center10     50   //Lengua Centro 


uint32_t lastSend = 0; 	
uint32_t lastEvent = 0;
uint8_t intValue1, intValue2, intValue3, intValue4;
uint8_t limitsHigh[10] = {limit1High, limit2High, limit3High, limit4Low, limit5High, limit6Low, limit7High, limit8High, limit9High, limit10High};
uint8_t limitsLow[10] = {limit1Low, limit2Low, limit3Low, limit4High, limit5Low, limit6High, limit7Low, limit8Low, limit9Low, limit10Low};


WiFly wifly;
OSCClient client(&wifly);
OSCServer server(&wifly);


void setup()
{

  char buf[32];

  Wire.begin();
  #define TWI_FREQ 400000L //Frecuencia bus I2C
  TWBR = ((F_CPU / TWI_FREQ) - 16) / 2;
  pinMode(ledDatos, OUTPUT);      // LED DATOS
  pinMode(releServos, OUTPUT);    // RELE
  pinMode(ledBat, OUTPUT);        // LED BATERIA 

  Serial.begin(115200);
  
  #if wiflyEnabled
    wifly.setupForUDP<HardwareSerial>(
      &Serial1,   //the serial you want to use (this can also be a software serial)
      115200, // if you use a hardware serial, I would recommend the full 115200
      true,	// should we try some other baudrates if the currently selected one fails?
      mySSID,  //Your Wifi Name (SSID)
      myPassword, //Your Wifi Password 
      "CAP",                 // Device name for identification in the network
      0,         // IP Adress of the Wifly. if 0 (without quotes), it will use dhcp to get an ip
      localPort,                    // WiFly receive port
      IP,       // Where to send outgoing Osc messages. "255.255.255.255" will send to all hosts in the subnet
      outPort,                     // outgoing port
      false	// show debug information on Serial
    );
    
    wifly.printStatusInfo(); //print some debug information 
    server.addCallback("/m1",&setMotor1);
    server.addCallback("/m2",&setMotor2);
    server.addCallback("/m3",&setMotor3);
    server.addCallback("/v1",&setVel1);
    server.addCallback("/v2",&setVel2);
    server.addCallback("/v3",&setVel3);
    server.addCallback("/r", &sReset);
    server.addCallback("/s",&setStop);
    server.addCallback("/t",&test);
  #endif
  
  digitalWrite(releServos, HIGH); // ARRANCA LA PLACA DE SERVOS (RELE ON)
  delay(100);
  servoReset(); // RESETEAMOS SERVOS A POSICION CENTRAL 
//  SD21(SERVO5, limit5Low);
//   SD21(SERVO9, limit9Low);
}


void loop()
{

 //if (Serial.available()) SD21(SERVO10, Serial.read()); //Busqueda de limites
 //if (Serial.available()) moveServo(SERVO9, Serial.read()); //Busqueda de centros

  #if wiflyEnabled
    if ((millis() - lastSend) > 30000) {  // interrupccion cada 30s para enviar estado de bateria y actualizacion de led
  
      digitalWrite(ledDatos, LOW);
  
      uint32_t bateria = (int)(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;
      
      client.sendInt(bateria,"/bt");
      //Serial.println(bateria);
        
      lastSend = millis();
  
      digitalWrite(ledDatos, HIGH);
      
    }
  
  
    if ((millis() - lastEvent) > 60000)  // INTERRUPCION PARA PONER EN REPOSO LOS SERVOS TRAS 60 SEG DE INACTIVIDAD
    {
      digitalWrite(releServos, LOW);
    }
    
    if(server.availableCheck(2)>0)
    {  
      lastEvent = millis(); // inicializo contador de actividad!
      //digitalWrite(releServos, HIGH); // Activamos la placa de motores
      digitalWrite(ledDatos, LOW); // led de datos ON
    }
    digitalWrite(ledDatos, HIGH); // led de datos ON
  #endif
}