#include <Wire.h>
#include <string.h>
#include <stdio.h>
#include <Servo.h>

uint8_t outbuf[6];		// array to store arduino output
char storebuf[200];		//array to store arduino input (transmitted from PC)
int cnt = 0;

Servo rightServo;
Servo leftServo;

int pinFan = 9;
int cntFan = 0;

void
receiveEvent (int howMany)
{
  char myinput[10];
  while (Wire.available ())
    {
      int c = Wire.receive ();	// receive byte as an integer

      sprintf (myinput, "%d", c);	// convert integer to character
      strncat (storebuf, myinput, 10);	// copy into the input buffer
      strncat (storebuf, ",", 1);	// add a comma
      cnt++;
      // after we read 50 bytes, print what we have so far
      if (cnt > 50)
	{
	  cnt = 0;
	  strcpy (storebuf, "");	// empty out the storage buffer
	}
    }
/* Just loop right 
  if (myinput[0] == '1')
    {
      left ();
    }
*/

  if (myinput[0] == '2')
    {
      right ();
    }
  if (myinput[0] == '3')
    {
      forward ();
    }
  if (myinput[0] == '4')
    {
      fan ();
    }    
}

void
left ()
{
  digitalWrite (pinFan, LOW);	// turn the Fan off
  for (int cnt = 0; cnt < 100; cnt++)
    {
      leftServo.write (65);
      rightServo.write (65);
      Servo::refresh ();
    }
}

void
right ()
{
  digitalWrite (pinFan, LOW);	// turn the Fan off
  for (int cnt = 0; cnt < 100; cnt++)
    {
      leftServo.write (115);
      rightServo.write (115);
      Servo::refresh ();
    }
}

void
forward ()
{
  digitalWrite (pinFan, HIGH);	// turn the Fan on
  for (int cnt = 0; cnt < 100; cnt++)
    {
      leftServo.write (105);
      rightServo.write (75);
      Servo::refresh ();
    }
}

void
fan ()
{
  digitalWrite (pinFan, HIGH);	// turn the Fan on
  for (int cnt = 0; cnt < 100; cnt++)
    {
   
    }
}

void
backward ()
{
}

void
requestEvent ()
{
  // Send some sample data back to the wiimote to be transmitted back to PC
  outbuf[0] = nunchuk_encode_byte (125);	// joystick X
  outbuf[1] = nunchuk_encode_byte (126);	// joystick Y
  outbuf[2] = nunchuk_encode_byte (227);	// Axis X
  outbuf[3] = nunchuk_encode_byte (241);	// Axis Y
  outbuf[4] = nunchuk_encode_byte (140);	// Axis Z
  outbuf[5] = nunchuk_encode_byte (1);    	// Press C button, byte[5] is buttons 
  Wire.send (outbuf, 6);			// send data packet
}

void
setup ()
{
  rightServo.attach (10);   // Attach to the servos
  leftServo.attach (11);
  pinMode (pinFan, OUTPUT);

  Wire.begin (0x52);		        // join i2c bus with address 0x52
  Wire.onReceive (receiveEvent);	// register recieve event
  Wire.onRequest (requestEvent);	// register reqeust event
}


void
loop ()
{
  //delay (75);
}

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