#include <OSCMessage.h>
#include <Ethernet.h>
#include <EthernetUdp.h>
#include <SPI.h>

#include <OSCBoards.h>

#ifdef BOARD_HAS_USB_SERIAL
#include <SLIPEncodedUSBSerial.h>
SLIPEncodedUSBSerial SLIPSerial( thisBoardsSerialUSB );
#else
#include <SLIPEncodedSerial.h>
 SLIPEncodedSerial SLIPSerial(Serial);
#endif


////  UTILITIES ///////// 


template <typename TYPE> void sendOSC(const char * adress, TYPE parameter) {
  OSCMessage OSCmsg(adress);
  OSCmsg.add(parameter);
  SLIPSerial.beginPacket(); 
  //Udp.beginPacket(outIp, outPort);
  OSCmsg.send(SLIPSerial); // send the bytes to the SLIP stream
//  OSCmsg.send(Udp); // send the bytes to the SLIP stream
  SLIPSerial.endPacket();
  //Udp.endPacket(); // mark the end of the OSC Packet
  OSCmsg.empty(); // free space occupied by message
}

void printTest(OSCMessage &msg) {
  sendOSC("/still", "up and running");
}

/////  STEPPER /////////



//void init_Stepper_OSC(OSCMessage &msg) {
//  float maximum = init_Stepper();
//  sendOSC("/stepper/maxPos",maximum );
//}
//
void moveTo_Stepper_OSC (OSCMessage &msg){
  long int distance = moveTo_Stepper(msg.getInt(0));
  sendOSC("/stepper/distanceToGo",distance );
//  while (!stepper.distanceToGo()==0)
//    stepper.run();
}

void move_Stepper_OSC (OSCMessage &msg){
  long int distance = move_Stepper(msg.getInt(0));
  sendOSC("/stepper/distanceToGo",distance );
}
//
//void moveToCm_Stepper_OSC (OSCMessage &msg){
//  long int distance = moveToCm_Stepper(msg.getFloat(0));
//  sendOSC("/stepper/distanceToGo/cm",distance );
//}
//
//void moveCm_Stepper_OSC (OSCMessage &msg){
//  long int distance = moveCm_Stepper(msg.getFloat(0));
//  sendOSC("/stepper/distanceToGo/cm",distance );
//}

void maxSpeed_Stepper (OSCMessage &msg){
  long int max_speed = maxSpeed_Stepper(msg.getInt(0));
  sendOSC("/stepper/maxSpeed",max_speed );
}

void acceleration_Stepper (OSCMessage &msg){
  long int acceleration = acceleration_Stepper(msg.getInt(0));
  sendOSC("/stepper/acceleration",acceleration );
}

void currentPos_Stepper_OSC(OSCMessage &msg) {
  sendOSC("/stepper/currentPos", stepper.currentPosition());
}


//////  OSC DECLARATIONS  /////////

void setup_OSC(){
  
  SLIPSerial.begin(115200);
//  delay (100);
  sendOSC("/ready", "to go");

  
}


void handleOSCIn() {


  //define OSC message to function relationships
  OSCMessage OSCin;
  int size;

  if (SLIPSerial.available()) {
    while (!SLIPSerial.endofPacket())
      if ( (size = SLIPSerial.available()) > 0)
      {
        while (size--)
          OSCin.fill(SLIPSerial.read());
      }
  }
      
  //Declare valid OSC messages here 
  if (!OSCin.hasError()) {
    OSCin.dispatch("/test", printTest);


//      //STEPPER CONTROL
//      OSCin.dispatch("/stepper/init", init_Stepper_OSC);
    OSCin.dispatch("/stepper/currentPos", currentPos_Stepper_OSC);  
    OSCin.dispatch("/stepper/moveTo", moveTo_Stepper_OSC);
    OSCin.dispatch("/stepper/move", move_Stepper_OSC);
//      OSCin.dispatch("/stepper/moveTo/cm", moveToCm_Stepper_OSC);
//      OSCin.dispatch("/stepper/move/cm", moveCm_Stepper_OSC);
    OSCin.dispatch("/stepper/maxSpeed", maxSpeed_Stepper);
    OSCin.dispatch("/stepper/acceleration", acceleration_Stepper);
//      OSCin.dispatch("/stepper/randomMove", randomMove_Stepper_OSC);
//
//      //LEDS
//      OSCin.dispatch("/led/value", valueLed_OSC);
//      OSCin.dispatch("/led/sensor", sensorLed_OSC);
//      OSCin.dispatch("/led/lighting", lightingLed_OSC);
  }    
//  }
}