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

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

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

#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)

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;
int lecturas = 10;

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}; 
  
#define MESSAGE_SIZE 16

char sensor[MESSAGE_SIZE] = { // Message template
  '/', 'f', 'b', '/',   // SENSOR MESSAGE TEMPLATE
  's' , '1' , B0, B0,
  ',', 'i', B0, B0, 
  B0 , B0 , B0, B0
};

char ejex[MESSAGE_SIZE] = { // Message template
  '/', 'f', 'b', '/',   // SENSOR MESSAGE TEMPLATE
  'x' ,  B0 , B0, B0,
  ',', 'i', B0, B0, 
  B0 , B0 , B0, B0
};

char ejey[MESSAGE_SIZE] = { // Message template
  '/', 'f', 'b', '/',   // SENSOR MESSAGE TEMPLATE
  'y' ,  B0 , B0, B0,
  ',', 'i', B0, B0, 
  B0 , B0 , B0, B0
};

char ejez[MESSAGE_SIZE] = { // Message template
  '/', 'f', 'b', '/',   // SENSOR MESSAGE TEMPLATE
  'z' ,  B0 , B0, B0,
  ',', 'i', B0, B0, 
  B0 , B0 , B0, B0
};

char pingBat[MESSAGE_SIZE] = { // Message template
  '/', 'f', 'b', '/',   // PING MESSAGE TEMPLATE
  'b' , 'a' , 't', B0,
  ',', 'i', B0, B0, 
  B0 , B0 , B0, B0
};


WiFly wifly;
void ini_wifly()
{
  char buf[32];
  wifly.begin(&Serial);
  
  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.print("MAC:");
  wifly.println(wifly.getMAC(buf, sizeof(buf)));
  wifly.print("IP: ");
  wifly.println(wifly.getIP(buf, sizeof(buf)));
  wifly.print("Netmask: ");
  wifly.println(wifly.getNetmask(buf, sizeof(buf)));
  wifly.print("Gateway: ");
  wifly.println(wifly.getGateway(buf, sizeof(buf)));
  wifly.print("LocalPort: ");
  wifly.println(wifly.getPort());
  wifly.print("HostPort: ");
  wifly.println(wifly.getHostPort());
  wifly.setDeviceID("Fembrana");
  wifly.print("DeviceID: ");
  wifly.println(wifly.getDeviceID(buf, sizeof(buf)));
  wifly.println("Fembrana ready");
}

void setup()
{

  Wire.begin();        // join i2c bus (address optional for master)
  Serial.begin(115200);  // start serial for output
  ini_wifly();
  //Turning on the ADXL345
  writeTo(DEVICE, 0x2D, 0);      
  writeTo(DEVICE, 0x2D, 16);
  writeTo(DEVICE, 0x2D, 8);
  
  writeTo(DEVICE, 0x31, 11);
  delay(15);
}

void loop()
{ 
  if ((millis() - lastSend) > 30000) {  // interrupccion cada 30s para enviar estado de bateria y actualizacion de led
    long bateria = analogRead(bat)/4;
    bateria = map(bateria, 102, 127, 0, 100);
    if( bateria > 100 ) bateria = 100;
    else if( bateria < 0 )  bateria = 0;

    pingBat[MESSAGE_SIZE-1] = (int)bateria;
    for(int b=0 ; b < MESSAGE_SIZE; b++) wifly.write(pingBat[b]);
    lastSend = millis();
  } 
  checkAdxl();
  checkAnalog();
}

void checkAdxl() 
{
    byte res = 4;
    averageADXL();   
    if ( (x > (x_old + res) ) || (x < (x_old - res) ) )
    {
      ejex[MESSAGE_SIZE-1] = x;
      for(int b=0 ; b < MESSAGE_SIZE; b++) wifly.write((unsigned char)ejex[b]); 
      delay(5);
      x_old = x;
    }
    if ( (y > (y_old + res) ) || (y < (y_old - res) ) )
    {
      ejey[MESSAGE_SIZE-1] = y;
      for(int b=0 ; b < MESSAGE_SIZE; b++) wifly.write((unsigned char)ejey[b]); 
      delay(5);
      y_old = y;
    }
    
    if ( (z > (z_old + res) ) || (z < (z_old - res) ) )
    {
      ejez[MESSAGE_SIZE-1] = z;
      for(int b=0 ; b < MESSAGE_SIZE; b++) wifly.write((unsigned char)ejez[b]); 
      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 ) )
    {

      sensor[MESSAGE_SIZE-1] = analogIn[i]/4;
      sensor[5] = i + 1 +'0';
      for(int b=0 ; b < MESSAGE_SIZE; b++)  
        wifly.write((unsigned char)sensor[b]); 
      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++)
  {
    writeTo(DEVICE, 0x31, 8);
    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,255);  
    temp_y = (((int)buff[3])<< 8) | buff[2];
    temp_y = map(temp_y,-lim,lim,0,255); 
    temp_z = (((int)buff[5]) << 8) | buff[4];
    temp_z = map(temp_z,-lim,lim,0,255); 
    x = (int)(temp_x + x);
    y = (int)(temp_y + y);
    z = (int)(temp_z + z);
  }
  x = (byte)(x / lecturas);
  y = (byte)(y / lecturas);
  z = (byte)(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
}