#include <MIDI.h>
#include <midi_Defs.h>
#include <midi_Message.h>
#include <midi_Namespace.h>
#include <midi_Settings.h>
#include <AccelStepper.h>

struct MySettings : public midi::DefaultSettings
{
  // static const bool UseRunningStatus = false; // Messes with my old equipment!
};
MIDI_CREATE_CUSTOM_INSTANCE(HardwareSerial, Serial1, MIDI, MySettings);


#define USBMIDI 1  //for Teensy with usbMIDI; comment out if not
#define MICROSTEP 1

#ifdef MICROSTEP
#define MS1 14
#define MS2 15
#define MS3 16
#endif


#define MOT1_STEP 19
#define MOT1_DIR 22
#define MOT2_STEP 20
#define MOT2_DIR 23
//#define MOT_DISABLE 23

#define STEPS_PER_REVOLUTION 3200
#define MOVEMENT_RANGE 1000  //movement range - negative and positive axis together (1.5 round in each direction)
#define MAX_SPEED 183050
#define ACCELERATION 57850

#define MIDI_CHANNEL 2


#define LED 13

long lastCommandX=0; //last received coordinates
long lastCommandY=0;
boolean haveNewCommandX=false;  
boolean haveNewCommandY=false;

// Define a stepper and the pins it will use
AccelStepper stepper1(1,MOT1_STEP,MOT1_DIR);
AccelStepper stepper2(1,MOT2_STEP,MOT2_DIR);

#ifdef USBMIDI
IntervalTimer ledTimer;
#endif


void setup()
{   
#ifdef MICROSTEP  
  pinMode(MS1,OUTPUT);
  pinMode(MS2,OUTPUT);
  pinMode(MS3,OUTPUT);
  
  digitalWrite(MS1,HIGH);
  digitalWrite(MS2,HIGH);
  digitalWrite(MS3,HIGH);
#endif

#ifdef USBMIDI
  usbMIDI.setHandleNoteOn(OnNoteOn) ;
  usbMIDI.setHandleControlChange(OnNoteOn) ;
#endif
  MIDI.setHandleNoteOn(OnNoteOn);
  MIDI.setHandleControlChange(OnNoteOn);
  MIDI.begin(MIDI_CHANNEL); 
  stepper1.setMaxSpeed(MAX_SPEED);
  stepper1.setAcceleration(ACCELERATION);
  stepper2.setMaxSpeed(MAX_SPEED);
  stepper2.setAcceleration(ACCELERATION);
 
 // pinMode(MOT_DISABLE,OUTPUT);
 // digitalWrite(MOT_DISABLE,LOW);
  
  pinMode(LED,OUTPUT);
 
  
}

void loop()
{    
#ifdef USBMIDI
  usbMIDI.read(MIDI_CHANNEL);
#endif
  MIDI.read(MIDI_CHANNEL);
  stepper1.run();
  stepper2.run();

   
}

void OnNoteOn(byte channel, byte note, byte velocity)
{

  #ifdef USBMIDI
   digitalWrite(LED,HIGH);
   ledTimer.begin(turnOffLed,100000);  
  #endif
   switch(note)
   {
     case 0:// x os
     {
       //haveNewCommandX=true;
       lastCommandX=((float)velocity/127.f-.5f)*MOVEMENT_RANGE;
       stepper1.moveTo(lastCommandX);
       break;  
     }
     case 1:// y axis
     {
       //haveNewCommandY=true;
       lastCommandY=((float)velocity/127.f-.5f)*MOVEMENT_RANGE;
        stepper2.moveTo(lastCommandY);
       break;
     }
     case 2:// speed
     {
        stepper1.setMaxSpeed(((float)velocity/127.f)*MAX_SPEED);
        stepper2.setMaxSpeed(((float)velocity/127.f)*MAX_SPEED);
        break;
     }
   }
   //usbMIDI.sendNoteOn(note,velocity,channel);
}

void turnOffLed()
{
   digitalWrite(LED,LOW);
   ledTimer.end();
}