// stepper Y 22, 21, 23
// stepper X 20, 19, 18
//           St, Di, En


#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 = 800 ;
const int HIGH_ACC = 800 ;
const int LOW_SPEED = 100 ;
const int LOW_ACC = 1000 ;

Atm_TeensyStep X_top_step;
Stepper X_top_stepper(20 , 19);
StepControl X_top_controller ;


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

void setup() {
  //Configure and start ethernet module (not needed for feeder)
  //SPI.setSCK(27); //only bottom
  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");
  pinMode(13, OUTPUT);

  // pinMode(18, OUTPUT);
  // digitalWrite(18, LOW);
  X_top_step.trace( Serial );
  X_top_step.begin(X_top_stepper, X_top_controller)
            .setEnablePin(18).enable(1)
            .setLimitType(1).setLimitPins(A3).limitReversed(true).limitThresholds(600, 800, 900, 1200);
  X_top_stepper.setMaxSpeed(HIGH_SPEED);
  X_top_stepper.setAcceleration(HIGH_ACC);
  X_top_stepper.setInverseRotation(true);

  //stepper.onOnchange(Machine &machine, optional int event = 0)
  //stepper.cycle(1000);
  X_top_step.move(100000);
  //controller.moveAsync(*stepper.motor);
  //pinMode(17, INPUT);
}



void loop() {
  automaton.run();
  Serial.print(analogRead(A3));
  Serial.print("   ");
  Serial.print(X_top_step.limitState[0]);
  Serial.print("   ");
  Serial.println(X_top_step.limitState[1]);
  delay(10);

}
