
#include <Arduino.h>

//Automaton and custom machines
#include <Automaton.h>
// #include "Atm_Teenstep.h"
// #include "Atm_Teenstep_OSC.h"
#include "Atm_AccelStepper.h"



//osc
#include <SPI.h>
#include <Ethernet.h>
#include <EthernetUdp.h>
#include <TeensyMAC.h>
#include <OSCBundle.h>


#include <Servo.h>


////////////////    Ethernet  /////////////////////////////
// Enter a MAC address and IP address for your controller below.
// The IP address will be dependent on your local network:
byte mac[] = {0x00, 0xAA, 0xBB, 0xCC, 0xDE, 0x02};
// IPAddress ip(192, 168, 1, 105);    //local IP of Arduino/Teensy
// unsigned int localPort = 8000;

// IPAddress outIp(192, 168, 1, 102);
// const unsigned int outPort = 9000;

// unsigned int localPort = 8888;      // local port to listen on (not needed for multicast)
IPAddress ipMulti(239, 200, 200, 200); // ipMidi Multicast address
unsigned int portMulti = 9977;   // ipMidi Mutlicast port 1
// buffers for receiving and sending data
byte packetBuffer[UDP_TX_PACKET_MAX_SIZE];       // buffer to hold incoming packet
byte sendBuffer1[] = {0x90, 0x14, 0x22};        // MIDI Note On to Multicast address
byte sendBuffer2[] = {0x80, 0x14, 0x00};        // MIDI Note Off to Multicast address
// An EthernetUDP instance to let us send and receive packets over UDP
EthernetUDP Udp;

///////////////////       OSC         ////////////////////////
OSCBundle bndl;

/////////////////// STEPPER machines  ////////////////////////
const int HIGH_SPEED = 2000 ;
const int HIGH_ACC = 600 ;
const int LOW_SPEED = 100 ;
const int LOW_ACC = 1000 ;

// Atm_Teenstep_OSC X_top_OSC;
// Atm_Teenstep X_top_step;
// Stepper X_top_stepper(20 , 19);
// StepControl X_top_controller ;
Atm_AccelStepper X_top_step;

// Atm_Teenstep_OSC Y_top_OSC;
// Atm_Teenstep Y_top_step;
// Stepper Y_top_stepper(22 , 21);
// StepControl Y_top_controller ;
Atm_AccelStepper Y_top_step;

/////////////////// Servo and pump  ////////////////////////

const int peristaltic_pwm_pin = 3;
const int peristaltic_dir_pin = 2;
bool peristaltic_direction = 0 ;

Servo pill_trap ;

void motorsOSC(OSCMessage &msg){
  msg.dispatch("/pill_trap", [](OSCMessage &msg){pill_trap.write(msg.getInt(0));});
  msg.dispatch("/peristaltic", [](OSCMessage &msg){analogWrite(peristaltic_pwm_pin, msg.getInt(0));});
}

/////////////////// Feeders serial communication  ////////////////////////
char feeder1_buffer[80];
Atm_command feeder1;
enum { CMD_BARREL,  CMD_PILL, CMD_PILL_MOVE, CMD_BARREL_MOVE};
const char feeder1list[] =
    "barrel pill pill_move barrel_move";
void feeder1_callback( int idx, int v, int up ) {
  //int pin = atoi( cmd.arg( 1 ) );
  Serial.print(v);
  Serial.println("  in callback");
  switch ( v ) {
    case CMD_BARREL:
      bndl.add("/feeder1/barrel/done").add(feeder1.arg( 1 ));
      Serial.println("got answer");
      return;
    case CMD_PILL:
      bndl.add("/feeder1/pill/done").add(feeder1.arg( 1 ));
      Serial.println("got answer");
      return;
    case CMD_BARREL_MOVE:
      bndl.add("/feeder1/barrel/move").add(feeder1.arg( 1 ));
      Serial.println("got answer");
      return;
    case CMD_PILL_MOVE:
      bndl.add("/feeder1/pill/move").add(feeder1.arg( 1 ));
      Serial.println("got answer");
      return;
  }
}

