#include <Arduino.h>

#include <Automaton.h>
//#include "Atm_lien/Atm_stepper.h"
#include "Atm_Teenstep.h"
//#include "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;
//
// ///////////////////       OSC         ////////////////////////
// OSCBundle bndl;

/////////////////// STEPPER machines  ////////////////////////
/*move are always meant in full steps on the motor output (after reduction)
so all motors have 200steps per turn */
const int BARREL_SPEED = 20 ;
const int BARREL_ACC = 800 ;
const int BARREL_DISTANCE = 10 ; //in full steps
const int BARREL_REDUCTION_RATIO = 26.85;
const int BARREL_MICROSTEP = 32;

static uint16_t BARREL_THRESHOLD[] = {880} ; //for sensor to trig
const int BARREL_INIBSENSOR = 6000;

const int PILL_SPEED = 20 ;
const int PILL_ACC = 1000 ;
const int PILL_DISTANCE = 200 ; //in full steps
const int PILL_MICROSTEP = 32;
const int PILL_THRESHOLD = 750 ;
const int PILL_MOVE_AFTER_DETECTION = 20 ;



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

Atm_analog barrel_sensor;


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

Atm_analog pill_sensor ;
uint16_t avgbuffer[16];

void barrel_homing(){
  barrel_step.move(5*BARREL_MICROSTEP*BARREL_REDUCTION_RATIO);
  //automaton.delay(BARREL_INIBSENSOR); //let the current position go
  automaton.delay(BARREL_INIBSENSOR);
  barrel_step.move(BARREL_DISTANCE*BARREL_MICROSTEP*BARREL_REDUCTION_RATIO);
  bool foundHome = false ;
  while(barrel_step.state() != barrel_step.ENABLED){
    // automaton.run();
    automaton.run();
    Serial.println(barrel_sensor.state());
    if(barrel_sensor.state()<BARREL_THRESHOLD[0]){
      barrel_step.emergencystop();
      barrel_stepper.setPosition(0);
      foundHome = true ;
      break ;
    }
  }
  if (!foundHome){Serial1.println("barrel 0 ");}
  else{Serial1.println("barrel 1 ");}

}


void barrel_move(int stepRel){
  Serial1.print("barrel_move ");
  Serial1.println(stepRel);
  barrel_step.move(stepRel*BARREL_MICROSTEP*BARREL_REDUCTION_RATIO);
}

void pill_move(int stepRel){
  Serial1.print("pill_move ");
  Serial1.println(stepRel);
  pill_step.move(stepRel* PILL_MICROSTEP);
}

void pill_next(){
  pill_step.move(PILL_DISTANCE * PILL_MICROSTEP);

  //make one turn, if nothing was seen by sensor try again up to 5 times
  bool foundPill = false ;
  while(pill_step.state() != pill_step.ENABLED){
    int value = pill_sensor.state();
    automaton.run();
    Serial.println(value);
    if(value<PILL_THRESHOLD){
      foundPill = true ;
      pill_step.move(PILL_MOVE_AFTER_DETECTION* PILL_MICROSTEP);
      break;}
  }
  if (!foundPill){Serial1.println("pill 0 ");}
  else{Serial1.println("pill 1 ");}

}

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

char cmd_buffer[80];
Atm_command cmd;
char cmd1_buffer[80];
Atm_command cmd1;
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_step.homing(0);
      barrel_homing();
      return;
    case CMD_BARREL_NEXT:
      barrel_move(BARREL_DISTANCE);
      if((!barrel_sensor.state())<BARREL_THRESHOLD[0]){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
  Serial1.begin(9600);
  // Serial2.begin(9600);
  delay(2000);
  Serial.println("Started Feeder");
  cmd.begin( Serial, cmd_buffer, sizeof( cmd_buffer ) )
    .trace(Serial)
    .list( cmdlist )
    .onCommand( cmd_callback );

  cmd1.begin( Serial1, cmd1_buffer, sizeof( cmd1_buffer ) )
    .trace(Serial)
    .list( cmdlist )
    .onCommand( cmd_callback );
  pinMode(13, OUTPUT);

  // pinMode(4, OUTPUT);
  // pinMode(4, LOW);
  barrel_step.trace( Serial );
  barrel_step.begin(barrel_stepper, barrel_controller)
        .setEnablePin(4).enableReversed(1);
        //.setLimitType(3).setLimitPins(A3).limitThresholds(800, 1000, 0, 100).homing(1);
  barrel_stepper.setMaxSpeed(BARREL_SPEED*BARREL_MICROSTEP);
  barrel_stepper.setAcceleration(BARREL_ACC*BARREL_MICROSTEP);
  barrel_stepper.setInverseRotation(true);
  barrel_sensor.begin(A3, 50);//.threshold(BARREL_THRESHOLD, 1)
              // .onChange( []( int idx, int v, int up ){Serial.println(up);} )
              // .onChange( true, []( int idx, int v, int up ){Serial.println(up);} )
              // .onChange( false, []( int idx, int v, int up ){Serial.println(up);} );

  pill_step.trace( Serial );
  pill_step.begin(pill_stepper, pill_controller)
        .setEnablePin(7).enableReversed(1);
  pill_stepper.setMaxSpeed(PILL_SPEED*PILL_MICROSTEP);
  pill_stepper.setAcceleration(PILL_ACC*PILL_MICROSTEP);
  pill_sensor.begin(A1, 50).average( avgbuffer, sizeof( avgbuffer ) );
  barrel_homing();
  //stepper.onOnchange(Machine &machine, optional int event = 0)
  //stepper.cycle(1000);
  //barrel_step.move(10000);
  // pill_step.move(10000);
  //controller.moveAsync(*stepper.motor);

}



void loop() {
  automaton.run();
  //Serial.println(barrel_sensor.state());

}
