#include <Arduino.h>

#include <Automaton.h>
#include "Atm_lien/Atm_stepper.h"
#include "Atm_lien/Atm_TeensyStep.h"

#include <SPI.h>
#include <Ethernet.h>
#include <EthernetUdp.h>
#include <TeensyMAC.h>
#include <OSCMessage.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, 204);    //local IP of Arduino/Teensy
//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;


/////////////////// STEPPER machines  ////////////////////////
const int HIGH_SPEED = 1000 ;
const int HIGH_ACC = 8000 ;
const int LOW_SPEED = 100 ;
const int LOW_ACC = 5000 ;
const int BARREL_DISTANCE = 8592 ;

Atm_TeensyStep barrel_step;
Stepper barrel_stepper(3, 2);
StepControl barrel_controller ;

Atm_analog barrel_sensor;

Atm_TeensyStep pill_step;
Stepper pill_stepper(6, 5);
StepControl pill_controller ;


void barrel_homing(){
  digitalWrite( 13, HIGH );
  barrel_stepper.setMaxSpeed(LOW_SPEED);
  barrel_stepper.setAcceleration(LOW_ACC);
  barrel_step.move(BARREL_DISTANCE);
  Serial.println(barrel_sensor.state());
  while(barrel_sensor.state()<500 && (barrel_step.state() == barrel_step.RUNNING)){ // &&
    //automaton.run();
    barrel_step.cycle();
    Serial.println(barrel_sensor.state());
  }
  barrel_step.stop();
  barrel_stepper.setPosition(0);
  digitalWrite( 13, LOW );

}

void barrel_next(){
  digitalWrite( 13, HIGH );
  barrel_stepper.setMaxSpeed(HIGH_SPEED);
  barrel_stepper.setAcceleration(HIGH_ACC);
  barrel_step.move(BARREL_DISTANCE);
}

//////////////  Serial command machine  /////////////////////
char cmd_buffer[80];
Atm_command cmd;
enum { CMD_TEST, CMD_BARREL_MOVE, CMD_BARREL_HOME, CMD_BARREL_NEXT };
const char cmdlist[] =
    "test barrel_move barrel_home barrel_next";
void cmd_callback( int idx, int v, int up ) {
  int pin = atoi( cmd.arg( 1 ) );
  Serial.print(v);
  Serial.println("  in callback");
  switch ( v ) {
    case CMD_TEST:
      digitalWrite( 13, HIGH );
      Serial.println("in test");
      return;
    case CMD_BARREL_MOVE:
      digitalWrite( 13, LOW );
      barrel_step.move(atoi( cmd.arg( 1 ) ));
      return;
    case CMD_BARREL_HOME:
      barrel_homing();
      return;
    case CMD_BARREL_NEXT:
      barrel_next();
      return;
  }
}




void setup() {
  //Configure and start ethernet module (not needed for feeder)
  //SPI.setSCK(27);
 //  Ethernet.init(15);//(10)
 //  teensyMAC(mac);
 //  Ethernet.begin(mac, ip);              // start the Ethernet and UDP:
 // //  Udp.beginMulti(ipMulti, portMulti);   // for modified Arduino library
 //  Udp.beginMulticast(ipMulti, portMulti); // for modified Teensy Ethernet library

  //Start serial
  Serial.begin(115200); // higher Baud rate for faster refresh, CHANGE IN SERIAL MONITOR
  delay(2000);
  Serial.println("Started");
  cmd.begin( Serial, cmd_buffer, sizeof( cmd_buffer ) )
    .trace(Serial)
    .list( cmdlist )
    .onCommand( cmd_callback );

  pinMode(13, OUTPUT);


  barrel_step.trace( Serial );
  barrel_step.begin(barrel_stepper, barrel_controller);
  barrel_stepper.setMaxSpeed(LOW_SPEED);
  barrel_stepper.setAcceleration(HIGH_ACC);
  barrel_sensor.begin(A1);

  pill_step.trace( Serial );
  pill_step.begin(pill_stepper, pill_controller);
  pill_stepper.setMaxSpeed(HIGH_SPEED);
  pill_stepper.setAcceleration(HIGH_ACC);
  //stepper.onOnchange(Machine &machine, optional int event = 0)
  //stepper.cycle(1000);
  //barrel_step.gotoStep(10000);
  //controller.moveAsync(*stepper.motor);

}

void loop() {
  automaton.run();

}