#include <XInput.h>

#define WAIT 4

#define DATA 14 //1pin
#define CMD  16 //2pin
#define ATT  10 //6pin
#define CLK  15 //7pin

// 調整はお好みで
#define ANMIN 0x68 // ステイックのあそび最小値
#define ANMAX 0x98 // ステイックのあそび最大値

uint8_t padStat[6];
uint8_t padOldStat[6];
uint8_t header[3];
uint8_t rumble[6]; // 振動ON/OFF [0][1]以外は常に0

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

  for(int i=0; i<6; i++)
    padOldStat[i] = 0xff;
  for(int i=0; i<5; i++)
    rumble[i] = 0; // 振動OFF

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

  delay(200);
}

void loop()
{
  int len;

  if((XInput.getRumbleRight() != 0) || (XInput.getRumbleLeft() != 0))
  {
    // 振動ON
    rumble[0] = 0x40;
    rumble[1] = 1;
  }
  else
  {
    // 振動OFF
    rumble[0] = 0;
    rumble[1] = 0;
  }

  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(rumble[i]);

  digitalWrite(ATT, HIGH);

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

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);
    digitalWrite(CLK, HIGH);
    data |= digitalRead(DATA) << i;
    delayMicroseconds(WAIT);
  }
  return data;
}

void digitalPad()
{
  uint8_t bt;

  bt = padStat[0];
  if(padOldStat[0] != bt)
  {
    XInput.setButton(DPAD_LEFT, !((bt >> 7) & 1));
    XInput.setButton(DPAD_DOWN, !((bt >> 6) & 1));
    XInput.setButton(DPAD_RIGHT, !((bt >> 5) & 1));
    XInput.setButton(DPAD_UP, !((bt >> 4) & 1));
    XInput.setButton(BUTTON_START, !((bt >> 3) & 1));
    XInput.setButton(BUTTON_BACK, !(bt & 1));
    padOldStat[0] = bt;
  }

  bt = padStat[1];
  if(padOldStat[1] != bt)
  {
    XInput.setButton(BUTTON_X, !((bt >> 7) & 1)); // □
    XInput.setButton(BUTTON_A, !((bt >> 6) & 1)); // ×
    XInput.setButton(BUTTON_B, !((bt >> 5) & 1)); // ○
    XInput.setButton(BUTTON_Y, !((bt >> 4) & 1)); // △ 
    XInput.setButton(BUTTON_RB, !((bt >> 3) & 1)); // R1
    XInput.setButton(BUTTON_LB, !((bt >> 2) & 1)); // L1
    XInput.setButton(BUTTON_R3, !((bt >> 1) & 1)); // R2
    XInput.setButton(BUTTON_L3, !(bt & 1)); // L2
    padOldStat[1] = bt;
  }
}

