/*
 midiInServoControl
 use the serial receive (pin 0) to receive MIDI note data
 - we use a pull up 680 ohm resistor at opto-coupler output - makes problem with uploading the sketch
 - might be too small - raise to couple of K

 check for the speed note - translate the velocity to pulse train speed
 count the steps - limit the number of steps - use the limit switches if needed
 check for the dir note - translate the velocity (0 or >0)to direction pin  

 hex = dec
---------------
 0x00 = 0
 0x01 = 1
 0x02 = 2
 0x03 = 3
 0x04 = 4
 0x05 = 6
 0x06 = 6
 0x07 = 7
 0x08 = 8
 0x09 = 9
 0x0A = 10
 0x0B = 11
 0x0C = 12
 0x0D = 13
 0x0E = 14
 0x0F = 15
 0x10 = 16
 0x11 = 17
 0x12 = 18
 0x13 = 19
 0x14 = 20
 0x15 = 21
 0x16 = 22
 0x17 = 23
 0x18 = 24
 0x19 = 25
 0x1A = 26
 0x1B = 27
 0x1C = 28
 0x1D = 29
 0x1E = 30
 0x1F = 31
 0x20 = 32
 
 0x28 = 40
 0x32 = 50
 0x45 = 69
 0x5A = 90
 0x64 = 100
 0x7F = 127
 0x80 = 128
 0xFF = 255
 
 */

/**
 http://www.schmalzhaus.com/EasyDriver/Examples/EasyDriverExamples.html
***/

// use the AccelStepper library with L297/L298 combination
#include <AccelStepper.h>

// since we use ISEL Stepper Driver, 2 driver pins are required 
// Define X stepper and the pins it will use - pin7 pulse train, pin6 dir
AccelStepper stepper1(1, 5, 4);
// Define Y stepper and the pins it will use - pin9 pulse train, pin8 dir
AccelStepper stepper2(1, 6, 7);

// first: 1 means DRIVER -> means that we use a driver like L297 (with Step and Direction pins)
// If an enable line is also needed, call setEnablePin() after construction. 
// You may also invert the pins using setPinsInverted(). 
// second: Arduino digital pin number for motor pin 1. Defaults to pin 2. 
// For a AccelStepper::DRIVER (interface==1), this is the Step input to the driver. 
// Low to high transition means to step) 
// third: Arduino digital pin number for motor pin 2. Defaults to pin 3. 
// For a AccelStepper::DRIVER (interface==1), this is the Direction input the driver. 
// High means forward. 

/**
  1. we receive a note for position: velocity 1-127 describes the final position
  - multiply by steps by motor turns to get to the final position
  2. second note is speed 1-127 - multiply to get the sensible speed
  - if speed 
  3. third note tells about the direction: 0 or >0
  - 
  4. do this all for X and Y
  
  5. we may need a zero positioning button to tell the machine the zero position
  - from this zero position we must always keep in memory the current position and the both 
  limits - left and write
 
  
**/


/*
 http://arduinomidilib.fortyseveneffects.com/a00025.html#ga414b3426cd08e148d612f94d3e462344
*/
// receive input midi data and translate to stepper control
#include <MIDI.h>

// we can use MIDI lib or not
const boolean usemidilib = true;

// use Rx Tx arduino pins 0, 1
const boolean usemidi = false;

const boolean printout = false;

#define MIDICH 1 // define the midi channel to listen to
#define LED 13   // LED pin on Arduino board

#define ZEROPOS 2   // zero position button - pulled up

#define TURNS1 1 // how many turns 
#define TURNS2 1 // how many turns 

// microstepping is the feature of stepper drivers: 1, 2, 4 for L297/298 in ISEL
#define STEPS1 1 // define the microsteps if used - 1=1, 1/2=2, 1/4=4 etc
#define STEPS2 1 // define the microsteps if used
// microsteps enlarge the resolutiuon -> multiply by steps per revolution 

// calculate steps per revolution based on above two
const long steps1 = STEPS1 * TURNS1;
const long steps2 = STEPS2 * TURNS2;
// from here on we use steps1 and steps2 to to check the positions

// limits for both
const long limit1 = -steps1 * TURNS1; // 200 steps per rotation * 3 rotations = 600
long zeroposition1 = 0; // this is check against a minus and plus limits 
// always remember the current position!
long currentposition1 = 0;

const long limit2 = steps2 * TURNS2;
long zeroposition2 = 0; // this is check against a minus and plus limits 
// always remember the current position!
long currentposition2 = 0;

// when limits reached we need to stop nad wait for the next command
// or turn the direction

