// How to send OSC messages from an Arduino.
// This Example is in the public domain.
#include "WiFlyHQ.h"
#include "ArdOSCForWiFlyHQ.h"
#include <Wire.h>

//uncomment this to use a software serial if you dont own a MEGA
//#include <SoftwareSerial.h>
//SoftwareSerial softSerial(softSerialRx,softSerialTx);

WiFly wifly;
OSCClient client(&wifly);
//create new osc message
OSCMessage global_mes;

int globalIntValue=0;
float globalFloatValue=1000;
char* globalString="foo";

#define lim 512
#define DEVICE 0x53    //ADXL345 device address
#define TO_READ 6        //num of bytes we are going to read each time (two bytes for each axis)

int lecturas = 1;


byte buff[TO_READ] ;    //6 bytes buffer for saving data read from the device
char str[512];                      //string buffer to transform data before writeing it to the serial port
int regAddress = 0x32;    //first axis-acceleration-data register on the ADXL345
long int x=0;
long int y=0;
long int z=0;
int x_old=0;
int y_old=0;
int z_old=0;


uint32_t lastSend = 0; 

#define   bat  A3

uint32_t analogPin[] = {
  A0,A1,A2}; 
uint32_t analogIn[] = {
  0,0,0}; 
uint32_t analogInOld[] = {
  0,0,0}; 
  
void setup()
{ 
  Wire.begin();        // join i2c bus (address optional for master)
  Serial.begin(115200);
  //use the convenient setup. Replace <HardwareSerial> by <SoftwareSerial> if you use one.
  wifly.setupForUDP<HardwareSerial>(
    &Serial,   //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?
    "hangar_oficines",  //Your Wifi Name (SSID)
    "m1cr0fug4s", //Your Wifi Password 
    "WiFly",                 // Device name for identification in the network
//    "172.0.26.255",         // IP Adress of the Wifly. if 0 (without quotes), it will use dhcp to get an ip
0,
    9000,                    // WiFly receive port
    "255.255.255.255",       // Where to send outgoing Osc messages. "255.255.255.255" will send to all hosts in the subnet
    8000,                     // outgoing port
    false	// show debug information on Serial
  );
  wifly.printStatusInfo(); //print some debug information 
  writeTo(DEVICE, 0x2D, 0x08);
//  writeTo(DEVICE, 0x31, 0x00); //2g
//  writeTo(DEVICE, 0x31, 0x01); //4g
  writeTo(DEVICE, 0x31, 0x02); //8g
//  writeTo(DEVICE, 0x31, 0x03); //16g
  delay(15);
}


void loop()
{
  //note that the destination adress is set by setting the remote host of the wifly!  
  //three ways of sending messages...
  if ((millis() - lastSend) > 1000) {  // interrupccion cada 1s para enviar estado de bateria y actualizacion de led
    int bateria = average(bat);
    bateria = map(bateria, 400, 508, 0, 100);
    if( bateria > 100 ) bateria = 100;
    else if( bateria < 0 )  bateria = 0;
    client.sendInt((int)bateria,"/fb/bat");
    lastSend = millis();
  } 
  checkAdxl();
  checkAnalog();
  

}

void checkAdxl() 
{
    byte res = 0;
    averageADXL();   
    if ( (x > (x_old + res) ) || (x < (x_old - res) ) )
    {
      client.sendInt(x,"/fb/x");
      //delay(5);
      x_old = x;
    }
    if ( (y > (y_old + res) ) || (y < (y_old - res) ) )
    {
      client.sendInt(y,"/fb/y");
      //delay(5);
      y_old = y;
    }
    
    if ( (z > (z_old + res) ) || (z < (z_old - res) ) )
    {
      client.sendInt(z,"/fb/z");
      //delay(5);
      z_old = z;
    }
    //Serial.println();
}

void checkAnalog() 
{
  byte res = 8;
  for(int i=0 ; i<3 ; i++)
  {

    analogIn[i] = (int)(average(analogPin[i]));

    if ((analogIn[i] > analogInOld[i] + res ) || (analogIn[i] < analogInOld[i] - res ) )
    {
      if (i==0) client.sendInt(analogIn[i],"/fb/s1");
      if (i==1) client.sendInt(analogIn[i],"/fb/s2");
      if (i==2) client.sendInt(analogIn[i],"/fb/s3");
      //delay(5);
      analogInOld[i] = analogIn[i];
    }
  }
}

int average(int anaPin)
{
  long total = 0;
  long average = 0;
  int count = 0;
  for(int i=0; i<lecturas; i++)
  {
    total = total + analogRead(anaPin);
  }

  average = total / lecturas;  
  return(average);

}

int averageADXL()
{
  int temp_x=0;
  int temp_y=0;
  int temp_z=0;
  x=0;
  y=0;
  z=0;
  
  for(int i=0; i<lecturas; i++)
  {
    readFrom(DEVICE, regAddress, TO_READ, buff); //read the acceleration data from the ADXL345
    temp_x = (((int)buff[1]) << 8) | buff[0]; 
    temp_x = map(temp_x,-lim,lim,0,1023);  
    temp_y = (((int)buff[3])<< 8) | buff[2];
    temp_y = map(temp_y,-lim,lim,0,1023); 
    temp_z = (((int)buff[5]) << 8) | buff[4];
    temp_z = map(temp_z,-lim,lim,0,1023); 
    x = (int)(temp_x + x);
    y = (int)(temp_y + y);
    z = (int)(temp_z + z);
  }
  x = (int)(x / lecturas);
  y = (int)(y / lecturas);
  z = (int)(z / lecturas);
}

//---------------- Functions
//Writes val to address register on device
void writeTo(int device, byte address, byte val) {
   Wire.beginTransmission(device); //start transmission to device 
   Wire.write(address);        // write register address
   Wire.write(val);        // write value to write
   Wire.endTransmission(); //end transmission
}

//reads num bytes starting from address register on device in to buff array
void readFrom(int device, byte address, int num, byte buff[]) {
  Wire.beginTransmission(device); //start transmission to device 
  Wire.write(address);        //writes address to read from
  Wire.endTransmission(); //end transmission
  
  Wire.beginTransmission(device); //start transmission to device
  Wire.requestFrom(device, num);    // request 6 bytes from device
  
  int i = 0;
  while(Wire.available())    //device may write less than requested (abnormal)
  { 
    buff[i] = Wire.read(); // read a byte
    i++;
  }
  Wire.endTransmission(); //end transmission
}