#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

bool update_stepper_status[NUM_OF_STEPPERS];
long int last_updated_status ;
int update_ms = 1000 ;

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


template <typename TYPE> void sendOSC(const char * adress, int first_param, TYPE parameter) {
  OSCMessage OSCmsg(adress);
  OSCmsg.add(first_param);
  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", 3, "up and running");
}



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



void homing_Stepper_OSC(OSCMessage &msg) {
  homing_steppers();
  sendOSC("/homing",3, "done" );
}


void setCurrentPosition_Stepper_OSC (OSCMessage &msg) {
  int index = msg.getInt(0);
  steppers[index].setCurrentPosition(msg.getInt(1));
  sendOSC("/setPosition/", index,  "position set");
}

void drivingMode_Stepper_OSC (OSCMessage &msg) {
  driveMode = msg.getInt(0);
  sendOSC("/driveMode/", 3,  "set to " + driveMode);
}

void moveTo_Stepper_OSC (OSCMessage &msg){
  int index = msg.getInt(0);
  long int distance = moveTo_Stepper(index, msg.getInt(1));
  sendOSC("/distanceToGo", index, distance );
//  while (!stepper.distanceToGo()==0)
//    stepper.run();
}

void move_Stepper_OSC (OSCMessage &msg){
  int index = msg.getInt(0);
  long int distance = move_Stepper(index, msg.getInt(1));
  sendOSC("/distanceToGo", index, distance );
}

void setSpeed_Stepper (OSCMessage &msg){
  int index = msg.getInt(0);
  steppers[index].setSpeed(msg.getInt(1));
  sendOSC("/speed", index, steppers[index].speed() );
}

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

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

void currentPos_Stepper_OSC(OSCMessage &msg) {
  int index = msg.getInt(0);
  sendOSC("/currentPos", index, steppers[index].currentPosition());
}

void status_Stepper_OSC(int stepper_index) {
  OSCMessage OSCmsg("/status");
  OSCmsg.add(stepper_index);  
  OSCmsg.add(steppers[stepper_index].currentPosition());
  //OSCmsg.add(steppers[stepper_index].targetPosition());
  OSCmsg.add(steppers[stepper_index].speed());
  //OSCmsg.add(steppers[stepper_index].maxSpeed());
  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 getStatus_stepper_OSC(OSCMessage &msg) {
  status_Stepper_OSC(msg.getInt(0)) ;
}


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

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

  
}


void handleOSCIn() {

  //send stepper status while it is running
  if (millis() - last_updated_status > update_ms ) {
    for(int i ; i < NUM_OF_STEPPERS ; i++ ) {
      if (update_stepper_status[i]) {
        status_Stepper_OSC(i)   ; 
      }
      if (!steppers[i].isRunning()) { update_stepper_status[i] = 0 ;}
      else update_stepper_status[i] = 1 ;
    }
    last_updated_status = millis() ;
  }

    //send stepper status while it is running
  if ((millis() - last_updated_status > update_ms) && update_stepper_status[0] ) {
  
    status_Stepper_OSC(0)   ; 
    
    if (!steppers[0].isRunning()) { update_stepper_status[0] = 0 ;}
    else update_stepper_status[0] = 1 ;
  
    last_updated_status = millis() ;
  }
    if ((millis() - last_updated_status > update_ms*1.5) && update_stepper_status[1]  ) {
  
    status_Stepper_OSC(1)   ; 
    
    if (!steppers[1].isRunning()) { update_stepper_status[1] = 0 ;}
    else update_stepper_status[1] = 1 ;
  
    last_updated_status = millis() ;
  }
  



  //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("/init", init_Stepper_OSC);
    OSCin.dispatch("/getStatus", getStatus_stepper_OSC);
    OSCin.dispatch("/setCurrentPosition", setCurrentPosition_Stepper_OSC);
    OSCin.dispatch("/currentPosition", currentPos_Stepper_OSC);  
    OSCin.dispatch("/driveMode", drivingMode_Stepper_OSC);
    OSCin.dispatch("/moveTo", moveTo_Stepper_OSC);
    OSCin.dispatch("/move", move_Stepper_OSC);
//      OSCin.dispatch("/moveTo/cm", moveToCm_Stepper_OSC);
//      OSCin.dispatch("/move/cm", moveCm_Stepper_OSC);
    OSCin.dispatch("/setSpeed", setSpeed_Stepper);
    OSCin.dispatch("/maxSpeed", maxSpeed_Stepper);
    OSCin.dispatch("/acceleration", acceleration_Stepper);
//      OSCin.dispatch("/randomMove", randomMove_Stepper_OSC);
//
//      //LEDS
//      OSCin.dispatch("/led/value", valueLed_OSC);
//      OSCin.dispatch("/led/sensor", sensorLed_OSC);
//      OSCin.dispatch("/led/lighting", lightingLed_OSC);
  }    
//  }
}