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

//ヌンチャク1キャリブレーション値
#define XMIN 23.0f  // X最小
#define XMAX 223.0f // X最大
#define XCEN 124.0f // Xセンター
#define YMIN 29.0f  // Y最小
#define YMAX 226.0f // Y最大
#define YCEN 128.0f // Yセンター
#define MARGIN 20 // スティックのあそび
#define DECODE(X) ((X ^ 0x17) + 0x17)

#define LED 21
#define MODESWITCH 7 //割り込みに使用
#define RUMBLE 20

uint8_t joyX, joyY, buttonZ, buttonC;
uint8_t nunchuck[6];
int count;

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

uint8_t buttonIN[10] = {10, 9, 8, 6, 5, 4, 16, 14, 15, 7};
uint8_t btstat[9];
static volatile uint8_t mode;
uint8_t st;

void (*pushButton[9])(); // スティック・ボタン処理ルーチン

void setup()
{
  pinMode(RUMBLE, OUTPUT);
  pinMode(LED, OUTPUT);
  for(int i=0; i<10; i++)
    pinMode(buttonIN[i], INPUT_PULLUP);
  
  Wire.begin();
  Wire.beginTransmission(0x52);
  Wire.write((uint8_t)0x40);
  Wire.write((uint8_t)0x00);
  Wire.endTransmission();
  
  //スティック設定
  fixLeftX = 128.0f / (XCEN - XMIN);
  fixRightX = 128.0f / (XMAX - XCEN);
  fixLeftY = 128.0f / (YCEN - YMIN);
  fixRightY = 128.0f / (YMAX - YCEN);
  // ヌンチャクコントローラー初期状態
  for(int i=0; i<2; i++)
  {
    stick[i] = 128;
    button[i] = 1; // オフ
  }

  // デジタルジョイスティック初期状態
  for(int i=0; i<9; i++)
    btstat[i] = 1;

  // ジョイスティック処理ルーチン収納
  pushButton[0] = padUp;
  pushButton[1] = padDown;
  pushButton[2] = padRight;
  pushButton[3] = padLeft;
  pushButton[4] = btnStart;
  pushButton[5] = btnSelect;
  pushButton[6] = btnA;
  pushButton[7] = btnB;
  pushButton[8] = btnC;
  
  XInput.setJoystickRange(0, 255);
  XInput.setAutoSend(false);
  XInput.begin();

  changeMode(); //スイッチ状態取得
  attachInterrupt(digitalPinToInterrupt(MODESWITCH), changeMode, CHANGE);
  
  delay(200);
}

void loop()
{
  // ヌンチャクコントローラー
  count = 0;
  Wire.requestFrom(0x52, 6);
  while(Wire.available())
  {
    nunchuck[count] = DECODE(Wire.read());
    count++;
  }

  if(count >= 5)
  {
    // X軸補正
    joyX = nunchuck[0];
    if(joyX < XCEN)
      joyX = ((float)joyX - XMIN) * fixLeftX;
    else if(joyX > XCEN)
      joyX = ((float)joyX - XCEN) * fixRightX + 128.0f;
    else
      joyX =128;

    // X軸のあそび
    if(joyX > (128 - MARGIN) && joyX < (128 + MARGIN))
      joyX = 128;
    if(joyX != stick[0])
    {
      if(mode)
        XInput.setJoystick(JOY_LEFT, joyX, joyY);
      else
        XInput.setJoystick(JOY_RIGHT, joyX, joyY);
      stick[0] = joyX;
    }

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

    if(nunchuck[5] & 1) 
      buttonZ = 1;
    else
      buttonZ = 0;
    if(buttonZ != button[0])
    {
      if(mode)
        XInput.setButton(TRIGGER_RIGHT, !buttonZ);
      else
        XInput.setButton(BUTTON_RB, !buttonZ);
      button[0] = buttonZ;
    }
    
    if(nunchuck[5] & 2)
      buttonC = 1;
    else
      buttonC = 0;
    if(buttonC != button[1])
    {
      if(mode)
        XInput.setButton(TRIGGER_LEFT, !buttonC);
      else
        XInput.setButton(BUTTON_LB, !buttonC);
      button[1] = buttonC;
    }

    XInput.send();
  }
  sendZero();

  // マイコンソフトジョイスティック
  for(int i=0; i<9; i++)
  {
    st = digitalRead(buttonIN[i]);
    if(btstat[i] != st)
    {
      (*pushButton[i])();
      btstat[i] = st;
      XInput.send();
    }
  }

  // 振動チェック
  if((XInput.getRumbleLeft() != 0) || (XInput.getRumbleRight() != 0))
    digitalWrite(RUMBLE, HIGH); // 振動ON
  else
    digitalWrite(RUMBLE, LOW); // 振動OFF
}

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

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

void padUp() // 上
{
  XInput.setButton(DPAD_UP, !(st & 1));
}

void padDown() // 下
{
  XInput.setButton(DPAD_DOWN, !(st & 1));
}

void padRight() // 右
{
  XInput.setButton(DPAD_RIGHT, !(st & 1));
}

void padLeft() // 左
{
  XInput.setButton(DPAD_LEFT, !(st & 1));
}

void btnStart() // START
{
  XInput.setButton(BUTTON_START, !(st & 1));
}

void btnSelect() // SELECT
{
  XInput.setButton(BUTTON_BACK, !(st & 1));
}

void btnA() // A
{
  XInput.setButton(BUTTON_B, !(st & 1));
}

void btnB() // B
{
  XInput.setButton(BUTTON_A, !(st & 1));
}

void btnC() // C
{
  if(mode)
    XInput.setButton(BUTTON_X, !(st & 1));
  else
    XInput.setButton(BUTTON_Y, !(st & 1));
}

void changeMode()
{
  if(digitalRead(buttonIN[9]) == HIGH)
  {
    digitalWrite(LED, HIGH);
    mode = 1;
  }
  else
  {
    digitalWrite(LED, LOW);
    mode = 0;
  }
}