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

#define debug  1

Servo servoBody;
Servo servoHead;

byte rele[12] = { 
  9, 10, 11, 12, 13, 14, 15, 1, 2, 3, 4, 5};

byte myMac[] = { 
  0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED };
byte myIp[]  = { 
  10, 1, 10, 11 };
int  serverPort  = 9000;
byte destIp[] = {
  10, 1, 10, 255 };      // TO SET ADDRESS IP COMPUTER
int  destPort = 8000;                  // TO SET SENDING PORT

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

uint32_t val_shift; //Valor del shift register
const uint8_t dataPin=8;
const uint8_t latchPin=7;
const uint8_t clockPin=9;
  
uint32_t an1, an2, an3, an4, an5, an6;

int ledState = LOW;             // ledState used to set the LED
long previousMillis = 0;        // will store last time LED was updated
long interval = 1000;           // interval at which to blink (milliseconds)

int counter = 0;


void resetshift()
{
  digitalWrite(latchPin, LOW);
  shiftOut(dataPin, clockPin, MSBFIRST, 0x00);
  shiftOut(dataPin, clockPin, MSBFIRST, 0x00);
  shiftOut(dataPin, clockPin, MSBFIRST, 0x00);
  digitalWrite(latchPin, HIGH);
}

void shiftWrite(uint8_t pin, uint8_t state)
{
  if (pin < 7) bitWrite(val_shift, pin + 1, state);
  else if (pin < 14) bitWrite(val_shift, pin + 2, state);
  else bitWrite(val_shift, pin + 3, state);
  digitalWrite(latchPin, LOW);
  byte registerThree = val_shift>>16;
  byte registerTwo = val_shift>>8;
  byte registerOne = val_shift;  
  shiftOut(dataPin, clockPin, MSBFIRST, registerThree);
  shiftOut(dataPin, clockPin, MSBFIRST, registerTwo);
  shiftOut(dataPin, clockPin, MSBFIRST, registerOne);
  digitalWrite(latchPin, HIGH);
}

void test()
{
  int countLimit = 12;
  
  for (int i=0; i<6; i++)
  {
    Serial.print("AN");
    Serial.print(i);
    Serial.print(" = ");
    Serial.println(analogRead(i));
  }

  for (int i=0; i<countLimit; i++)
  {
    if(i == 0)
      shiftWrite(countLimit-1, LOW);
    else
      shiftWrite(i-1, LOW);

    shiftWrite(i, HIGH);

    Serial.print("rele ");
    Serial.print(i);
    Serial.println(": ON ");
    delay(200);
  }  

}


void sendOSC()
{

  an1 = analogRead(0);
  an2 = analogRead(1);
  an3 = analogRead(2);
  an4 = analogRead(3);
  an5 = analogRead(4);
  an6 = analogRead(5);
  
  message.setAddress(destIp,destPort);
  message.setZ_OSCMessage("/robot/analog" , "iiiiii", &an1, &an2, &an3, &an4, &an5, &an6);
  client.send(&message); 
  message.flush();

}


void cmdProcess()
{

  uint32_t intValue;
  uint8_t motorSpeed;

  if( !strcmp( rcvMes->getZ_OSCAddress() , "/rele" ) )
  {
    intValue = rcvMes->getInteger32(0);
    word2rele( intValue );
    
    if(debug)
    {
      Serial.print("Rele state: ");
      Serial.println(intValue);
    }
  }
  else if( !strcmp( rcvMes->getZ_OSCAddress() , "/servo1" ) )
  {
    intValue = rcvMes->getInteger32(0);
    servoBody.write( intValue ); 
    
    if(debug)
    {
      Serial.print("Rele state: ");
      Serial.println(intValue);
    }
  }
  else if( !strcmp( rcvMes->getZ_OSCAddress() , "/servo2" ) )
  {
    intValue = rcvMes->getInteger32(0);
    servoHead.write( intValue ); 
    
    if(debug)
    {
      Serial.print("Rele state: ");
      Serial.println(intValue);
    }
  }

}


void logMessage()

{
  uint16_t i;
  byte *ip=rcvMes->getIpAddress();

  uint32_t intValue;
  float floatValue;
  char *stringValue;

  if(debug)
  {
    Serial.print(ip[0],DEC);
    Serial.print(".");
    Serial.print(ip[1],DEC);
    Serial.print(".");
    Serial.print(ip[2],DEC);
    Serial.print(".");
    Serial.print(ip[3],DEC);
    Serial.print(":");

    Serial.print(rcvMes->getPortNumber());
    Serial.print(" ");
    Serial.print(rcvMes->getZ_OSCAddress());
    Serial.print(" ");
    Serial.print(rcvMes->getTypeTags());
    Serial.print("--");  
  }

  for(i=0 ; i<rcvMes->getArgsNum(); i++){

    switch( rcvMes->getTypeTag(i) ){

    case 'i':       
      intValue = rcvMes->getInteger32(i);

      Serial.print(intValue);
      Serial.print(" ");
      break; 


    case 'f':        
      floatValue = rcvMes->getFloat(i);

      Serial.print(floatValue);
      Serial.print(" ");
      break; 


    case 's':         
      stringValue = rcvMes->getString(i);

      Serial.print(stringValue);
      Serial.print(" ");
      break; 

    }


  }

}

void word2rele(int wordInput)
{

  for (int i=0; i<16; i++)
  {
   
    if( bitRead(wordInput, i) )
      shiftWrite(i, HIGH);
    else
      shiftWrite(i, LOW);
     
  }

}

void servoTest(int servoDelay)
{

 for(int counter = 0 ; counter <= 180; counter +=2) { 
    servoBody.write(counter);
    servoHead.write(counter);     
    delay(servoDelay);                            
  } 

  for(int counter = 180 ; counter >= 0; counter -=2) { 
    servoBody.write(counter); 
    servoHead.write(counter);     
    delay(servoDelay);                            
  } 
    
  
}


void setup() {
  // set up serial port
  Serial.begin(9600);

  Ethernet.begin(myMac , myIp); 

  server.sockOpen(serverPort);

  servoBody.attach(3);
  servoHead.attach(4);

  // Set the output pins for the DAC control. This pins are defined in the library
  pinMode(2, OUTPUT);
  pinMode(3, OUTPUT);
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);

  pinMode(dataPin, OUTPUT);
  pinMode(latchPin, OUTPUT);  
  pinMode(clockPin, OUTPUT); 
  resetshift();

  // pin13 LED
  pinMode(13, OUTPUT);

  if(debug)
  {
    Serial.println("Starting...");  
  }

}



void loop() {

  //test();
  //servoTest(30);
  
  unsigned long currentMillis = millis();
 
  if(currentMillis - previousMillis > interval) {
    previousMillis = currentMillis;      
    sendOSC();
  }
  
  
  if(server.available())
  {
    rcvMes=server.getMessage();
    //logMessage();
    cmdProcess();

  } 


  

}