// 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";
const char myPassword[] = "wifpanspermia";
const char *IP = "192.168.2.10";  // receiber computer ip = showservernew
/*
const char mySSID[] = "xarxasat";
const char myPassword[] = "connexio$a$xarxasat";
const char *IP = "192.168.3.9";
*/
// 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 RIGHT EYE X
#define SERVO2   0x40  // RIGHT EYE Y
#define SERVO3   0x41  // LEFT EYE X
#define SERVO4   0x42  // LEFT EYE Y
#define SERVO5   0x43  // LEFT PARP
#define SERVO6   0x44  // RIGHT PARP
#define SERVO7   0x45  // LEFT EAR
#define SERVO8   0x46  // RIGHT EAR
#define SERVO9   0x47  // MOUTH
#define SERVO10  0x48  // TONGUE

#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    10  // RIGHT EYE X IN
#define limit1High   100 // OUT
#define center1      50
#define limit2Low    20  // RIGHT EYE Y DOWN
#define limit2High   100 // UP
#define center2      60
#define limit3Low    20  // LEFT EYE X OUT
#define limit3High   120  // IN 
#define center3      40  // 
#define limit4Low    14  // LEFT EYE Y UP
#define limit4High   90  // DOWN
#define center4      52

//Orejas y parpados
#define limit5Low    7  // LEFT PARP CLOSE
#define limit5High   75 // OPEN
#define center5      55
#define limit6Low    12 // RIGHT PARP OPEN
#define limit6High   75 // CLOSE
#define center6      65
#define limit7Low    3  // RIGHT EAR CLOSE
#define limit7High   90 // OPEN
#define center7      10
#define limit8Low    1  // LEFT EAR CLOSE
#define limit8High   90 // OPEN
#define center8      5

//Lengua y boca
#define limit9Low    1  // MOUTH OPEN
#define limit9High   100 // CLOSE
#define center9      100 // ;)
#define limit10Low   6   // TONGUE OUT
#define limit10High  105 // IN
#define center10     40

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()
{
  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 "192.168.2.22"
      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
    );
    // * Flush the incoming data from the WiFly.
    // * @param timeout - the number of milliseconds to wait for additional data to flush. Default is 500msecs.
    // wifly.flushRx(1);
    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);
    
    server.addCallback("/kp/ullxDRE",&setULLdreX);
    server.addCallback("/kp/ullxESQ",&setULLesqX);
    server.addCallback("/kp/ullyDRE",&setULLdreY);
    server.addCallback("/kp/ullyESQ",&setULLesqY);
    
    server.addCallback("/kp/orelDRE",&setOREdre);
    server.addCallback("/kp/orelESQ",&setOREesq);
    server.addCallback("/kp/parpDRE",&setPARdre);
    server.addCallback("/kp/parpESQ",&setPAResq);
    
    server.addCallback("/kp/boca",&setBoca);
    server.addCallback("/kp/lleng",&setLleng);
    
    
  #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 ((millis() - lastEvent) < 2) { // ELIMINA BUFFER SI LLEGAN MENSAJES A MAS X VECES POR SEGUNDO
        //wifly.flushRx();
        //Serial1.read();
    } */    
    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 OFF
  #endif
}