#include <Wire.h>
#include <Servo.h> 
#include <Stepper.h>

unsigned long tStep; // total
unsigned long otime; // old
unsigned long ntime; // new

int rev = 200; // num steps in 1 stepper rev
int pos1 = 90; // localization
int pos2 = 90; // localization
int p1 = 0;    // pause button input comparitor
int p2 = 0;    // pause button cycle reference
int paused = 0;// is paused?

Servo myservo;  // create servo object to control a servo 

// gnd    => 75144 pins: 4, 5, 12, 13
// +5vdc  => 75144 pins: 1, 9, 16
// +12vdc => 75144 pin:  8
// stepper wire colors: b, y, g, r => 75144 pins: 14, 11, 6, 3
Stepper stepper1(rev, 4, 5, 6, 7);   // => 75144 pins: 15, 10, 7, 2
Stepper stepper2(rev, 8, 9, 10, 11); // => 75144 pins: 15, 10, 7, 2

void setup(){
  Serial.begin(115200);// for debugging
  pinMode(13, OUTPUT); // pause indicator    
  myservo.attach(2);   // attaches the servo on pin 2 to the servo object 
  nunchuck_init();     // get to i2c
  Serial.print ("Finished setup\n");
}

void loop(){
  nunchuck_get_data();
  nunchuck_print_data();
  p1 = digitalRead(3); // pause button input
  if(p1 != p2 && p1 == HIGH){ paused = !paused; } // debounce
  if(paused){
    digitalWrite(13, HIGH);   // set the LED on
    delay(1000);              // pause
  }else{
    digitalWrite(13, LOW);    // set the LED off

    int loc1 = analogRead(0); // set new position
    int rpm1 = analogRead(1); // and feed
    rpm1 = map(rpm1, 0, 1023, 10, 150); // calibrate
    loc1 = map(loc1, 0, 1023, 0, 179);  // calibrate
// controlled to move within 180 degrees
    if(loc1 < pos1){           // one step at a time
      stepper1.setSpeed(rpm1); // initiate feed
      stepper1.step(-1);       // move towards location
      pos1--;                  // we moved
    }else if(loc1 > pos1){
      stepper1.setSpeed(rpm1); // initiate feed
      stepper1.step(1);        // move towards location
      pos1++;                  // we moved
    }
  
    int loc2 = analogRead(2);           // set new position
    loc2 = map(loc2, 0, 1023, 0, 179);  // calibrate
    myservo.write(loc2);                // set servo position

    int rpm2 = analogRead(3); // and feed
    rpm2 = map(rpm2, 0, 1023, 10, 600); // calibrate

//speed controlled continuous rotation
    stepper2.setSpeed(rpm2); // initiate feed
    stepper2.step(1);        // preset direction
    tStep++;                 // keep track of total
    if(tStep%200 == 0){         
      int numRevs = tStep/200;
      ntime = millis() - otime;
      int trueRpm = 60000/ntime;
      Serial.print(numRevs);
      Serial.print(" ");
      Serial.print(trueRpm);
      Serial.println(" rpm;");
      otime = ntime;
    }
    
  }
  p2 = p1;
}

//
// Nunchuck functions
//

static uint8_t nunchuck_buf[6];   // array to store nunchuck data,

// initialize the I2C system, join the I2C bus,
// and tell the nunchuck we're talking to it
void nunchuck_init()
{ 
  Wire.begin();	                // join i2c bus as master
  Wire.beginTransmission(0x52);	// transmit to device 0x52
  Wire.send(0x40);		// sends memory address
  Wire.send(0x00);		// sends sent a zero.  
  Wire.endTransmission();	// stop transmitting
}

// Send a request for data to the nunchuck
// was "send_zero()"
void nunchuck_send_request()
{
  Wire.beginTransmission(0x52);	// transmit to device 0x52
  Wire.send(0x00);		// sends one byte
  Wire.endTransmission();	// stop transmitting
}

// Receive data back from the nunchuck, 
int nunchuck_get_data()
{
    int cnt=0;
    Wire.requestFrom (0x52, 6);	// request data from nunchuck
    while (Wire.available ()) {
      // receive byte as an integer
      nunchuck_buf[cnt] = nunchuk_decode_byte(Wire.receive());
      cnt++;
    }
    nunchuck_send_request();  // send request for next data payload
    // If we recieved the 6 bytes, then go print them
    if (cnt >= 5) {
     return 1;   // success
    }
    return 0; //failure
}

// Print the input data we have recieved
// accel data is 10 bits long
// so we read 8 bits, then we have to add
// on the last 2 bits.  That is why I
// multiply them by 2 * 2
void nunchuck_print_data()
{ 
  static int i=0;
  int joy_x_axis = nunchuck_buf[0];
  int joy_y_axis = nunchuck_buf[1];
  int accel_x_axis = nunchuck_buf[2]; // * 2 * 2; 
  int accel_y_axis = nunchuck_buf[3]; // * 2 * 2;
  int accel_z_axis = nunchuck_buf[4]; // * 2 * 2;

  int z_button = 0;
  int c_button = 0;

  // byte nunchuck_buf[5] contains bits for z and c buttons
  // it also contains the least significant bits for the accelerometer data
  // so we have to check each bit of byte outbuf[5]
  if ((nunchuck_buf[5] >> 0) & 1) 
    z_button = 1;
  if ((nunchuck_buf[5] >> 1) & 1)
    c_button = 1;

  if ((nunchuck_buf[5] >> 2) & 1) 
    accel_x_axis += 2;
  if ((nunchuck_buf[5] >> 3) & 1)
    accel_x_axis += 1;

  if ((nunchuck_buf[5] >> 4) & 1)
    accel_y_axis += 2;
  if ((nunchuck_buf[5] >> 5) & 1)
    accel_y_axis += 1;

  if ((nunchuck_buf[5] >> 6) & 1)
    accel_z_axis += 2;
  if ((nunchuck_buf[5] >> 7) & 1)
    accel_z_axis += 1;

  Serial.print(i,DEC);
  Serial.print("\t");
  
  Serial.print("joy:");
  Serial.print(joy_x_axis,DEC);
  Serial.print(",");
  Serial.print(joy_y_axis, DEC);
  Serial.print("  \t");

  Serial.print("acc:");
  Serial.print(accel_x_axis, DEC);
  Serial.print(",");
  Serial.print(accel_y_axis, DEC);
  Serial.print(",");
  Serial.print(accel_z_axis, DEC);
  Serial.print("\t");

  Serial.print("but:");
  Serial.print(z_button, DEC);
  Serial.print(",");
  Serial.print(c_button, DEC);

  Serial.print("\r\n");  // newline
  i++;
}

// Encode data to format that most wiimote drivers except
// only needed if you use one of the regular wiimote drivers
char nunchuk_decode_byte (char x)
{
  x = (x ^ 0x17) + 0x17;
  return x;
}