void analogPad()
{
  static int rx = 0x80, ry = 0x80, lx = 0x80, ly = 0x80;
  uint8_t bt;

  bt = padStat[0];
  if(padOldStat[0] != bt)
  {
    XInput.setButton(DPAD_LEFT, !((bt >> 7) & 1));
    XInput.setButton(DPAD_DOWN, !((bt >> 6) & 1));
    XInput.setButton(DPAD_RIGHT, !((bt >> 5) & 1));
    XInput.setButton(DPAD_UP, !((bt >> 4) & 1));
    XInput.setButton(BUTTON_START, !((bt >> 3) & 1));
    XInput.setButton(BUTTON_R3, !((bt >> 2) & 1)); // LSW
    XInput.setButton(BUTTON_L3, !((bt >> 1) & 1)); // RSW
    XInput.setButton(BUTTON_BACK, !(bt & 1));
    padOldStat[0] = bt;
  }

  bt = padStat[1];
  if(padOldStat[1] != bt)
  {
    XInput.setButton(BUTTON_X, !((bt >> 7) & 1)); // □
    XInput.setButton(BUTTON_A, !((bt >> 6) & 1)); // ×
    XInput.setButton(BUTTON_B, !((bt >> 5) & 1)); // ○
    XInput.setButton(BUTTON_Y, !((bt >> 4) & 1)); // △ 
    XInput.setButton(BUTTON_RB, !((bt >> 3) & 1)); // R1
    XInput.setButton(BUTTON_LB, !((bt >> 2) & 1)); // L1
    XInput.setButton(TRIGGER_RIGHT, !((bt >> 1) & 1)); // R2
    XInput.setButton(TRIGGER_LEFT, !(bt & 1)); // L2
    padOldStat[1] = bt;
  }

  bt = padStat[2];
  if(padOldStat[2] != bt)
  {
    if((bt > ANMIN) && (bt < ANMAX))
      rx = 0x80;
    else
      rx = bt;
    XInput.setJoystick(JOY_RIGHT, rx, ry);
    padOldStat[2] = bt;
  }

  bt = padStat[3];
  if(padOldStat[3] != bt)
  {
    if((bt > ANMIN) && (bt < ANMAX))
      ry = 0x80;
    else
      ry = 255 - bt;
    XInput.setJoystick(JOY_RIGHT, rx, ry);
    padOldStat[3] = bt;
  }

  bt = padStat[4];
  if(padOldStat[4] != bt)
  {
    if((bt > ANMIN) && (bt < ANMAX))
      lx = 0x80;
    else
      lx = bt;
    XInput.setJoystick(JOY_LEFT, lx, ly);
    padOldStat[4] = bt;
  }

  bt = padStat[5];
  if(padOldStat[5] != bt)
  {
    if((bt > ANMIN) && (bt < ANMAX))
      ly = 0x80;
    else
      ly = 255 - bt;
    XInput.setJoystick(JOY_LEFT, lx, ly);
    padOldStat[5] = bt;
  }
}

void negcon()
{
  static int rx = 0x80, ry = 0, lx = 0, ly = 0;
  uint8_t bt;

  bt = padStat[0];
  if(padOldStat[0] != bt)
  {
    XInput.setButton(DPAD_LEFT, !((bt >> 7) & 1));
    XInput.setButton(DPAD_DOWN, !((bt >> 6) & 1));
    XInput.setButton(DPAD_RIGHT, !((bt >> 5) & 1));
    XInput.setButton(DPAD_UP, !((bt >> 4) & 1));
    XInput.setButton(BUTTON_START, !((bt >> 3) & 1));
    padOldStat[0] = bt;
  }

  bt = padStat[1];
  if(padOldStat[1] != bt)
  {
    XInput.setButton(BUTTON_B, !((bt >> 5) & 1)); // ○
    XInput.setButton(BUTTON_Y, !((bt >> 4) & 1)); // △ 
    XInput.setButton(BUTTON_RB, !((bt >> 3) & 1)); // R1
    padOldStat[1] = bt;
  }

  bt = padStat[2];
  if(padOldStat[2] != bt)
  {
    if((bt > ANMIN) && (bt < ANMAX))
      rx = 0x80;
    else
      rx = bt;
    XInput.setJoystick(JOY_LEFT, rx, 0x80);
    padOldStat[2] = bt;
  }

  bt = padStat[3];
  if(padOldStat[3] != bt)
  {
    if(bt < 0x20)
       XInput.setButton(BUTTON_A, false);
    else
       XInput.setButton(BUTTON_A, true);
    padOldStat[3] = bt;
  }

  bt = padStat[4];
  if(padOldStat[4] != bt)
  {
    if(bt < 0x20)
       XInput.setButton(BUTTON_X, false);
    else
       XInput.setButton(BUTTON_X, true);
    padOldStat[4] = bt;
  }

  bt = padStat[5];
  if(padOldStat[5] != bt)
  {
    if(bt < 0x20)
       XInput.setButton(BUTTON_LB, false);
    else
       XInput.setButton(BUTTON_LB, true);
    padOldStat[5] = bt;
  }
}