#include <PS2X_lib.h>  //for v1.6
#include <MsTimer2.h>
//******************************************************************

#define PS2_DAT        13  //14    
#define PS2_CMD        11  //15
#define PS2_SEL        10  //16
#define PS2_CLK        12  //17

//******************************************************************

#define pressures   false
#define rumble      false

PS2X ps2x; // create PS2 Controller Class

int error = 0;
byte type = 0;
byte vibrate = 0;
int SerialSend;
uint8_t MyMode1_Val;
uint8_t MyMode2_Val;
uint8_t MyMode3_Val;
uint8_t MyMode4_Val;
uint8_t MyMode5_Val;

void setup() {

  Serial.begin(57600);//通信速度
  pinMode(5, OUTPUT);//ＬＥＤ用Ｉ／Ｏ
  MyMode1_Val = 0;
  analogWrite(5, 0);//ＬＥＤ明るさを０（消灯）
  delay(300);
  
  error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);

  if (error == 0) {

  }
  else if (error == 1)
    Serial.println("エラー１");
  else if (error == 2)
    Serial.println("エラー２");
  else if (error == 3)
    Serial.println("エラー３");
  type = ps2x.readType();

  switch (type) {
    case 0:
      Serial.print("Unknown Controller type found ");
      break;
    case 1:
      //コントローラーが見つかった
      break;
    case 2:
      Serial.print("GuitarHero Controller found ");
      break;
    case 3:
      Serial.print("Wireless Sony DualShock Controller found ");
      break;
  }
}
void flash() {

  uint8_t KeyPad1;
  uint8_t KeyPad2;
  uint8_t KeyPad3;

  ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed

  //ここでボタンなどの状態を取得して、送信用のデータを作成する。
  KeyPad1 = ps2x.Button(PSB_PAD_UP) + ps2x.Button(PSB_PAD_DOWN) * 2 + ps2x.Button(PSB_PAD_RIGHT) * 4 + ps2x.Button(PSB_PAD_LEFT) * 8 +  ps2x.Button(PSB_L1) * 16 + ps2x.Button(PSB_R1) * 32 + ps2x.Button(PSB_L3) * 64;
  KeyPad2 = ps2x.Button(PSB_CIRCLE) + ps2x.Button(PSB_CROSS) * 2 + ps2x.Button(PSB_SQUARE) * 4 + ps2x.Button(PSB_TRIANGLE) * 8  + ps2x.Button(PSB_L2) * 16 + ps2x.Button(PSB_R2) * 32 + ps2x.Button(PSB_R3) * 64;
  KeyPad3 = ps2x.Button(PSB_START)  + ps2x.Button(PSB_SELECT)  * 2 + MyMode1_Val * 4 + MyMode2_Val * 8 + MyMode3_Val * 16 + MyMode4_Val * 32 + MyMode5_Val * 64;

  uint8_t right_X = constrain(ps2x.Analog(PSS_RX), 0, 254);
  uint8_t right_Y = constrain(ps2x.Analog(PSS_RY), 0, 254);
  uint8_t left_X = constrain(ps2x.Analog(PSS_LX), 0, 254);
  uint8_t left_Y = constrain(ps2x.Analog(PSS_LY), 0, 254);
  uint8_t mode1 = constrain(KeyPad1, 0, 254);
  uint8_t mode2 = constrain(KeyPad2, 0, 254);
  uint8_t mode3 = constrain(KeyPad3, 0, 254);
  
  //ここで実際の送信をしている。
  Serial.write(255); // header
  Serial.write(left_X);//左スティックのＸ
  Serial.write(left_Y);//左スティックのＹ
  Serial.write(right_X);//右スティックのＸ
  Serial.write(right_Y);//右スティックのＹ
  Serial.write(mode1);//十字キー、Ｌ１，Ｒ１，左スティック押し込みスイッチ
  Serial.write(mode2);//十字キー、Ｌ２，Ｒ２，左スティック押し込みスイッチ
  Serial.write(mode3);//スタート、セレクト、拡張用信号（５個）
  SerialSend = 0;
}


void loop() {

  if (error == 1) { //エラーの場合の送信内容
    Serial.write(255); // header
    Serial.write(125);
    Serial.write(125);
    Serial.write(125);
    Serial.write(125);
    Serial.write(254);//６文字でやめる
    delay(100);
    return;
  }
  if (type == 2) { //Guitar Hero Controller

  }  else { //DualShock Controller
    if (SerialSend == 0) {
      SerialSend = 1;
      MsTimer2::set(25, flash);     // 25ms毎にflash( )割込み関数を呼び出す様に設定
      MsTimer2::start();             // タイマー割り込み開始
      if (ps2x.ButtonPressed(PSB_SELECT)) {//セレクトボタン押した場合。
        if (MyMode1_Val == 0) {
          MyMode1_Val = 1;
          analogWrite(5, 50);//LEDON
        } else {
          MyMode1_Val = 0;
          analogWrite(5, 0);//LEDON
        }
      }
    }
  }
}