#define SPEEDFACTOR1 2 // multiply speed by this factor 
#define SPEEDFACTOR2 2 // multiply speed by this factor 

long speedlimit1 = 64 * SPEEDFACTOR1;
long speedlimit2 = 64 * SPEEDFACTOR2;

// variables
// midi read
byte midichannel = MIDICH;
byte midinote = 0;
byte midivelocity = 0;

// globals to check with the RxTx serial MIDI IN OUT
// may not be needed if MIDI lib works
byte commandByte;
byte noteByte;
byte velocityByte;

char ledstate = LOW;
long lastledtime = millis();

// steppers definitions

// adapted midi values
long midipos1 = 0;
long midispeed1 = 64 * SPEEDFACTOR1;
long mididir1 = 0;

long midipos2 = 0;
long midispeed2 = 64 * SPEEDFACTOR2;
long mididir2 = 0;

// speed and position -> these two need to be long!
// direction
long pos1 = 200;
long speed1 = 64 * SPEEDFACTOR1;
byte dir1 = 0;

long pos2 = 200;
long speed2 = 64 * SPEEDFACTOR2;
byte dir2 = 0;

// temporary globals - before conversion  
byte mpos1;
byte mpos2;
byte mspeed1;
byte mspeed2;
byte mdir1;
byte mdir2;

// remember previous values
long prevpos1 = 0;
long prevspeed1 = 50;
byte prevdir1 = 0;

long prevpos2 = 0;
long prevspeed2 = 50;
byte prevdir2 = 0;

void setup() {

  pinMode(LED, OUTPUT); // blink on receive
  digitalWrite(LED, LOW); // blink on receive
  
  pinMode(ZEROPOS, INPUT);
  digitalWrite(ZEROPOS, HIGH); // turn on pull up resistor
  // set the hardware interrupt for zeroposition pin 
  //attachInterrupt(0, setZeroPosition, LOW); // int.0 on pin2	int.1 on pin 3 // LOW FALLING RISING CHANGE HIGH
 
  // output pins for stepper drivers get the output mode with the library
  // AccelStepper lib
  stepper1.setMaxSpeed(speedlimit1); // steps per s; 50, 127 works
  stepper1.setAcceleration(100); //Calling setAcceleration() is expensive, since it requires a square root to be calculated. 
  stepper1.setMinPulseWidth(1000); //The minimum pulse width in microseconds - 10ms is ok 
  stepper1.setCurrentPosition(0); //Resets the current position of the motor, so that wherever the motor happens to be right now is considered to be the new 0 position. 
  stepper1.setSpeed(speedlimit1); //this allows to take in the new value
  // first move - one round 
  stepper1.moveTo(200); // long absolute
  //stepper1.move(100); // long relative move 
  
  stepper2.setMaxSpeed(speedlimit2); // steps per s; 50 works
  stepper2.setAcceleration(100);
  stepper2.setMinPulseWidth(1000); // 10ms
  stepper2.setCurrentPosition(0); //Resets the current position of the motor, so that wherever the motor happens to be right now is considered to be the new 0 position. 
  stepper2.setSpeed(speedlimit2); //this allows to take in the new value
  // first move - one round 
  stepper2.moveTo(200); //stepper2.moveTo(1000000);
      
  if(usemidilib){ 
    // Initiate MIDI communications
    //MIDI.begin(MIDI_CHANNEL_OMNI); // listen to all channels   
    //MIDI.begin();     // Launch MIDI with default options - input channel set to 1          	
    MIDI.begin(MIDICH); // input channel is set to predefined value
  }

  // if we use the Tx Rx pins with MIDI IN OUT we need to use MIDI baud rate 
  // Rx pin0, Tx pin1
  if(usemidi)   Serial.begin(31250);  //  Set MIDI baud rate
  else       Serial.begin(9600);

}

