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

WiiChuck nunchuck = WiiChuck();
Servo servo1;  // create servo object to control a servo 
                // a maximum of eight servo objects can be created 
Servo servo2;
Servo servo3;

int pin_servo1 = 13;
int pin_servo2 = 12;
int pin_servo3 = 11;

int pin_pot1 = 0;
int val1;

int pos = 0;    // variable to store the servo position 

#define FRAME_DELAY 250 // in 4s of milliseconds; 

byte walkForward[6][3] = {
  {70,50,70},
  {30,50,110},
  {30,96,110},
  {70,96,70},
  {110,96,30},
  {110,50,30}
}; //used to alternate 2 from 50 to 80

byte walkLeft[4][3] = {
  {70,96,70},
  {110,96,30},
  {110,73,30},
  {70,73,70}
};

byte walkRight[4][3] = {
  {70,50,70},
  {30,50,110},
  {30,73,110},
  {70,73,70}
};

int curFrameIndex;
int lastFrameIndex;
byte* curFrame;
byte* lastFrame;
byte numFrames;
byte curFrameSet[6][3];
unsigned long time;
unsigned long lastFrameTime;


byte walkState;
#define STATE_FORWARD 1
#define STATE_BACKWARD 2
#define STATE_IDLE 3

byte turnState;
#define STATE_LEFT 4
#define STATE_RIGHT 5
#define STATE_STRAIGHT 6


void setup() { 
  Serial.begin(115200);
   
  servo1.attach(pin_servo1);  // attaches the servo on pin 9 to the servo object 
  servo2.attach(pin_servo2);
  servo3.attach(pin_servo3);

  lastFrame = walkForward[0];
  curFrame = walkForward[1];
  curFrameIndex = 1;
  
  nunchuck.begin();
  nunchuck.update();
  
  lastFrameTime = millis();
} 
 
 
void loop() { 
  val1 = analogRead(pin_pot1);
  val1 = map(val1, 0, 1023, 0, 179);
    
  nunchuck.update();
  
  
  if(nunchuck.buttonC) {
    // If we have the C button pressed, send our
    // readouts to the computer.  Reading the raw
    // values lets us calibrate the program
    // appropriately
    Serial.print(nunchuck.readRoll());
    Serial.print(", ");
    Serial.print(nunchuck.readPitch()); //if used, would use about 50 to 150 or so
    Serial.print(" : ");
    Serial.print(int(nunchuck.joyX));
    Serial.print(", ");
    Serial.print(int(nunchuck.joyY));
    Serial.println();
    
  }
  
  // here is where we take care of walking motions
  if(nunchuck.buttonC) {
    // if the C button is pressed, figure out how we should be moving -
    //  forward & left?  sitting still?  backward? etc
    if (nunchuck.joyY>180) { //forward
      walkState = STATE_FORWARD;
    } else if (nunchuck.joyY<80) { //back
      walkState = STATE_BACKWARD;
    } else {
      walkState = STATE_IDLE; 
    }
    
    if (nunchuck.joyX < 80) {
      turnState = STATE_LEFT;
    } else if (nunchuck.joyX > 180) {
      turnState = STATE_RIGHT; 
    } else {
      turnState = STATE_STRAIGHT; 
    }
    
    
    if (walkState!=STATE_IDLE) {
      // if we aren't sitting still, then...
     
      // advance the frame
      time = millis() - lastFrameTime;
      if(turnState==STATE_STRAIGHT) numFrames=sizeof(walkForward);
      else if (turnState==STATE_LEFT) numFrames=sizeof(walkLeft);
      else if (turnState==STATE_RIGHT) numFrames=sizeof(walkRight);
      numFrames=numFrames/3;
           
      if(time > FRAME_DELAY) {
        time = time - FRAME_DELAY;
        lastFrameTime = lastFrameTime + FRAME_DELAY;
        
        if(walkState==STATE_FORWARD) {
          // if we're moving forward, then previous frame is curFrame-1
          curFrameIndex++; 
          if(curFrameIndex > (numFrames-1)) curFrameIndex=0;
          
          if(curFrameIndex == 0) lastFrameIndex=numFrames-1;
          else lastFrameIndex=curFrameIndex-1;         
        } else if(walkState==STATE_BACKWARD) {
          // if we're going backward through the frames, then previous frame is curFrame+1
          curFrameIndex--;
          if(curFrameIndex==-1) curFrameIndex = numFrames-1;
          
          if(curFrameIndex == numFrames-1) lastFrameIndex=0;
          else lastFrameIndex=curFrameIndex+1;
        }
        
        // choose the current & last frames based on how we're moving
        if(turnState==STATE_STRAIGHT) curFrame = walkForward[curFrameIndex];
        else if (turnState==STATE_LEFT) curFrame = walkLeft[curFrameIndex];
        else if (turnState==STATE_RIGHT) curFrame = walkRight[curFrameIndex];
        
        if (turnState==STATE_STRAIGHT) lastFrame = walkForward[lastFrameIndex];
        else if (turnState==STATE_LEFT) lastFrame = walkLeft[lastFrameIndex];
        else if (turnState==STATE_RIGHT) lastFrame = walkRight[lastFrameIndex];
      }

      // set the servos to the correct position
      // use map to set it to something between the previous frame and the
      // current frame based on where we are between frames
      // so, if we're halfway between frames, servos will go halfway between
      // where the previous and current frames are
      // 
      servo1.write(map(time, 0, FRAME_DELAY, lastFrame[0], curFrame[0]));
      servo2.write(map(time, 0, FRAME_DELAY, lastFrame[1], curFrame[1]));
      servo3.write(map(time, 0, FRAME_DELAY, lastFrame[2], curFrame[2]));
    } else {
      // if we're sitting still, then just center all the servos
      servo1.write(73);
      servo2.write(73);
      servo3.write(73); 
    }
  } else {
    // c button isn't pressed
    // get timing stuff for frames initialized
    // lastFrame = walkForward[0];
    // curFrame = walkForward[0];
    curFrameIndex = 0;
    lastFrameTime=millis();
    
    if(nunchuck.buttonZ) {
      // if Z is pressed, then set  the servos based on joystick and roll values
      servo1.write(map(nunchuck.joyX, 235, 25, 10, 139)); //mid 125
      servo2.write(map(nunchuck.readRoll(), 40, -40, 10, 139));
      servo3.write(map(nunchuck.joyY, 31, 231, 10, 139)); //mid 131
    } else {
      // the C button wasn't pressed, and now we know the Z button isn't pressed.
      // just center the servos
      servo1.write(73);
      servo2.write(73); 
      servo3.write(73);
    }
  }
  
  
  delay(1);
}