#define USE_NEW_WAY_INIT 1 // use "The New Way" of initialization <http://wiibrew.org/wiki/Wiimote#The_New_Way>
#define WII_IDENT_LEN ((byte)6)
#define WII_TELEGRAM_LEN ((byte)6)
#define WII_NUNCHUCK_TWI_ADR ((byte)0x52)
#include <Wire.h>
#include <string.h>
#include <utility/twi.h>
#undef int
#include <stdio.h>
uint8_t outbuf[WII_TELEGRAM_LEN]; // array to store arduino output
int cnt = 0;
int ledPin = 13;
const int xsensorMin = 2;
const int xsensorMax = 255;
const int ysensorMin = 2;
const int ysensorMax = 255;
//const int row[8] = {  2, 3, 4, 5, 6, 7, 8, 9 }; //non BCD
//const int col[8] = { 10,11,12,13,14,15,16,17 }; //non BCD
const int colOut[3] =  { 2, 3, 4 };
const int rowOut[3] =  { 9,10,11 };
int pixels[8][8];           
int x = 4;
int y = 4;

void setup() {
  // initialize the serial port:
  Serial.begin(19200);
  Wire.begin(); // initialize i2c
  #define TWI_FREQ_NUNCHUCK 400000L
  TWBR = ((CPU_FREQ / TWI_FREQ_NUNCHUCK) - 16) / 2;
  nunchuck_init(0); // send the initialization handshake
  byte i;
  if(readControllerIdent(outbuf) == 0){
    Serial.print("Ident=");
    for (i = 0; i < WII_TELEGRAM_LEN; i++){
      Serial.print(outbuf[i], HEX);
      Serial.print(' ');
    }
    Serial.println();
  }

  for (int i = 0; i < 3; i++) {
    pinMode(colOut[i], OUTPUT);
    digitalWrite(colOut[i], LOW);   //col output 
  }
  for (int i = 0; i < 3; i++) {
    pinMode(rowOut[i], OUTPUT);
  }

  // initialize the pixel matrix:
  for (int x = 0; x < 8; x++) {
    for (int y = 0; y < 8; y++) {
      pixels[x][y] = LOW;
    }
  }
  Serial.println("Finished setup");
}

void loop() {
  Wire.requestFrom (WII_NUNCHUCK_TWI_ADR, WII_TELEGRAM_LEN); // request data from nunchuck
  for (cnt = 0; (cnt < WII_TELEGRAM_LEN) && Wire.available (); cnt++){
    outbuf[cnt] = nunchuk_decode_byte (Wire.receive ()); // receive byte as an integer
    digitalWrite (ledPin, HIGH); // sets the LED on
  }
  clearTwiInputBuffer();
  if (cnt >= WII_TELEGRAM_LEN){  // If we recieved the 6 bytes, then go print them
    print ();
    readSensors();
    refreshScreen();
  }
  send_zero (); // send the request for next bytes
  delay (100);
  digitalWrite (ledPin, LOW); // sets the LED off so it flashes with every 'word' (6 wii bytes)
}

byte nunchuck_init (unsigned short timeout){
  byte rc = 1;
  #ifndef USE_NEW_WAY_INIT
    Wire.beginTransmission (WII_NUNCHUCK_TWI_ADR); // transmit to device 0x52
    Wire.send (0x40); // sends memory address
    Wire.send (0x00); // sends sent a zero.
    Wire.endTransmission (); // stop transmitting
  #else
    unsigned long time = millis();
    do{
      Wire.beginTransmission (WII_NUNCHUCK_TWI_ADR); // transmit to device 0x52
      Wire.send (0xF0); // sends memory address
      Wire.send (0x55); // sends data.
      if(Wire.endTransmission() == 0){ // stop transmitting
        Wire.beginTransmission (WII_NUNCHUCK_TWI_ADR); // transmit to device 0x52
        Wire.send (0xFB); // sends memory address
        Wire.send (0x00); // sends sent a zero.
        if(Wire.endTransmission () == 0){ // stop transmitting
          rc = 0;
        }
      }
    }
    while (rc != 0 && (!timeout || ((millis() - time) < timeout)));
  #endif
  return rc;
}