void loop() {
  // timecheck to turn off led  
  BlinkLedOff();

  // check if zeropisition switch pushed
  if(digitalRead(ZEROPOS)==LOW){
    setZeroPosition();
  }

  if(usemidilib){
    // we can use the interrupt function above or this below 

    // Read messages from the serial port using the main input channel. 
    if (MIDI.read() && MIDI.check()) {  // Is there a MIDI message incoming?
      //if(MIDI.check()) // check if a valid message is stored in the structure 
      // types: NoteOff NoteOn ControlChange AfterTouchPoly AfterTouchChannel PitchBend ProgramChange
      switch(MIDI.getType()) {		// Get the type of the message we caught
        case NoteOn:               // If it is a NoteOn
        //case ProgramChange:               // If it is a Program Change
        //Get the channel of the message stored in the structure. 
        //Channel range is 1 to 16. For non-channel messages, this will return 0
        // if correct channel for this machine - read further - or ignore
        midichannel = MIDI.getChannel(); //MIDI.getInputChannel();
            
        if(midichannel==MIDICH){
          // Get the first data byte of the last received message. 
          midinote = MIDI.getData1(); // get the note value = pitch
          // Get the second data byte of the last received message. 
          midivelocity = MIDI.getData2(); // get the note velocity

          BlinkLed();	// Blink the LED now that motor data is on their way

          // do some action based on note received
          checkMIDI(midinote, midivelocity);
          
        }
        break;
      }
    }

  } else { 
    // use above or below
    //if (Serial.available() > 0) {
    if (Serial.available()){
      // filter by note - these are hex values!
      //commandByte = Serial.read();//read first byte
      //midichannel==
      // if command byte is channel
      //if(midichannel==MIDICH){
        // if command byte is noteon
        //if(midichannel==MIDICH){
      //      BlinkLed(); // Blink the LED 
        //    noteByte = Serial.read();//read next byte
          //  midinote = 0; // translate note hex byte to dec  
            //velocityByte = Serial.read();//read final byte
            //checkMIDI();
        //}
      //}
    }
  }
  
  //testSteppers();
  steppersPosSpeedDir();

}

void setZeroPosition() {
  // redefine the global zeroposition variable for both
  stepper1.setCurrentPosition(0); //Resets the current position of the motor, so that wherever the motor happens to be right now is considered to be the new 0 position. 
  stepper2.setCurrentPosition(0); //Resets the current position of the motor, so that wherever the motor happens to be right now is considered to be the new 0 position. 
}

// check what to do with received midi
void checkMIDI(byte note, byte velocity){
  
  // motor 1
  // goto position
  if(note==1){
    // this would be: with stepper1 go to position 0-127 - we need some multiplying
    if (velocity == 0) {
      // stop
      mpos1 = 0;
    } else {
      mpos1 = velocity;
    } 
  // with speed
  } else if(note==2) { 
    // this could be: speed for stepper1 0-127 - we need some multiplying here
    if (velocity == 0) {
      // stop
      mspeed1 = 0;
    } else {
      mspeed1 = velocity;
    }  
  // use direction
  } else if(note==3) { 
    // this could be: direction for stepper1 0 or >0
    if (velocity == 0) {
      // left
      mdir1 = 0;
    } else {
       // right 
      mdir1 = 1;    
    }
    
    // motor 2
  } else if(note==4) { 
    // this could be: go with stepper2 to position 0-127 - we need some multiplying
    if (velocity == 0) {
      // stop
      mpos2 = 0;
    } else {
      mpos2 = velocity;
    }  
  } else if(note==5) { 
    // this could be: speed for stepper2 0-127 - we need some multiplying here
    if (velocity == 0) {
      // stop
      midispeed2 = 0;
    } else {
      midispeed2 = velocity;
    }  
  } else if(note==6) { 
    // this would be: direction for stepper2 0 or >0
    if (velocity == 0) {
      // left
       mdir2 = 0;
    } else {
       // right
       mdir2 = 1;
    }  
  }  
  // with these globals adapt the stepper values
  adaptValues();
  
}  

void adaptValues(){
  long pfactor1;
  long pfactor2;
  long sfactor1;
  long sfactor2;
  
  pfactor1 = limit1 / 127;
  pfactor2 = limit2 / 127;

  // adapt position number from max 127 to max limit1 
  midipos1 = mpos1 * pfactor1;
  midipos2 = mpos2 * pfactor2;
  
  if(midipos1>limit1) midipos1=limit1; 
  if(midipos2>limit2) midipos2=limit2; 

  // speed * SPEEDFACTOR2
  sfactor1 = speedlimit1 / 127;
  sfactor2 = speedlimit2 / 127;
  midispeed1 = mspeed1 * sfactor1;
  midispeed2 = mspeed2 * sfactor2;
  if(midispeed1>speedlimit1) midispeed1=speedlimit1; 
  if(midispeed2>speedlimit2) midispeed2=speedlimit2; 

  // dir is ok
  
}

