#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 ;
const int BARREL_DISTANCE = 10 ; //in full steps

const int REDUCTION_RATIO = 26.85;

const int PILL_MICROSTEP = 32;
const int BARREL_MICROSTEP = 1;

const int BARREL_THRESHOLD = 950 ;

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 ;

Atm_analog pill_sensor ;


void barrel_homing(){
  digitalWrite( 13, HIGH );
  barrel_stepper.setMaxSpeed(LOW_SPEED);
  barrel_stepper.setAcceleration(LOW_ACC);
  barrel_step.move(BARREL_DISTANCE*BARREL_MICROSTEP*REDUCTION_RATIO);
  Serial.println(barrel_sensor.state());
  // while(barrel_sensor.state()>900 && (barrel_step.state() == barrel_step.RUNNING)){ // &&
  //
  //   //barrel_step.cycle();
  //   //Serial.println(barrel_sensor.state());
  // }
  // barrel_step.stop();
  // barrel_stepper.setPosition(0);
  bool foundHome = false ;
  while(barrel_step.state() != barrel_step.STOPPING){
    // automaton.run();
    barrel_step.cycle();
    if(barrel_sensor.state()<BARREL_THRESHOLD){
      barrel_step.stop();
      barrel_stepper.setPosition(0);
      foundHome = true ;
      break ;
    }
  }
  digitalWrite( 13, LOW );
  if (!foundHome){Serial.println("HOMING FAILED - CHECK SENSOR");}
  else{Serial.println("homing done");}

}


void barrel_move(int stepRel){
  barrel_stepper.setMaxSpeed(HIGH_SPEED);
  barrel_stepper.setAcceleration(HIGH_ACC);
  barrel_step.move(stepRel);
}

void pill_move(int stepRel){
  pill_stepper.setMaxSpeed(HIGH_SPEED);
  pill_stepper.setAcceleration(HIGH_ACC);
  pill_step.move(stepRel);
}

void pill_next(){
  pill_stepper.setMaxSpeed(HIGH_SPEED);
  pill_stepper.setAcceleration(HIGH_ACC);

  //make one turn, if nothing was seen by sensor try again up to 5 times

  // for (int retries = 0 ; retries < 5; retries
  // pill_step.move(200*PILL_MICROSTEP);

}

//////////////  Serial command machine  /////////////////////

char cmd_buffer[80];
Atm_command cmd;
enum { CMD_TEST, CMD_BARREL_MOVE, CMD_BARREL_HOME, CMD_BARREL_NEXT,
          CMD_PILL, CMD_PILL_MOVE };
const char cmdlist[] =
    "test barrel_move barrel_home barrel_next pill pill_move";
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:
      barrel_move(atoi( cmd.arg( 1 ) ));
      return;
    case CMD_BARREL_HOME:
      barrel_homing();
      return;
    case CMD_BARREL_NEXT:
      barrel_move(BARREL_DISTANCE);
      if(!barrel_sensor.state()>BARREL_THRESHOLD){barrel_homing();}
      return;
    case CMD_PILL:
      pill_next();
      return;
    case CMD_PILL_MOVE:
      pill_move(atoi( cmd.arg( 1 ) ));
      return;
  }
}


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

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_stepper.setInverseRotation(true);
  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);
  pill_sensor.begin(A3);
  //stepper.onOnchange(Machine &machine, optional int event = 0)
  //stepper.cycle(1000);
  //barrel_step.gotoStep(10000);
  //controller.moveAsync(*stepper.motor);

}



void loop() {
  automaton.run();

}
