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



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

/////  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 );
}

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 );


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

void setup_OSC(){
  OSCMessage OSCmsg("/connected");


}



void handleOSCIn() {

//  //check ethernet connection periodically
//  if ( millis() - ethernetTestTimer > 5000 ) {
//    ethernetTestTimer = millis() ;  
//    //Serial.println(Ethernet.maintain());    
//  }
  
  //define OSC message to function relationships
  OSCMessage OSCin;
  int size;

  if ( (size = Udp.parsePacket()) > 0) {
    while (size--)
    OSCin.fill(Udp.read());

    //Declare valid OSC messages here 
    if (!OSCin.hasError()) {
      OSCin.dispatch("/test", printTest);
      OSCin.dispatch("/mode", mode_OSC);
      
      //SENSOR
      OSCin.dispatch("/sensor/state", sensorState_OSC);
      OSCin.dispatch("/sensor/time", sensorTimeOSC);
      OSCin.dispatch("/sensor/scale/reset", resetScale_OSC);
      OSCin.dispatch("/sensor/scale/current", currentScale_OSC);

      //CC MOTOR CONTROL
      OSCin.dispatch("/CC/speed", setSpeed_CCMotor_OSC);
      OSCin.dispatch("/CC/cs", checkCS_CCMotor_OSC);
      
      //STEPPER CONTROL
      OSCin.dispatch("/stepper/init", init_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);
    }    
  }
}