//
// Freqin' Wii - by Laen -  http://www.laen.org
//    Uses freqout by Paul Badger - http://www.arduino.cc/playground/Main/Freqout
//    Uses Wii Nunchuck code by Chad Philips - http://www.windmeadow.com/node/42
//    Uses the Wiichuck adapter by Tod E. Kurt - http://todbot.com/
/*
   INSTRUCTIONS:
 Hook a speaker to the output pin (defined below, 13 by default),
 and hook the Wii adapter up to analog pins 2-5, preferably using the Wiichuck adapter.
 
 Tilt the wii from side to side to adjust the tone, and tilt forward and back to adjust
 the octave.  Push the joystick forward and back to adjust the length of the note.
 Push the Z button to play the note.  
 */


/*  freqout(freq, t)  // freq in hz, t in ms
 *   Paul Badger 2007
 *   a simple tone generation function
 *   generates square waves of arbitrary frequency and duration
 *   program also includes a top-octave lookup table & transposition function
 */

#include <Wire.h>
#include <math.h>  // requires an Atmega168 chip 

int outpin=13;    // Connect your speaker here.

float ps;         // variable for pow pitchShift routine
float noteval;    // The note we play
int dur = 100;    // ..how long we play it.

// note values for two octave scale
// divide them by powers of two to generate other octaves
float A     = 14080;
float AS    = 14917.2;
float B     = 15804.3;
float C     = 16744;
float CS    = 17739.7;
float D     = 18794.5;
float DS    = 19912.1;
float E     = 21096.2;
float F     = 22350.6;
float FS    = 23679.6;
float G     = 25087.7;
float GS    = 26579.5;
float A2    = 28160;
float A2S   = 29834.5;
float B2    = 31608.5;
float C2    = 33488.1;
float C2S   = 35479.4;
float D2    = 37589.1;
float D2S   = 39824.3;
float E2    = 42192.3;
float F2    = 44701.2;
float F2S   = 47359.3;
float G2    = 50175.4;
float G2S   = 53159;
float A3    = 56320;

//rhythm values
int wh = 1024;
int h  = 512;
int dq = 448;
int q = 256;
int qt = 170;
int de = 192;
int e = 128;
int et = 85;
int dsx = 96;
int sx = 64;
int thx = 32;

// major scale just for demo, hack this

float majScale[] = { 
  A,  B,  CS,  D,  E,  FS,  GS,  A2,   B2,  C2S,  D2,  E2,  F2S,  G2S,  A3};

uint8_t outbuf[6];		// array to store arduino output
int cnt = 0;

void freqout(int freq, int t)  // freq in hz, t in ms
{
  int hperiod;                               //calculate 1/2 period in us
  long cycles, i;
  pinMode(outpin, OUTPUT);                   // turn on output pin

  hperiod = (500000 / freq) - 7;             // subtract 7 us to make up for digitalWrite overhead

  cycles = ((long)freq * (long)t) / 1000;    // calculate cycles

  for (i=0; i<= cycles; i++){              // play note for t ms 
    digitalWrite(outpin, HIGH); 
    delayMicroseconds(hperiod);
    digitalWrite(outpin, LOW); 
    delayMicroseconds(hperiod - 1);     // - 1 to make up for digitaWrite overhead
  }
  pinMode(outpin, INPUT);                // shut off pin to avoid noise from other operations

}

void setup()
{
  // This sets up analog pins 2 and 3 to provide power and ground.
  byte pwrpin = PC3;
  byte gndpin = PC2;
  DDRC |= _BV(pwrpin) | _BV(gndpin);
  PORTC &=~ _BV(gndpin);
  PORTC |=  _BV(pwrpin);
  delay(100);  // wait for things to stabilize
  beginSerial (115200);
  //  Serial.print ("Finished setup\n");
  Wire.begin ();		// join i2c bus with address 0x52
  nunchuck_init (); // send the initilization handshake
}

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
loop ()
{

  Wire.requestFrom (0x52, 6);	// request data from nunchuck
  while (Wire.available ())
  {
    outbuf[cnt] = nunchuk_decode_byte (Wire.receive ());	// receive byte as an integer
    cnt++;
  }

  // If we recieved the 6 bytes, then go print them
  if (cnt >= 5)
  {
    int joy_x_axis = outbuf[0];
    int joy_y_axis = outbuf[1];
    int accel_x_axis = outbuf[2] * 2 * 2; 
    int accel_y_axis = outbuf[3] * 2 * 2;
    int accel_z_axis = outbuf[4] * 2 * 2;

    int z_button = 0;
    int 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; }

    ps = (accel_y_axis/100)-3;
    if (ps < 0) ps = 0;
    noteval = (accel_x_axis/2)*pow(2,ps);
    dur     = joy_y_axis;
    if (c_button == 0) {
      int d;
      pinMode(outpin,OUTPUT);
      for (d=0;d<500;d++) {
        digitalWrite(outpin,HIGH);
        delayMicroseconds(d);
        digitalWrite(outpin,LOW);
        delayMicroseconds(d*2);
      }
      /*        for (d=500;d>0;d--) {
       digitalWrite(outpin,HIGH);
       delayMicroseconds(d);
       digitalWrite(outpin,LOW);
       delayMicroseconds(d/4);
       }*/
    }
    if (z_button == 0) {
      freqout(noteval,dur);
    }
    print();
  }

  cnt = 0;
  send_zero (); // send the request for next bytes
  delay (dur);
}

// 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
print ()
{
  int joy_x_axis = outbuf[0];
  int joy_y_axis = outbuf[1];

  int z_button = 0;
  int c_button = 0;

  int accel_x_axis = outbuf[2] * 2 * 2; 
  if ((outbuf[5] >> 2) & 1) { 
    accel_x_axis += 2; 
  }
  if ((outbuf[5] >> 3) & 1) { 
    accel_x_axis += 1; 
  }

  int accel_y_axis = outbuf[3] * 2 * 2;
  if ((outbuf[5] >> 4) & 1) { 
    accel_y_axis += 2; 
  }
  if ((outbuf[5] >> 5) & 1) { 
    accel_y_axis += 1; 
  }

  int accel_z_axis = outbuf[4] * 2 * 2;
  if ((outbuf[5] >> 6) & 1) { 
    accel_z_axis += 2; 
  }
  if ((outbuf[5] >> 7) & 1) { 
    accel_z_axis += 1; 
  }

  // 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; 
  }


  Serial.print (joy_x_axis, DEC);
  Serial.print ("\t");

  Serial.print (joy_y_axis, DEC);
  Serial.print ("\t");

  Serial.print (accel_x_axis, DEC);
  Serial.print ("\t");

  Serial.print (accel_y_axis, DEC);
  Serial.print ("\t");

  Serial.print (accel_z_axis, DEC);
  Serial.print ("\t");

  Serial.print (z_button, DEC);
  Serial.print ("\t");

  Serial.print (c_button, DEC);
  Serial.print ("\t");

  Serial.print ("\r\n");
}

// 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;
}