char feeder2_buffer[80];
Atm_command feeder2;
enum { CMD_BARREL2,  CMD_PILL2, CMD_PILL_MOVE2, CMD_BARREL_MOVE2};
const char feeder2list[] =
    "barrel pill pill_move barrel_move";
void feeder2_callback( int idx, int v, int up ) {
  //int pin = atoi( cmd.arg( 1 ) );
  Serial.print(v);
  Serial.println("  in callback");
  switch ( v ) {
    case CMD_BARREL2:
      bndl.add("/feeder2/barrel/done").add(feeder2.arg( 1 ));
      Serial.println("got answer");

      return;
    case CMD_PILL2:
      bndl.add("/feeder2/pill/done").add(feeder2.arg( 1 ));
      Serial.println("got answer");
      return;
    case CMD_BARREL_MOVE2:
      bndl.add("/feeder2/barrel/move").add(feeder2.arg( 1 ));
      Serial.println("got answer");
      return;
    case CMD_PILL_MOVE2:
      bndl.add("/feeder2/pill/move").add(feeder2.arg( 1 ));
      Serial.println("got answer");
      return;
  }
}

void feederOSC(OSCMessage &msg){

  msg.dispatch("/feeder1/barrel_home", [](OSCMessage &msg){Serial1.println("barrel_home ");Serial.println("barrel1");});
  msg.dispatch("/feeder1/pill", [](OSCMessage &msg){Serial1.println("pill ");Serial.println("pill1");});
  msg.dispatch("/feeder1/barrel_move", [](OSCMessage &msg){Serial1.print("barrel_move ");Serial1.println(msg.getInt(0));Serial1.println(" ");Serial.println("pill1");});
  msg.dispatch("/feeder1/pill_move", [](OSCMessage &msg){Serial1.print("pill_move ");Serial1.println(msg.getInt(0));Serial1.println(" ");Serial.println("pill1");});
  msg.dispatch("/feeder2/barrel_home", [](OSCMessage &msg){Serial2.println("barrel_home ");Serial.println("barrel2");});
  msg.dispatch("/feeder2/pill", [](OSCMessage &msg){Serial2.println("pill ");Serial.println("pill2");});
  msg.dispatch("/feeder2/barrel_move", [](OSCMessage &msg){Serial2.print("barrel_move ");Serial2.println(msg.getInt(0));Serial2.println(" ");Serial.println("pill2");});
  msg.dispatch("/feeder2/pill_move", [](OSCMessage &msg){Serial2.print("pill_move ");Serial2.print(msg.getInt(0));Serial2.println(" ");Serial.println(msg.getInt(0));});
}




//////////////  Setup  /////////////////////


