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

#define AWAKE  4 // Despertar WIFI
#define PANEL A8 //Entrada panel
#define BAT   A7 //Entrada bateria

//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);
OSCServer server(&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 = 10;

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; 

uint32_t analogPin[] = {
  A0,A1,A2}; 
uint32_t analogIn[] = {
  0,0,0}; 
uint32_t analogInOld[] = {
  0,0,0}; 


const char mySSID[] = "Whale02";  
const char myKey[] = "6f73747261";
//const char mySSID[] = "mid";
//const char myPassword[] = "";
const char *IP = "192.168.2.255"; // IP fija del SCK si se activa en la funcion setup!!!!!!!!!!!!!!!!!!
const uint16_t outPort = 8000;
const uint16_t localPort = 9000; 

void setup()
{ 
  Wire.begin();        // join i2c bus (address optional for master)
  Serial1.begin(115200);
  Serial.begin(9600);

  pinMode(AWAKE, OUTPUT);

  wifly.begin(&Serial1);

  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.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

  digitalWrite(AWAKE, HIGH); 
  delay(100);
  digitalWrite(AWAKE, LOW); 

  server.addCallback("/sleep",&sleep);

  delay(15);
}


void loop()
{

  if(Serial.available())
  {
    char cmd = Serial.read();  
    if (cmd == 'W')
    {
      Serial.println("Wifi Awake! ");
      digitalWrite(AWAKE, HIGH); 
      delay(100);
      digitalWrite(AWAKE, LOW); 

    }
    if (cmd == 'S')
    {
      Serial.println("Wifi Sleep! ");
      wifly.sleep();
      digitalWrite(AWAKE, LOW);    
    }
  }


  if(server.availableCheck()>0)
  {
    //callback after process
    //Serial.println("data received");
  }


  if ((millis() - lastSend) > 2000) 
  {  // interrupccion cada 10s para enviar estado de bateria y del panel

    // LECTURA DE LA BATERIA EN %
    uint32_t bateria = average(BAT);
    bateria = map(bateria, 613, 840, 0, 100);
    if( bateria > 100 ) bateria = 100;
    else if( bateria < 0 )  bateria = 0;

    client.sendInt(bateria,"/bat");
    delay(10);
    Serial.print("Bateria: ");
    Serial.println(bateria);


    // LECTURA DEL PANEL EN mV
    uint32_t panel_mVolts = 5.25*average(PANEL)*5000./1023;
    if (panel_mVolts > 500) panel_mVolts = panel_mVolts + 750; //Tension del diodo de proteccion
    client.sendInt(panel_mVolts,"/panel");
    delay(10);
    Serial.print("Panel: ");
    Serial.println(panel_mVolts);


    // AWAKE WIFI
    if(panel_mVolts > 8000) // si el voltaje del panel es mayor que 8V arrancamos el wifi!
    {
      digitalWrite(AWAKE, HIGH); 
      delayMicroseconds(100);
      digitalWrite(AWAKE, LOW);      
    }

    // CHECK WIFI
    if (!wifly.isAssociated()) {
      Serial.println("Conexion perdida, reiniciando... ");
     /*
      // 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.reboot();
    } 

    lastSend = millis();
    
  } 

  checkAdxl();

  delay(50);

}


void sleep(OSCMessage *_mes)
{
  //get 1st argument(int32)
  uint32_t sleepFlag = _mes->getArgInt32(0);
  //SLEEP MODE
  if (sleepFlag==1)
  {
    Serial.println("OSC Sleep command received!");
    if(wifly.sleep())
      Serial.println("Wifi sleeping!");
    else
      Serial.println("Wifi sleep mode fail!");
  } 
}



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.print("Acell: ");
   Serial.print(x);
   Serial.print("  ");
   Serial.print(y);
   Serial.print("  ");
   Serial.println(z); 
   */

  //using a global message object and some global variables works as well
  global_mes.beginMessage("/accel");
  global_mes.addArgInt32(x);
  global_mes.addArgInt32(y);
  global_mes.addArgInt32(z);

  client.send(&global_mes);
  global_mes.flush(); //object data clear

}


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
}