//#define debug

#include <debug.h>
#include <makros.h>

#include <Servo.h>
#include <Wire.h>
#include "WiiNunchuck.h"

/*
  This is a basic demo of using the WiiNunchuck library.
 Written by Grant Emsley - http://arduino.emsley.ca
 */

Servo servo1;
Servo servo2;

void setup()
{
#ifdef debug
  Serial.begin(57600);
  while(!Serial) ;
  Serial.print("WiiNunchuck Demo ready\n");
#endif

  // This initializes the nunchuck. If it can't initialize, or doesn't see the nunchuck, it waits forever.
  // It takes one option, a 1 for power, or 0 for no power. If power is turned on, it will output + on analog 3, - on analog 2.
  // I recommend using 0 and powering it through the 3.3v power connector if possible.  Use this only if you are using the little nunchuck adapter.
  Nunchuck.init(0);
  servo1.attach(5);
  servo1.write(90);
  servo2.attach(6);
  servo2.write(90);
}

void loop()
{
  Nunchuck.getData();
  // Prints data for all the sensors in a table format
  Nunchuck.printData();

  if (!(Nunchuck.zbutton() == 0)) {
    int joyx = Nunchuck.joyx();
    joyx = map(joyx, 0, 255, 0, 180);
    servo1.write(joyx);

    int joyy = Nunchuck.joyy();
    joyy = map(joyy, 0, 255, 0, 180);
    servo2.write(joyy);
  }
  else if (!(Nunchuck.cbutton() == 0)) {
    int joyx = Nunchuck.accelx();
    joyx = map(joyx, 300, 700, 0, 180);
    servo1.write(joyx);

    int joyy = Nunchuck.accely();
    joyy = map(joyy, 300, 700, 0, 180);
    servo2.write(joyy);
  } 
  else {
    servo1.write(90);
    servo2.write(90);
  }
//  delay(10);
}