#include <Wire.h>
#include <XInput.h>

//ヌンチャク1キャリブレーション値
#define N1XMIN 22.0f  // X最小
#define N1XMAX 219.0f // X最大
#define N1XCEN 120.0f // Xセンター

#define N1YMIN 38.0f  // Y最小
#define N1YMAX 236.0f // Y最大
#define N1YCEN 135.0f // Yセンター

//ヌンチャク2キャリブレーション値
#define N2XMIN 24.0f  // X最小
#define N2XMAX 220.0f // X最大
#define N2XCEN 124.0f // Xセンター

#define N2YMIN 30.0f  // Y最小
#define N2YMAX 224.0f // Y最大
#define N2YCEN 127.0f // Yセンター

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

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

#define LED 21
#define SW  7

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

uint8_t joyX, joyY, buttonZ, buttonC;

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

// スティック補正値
float fixLeftX[2], fixRightX[2], fixLeftY[2], fixRightY[2];
uint8_t stick[4]; // スティックの状態
uint8_t button[4]; // ボタンの状態

static volatile bool mode = false;// ボタン切り替え

void setup()
{
  pinMode(LED, OUTPUT);
  pinMode(SW, INPUT); // 10KΩでPULLUP済
   
  //スティック設定
  fixLeftX[0] = 128.0f / (N1XCEN - N1XMIN);
  fixRightX[0] = 128.0f / (N1XMAX - N1XCEN);
  fixLeftY[0] = 128.0f / (N1YCEN - N1YMIN);
  fixRightY[0] = 128.0f / (N1YMAX - N1YCEN);
  fixLeftX[1] = 128.0f / (N2XCEN - N2XMIN);
  fixRightX[1] = 128.0f / (N2XMAX - N2XCEN);
  fixLeftY[1] = 128.0f / (N2YCEN - N2YMIN);
  fixRightY[1] = 128.0f / (N2YMAX - N2YCEN);

  // スティック初期状態
  for(int i=0; i<4; i++)
    stick[i] = 128;
  
  // ボタン初期状態
  for(int i=0; i<4; i++)
    button[i] = 1; // オフ

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

  digitalWrite(LED, LOW);

  attachInterrupt(digitalPinToInterrupt(SW), changeMode, FALLING);

  XInput.setJoystickRange(0, 255);
  XInput.setAutoSend(false);
  XInput.begin();

  delay(200);
}


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)
    {
      joyX = ((float)joyX - N1XMIN) * fixLeftX[0];
    }
    else if(joyX > N1XCEN)
    {
      joyX = ((float)joyX - N1XCEN) * fixRightX[0] + 128.0f;
    }
    else
      joyX =128;
    // X軸のあそび
    if(joyX > (128 - MARGIN) && joyX < (128 + MARGIN))
      joyX = 128;
    if(joyX != stick[0])
    {
      XInput.setJoystick(JOY_LEFT, joyX, joyY);
      stick[0] = joyX;
    }

    // Y軸補正
    if(joyY < N1YCEN)
    {
      joyY = ((float)joyY - N1YMIN) * fixLeftY[0];
    }
    else if(joyY > N1YCEN)
    {
      joyY = ((float)joyY - N1YCEN) * fixRightY[0] + 128.0f;
    }
    else
      joyY =128;
    // Y軸のあそび
    if(joyY > (128 - MARGIN) && joyY < (128 + MARGIN))
      joyY = 128;
    if(joyY != stick[1])
    {
      XInput.setJoystick(JOY_LEFT, joyX, joyY);
      stick[1] = joyY;
    }

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

    if(buttonZ != button[1])
    {
      XInput.setButton(BUTTON_B, !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)
    {
      joyX = ((float)joyX - N2XMIN) * fixLeftX[1];
    }
    else if(joyX > N2XCEN)
    {
      joyX = ((float)joyX - N2XCEN) * fixRightX[1] + 128.0f;
    }
    else
      joyX =128;
    // X軸のあそび
    if(joyX > (128 - MARGIN) && joyX < (128 + MARGIN))
      joyX = 128;
    if(joyX != stick[2])
    {
      if(mode)
        XInput.setJoystick(JOY_RIGHT, joyX, joyY);
      else
      {
        if(joyX < 128)
        {
          XInput.setButton(BUTTON_LB, true);
          XInput.setButton(BUTTON_RB, false);
        }
        else if(joyX > 128)
        {
          XInput.setButton(BUTTON_LB, false);
          XInput.setButton(BUTTON_RB, true);
        }
        else
        {
          XInput.setButton(BUTTON_LB, false);
          XInput.setButton(BUTTON_RB, false);
        }
      }
      stick[2] = joyX;
    }

    // Y軸補正
    if(joyY < N2YCEN)
    {
      joyY = ((float)joyY - N2YMIN) * fixLeftY[1];
    }
    else if(joyY > N2YCEN)
    {
      joyY = ((float)joyY - N2YCEN) * fixRightY[1] + 128.0f;
    }
    else
      joyY =128;
    // Y軸のあそび
    if(joyY > (128 - MARGIN) && joyY < (128 + MARGIN))
      joyY = 128;
    if(joyY != stick[3])
    {
      if(mode)
        XInput.setJoystick(JOY_RIGHT, joyX, joyY);
      else
      {
        if(joyY < 128)
        {
          XInput.setButton(TRIGGER_RIGHT, true);
          XInput.setButton(TRIGGER_LEFT, false);
        }
        else if(joyY > 128)
        {
          XInput.setButton(TRIGGER_RIGHT, false);
          XInput.setButton(TRIGGER_LEFT, true);
        }
        else
        {
          XInput.setButton(TRIGGER_RIGHT, false);
          XInput.setButton(TRIGGER_LEFT, false);
        }
      }
      stick[3] = joyY;
    }

    if(buttonC != button[2])
    {
      if(mode)
        XInput.setButton(BUTTON_BACK, !buttonC);
      else
        XInput.setButton(BUTTON_Y, !buttonC);
      button[2] = buttonC;
    }

    if(buttonZ != button[3])
    {
      if(mode)
        XInput.setButton(BUTTON_START, !buttonZ);
      else
        XInput.setButton(BUTTON_X, !buttonZ);
      button[3] = buttonZ;
    }
  }

  XInput.send();
  
  changeNunchuck();
  sendZero();
  delay(10);
}

// ボタン切り替え
void changeMode()
{
  mode = !mode;
  digitalWrite(LED, mode);
  delay(500);
}

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