// AccelStepper lib
// check details
void steppersPosSpeedDir() {

  boolean change1=false;
  boolean change2=false;
  // here we use midi globals to adapt stepper globals 
  // stepper receives moveTo position command
  // and runs 
  // setting setSpeed(0) allows to take in the new value for moveTo()!
  // if some action occurs - like new note 
  //stepper1.setSpeed(0);
  //stepper2.setSpeed(0);
  // we have globals: midipos1, midispeed1, mididir1
  // ADAPT HERE THE VALUES
  // check for previous values - allow to change on the fly
  if(midipos1!=prevpos1){
    prevpos1 = midipos1; // remember anew
    change1 = true;
  }
  if(midispeed1!=prevspeed1){
    prevspeed1 = midispeed1; // remember anew
    change1 = true;
  }
  if(mididir1!=prevdir1){
    prevdir1 = mididir1; // remember anew
    change1 = true;
  }
  
  if(change1 == true) {
    stepper1.setSpeed(0); // this allows to change the moveTo value
    stepper1.moveTo(midipos1);  
    stepper1.setSpeed(midispeed1);
  }

  if(midipos2!=prevpos2){
    prevpos2 = midipos2; // remember anew
    change2 = true;
  }
  if(midispeed2!=prevspeed2){
    prevspeed2 = midispeed2; // remember anew
    change2 = true;
  }
  if(mididir2!=prevdir2){
    prevdir2 = mididir2; // remember anew
    change2 = true;
  }
  
  if(change2 == true) {
    stepper2.setSpeed(0); // this allows to change the moveTo value
    stepper2.moveTo(midipos2);  
    stepper2.setSpeed(midispeed2);
  }

  //stepper1.currentPosition()

  // Change direction at the limits
  if (stepper1.distanceToGo() == 0) stepper1.moveTo(-stepper1.currentPosition());
  if (stepper2.distanceToGo() == 0) stepper2.moveTo(-stepper2.currentPosition());
 
  // stepper1.distanceToGo() is long
  // this would mean: if distance is reached - turn direction
  if (stepper1.distanceToGo() == 0)  {
    pos1 = -pos1;
    stepper1.moveTo(pos1);
  }

  if (stepper2.distanceToGo() == 0)  {
    pos2 = -pos2;
    stepper2.moveTo(pos2);
  }
  
  // moveTo argument should be limited to what we can handle
  // currentPosition() -> the current step
  // distanceToGo() -> how far still to go
  // setMaxSpeed(300); // steps per ?
  // setAcceleration(100);
  //moveTo(600)

  // this must hppen as often as possible - in the  main loop
  // - one step per loop
  // run steppers
  stepper1.run();
  stepper2.run();

  if(printout) {
    Serial.print(" current position: ");
    Serial.print(stepper1.currentPosition());
    Serial.print(", speed: ");
    Serial.print(stepper1.speed());
    Serial.print(", speed limit: ");
    Serial.println(midispeed1);
    // some delay
    delay(10);
  }

}


void testSteppers() {
  // this must be in the loop
  // stepper receives motoTo position command
  // and runs 
  // setting this allows to take in the new value for moveTo()!
  // if some action occurs - like new note 
  //stepper1.setSpeed(0);
  //stepper2.setSpeed(0);
  
  // moveTo argument should be limited to what we can handle
  // currentPosition() -> the current step
  // distanceToGo() -> how far still to go
  // setMaxSpeed(300); // steps per ?
  // setAcceleration(100);
  //moveTo(600)
 
  //stepper1.moveTo(600); // one round = 200
  //stepper2.moveTo(600); // one round = 200

  // Change direction at the limits and go back
  if (stepper1.distanceToGo() == 0){
    // change speed?
    //stepper1.setMaxSpeed(200); // steps per sec?
    //stepper1.setAcceleration(50);
    stepper1.moveTo(-stepper1.currentPosition());
  }  
  if (stepper2.distanceToGo() == 0){
    stepper2.moveTo(-stepper2.currentPosition());
  }  
  
  // must be in the loop each loop cycle!
  stepper1.run();
  stepper2.run();

  /**
  Serial.print(" values: ");
  Serial.print(stepper1.currentPosition());
  Serial.print(", ");
  Serial.println(stepper2.currentPosition());
  // some delay
  delay(10);
  **/
}

void checkMIDI2(){
  do{
    if (Serial.available()){
      commandByte = Serial.read();//read first byte
      noteByte = Serial.read();//read next byte
      velocityByte = Serial.read();//read final byte
      BlinkLed(); // Blink the LED a number of times 
    }
  }
  while (Serial.available() > 2); //when at least three bytes available
}

// Basic blink function
void BlinkLed() {
  digitalWrite(LED, HIGH);
  lastledtime = millis();
  ledstate = HIGH;
}
void BlinkLedOff() {
  if(ledstate == HIGH) {
    if(millis()-lastledtime > 10){ //ms
      digitalWrite(LED, LOW);
      lastledtime = millis();
      ledstate = LOW;
    }  
  }
}