#include "Arduino.h"
#include "DualVNH5019MotorShield.h"
#include <digitalWriteFast.h>  
 
// Code Organization and PI Controller Written  by Steven Jens Jorgensen (stevenjj@utexas.edu)
// Tuned for final presentation by Matthew Horn (mwhorn@utexas.edu)
 
// library for high performance reads and writes by jrraines
// see http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1267553811/0
// and http://code.google.com/p/digitalwritefast/
// Code sample is from http://www.hessmer.org/blog/2011/01/30/quadrature-encoder-too-fast-for-arduino/
// It turns out that the regular digitalRead() calls are too slow and bring the arduino down when
// I use them in the interrupt routines while the motor runs at full speed creating more than
// 40000 encoder ticks per second per motor.
// Quadrature encoders
// Left encoder
#define c_LeftEncoderInterrupt 4
#define c_LeftEncoderPinA 19
#define c_LeftEncoderPinB 25
#define LeftEncoderIsReversed
const int buttonPin = 43;     // the number of the pushbutton pin
const int ledPin =  13;      // the number of the LED pin
int buttonState = 0;  
float ENCODER_RES = 64.0; //per revolution
float GEARBOX_RATIO = 131.25; //131.25:1
volatile bool _LeftEncoderBSet;
volatile long _LeftEncoderTicks = 0;
DualVNH5019MotorShield md;
// 40/16 --> 2.5 <-- Output/Input Spin output to 180deg. set goToDeg(180*2.5)
int MaxSpeed = 350; //400; // Current to send motor 0-400 From spec sheet.

unsigned long time_delay_toSnap = 1000; // Time Delay in milliseconds
unsigned long time_delay_afterSnap = 2000; // Time Delay in milliseconds
unsigned long time_delay_reload = 3000; // Time Delay in milliseconds
unsigned long time_start  = 0;

void stopIfFault()
{
  if (md.getM2Fault())
  {
    Serial.println("M2 fault");
    while(1);
  }
}
void setup()
{
  Serial.begin(115200); //baudrate

  // Quadrature encoders
  // Left encoder  
  pinMode(c_LeftEncoderPinA, INPUT);      // sets pin A as input
  digitalWrite(c_LeftEncoderPinA, LOW);  // turn on pullup resistors
  pinMode(c_LeftEncoderPinB, INPUT);      // sets pin B as input
  digitalWrite(c_LeftEncoderPinB, LOW);  // turn on pullup resistors
  attachInterrupt(c_LeftEncoderInterrupt, HandleLeftMotorInterruptA, RISING);
  // initialize the LED pin as an output:
  pinMode(ledPin, OUTPUT);      
  // initialize the pushbutton pin as an input:
  pinMode(buttonPin, INPUT);    
  
  Serial.println("Dual VNH5019 Motor Shield");
  //goToDeg(-2.5);
  md.init();
}
void setMotor2(int Des_speed){
  md.setM2Speed(Des_speed);
  stopIfFault();    
  //Serial.print("M1 current: ");
  //Serial.println(md.getM1CurrentMilliamps());    
}
float ticksToDeg(int ticks){
  return float(ticks)/ENCODER_RES/GEARBOX_RATIO*360.0; 
}
int maxEffort(float effort){
  if (effort > MaxSpeed){
    return int(MaxSpeed);
  }
  else if (effort < -MaxSpeed){
    return int(-MaxSpeed);
  }
  else{
    return int(effort);
  }  
}
void goToDeg(float des_deg){
  float cur_deg = ticksToDeg(_LeftEncoderTicks);
  float e = des_deg - cur_deg;
  float e_tot = 0;  
  while (abs(int(e)) > 0.05){
    //Serial.print(" Degs: ");  
    //Serial.println(ticksToDeg(_LeftEncoderTicks));    
    cur_deg = ticksToDeg(_LeftEncoderTicks);
    e = des_deg - cur_deg;
    e_tot = e_tot + e;
    float k = 5;
    float i = 0.5;
    setMotor2(maxEffort( k*e+ i*e_tot ));
  }
  setMotor2(0);
}
void loop()
{
  //setMotor2(250);
  //Serial.println(_LeftEncoderTicks);
  //Serial.print("\t");
  //Serial.print("\n");
   buttonState = digitalRead(buttonPin);
  if (buttonState == HIGH) {     
    // turn LED on:    
    digitalWrite(ledPin, HIGH);  
    Serial.println(" Resetting Encoder to 0 ..."); 
    Serial.println(" BUTTON PUSHED"); 
    _LeftEncoderTicks -= _LeftEncoderTicks; // 0 Encoder Ticks
    Serial.print(" Degs: ");  
    Serial.println(ticksToDeg(_LeftEncoderTicks)); 
    delay(500);
    Serial.println(" Loading to 180 deg"); 
    MaxSpeed = MaxSpeed/4;
    goToDeg(-92);
    Serial.print(" Degs: ");  
    Serial.println(ticksToDeg(_LeftEncoderTicks));      
    delay(6000);
    Serial.println(" Loading to Finish");
   MaxSpeed = MaxSpeed*4; 
    goToDeg(-185);
    Serial.print(" Degs: ");  
    Serial.println(ticksToDeg(_LeftEncoderTicks));
    //
    delay(3000);
  } 
  else {
    // turn LED off:
    digitalWrite(ledPin, LOW); 
  }
  //Serial.println(" Loading to 195 deg"); 
  //goToDeg(195*2.5);
 // Serial.print(" Degs: ");  
  //Serial.println(ticksToDeg(_LeftEncoderTicks));      
  //delay(2000);
  //  Serial.println(" Going to -45.0"); 
  //  goToDeg(-45.0);
  //    Serial.print(" Degs: ");  
  //    Serial.println(ticksToDeg(_LeftEncoderTicks));    
  //    delay(2000);

  //  Serial.print(" Degs: ");  
  //  Serial.println(ticksToDeg(_LeftEncoderTicks));      
  // if (digitalRead(buttonPin) == HIGH) {
  //     setMotor2(MaxSpeed/2); // Snap!
  //     delay(time_delay_toSnap);    
  //     Serial.println(_LeftEncoderTicks);
  //     
  //     setMotor2(0); // Stop motor
  //     delay(time_delay_afterSnap); // Wait To Gulp
  //     Serial.println(_LeftEncoderTicks);
  //     
  //     setMotor2(MaxSpeed/2); // Reload
  //     delay(time_delay_reload);
  //     
  //     setMotor2(0); // Stop Motor
  //     Serial.println(_LeftEncoderTicks);
  //     delay(2000); // demo Delay
  // }

  //delay(20);
}
// Interrupt service routines for the left motor's quadrature encoder
void HandleLeftMotorInterruptA()
{
  // Test transition; since the interrupt will only fire on 'rising' we don't need to read pin A
  _LeftEncoderBSet = digitalReadFast(c_LeftEncoderPinB);   // read the input pin
  // and adjust counter + if A leads B
#ifdef LeftEncoderIsReversed
  _LeftEncoderTicks -= _LeftEncoderBSet ? -1 : +1;
#else
  _LeftEncoderTicks += _LeftEncoderBSet ? -1 : +1;
#endif
}