void setup() {
  //  ETHERNET (not needed for feeder)
  //SPI.setSCK(27); //only bottom
  Ethernet.init(15);//(10)
  teensyMAC(mac);
  Ethernet.begin(mac);              // start the Ethernet and UDP:
  Udp.beginMulticast(ipMulti, portMulti); // for modified Teensy Ethernet library
  // Udp.begin(localPort);

  delay(2000);
  //  SERIAL
  Serial.begin(115200); // higher Baud rate for faster refresh, CHANGE IN SERIAL MONITOR

  Serial.println("Started Top");

  //  LED


  //  STEPPERS

  X_top_step.begin(20, 19).setEnablePin(18).pinReversed(0)//.trace(Serial)
             .enable()
   .limitLow_set(1, 17, 1).limitLow_isHard(1)
   .limitHigh_set(1, 16, 1).limitHigh_isHard(1)
   .setMaxSpeed(HIGH_SPEED).setAcceleration(HIGH_ACC);
  X_top_step.onChangeposition([](int idx, int v, int up){bndl.add("/X_top/step").add(v).add(up);});
  X_top_step.onChangestate([](int idx, int v, int up){bndl.add("/X_top/state").add(v);});
  X_top_step.onOnlimitlow([](int idx, int v, int up){bndl.add("/X_top/limitLow").add(v);});
  X_top_step.onOnlimithigh([](int idx, int v, int up){bndl.add("/X_top/limitHigh").add(v);});
  X_top_step.onOntarget([](int idx, int v, int up){bndl.add("/X_top/onTarget").add(v);});

  Y_top_step.begin(22, 21)//.pinReversed(1)//.trace(Serial)
             .enable()
   .limitLow_set(1, 7, 1).limitLow_isHard(1)
   .limitHigh_set(1, 8, 1).limitHigh_isHard(1)
   .setMaxSpeed(HIGH_SPEED).setAcceleration(HIGH_ACC);
  Y_top_step.onChangeposition([](int idx, int v, int up){bndl.add("/Y_top/step").add(v).add(up);});
  Y_top_step.onChangestate([](int idx, int v, int up){bndl.add("/Y_top/state").add(v);});
  Y_top_step.onOnlimitlow([](int idx, int v, int up){bndl.add("/Y_top/limitLow").add(v);});
  Y_top_step.onOnlimithigh([](int idx, int v, int up){bndl.add("/Y_top/limitHigh").add(v);});
  Y_top_step.onOntarget([](int idx, int v, int up){bndl.add("/Y_top/onTarget").add(v);});


  // Y_top_step.trace( Serial );
  // Y_top_step.begin(Y_top_stepper, Y_top_controller)
  //           .setEnablePin(23).enableReversed(1)
  //           //limit pin is on 7 and 8
  //            .setLimitType(2).setLimitPins(8, 7).limitReversed(true);
  // Y_top_stepper.setMaxSpeed(600);
  // Y_top_stepper.setAcceleration(1500);
  // Y_top_stepper.setInverseRotation(false);
  // Y_top_OSC.begin(Y_top_step, Udp, bndl, "/Y_top");
  // //OSC feedback
  // Y_top_step.onChangeposition([](int idx, int v, int up){bndl.add("/Y_top_OSC/step").add(v);});
  // Y_top_step.onChange([](int idx, int v, int up){bndl.add("/Y_top_OSC/state").add(v);});
  // Y_top_step.onLimitlow([](int idx, int v, int up){bndl.add("/Y_top_OSC/limitLow").add(v);});
  // Y_top_step.onLimithigh([](int idx, int v, int up){bndl.add("/Y_top_OSC/limitHigh").add(v);});
  // // Y_top_step.onChangeposition([] ( int idx, int v, int up ) {
  // //      bndl.
  // //    });
  //
  //
  // X_top_step.trace( Serial );
  // X_top_step.begin(X_top_stepper, X_top_controller)
  //           .setEnablePin(18)//.enableReversed(1);//.enable(1);
  //           .setLimitType(2).setLimitPins(16, 17).limitReversed(true);
  // X_top_stepper.setMaxSpeed(HIGH_SPEED);
  // X_top_stepper.setAcceleration(HIGH_ACC);
  // X_top_stepper.setInverseRotation(false);
  // X_top_OSC.begin(X_top_step, Udp, bndl, "/X_top");
  // X_top_step.onChangeposition([](int idx, int v, int up){bndl.add("/X_top_OSC/step").add(v);});
  // X_top_step.onChange([](int idx, int v, int up){bndl.add("/X_top_OSC/state").add(v);});
  // X_top_step.onLimitlow([](int idx, int v, int up){bndl.add("/X_top_OSC/limitLow").add(v);});
  // X_top_step.onLimithigh([](int idx, int v, int up){bndl.add("/X_top_OSC/limitHigh").add(v);});
  // X_top_step.onOnchangeposition();




  // FEEDERS
  Serial1.begin(9600);
  delay(100);
  feeder1.begin( Serial1, feeder1_buffer, sizeof( feeder1_buffer ) )
    .trace(Serial)
    .list( feeder1list )
    .onCommand( feeder1_callback );

  Serial2.begin(9600);
  delay(100);
  feeder2.begin( Serial2, feeder2_buffer, sizeof( feeder2_buffer ) )
    .trace(Serial)
    .list( feeder2list )
    .onCommand( feeder2_callback );

  //  Servo controlling pill trap
  pill_trap.attach(14);

  //  Peristaltic pump
  pinMode(peristaltic_dir_pin, OUTPUT);
  digitalWrite(peristaltic_dir_pin, peristaltic_direction);
  pinMode(peristaltic_pwm_pin, OUTPUT);


}



