#include <SPI.h>
#include <Ethernet.h>   // version IDE 0022
#include "Z_OSC.h"    // version IDE 0022
#include <EEPROM.h>


#define TIMEOUT 1000		/* 1 second */
#define MAX_READ_REGS 125
#define MAX_WRITE_REGS 125
#define MAX_RESPONSE_LENGTH 256
#define PRESET_QUERY_SIZE 256
/* errors */
#define PORT_ERROR -5
#define DEBUG  0
/*pins*/
#define TXENABLE  3

///////////////////////////
Z_OSCMessage *rcvMes;
Z_OSCClient client;
Z_OSCServer server;
Z_OSCMessage message;  

///////////////////////////
byte Macel[8]={
  0x01, 0x03, 0x00, 0x06, 0x00, 0x02, 0x24, 0x0A};
byte Mrpm[8]={
  0x01, 0x03, 0x51, 0x08, 0x00, 0x01, 0x15, 0x34};
byte Mcurrent[8]={
  0x01, 0x03, 0x51, 0x07, 0x00, 0x01, 0x25, 0x37};

byte Mfw[10]={
  0x01, 0x10, 0x00, 0x05, 0x01, 0x02, 0x12, 0x82, 0x2A, 0xC4};
//  0x01, 0x10, 0x00, 0x05, 0x01, 0x02, 0x00, 0x02, 0x2A, 0xC4};
byte Mstop[10]={
  0x01, 0x10, 0x00, 0x05, 0x01, 0x02, 0x12, 0x81, 0x6A, 0xC5};
//  0x01, 0x10, 0x00, 0x05, 0x01, 0x02, 0x00, 0x01, 0x6A, 0xC5};

///////////////////////////////////////////////////////////////  
// SET THE RIGHT ADDRESS FOR ARDUINO 
///////////////////////////////////////////////////////////////
byte myMac[]= {
  0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED }; // TO SET ADDRESS MAC ARDUINO CARD
byte myIp[] = { 
  192, 168, 1, 11 };      // TO SET ADDRESS IP ARDUINO CARD  
int  serverPort = 8000;                // TO SET RECEIVING PORT
byte gateway[] = { 
  192, 168, 1, 1 };    // ROUTER
byte subnet[] = {
  255, 255, 255, 0 };    // SUBNET
byte destIp[] = {
  192, 168, 1, 2};      // TO SET ADDRESS IP COMPUTER
int  destPort = 9000;                  // TO SET SENDING PORT
///////////////////////////////////////////////////////////////  

int response_length;
byte data[10];
long int dataRec1;
long int dataRec2;
byte tempByte;


void setup() {


  pinMode(2,OUTPUT);
  pinMode(TXENABLE,OUTPUT);
  pinMode(9,OUTPUT); 
  
  digitalWrite(TXENABLE, LOW); // pin seleccion
  digitalWrite(2, HIGH); // led
  digitalWrite(9, LOW); // Rele


  //Ethernet.begin(serverMac ,serverIp);
  Ethernet.begin(myMac ,myIp ,gateway ,subnet);

  //setting osc receive server
  server.sockOpen(serverPort);

  Serial.begin(9600);

  if(DEBUG)
  {
    message.setAddress(destIp, destPort);   
    message.setZ_OSCMessage("/is5/" , "s", "Arrancando OSC...");
    client.send(&message);
  }

}

void loop() {

  if ( server.available() ) {   //check if osc arrive 
    rcvMes = server.getMessage();
    //rcvMes->setPortNumber(destPort);

    oscProcess();

    response_length = receive_response(data);

    if(response_length)
    {

      if(DEBUG)
      {   
        Serial.print("  Received:");
        Serial.print(response_length);
        Serial.print("  Data: ");
        for(int i=0; i<10 ; i++)
        { 
          Serial.print(data[i]);
          Serial.print("  ");
        }
        Serial.println();
      }

      if(data[2] == 2) // un dato de 2 bytes
      {
        
        dataRec1 = (data[3]<<8) + data[4];
        message.setAddress(destIp, destPort);
        message.setZ_OSCMessage("/is5/data/" , "i", &dataRec1);
        client.send(&message);
      }
      else if(data[2] == 4) // dos datos de 2 bytes
      {
        
        dataRec1 = (data[3]<<8) + data[4];
        dataRec2 = (data[5]<<8) + data[6];
        
        message.setAddress(destIp, destPort);
        message.setZ_OSCMessage("/is5/data/" , "ii", &dataRec1, &dataRec2);
        client.send(&message);
        message.flush();
      }
      else
      {
        message.setAddress(destIp, destPort);
        message.setZ_OSCMessage("/is5/error/" , "iiiiiiiii", data[0]&0xFF,data[1]&0xFF,data[2]&0xFF,data[3]&0xFF,data[4]&0xFF,data[5]&0xFF,data[6]&0xFF,data[7]&0xFF);
        client.send(&message);
        message.flush();
      }



    }

  } 

}


