#include <Wire.h>
#include <Joystick.h>

//ヌンチャク1範囲
#define N1XCEN 120
#define N1XMIN 23
#define N1XMAX 219

#define N1YCEN 136
#define N1YMIN 38
#define N1YMAX 235

//ヌンチャク2範囲
#define N2XCEN 123
#define N2XMIN 25
#define N2XMAX 218

#define N2YCEN 129
#define N2YMIN 30
#define N2YMAX 222

#define MARGIN 20 //スティックのあそび

//ヌンチャク制御ピン
#define GATE1 4
#define GATE2 5

#define DECODE(X) ((X ^ 0x17) + 0x17)

Joystick_ Joystick =
  Joystick_(JOYSTICK_DEFAULT_REPORT_ID, JOYSTICK_TYPE_JOYSTICK,
  4, 0,              //4ボタン 0ハットスイッチ
  true, true, false, //X軸あり, Y軸あり, Z軸なし
  true, true, false,  //Rxあり, Ryあり, Rzなし
  false, false,        //rudderなし, throttleなし
  false, false, false); //acceleratorなし, brakeなし, steeringなし

uint8_t joyX, joyY, buttonZ, buttonC;

uint8_t nunchuck1[6];
uint8_t nunchuck2[6];
int count;
bool selflag = true;

uint8_t stick1[2], stick2[2]; //スティックの状態
uint8_t button[4]; //ボタンの状態

void setup()
{
  //スティック状態初期化
  stick1[0] = N1XCEN; //X軸の中心
  stick1[1] = N1YCEN; //Y軸の中心
  stick2[0] = N2XCEN; //X軸の中心
  stick2[1] = N2YCEN; //Y軸の中心
  
  //ボタン状態初期化
  for(int i=0; i<4; i++)
    button[i] = 1; //オフの状態

  joystickInit();
  nunchuckInit();

  digitalWrite(GATE1, HIGH);
  digitalWrite(GATE2, LOW);
}

void loop()
{
  count = 0;
  Wire.requestFrom(0x52, 6);
  while(Wire.available())
  {
    nunchuck1[count] = DECODE(Wire.read());
    count++;
  }

  if(count >= 5)
  {
    //ヌンチャク1の状態取得
    setJoystick(nunchuck1);

    //X軸のあそび
    if(joyX > (N1XCEN - MARGIN) && joyX < (N1XCEN + MARGIN))
      joyX = N1XCEN;
    if(joyX != stick1[0])
    {
      Joystick.setXAxis(joyX);
      stick1[0] = joyX;
    }
    //Y軸のあそび
    if(joyY > (N1YCEN - MARGIN) && joyY < (N1YCEN + MARGIN))
      joyY = N1YCEN;
    if(joyY != stick1[1])
    {
      Joystick.setYAxis(joyY);
      stick1[1] = joyY;
    }

    if(buttonC != button[0])
    {
      Joystick.setButton(0, !buttonC);
      button[0] = buttonC;
    }

    if(buttonZ != button[1])
    {
      Joystick.setButton(1, !buttonZ);
      button[1] = buttonZ;
    }
  }

  changeNunchuck();
  sendZero();
  delay(10);

  count = 0;
  Wire.requestFrom(0x52, 6);
  while(Wire.available())
  {
    nunchuck2[count] = DECODE(Wire.read());
    count++;
  }

  if(count >= 5)
  {
    //ヌンチャク2の状態取得
    setJoystick(nunchuck2);

    //X軸のあそび
    if(joyX > (N2XCEN - MARGIN) && joyX < (N2XCEN + MARGIN))
      joyX = N2XCEN;
    if(joyX != stick2[0])
    {
      Joystick.setRxAxis(joyX);
      stick2[0] = joyX;
    }
    //Y軸のあそび
    if(joyY > (N2YCEN - MARGIN) && joyY < (N2YCEN + MARGIN))
      joyY = N2YCEN;
    if(joyY != stick2[1])
    {
      Joystick.setRyAxis(joyY);
      stick2[1] = joyY;
    }
    
    if(buttonC != button[2])
    {
      Joystick.setButton(2, !buttonC);
      button[2] = buttonC;
    }

    if(buttonZ != button[3])
    {
      Joystick.setButton(3, !buttonZ);
      button[3] = buttonZ;
    }
  }

  changeNunchuck();
  sendZero();
  delay(10);
}

void sendZero()
{
  Wire.beginTransmission(0x52);
  Wire.write(0x00);
  Wire.endTransmission();
}

void changeNunchuck()
{
  if(selflag)
  {
    digitalWrite(GATE2, HIGH);
    digitalWrite(GATE1, LOW);
  }
  else
  {
    digitalWrite(GATE1, HIGH);
    digitalWrite(GATE2, LOW);
  }

  selflag = !selflag;
}

void setJoystick(uint8_t* nunchuck)
{
  joyX = nunchuck[0];
  joyY = nunchuck[1];
  buttonZ = 0;
  buttonC = 0;

  if(nunchuck[5] & 1) 
    buttonZ = 1;
  if(nunchuck[5] & 2)
    buttonC = 1;
}

void joystickInit()
{
  Joystick.begin(true);
  Joystick.setXAxisRange(N1XMIN, N1XMAX);
  Joystick.setYAxisRange(N1YMAX, N1YMIN);
  Joystick.setXAxis(N1XCEN);
  Joystick.setYAxis(N1YCEN);
  Joystick.setRxAxisRange(N2XMIN, N2XMAX);
  Joystick.setRyAxisRange(N2YMAX, N2YMIN);
  Joystick.setRxAxis(N2XCEN);
  Joystick.setRyAxis(N2YCEN);
}

void nunchuckInit()
{
  pinMode(GATE1, OUTPUT);
  pinMode(GATE2, OUTPUT);

  //まとめて初期化
  digitalWrite(GATE1, HIGH);
  digitalWrite(GATE2, HIGH);

  Wire.begin();
  Wire.beginTransmission(0x52);
  Wire.write((uint8_t)0x40);
  Wire.write((uint8_t)0x00);
  Wire.endTransmission();
}