#include "Wire.h"
#include "wiimote.h"

#define DATA 8 //1pin
#define CMD  7 //2pin
#define ATT  6 //6pin
#define CLK  5 //7pin
#define WAIT 10

uint8_t padStat[6];
uint8_t padOldStat[6];
uint8_t header[3];

// Classic Controller Buttons
int bdl = 0; // D-Pad Left state
int bdr = 0; // D-Pad Right state
int bdu = 0; // D-Pad Up state
int bdd = 0; // D-Pad Down state
int ba = 0; // A button state
int bb = 0; // B button state
int bx = 0; // X button state
int by = 0; // Y button state
int bl = 0; // L button state
int br = 0; // R button state
int bm = 0; // MINUS button state
int bp = 0; // PLUS button state
int bhome = 0; // HOME button state
int bzl = 0; // ZL button state
int bzr = 0; // ZR button state
byte lx = 31;
byte ly = 31;
byte rx = 16;
byte ry = 16;

// Wiimote button data stream
byte *stream_callback(byte *buffer)
{
  wiimote_write_buffer(buffer, bdl, bdr, bdu, bdd, ba, bb, bx, by, bl, br,
      bm, bp, bhome, lx, ly, rx, ry, bzl, bzr);

  return buffer;
}

void setup()
{
  pinMode(DATA, INPUT_PULLUP);
  pinMode(CMD, OUTPUT);
  pinMode(ATT, OUTPUT);
  pinMode(CLK, OUTPUT);
  digitalWrite(CMD, HIGH);
  digitalWrite(ATT, HIGH);
  digitalWrite(CLK, HIGH);

  // Prepare wiimote communications
  wiimote_stream = stream_callback;
  wiimote_init();

  for(int i=0; i<1; i++)
  {
    padOldStat[i] = 0;
    padOldStat[i + 2] = 16;
    padOldStat[i + 4] = 31;
  }
}

void loop()
{
  int len;

  digitalWrite(ATT, LOW);

  // コントローラー情報
  header[0] = readData(0x01);
  header[1] = readData(0x42);
  header[2] = readData(0x00);

  // ボタン情報
  len = (header[1] & 0xf) << 1; // 送られてくるデータ数
  for(int i=0; i<len; i++)
    padStat[i] = readData(0);

  digitalWrite(ATT, HIGH);

  switch(header[1])
  {
    case 0x41: // デジタル
      digitalPad();
      break;
    case 0x73: // アナログ
      analogPad();
      break;
    case 0x23: // ネジコン
      negcon();
      break;
  }
}

byte readData(byte command)
{
  byte data = 0;

  delayMicroseconds(WAIT);
  for (int i=0; i<8; i++)
  {
    digitalWrite(CMD, (command >> i) & 1);
    digitalWrite(CLK, LOW);
    delayMicroseconds(WAIT);
    data |= digitalRead(DATA) << i;
    digitalWrite(CLK, HIGH);
    delayMicroseconds(WAIT);
  }
  return data;
}

void digitalPad()
{
  uint8_t bt;
  
  bt = ~padStat[0];
  if(padOldStat[0] != bt)
  {
    bdl = (bt >> 7) & 1; // 左
    bdd = (bt >> 6) & 1; // 下
    bdr = (bt >> 5) & 1; // 右
    bdu = (bt >> 4) & 1; // 上
    bp = (bt >> 3) & 1;  // Start
    bm = bt & 1;         // Select
    padOldStat[0] = bt;
  }

  bt = ~padStat[1];
  if(padOldStat[1] != bt)
  {
    by = (bt >> 7) & 1; // □
    bb = (bt >> 6) & 1; // ×
    ba = (bt >> 5) & 1; // ○
    bx = (bt >> 4) & 1; // △
    br = (bt >> 3) & 1;  // R1
    bl = (bt >> 2) & 1;  // L1
    bzr = (bt >> 1) & 1; // R2
    bzl = bt & 1;        // L2
    padOldStat[1] = bt;
  }
}

void analogPad()
{
  uint8_t bt;

  bt = ~padStat[0];
  if(padOldStat[0] != bt)
  {
    bdl = (bt >> 7) & 1; // 左
    bdd = (bt >> 6) & 1; // 下
    bdr = (bt >> 5) & 1; // 右
    bdu = (bt >> 4) & 1; // 上
    bp = (bt >> 3) & 1;  // Start
    bhome = (bt >> 2) & 1; // R SW
    //bhome = (bt >> 1) & 1; // L SW
    bm = bt & 1;         // Select
    padOldStat[0] = bt;
  }

  bt = ~padStat[1];
  if(padOldStat[1] != bt)
  {
    by = (bt >> 7) & 1; // □
    bb = (bt >> 6) & 1; // ×
    ba = (bt >> 5) & 1; // ○
    bx = (bt >> 4) & 1; // △
    br = (bt >> 3) & 1;  // R1
    bl = (bt >> 2) & 1;  // L1
    bzr = (bt >> 1) & 1; // R2
    bzl = bt & 1;        // L2
    padOldStat[1] = bt;
  }

  // アナログ右
  bt = padStat[2] >> 3;
  if(padOldStat[2] != bt)
  {
    rx = bt;
    padOldStat[2] = bt;
  }
  bt = padStat[3] >> 3;
  if(padOldStat[3] != bt)
  {
    ry = bt;
    padOldStat[3] = bt;
  }

  // アナログ左
  bt = padStat[4] >> 2;
  if(padOldStat[4] != bt)
  {
    lx = bt;
    padOldStat[4] = bt;
  }
  bt = padStat[5] >> 2;
  if(padOldStat[5] != bt)
  {
    ly = bt;
    padOldStat[5] = bt;
  }
}

void negcon()
{
  uint8_t bt;

  bt = ~padStat[0];
  if(padOldStat[0] != bt)
  {
    bdl = (bt >> 7) & 1; // 左
    bdd = (bt >> 6) & 1; // 下
    bdr = (bt >> 5) & 1; // 右
    bdu = (bt >> 4) & 1; // 上
    bp = (bt >> 3) & 1;  // Start
    padOldStat[0] = bt;
  }

  bt = ~padStat[1];
  if(padOldStat[1] != bt)
  {
    ba = (bt >> 5) & 1; // ○
    bx = (bt >> 4) & 1; // △
    br = (bt >> 3) & 1;  // R1
    padOldStat[1] = bt;
  }

  // ねじり
  bt = padStat[2] >> 2;
  if(padOldStat[2] != bt)
  {
    lx = bt;
    padOldStat[2] = bt;
  }
  // Iボタン
  bt = padStat[3];
  if(padOldStat[3] != bt)
  {
    if(bt < 0x20)
      bb = 0;
    else
      bb = 1;
    padOldStat[3] = bt;
  }
  // IIボタン
  bt = padStat[4];
  if(padOldStat[4] != bt)
  {
    if(bt < 0x20)
      by = 0;
    else
      by = 1;
    padOldStat[4] = bt;
  }
  // L1
  bt = padStat[5];
  if(padOldStat[5] != bt)
  {
    if(bt < 0x20)
      bl = 0;
    else
      bl = 1;
    padOldStat[5] = bt;
  }
}