#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>
// 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;

// Atm_stepper stepper;
Atm_TeensyStep stepper;
Stepper A_stepper(2, 1);
StepControl controller ;

class Atm_blink : public Machine {

 public:
   Atm_blink( void ) : Machine() {};
   short pin;
   atm_timer_millis timer;
   OSCMessage _msg ;
   EthernetUDP* _udpRef ;
   //Atm_blink& onOSC(OSCMessage& msg );

   enum { IDLE, LED_ON, LED_OFF }; // STATES
   enum { EVT_TIMER, EVT_ON, EVT_OFF, ELSE }; // EVENTS
   enum { ENT_ON, ENT_OFF }; // ACTIONS

   Atm_blink & begin( int attached_pin, uint32_t blinkrate, EthernetUDP& udpRef  ) {
     const static state_t state_table[] PROGMEM = {
       /*            ON_ENTER    ON_LOOP  ON_EXIT  EVT_TIMER  EVT_ON  EVT_OFF  ELSE */
       /* IDLE    */  ENT_OFF,        -1,      -1,        -1, LED_ON,      -1,   -1,
       /* LED_ON  */   ENT_ON,        -1,      -1,   LED_OFF,     -1,    IDLE,   -1,
       /* LED_OFF */  ENT_OFF,        -1,      -1,    LED_ON,     -1,    IDLE,   -1,
     };
     Machine::begin( state_table, ELSE );
     pin = attached_pin;
     timer.set( blinkrate );
     pinMode( pin, OUTPUT );
     this->_udpRef =   &udpRef;

     return *this;
   }

   int event( int id ) {
     switch ( id ) {
       case EVT_TIMER :
         return timer.expired( this );

   }
 }

   void action( int id ) {

     switch ( id ) {
       case ENT_ON :
         digitalWrite( pin, HIGH );


         return;

       case ENT_OFF :
         digitalWrite( pin, LOW );

         return;
     }
   }

   Atm_blink& trigger( int event ) {
     Machine::trigger( event );
     return *this;
   }

   /* Optionally override the default state() method
    * Control what the machine returns when another process requests its state
    */
    // Atm_led& Atm_led::on( void ) {
    //   trigger( EVT_ON );
    //   return *this;


   Atm_blink& onOSC(OSCMessage& msg ){
     Serial.println("OSC");
     if(msg.fullMatch("/blink")){
       Serial.println("ouais gros");
       // int blinkstate = _msg.getInt(0);
       // blinkstate ? trigger(EVT_ON) : trigger(EVT_OFF);
       _msg = new OSCMessage("/on");
      _msg.add("youpi");
      _udpRef->beginPacket(ipMulti, portMulti);
      _msg.send(*_udpRef); // send the bytes to the SLIP stream
      _udpRef->endPacket(); // mark the end of the OSC Packet
      _msg.empty();
       return *this;
     }
   }


};

Atm_blink led;

void test(OSCMessage &msg, int addrOffset){
  Serial.println("test");
msg.getInt(0)?led.trigger( led.EVT_ON ):led.trigger( led.EVT_OFF );
}



void setup() {
 //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
 Serial.begin(115200); // higher Baud rate for faster refresh, CHANGE IN SERIAL MONITOR

 // led.begin( 14, 200, Udp);        // Setup a blink machine on pin 4
 // led.trigger( led.EVT_ON );  // Turn it on

  delay(1000);

  // stepper.trace( Serial );
  // Serial.println("Started");
  // stepper.begin(A_stepper);
  // stepper.cycle(1000);
  // Serial.println("Started");
  // stepper.gotoStep(1000);
  // controller.moveAsync(*stepper.motor);
  // delay(1000);
  // controller.moveAsync(*stepper.motor);
  // delay(10000);
  // stepper.motor->setMaxSpeed(30000);
  // stepper.motor->setAcceleration(10000);
  // stepper.gotoStep(-100000);
  // controller.moveAsync(*stepper.motor);
  // Serial.println("Started");
  // stepper.trigger(stepper.EVT_GOTO);
  // pinMode(00, OUTPUT);
  // digitalWrite(0, HIGH);
}

void loop() {
 //  automaton.run();
 // OSCMessage msgIn;
 // int size;
  Serial.println("msgIn");
 // if( (size = Udp.parsePacket())>0)
 // {
 //   while(size--)
 //     msgIn.fill(Udp.read());
 //
 //    if(!msgIn.hasError()){
 //      // msgIn.route("/blink", test);
 //      // led.onOSC(msgIn);
 //    }
 //
 //
 // }

 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

 // led.cycle();

 delay(100);
 //UDPsend();   // sending of packets; do not use with UDPreceive because it's including delay()!!
 //UDPreceive();

}