void loop() {

  Udp.beginPacket(ipMulti, portMulti);
  automaton.run();
  // Udp.beginPacket(outIp, outPort);
  bndl.send(Udp); // send the bytes to the SLIP stream
  Udp.endPacket(); // mark the end of the OSC Packet
  bndl.empty(); // empty the bundle to free room for a new one

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

     if(!msgIn.hasError()){
       // Serial.println("got OSC");
       // msgIn.route("/blink", test);
       feederOSC(msgIn);
       msgIn.dispatch("/X_top/move", [](OSCMessage &msg){X_top_step.move(msg.getInt(0));});
       msgIn.dispatch("/X_top/moveTo", [](OSCMessage &msg){X_top_step.moveTo(msg.getInt(0));});
       msgIn.dispatch("/X_top/rotate", [](OSCMessage &msg){X_top_step.rotate(msg.getInt(0));});
       msgIn.dispatch("/X_top/stop", [](OSCMessage &msg){X_top_step.stop();});
       msgIn.dispatch("/X_top/emergency_stop", [](OSCMessage &msg){X_top_step.emergency_stop();});
       msgIn.dispatch("/X_top/homing", [](OSCMessage &msg){X_top_step.homing(msg.getInt(0));});
       msgIn.dispatch("/X_top/setMaxSpeed", [](OSCMessage &msg){X_top_step.setMaxSpeed(msg.getInt(0));});
       msgIn.dispatch("/X_top/setHomingSpeed", [](OSCMessage &msg){X_top_step.setHomingSpeed(msg.getInt(0));});
       msgIn.dispatch("/X_top/setAcceleration", [](OSCMessage &msg){X_top_step.setAcceleration(msg.getInt(0));});

       msgIn.dispatch("/Y_top/move", [](OSCMessage &msg){Y_top_step.move(msg.getInt(0));});
       msgIn.dispatch("/Y_top/moveTo", [](OSCMessage &msg){Y_top_step.moveTo(msg.getInt(0));});
       msgIn.dispatch("/Y_top/rotate", [](OSCMessage &msg){Y_top_step.rotate(msg.getInt(0));});
       msgIn.dispatch("/Y_top/stop", [](OSCMessage &msg){Y_top_step.stop();});
       msgIn.dispatch("/Y_top/emergency_stop", [](OSCMessage &msg){Y_top_step.emergency_stop();});
       msgIn.dispatch("/Y_top/homing", [](OSCMessage &msg){Y_top_step.homing(msg.getInt(0));});
       msgIn.dispatch("/Y_top/setMaxSpeed", [](OSCMessage &msg){Y_top_step.setMaxSpeed(msg.getInt(0));});
       msgIn.dispatch("/Y_top/setHomingSpeed", [](OSCMessage &msg){Y_top_step.setHomingSpeed(msg.getInt(0));});
       msgIn.dispatch("/Y_top/setAcceleration", [](OSCMessage &msg){Y_top_step.setAcceleration(msg.getInt(0));});
       // feederOSC(msgIn);
       motorsOSC(msgIn);
       // Y_top_step.onOSC(msgIn);
       // peristaltic.onOSC(msgIn);
     }
  }





  // Serial.print(analogRead(A3));
  // Serial.print("   ");
   // Serial.print(X_top_step.limitState[0]);
   // Serial.print("   ");
   // Serial.println(X_top_step.limitState[1]);

  // OSCMessage msg("/analog/0");
  // msg.add((int32_t)analogRead(0));
  //
  // Udp.beginPacket(ipMulti, portMulti);
  // msg.send(Udp); // send the bytes to the SLIP stream
  // Udp.endPacket(); // mark the end of the OSC Packet
  // msg.empty(); // free space occupied by message

  delay(1);

}