char nunchuk_decode_byte (char x){
  #ifndef USE_NEW_WAY_INIT
    x = (x ^ 0x17) + 0x17;
  #endif
  return x;
}

byte readControllerIdent(byte* pIdent){
  byte rc = 1;
  Wire.beginTransmission (WII_NUNCHUCK_TWI_ADR); // transmit to device 0x52
  Wire.send (0xFA); // sends memory address of ident in controller
  if(Wire.endTransmission () == 0){ // stop transmitting
    byte i;
    Wire.requestFrom (WII_NUNCHUCK_TWI_ADR, WII_TELEGRAM_LEN); // request data from nunchuck
    for (i = 0; (i < WII_TELEGRAM_LEN) && Wire.available (); i++){
      pIdent[i] = Wire.receive(); // receive byte as an integer
    }
    if(i == WII_TELEGRAM_LEN){
      rc = 0;
    }
  }
  return rc;
}

void clearTwiInputBuffer(void){
  while( Wire.available ())
  Wire.receive ();
}

void send_zero (){
  for(byte i = 0; i < 3; i++){
    Wire.beginTransmission (WII_NUNCHUCK_TWI_ADR); // transmit to device 0x52
    Wire.send (0x00); // sends one byte
    Wire.endTransmission (); // stop transmitting
  }
}

void print (){
  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;
  if ((outbuf[5] >> 0) & 1){ z_button = 1; }
  if ((outbuf[5] >> 1) & 1){ c_button = 1; }

  Serial.print ("x:");
  Serial.print (joy_x_axis, DEC);
  Serial.print ("\ty:");
  Serial.print (joy_y_axis, DEC);
  Serial.print ("\tX:");
  Serial.print (accel_x_axis, DEC);
  Serial.print ("\tY:");
  Serial.print (accel_y_axis, DEC);
  Serial.print ("\tZ:");
  Serial.print (accel_z_axis, DEC);
  Serial.print ("\tz:");
  Serial.print (z_button, DEC);
  Serial.print ("\tc:");
  Serial.print (c_button, DEC);
  Serial.print ("\r\n");
}

void readSensors() {
  int joy_x_axis = outbuf[0];// * 2 * 2
  int joy_y_axis = outbuf[1];// * 2 * 2
  
  pixels[x][y] = LOW; // set last position in memory to off
  x = map(joy_x_axis, xsensorMin, xsensorMax, 0, 7);
  y = map(joy_y_axis, ysensorMin, ysensorMax, 7, 0);
  pixels[x][y] = HIGH;
  Serial.print (x, DEC);
  Serial.print ("\t");
  Serial.print (y, DEC);
  Serial.print ("\r\n");

}

void refreshScreen() {
  for (int thisRow = 0; thisRow < 8; thisRow++) {  // iterate over the rows (cathodes)
    for (int i = 0; i < 3; i++) {  // take the row pin (cathode) high:
      digitalWrite(rowOut[bitRead(byte(thisRow), i)] , LOW);
    }
    for (int thisCol = 0; thisCol < 8; thisCol++) { // iterate over the cols (anodes)
      int thisPixel = pixels[thisRow][thisCol]; // get the state of the current pixel
      for (int i = 0; i < 3; i++) { // when the row is HIGH and the col is LOW, the LED where they meet turns on
        digitalWrite(colOut[bitRead(byte(thisCol), i)] , thisPixel);
      }
      if (thisPixel == HIGH) {  // turn the pixel off
        delay(10);
        for (int i = 0; i < 3; i++) {
          digitalWrite(colOut[bitRead(byte(thisCol), i)] , LOW);
        }
      }
    }
    for (int i = 0; i < 3; i++) {  // take the row pin low to turn off the whole row
      digitalWrite(rowOut[bitRead(byte(thisRow), i)] , HIGH);
    }
  }

}