void oscProcess(){

  if( !strcmp( rcvMes->getZ_OSCAddress() , "/is5/rpm" ) ){
    long int rpm = rcvMes->getInteger32(0);
    sendData(Mrpm);
  }  
  if( !strcmp( rcvMes->getZ_OSCAddress() , "/is5/acel" ) ){
    long int acel = rcvMes->getInteger32(0);  
    sendData(Macel);
  }  
  if( !strcmp( rcvMes->getZ_OSCAddress() , "/is5/current" ) ){
    long int current = rcvMes->getInteger32(0);
    sendData(Mcurrent);
  }  
  if( !strcmp( rcvMes->getZ_OSCAddress() , "/is5/ON" ) ){
    long int MotorOn = rcvMes->getInteger32(0);
    if(MotorOn)
      digitalWrite(9, HIGH);
    else
      digitalWrite(9, LOW);
   
  }  

}

void sendData(byte *array2send)
{

  //while( Serial.available() )  // Limpiamos buffer de entrada!
  //  tempByte = Serial.read();
    
  for( int i=0 ; i<10 ; i++ )    
    data[i] = 0;

  digitalWrite( TXENABLE, HIGH);
  delay(100);

  for( int i=0 ; i<8 ; i++ )    
  {
    Serial.write(array2send[i]); 
    delayMicroseconds(1600); // distancia minima entre caracter
    //delay(2); // distancia minima entre caracter
  }
  
  Serial.flush(); // esperamos a que se envien todos los datos
  
  digitalWrite( TXENABLE, LOW); 



}



int receive_response(byte *received_string)
{

  int bytes_received = 0;
  int i = 0;

  /* wait for a response; this will block! */
        
  delay(50); // INTENTAR BAJAR ESTE DELAY AL MINIMO POSIBLE!!!!!

/*
  // EL WHILE NO ESPERA LO QUE TIENE QUE ESPERAR... POR ESO EL DELAY SUPERIOR...
  while( !Serial.available() ) {
    delay(1);
    if (i++ > TIMEOUT)
      return bytes_received;
  }
*/

  // FIXME: does Serial.available wait 1.5T or 3.5T before exiting the loop? 
  while(Serial.available()) {
    
   digitalWrite(2, LOW);
   delay(20);
   digitalWrite(2,HIGH);
   delay(20);
   
    received_string[bytes_received] = Serial.read();

    bytes_received++;

    if (bytes_received >= MAX_RESPONSE_LENGTH)
      return PORT_ERROR;
  }

  //Serial.println();  

  return (bytes_received);
}




unsigned int crc(unsigned char *buf, int start, int cnt)
{
  int i, j;
  unsigned temp, temp2, flag;

  temp = 0xFFFF;

  for (i = start; i < cnt; i++) {
    temp = temp ^ buf[i];

    for (j = 1; j <= 8; j++) {
      flag = temp & 0x0001;
      temp = temp >> 1;
      if (flag)
        temp = temp ^ 0xA001;
    }
  }

  /* Reverse byte order. */

  temp2 = temp >> 8;
  temp = (temp << 8) | temp2;
  temp &= 0xFFFF;

  return (temp);

}


void modbus_query(unsigned char *packet, size_t string_length)
{

  int temp_crc;

  temp_crc = crc(packet, 0, string_length);

  Serial.print("CRC: ");
  Serial.print( temp_crc >> 8 );
  Serial.print("  ");
  Serial.println( temp_crc & 0x00FF );

}