#include < Wire.h >
 #include < Streaming.h >
 
 //WM+ stuff
 byte data[6]; //six data bytes
 int yaw, pitch, roll; //three axes
 int yaw0, pitch0, roll0; //calibration zeroes
 
 //nunchuck stuff
 uint8_t outbuf[6]; // array to store arduino output
 int cnt = 0;
 int joy_x_axis, joy_y_axis, accel_x_axis, accel_y_axis, accel_z_axis;
 boolean z_button, c_button;
 
 void setup(){
 	Serial.begin(115200);
 	Serial << "WM+ and Nunchuck tester" << endl;
 	pinMode(3, OUTPUT);
 	pinMode(4, OUTPUT);
 	digitalWrite(3, HIGH);
 	digitalWrite(4, HIGH);
 	Wire.begin();
 	switchtowmp();
 	wmpOn();
 	calibrateZeroes();
 	switchtonunchuck();
 	nunchuck_init();
 }
 
 void loop(){
 	switchtowmp();
 	receiveData();
 //receiveRaw();
 	switchtonunchuck();
 	receive_nunchuck_data();
 	Serial << yaw << "\t" << pitch << "\t" << roll << "\t" << joy_x_axis << "\t" << joy_y_axis << "\t";
 	Serial << accel_x_axis << "\t" << accel_y_axis << "\t" << accel_z_axis;
 	Serial << "\t" << _DEC(z_button) << "\t" << _DEC(c_button) << endl;
 	delay(100); 
 }
 
 void nunchuck_init ()
 {
 Wire.beginTransmission (0x52);// transmit to device 0x52
 Wire.send (0x40); // sends memory address
 Wire.send (0x00); // sends sent a zero. 
 Wire.endTransmission (); // stop transmitting
 }
 
 void send_zero ()
 {
 Wire.beginTransmission (0x52);// transmit to device 0x52
 Wire.send (0x00); // sends one byte
 Wire.endTransmission (); // stop transmitting
 }
 
 void receive_nunchuck_data(){
 	Wire.requestFrom(0x52, 6);
 	for (int i=0;i<6;i++){
 		data[i]=Wire.receive();
 	}
 	make_nunchuck_data();
 	send_zero();
 }
 
 void make_nunchuck_data ()
 {
 	for(int i=0;i<6;i++){
 		outbuf[i]=nunchuck_decode_byte(data[i]);
 	}
 	joy_x_axis = outbuf[0];
 	joy_y_axis = outbuf[1];
 	accel_x_axis = outbuf[2] * 2 * 2;
 	accel_y_axis = outbuf[3] * 2 * 2;
 	accel_z_axis = outbuf[4] * 2 * 2;
 	z_button = 0;
 	c_button = 0;
 
 // byte outbuf[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 ((outbuf[5] >> 0) & 1)
 	{
 		z_button = 1;
 	}
 	if ((outbuf[5] >> 1) & 1)
 	{
 		c_button = 1;
 	}
 	if ((outbuf[5] >> 2) & 1)
 	{
 		accel_x_axis += 2;
 	}
 	if ((outbuf[5] >> 3) & 1)
 	{
 		accel_x_axis += 1;
 	}
 	if ((outbuf[5] >> 4) & 1)
 	{
 		accel_y_axis += 2;
 	}
 	if ((outbuf[5] >> 5) & 1)
 	{
 		accel_y_axis += 1;
 	}
 	if ((outbuf[5] >> 6) & 1)
 	{
 		accel_z_axis += 2;
 	}
 	if ((outbuf[5] >> 7) & 1)
 	{
 		accel_z_axis += 1;
 	}
 }
 
 char nunchuck_decode_byte (char x)
 {
 	x = (x ^ 0x17) + 0x17;
 	return x;
 }
 
 void wmpOn(){
 Wire.beginTransmission(0x53);//WM+ starts out deactivated at address 0x53
 Wire.send(0xfe); //send 0x04 to address 0xFE to activate WM+
 Wire.send(0x04);
 Wire.endTransmission(); //WM+ jumps to address 0x52 and is now active
 }
 
 void wmpOff(){
 	Wire.beginTransmission(82);
 Wire.send(0xf0);//address then
 Wire.send(0x55);//command
 //Wire.send(0x00);
 //Wire.send(0xfb);
 Wire.endTransmission();
 }
 
 void wmpSendZero(){
 Wire.beginTransmission(0x52);//now at address 0x52
 Wire.send(0x00); //send zero to signal we want info
 Wire.endTransmission();
 }
 
 void calibrateZeroes(){
 	for (int i=0;i<10;i++){
 		wmpSendZero();
 		Wire.requestFrom(0x52,6);
 		for (int i=0;i<6;i++){
 			data[i]=Wire.receive();
 		}
 yaw0+=(((data[3] >> 2) << 8)+data[0])/10;//average 10 readings
 pitch0+=(((data[4] >> 2) << 8)+data[1])/10;// for each zero
 roll0+=(((data[5] >> 2) << 8)+data[2])/10;
 }
 Serial.print("Yaw0:");
 Serial.print(yaw0);
 Serial.print(" Pitch0:");
 Serial.print(pitch0);
 Serial.print(" Roll0:");
 Serial.println(roll0);
 }
 
 void receiveData(){
 wmpSendZero(); //send zero before each request (same as nunchuck)
 Wire.requestFrom(0x52,6); //request the six bytes from the WM+
 for (int i=0;i<6;i++){
 	data[i]=Wire.receive();
 }
 //see http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus
 //for info on what each byte represents
 yaw=((data[3] >> 2) << 8)+data[0]-yaw0; 
 pitch=((data[4] >> 2) << 8)+data[1]-pitch0;
 roll=((data[5] >> 2) << 8)+data[2]-roll0; 
 }
 
 void receiveRaw(){
 wmpSendZero(); //send zero before each request (same as nunchuck)
 Wire.requestFrom(0x52,6);//request the six bytes from the WM+
 for (int i=0;i<6;i++){
 	data[i]=Wire.receive();
 }
 
 yaw=((data[3] >> 2) << 8)+data[0];
 pitch=((data[4] >> 2) << 8)+data[1];
 roll=((data[5] >> 2) << 8)+data[2];
 }
 
 void switchtonunchuck(){
 	digitalWrite(3, LOW);
 	digitalWrite(4, LOW);
 	digitalWrite(4, HIGH);
 }
 
 void switchtowmp(){
 	digitalWrite(3, LOW);
 	digitalWrite(4, LOW);
 	digitalWrite(3, HIGH);
 }