#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 ACCEMG 50 //加速度センサーのあそび

//ヌンチャク制御ピン
#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, true,  //Rxあり, Ryあり, Rzあり
  true, true,        //rudderあり, throttleあり
  true, true, true); //acceleratorあり, brakeあり, steeringあり

uint8_t joyX, joyY, accelX, accelY, accelZ, buttonZ, buttonC;

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

uint8_t stick1[2], stick2[2]; //スティックの状態
uint8_t accel1[3], accel2[3]; //加速度センサーの状態
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<3; i++)
  {
    accel1[i] = 128;
    accel2[i] = 128;
  }
  
  //ボタン状態初期化
  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;
    }

    //センサーX軸のあそび
    if(accelX > (128 - ACCEMG) && accelX < (128 + ACCEMG))
      accelX = 128;
    if(accelX != accel1[0])
    {
      Joystick.setRxAxis(accelX);
      accel1[0] = accelX;
    }
    //センサーY軸のあそび
    if(accelY > (128 - ACCEMG) && accelY < (128 + ACCEMG))
      accelY = 128;
    if(accelY != accel1[1])
    {
      Joystick.setRyAxis(accelY);
      accel1[1] = accelY;
    }
    //センサーZ軸のあそび
    if(accelZ > (128 - ACCEMG) && accelZ < (128 + ACCEMG))
      accelZ = 128;
    if(accelZ != accel1[2])
    {
      Joystick.setRzAxis(accelZ);
      accel1[2] = accelZ;
    }

    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.setThrottle(joyX);
      stick2[0] = joyX;
    }
    //Y軸のあそび
    if(joyY > (N2YCEN - MARGIN) && joyY < (N2YCEN + MARGIN))
      joyY = N2YCEN;
    if(joyY != stick2[1])
    {
      Joystick.setRudder(joyY);
      stick2[1] = joyY;
    }
    
    //センサーX軸のあそび
    if(accelX > (128 - ACCEMG) && accelX < (128 + ACCEMG))
      accelX = 128;
    if(accelX != accel2[0])
    {
      Joystick.setAccelerator(accelX);
      accel2[0] = accelX;
    }
    //センサーY軸のあそび
    if(accelY > (128 - ACCEMG) && accelY < (128 + ACCEMG))
      accelY = 128;
    if(accelY != accel2[1])
    {
      Joystick.setBrake(accelY);
      accel2[1] = accelY;
    }
    //センサーZ軸のあそび
    if(accelZ > (128 - ACCEMG) && accelZ < (128 + ACCEMG))
      accelZ = 128;
    if(accelZ != accel2[2])
    {
      Joystick.setSteering(accelZ);
      accel2[2] = accelZ;
    }
    
    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];
  accelX = nunchuck[2];
  accelY = nunchuck[3];
  accelZ = nunchuck[4];
  buttonZ = 0;
  buttonC = 0;

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

  if(nunchuck[5] & 4) 
    accelX += 1;
  if(nunchuck[5] & 8)
    accelX += 2;

  if(nunchuck[5] & 16) 
    accelY += 1;
  if(nunchuck[5] & 32)
    accelY += 2;

  if(nunchuck[5] & 64) 
    accelZ += 1;
  if(nunchuck[5] & 128)
    accelZ += 2;
}

void joystickInit()
{
  Joystick.begin(true);
  Joystick.setXAxisRange(N1XMIN, N1XMAX);
  Joystick.setYAxisRange(N1YMAX, N1YMIN);
  Joystick.setXAxis(N1XCEN);
  Joystick.setYAxis(N1YCEN);
  Joystick.setRxAxisRange(0, 255);
  Joystick.setRyAxisRange(0, 255);
  Joystick.setRzAxisRange(0, 255);
  Joystick.setRxAxis(128);
  Joystick.setRyAxis(128);
  Joystick.setRzAxis(128);
  Joystick.setThrottleRange(N2XMIN, N2XMAX);
  Joystick.setRudderRange(N2YMAX, N2YMIN);
  Joystick.setThrottle(N2XCEN);
  Joystick.setRudder(N2YCEN);
  Joystick.setAcceleratorRange(0, 255);
  Joystick.setBrakeRange(0, 255);
  Joystick.setSteeringRange(0, 255);
  Joystick.setAccelerator(128);
  Joystick.setBrake(128);
  Joystick.setSteering(128);
}

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();
}