// DRESKE
// ARDUINO MEGA

// TO DO:
// - Mapear led de bateria 
// - Configuracion con IP manual
//
// COFIGURACION SET COMM SIZE 1024
// COFIGURACION SET COMM TIME 1

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

//const char mySSID[] = "gira_li";  
//const char myPassword[] = "wifpanspermia";
//const char *IP = "192.168.1.31";

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

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

#define debug  0

#define   bat            A8
#define   ledBat         22  
#define   ledDatos       2
#define   batThreshold   155


uint32_t analogPin[] = {
  A2,A3,A4,A5,A6,A7}; 
uint32_t analogIn[] = {
  0,0,0,0,0,0}; 
uint32_t analogInOld[] = {
  0,0,0,0,0,0}; 
uint8_t  pwmOut[] = {
  5,6,7,8};
uint8_t  digitalPin[] = {
  23,25,27,29,31,33,35,37,39,41,43,45,47,49,51,53};
uint8_t digitalIn[] = {
  0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; 
uint8_t digitalInOld[] = {
  0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; 



boolean datoPrevio = true;
uint32_t counter=0;
uint32_t error=0;
uint8_t tick=0;
uint8_t tempByte = 0;
uint8_t serialCount=0;
uint32_t lastSend = 0; 	
uint32_t lastEvent = 0;
uint32_t intValue1, intValue2, intValue3, intValue4; 	
unsigned long prevMillis = 0;
unsigned long prevMillis2 = 0;

#define MESSAGE_SIZE 16

char incomming_message[] = { // Message template commands
  '/', 'd', 'r', '/',   // ANALOG OUT TEMPLATE
  'p' , 'w' , 'm', B0,
  ',', 'i', B0, B0, 
  B0 , B0 , B0, B0   // 4 analog outs =  4 integers
};

char digi[] = { // Message template commands
  '/', 'd', 'r', '/',   // DIGITAL OUT TEMPLATE
  'd' , '1' , B0, B0,
  ',', 'i', B0, B0, 
  B0 , B0 , B0, B0   // 16 digital outs = 16 bit (2 integers)
};

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

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

char err[MESSAGE_SIZE] = { // Message template
  '/', 'd', 'r', '/',   // ERROR MESSAGE TEMPLATE
  'e' , 'r' , 'r', B0,
  ',', 'i', B0, B0, 
  B0 , B0 , B0, B0
};


WiFly wifly;

void setup()
{

  char buf[32];

  Wire.begin();
  pinMode(ledDatos, OUTPUT);      // LED DATOS
  pinMode(ledBat, OUTPUT);        // LED BATERIA 

  digitalWrite(ledBat, LOW);        // LED BATERIA 

  for(int j=0; j<MESSAGE_SIZE ; j++)
    pinMode(digitalPin[j], INPUT);        // ENTRADAS DIGITALES

  for(int j=0; j<MESSAGE_SIZE ; j++)
    digitalWrite(digitalPin[j], HIGH);        // ENTRADAS DIGITALES

  Serial.begin(115200);
  Serial1.begin(115200);
  wifly.begin(&Serial1);

  /* Join wifi network if not already associated */
  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.setIP(const __FlashStringHelper *buf)  // EN CASO DE USAR IP FIJA
  //wifly.setNetmask(const char *buf)

  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("Dreske");
  wifly.print("DeviceID: ");
  wifly.println(wifly.getDeviceID(buf, sizeof(buf)));
  wifly.println("Dreske ready");

  if(debug)
  {
    Serial.print("MAC:");
    Serial.println(wifly.getMAC(buf, sizeof(buf)));
    Serial.print("IP: ");
    Serial.println(wifly.getIP(buf, sizeof(buf)));
    Serial.print("Netmask: ");
    Serial.println(wifly.getNetmask(buf, sizeof(buf)));
    Serial.print("Gateway: ");
    Serial.println(wifly.getGateway(buf, sizeof(buf)));
    Serial.print("LocalPort: ");
    Serial.println(wifly.getPort());
    Serial.print("HostPort: ");
    Serial.println(wifly.getHostPort());
    Serial.print("DeviceID: ");
    Serial.println(wifly.getDeviceID(buf, sizeof(buf)));
    Serial.println("Dreske ready");
  }


}


void loop()
{

  if ((millis() - lastSend) > 30000) {  // interrupccion cada 30s para enviar estado de bateria y actualizacion de led

    digitalWrite(ledDatos, LOW);

    long bateria = analogRead(bat)/4;

    //Serial.print("bat ");
    //Serial.print(bateria);
    //delay(1000);
    
    if( bateria < batThreshold)      // Bat MAX 15V = 213
      digitalWrite(ledBat, LOW);    // Bat 11V = 155 -> threshold LED
    else                             // Bat min 10V = 141
    digitalWrite(ledBat, HIGH);
    
   
    

    bateria = map(bateria, 141, 213, 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();

    digitalWrite(ledDatos, HIGH);

  }


  if ((millis() - prevMillis) > 5) { 
    prevMillis = millis();
    checkAnalogOne(counter);
    counter++;
    if(counter == 6)
      counter = 0;
  }

  if ((millis() - prevMillis2) > 23) { 
    prevMillis2 = millis();
    checkDigital();
  }

  //checkAnalog();     // chequeamos entradas analogicas!!!
  checkIncomming();  // chequeamos los mensajes por wifi!!!



}


void checkAnalog() 
{

  digitalWrite(ledDatos, LOW); // led de datos ON

  for(int i=0 ; i<6 ; i++)
  {

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

    analogIn[i] = analogRead(analogPin[i]);


    if (  ( analogIn[i] != analogInOld[i] ) && (analogIn[i] > analogInOld[i] + 8 ) && (analogIn[i] < analogInOld[i] - 8 ) )
    {

      sensor[MESSAGE_SIZE-1] = analogIn[i]/4;
      sensor[5] = i + 1 +'0';

      for(int b=0 ; b < MESSAGE_SIZE; b++)
      {
        Serial1.write((unsigned char)sensor[b]); 
        //Serial.print((unsigned char)sensor[b]);
        //Serial.print(" ");

      }
      //Serial.println();
      //delay(100);

      analogInOld[i] = analogIn[i];

      if(debug)
      {
        //Serial.println(sensor);     
        Serial.print("sensor ");
        Serial.print(i+1, DEC);
        Serial.print(" > ");
        Serial.println( (unsigned char)sensor[MESSAGE_SIZE-1] , DEC);
      }
    }

  }

  digitalWrite(ledDatos, HIGH); // led de datos ON

}



void checkAnalogOne(int i) 
{

  digitalWrite(ledDatos, LOW); // led de datos ON


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

  //analogIn[i] = analogRead(analogPin[i]);


  if (  ( analogIn[i] != analogInOld[i] )  )
  {

    sensor[MESSAGE_SIZE-1] = analogIn[i]/4;
    sensor[5] = i + 1 +'0';

    for(int b=0 ; b < MESSAGE_SIZE; b++)
    {
      Serial1.write((unsigned char)sensor[b]); 
      //Serial.print((unsigned char)sensor[b]);
      //Serial.print(" ");

    }
    //Serial.println();
    //delay(100);

    analogInOld[i] = analogIn[i];

    if(debug)
    {
      //Serial.println(sensor);     
      Serial.print("sensor ");
      Serial.print(i+1, DEC);
      Serial.print(" > ");
      Serial.println( (unsigned char)sensor[MESSAGE_SIZE-1] , DEC);
    }
  }


  digitalWrite(ledDatos, HIGH); // led de datos ON

}



int average(int anaPin)
{
  int lecturas = 4;
  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);

}



void  checkDigital() 
{

  digitalWrite(ledDatos, LOW); // led de datos ON

  for(int i=0 ; i<16 ; i++)
  {
    digitalIn[i] = digitalRead(digitalPin[i]);

    if ( digitalIn[i] != digitalInOld[i])
    {

      delay(10);
      digi[MESSAGE_SIZE-1] = digitalIn[i];

      if(i<9)
      {
        digi[5] = i+ 1 +'0';
        digi[6] = B0;
      }
      else
      {
        digi[5] = (i+1)/10 + '0' ;
        digi[6] = (i+1)%10 + '0';
      }

      for(int b=0 ; b < MESSAGE_SIZE; b++)
        wifly.write(digi[b]);

      digitalInOld[i] = digitalIn[i];

      if(debug)
      {
        Serial.println(digi);     
        Serial.print(" din:");
        Serial.print(i, DEC);
        Serial.print(" val:");
        Serial.println( digitalIn[i] , DEC);
      }
    }

  }

  digitalWrite(ledDatos, HIGH); // led de datos ON

}  



void checkIncomming() {  // CHEQUEO DE MENSAJES DE ENTRADA PARA CONTROL DE PWM

  if(wifly.available() ) 
  {

    lastEvent = millis(); // inicializo contador de actividad!

    digitalWrite(ledDatos, LOW); // led de datos ON

    tempByte = wifly.read();
    //Serial.println(tempByte);

    if ( (tempByte == '/') && datoPrevio ) // buscamos el primer '/' despues de la coma
    {
      if(serialCount > 15) // Compruebo una vez recibido todo el array
      {
        datoPrevio = 0;
        serialCount = 0;
        for(int j=0; j<MESSAGE_SIZE ; j++)
          incomming_message[j] = '/0';

      }
    }

    //Serial.println(serialCount);

    if ( (tempByte == ',') && (serialCount<12) )
      serialCount = 8;   // re-sincronizacion en cada ','

    incomming_message[serialCount] = tempByte;

    if( incomming_message[8] == ',' )  // si llega la coma en la posicion correcta -> dato valido
      datoPrevio = true;
    else
      datoPrevio = false;

    serialCount++;

    if( (serialCount >= MESSAGE_SIZE) && !datoPrevio )
    {
      error++;
      Serial.print("Error:");
      Serial.println(error);
      serialCount=0;
      for(int j=0; j<MESSAGE_SIZE ; j++)
        incomming_message[j] = '/0'; 
    }
    else if( (serialCount >= MESSAGE_SIZE) && datoPrevio )
    {
      // Check OSC Message

      /* SALIDAS DIGITALES OK
       
       if( (incomming_message[0] == '/') && ( incomming_message[1] == 'd' ) && ( incomming_message[2] == 'r' ) 
       && ( incomming_message[3] == '/' ) && ( incomming_message[4] == 'd' )  && ( incomming_message[5] == 'i' ) 
       && ( incomming_message[6] == 'g' ) )  
       
       // MENSAJE DE CONTROL DE MOTOR
       {
       
       intValue2 = incomming_message[14];
       intValue1 = incomming_message[15];
       
       for(int j=0; j<8 ; j++)
       {
       digitalWrite(digiOut[j], bitRead(intValue1, j));
       digitalWrite(digiOut[j+8], bitRead(intValue2, j));
       }
       
       Serial.print("OK: ");
       Serial.print((unsigned char)intValue1);
       Serial.print(" ");
       Serial.println((unsigned char)intValue2);
       
       */

      if( (incomming_message[0] == '/') && ( incomming_message[1] == 'd' ) && ( incomming_message[2] == 'r' ) 
        && ( incomming_message[3] == '/' ) && ( incomming_message[4] == 'p' )  && ( incomming_message[5] == 'w' ) 
        && ( incomming_message[6] == 'm' ) )  

        // MENSAJE DE CONTROL DE MOTOR
      {

        intValue4 = incomming_message[12];
        intValue3 = incomming_message[13];
        intValue2 = incomming_message[14];
        intValue1 = incomming_message[15];

        analogWrite(pwmOut[0], (unsigned char)intValue1);
        analogWrite(pwmOut[1], (unsigned char)intValue2);
        analogWrite(pwmOut[2], (unsigned char)intValue3);
        analogWrite(pwmOut[3], (unsigned char)intValue4);       

        if(debug)
        {
          Serial.print("OK: ");
          Serial.print((unsigned char)intValue1);
          Serial.print(" ");
          Serial.print((unsigned char)intValue2);
          Serial.print(" ");
          Serial.print((unsigned char)intValue3);
          Serial.print(" ");
          Serial.println((unsigned char)intValue4);
        }

      }


      //-----------------------------------
      //Serial.print(incomming_message);
      //Serial.print(" ");
      //Serial.print((unsigned char)incomming_message[14], DEC);
      //Serial.print(" ");
      //Serial.println((unsigned char)incomming_message[15], DEC);
    }


    digitalWrite(ledDatos, HIGH);  //APAGAMOS LED DE DATOS

  }

}