/*
KRR-ESP release2

RCB-4HVをPS4コントローラで操縦できるようにします。
配線方法はKRR-5FHやKRI-3と同じでRCB-4HVのSIO5〜SIO8に接続できます。

〜制作環境〜
Arduino IDE 2.0.3
Use a ps4 controller with an esp32(PS4Controller.h)
https://github.com/AzSaSiN/PS4-esp32

〜使用方法〜
１．回路図に従って回路を構成してください。D1mini(ESP32)での製作を前提としています。
　　　https://herdruk.web.fc2.com/KRR-ESP32.PNG
２．PS4コントローラのMACアドレスをこの使用するESP32と同じにしてください。
    MACアドレスを調べるスケッチは以下のもの が使用できます。
　　　https://qiita.com/Geek493/items/8402ad875b88822e75ab
３．ESP32にこのスケッチを書き込んでください。
　　ESP32-WROOM-32、ESP32-WROOM-32D、ESP32-WROOM-32Eで動作確認しています。
４．RCB-4HVのSIO5〜SIO8に接続してください。通信速度は自動判定です。
    1.25Mbpsで動作時はD1mini上の青LEDが点灯します。
　　115200kbpsで動作時はD1mini上の青LEDが消灯します。
５．RCB-4HVとESP32が正しく通信できていればESP32とPS4コントローラがペアリングできるようになります。
　　※RCB-4HVと未接続の場合はPS4コントローラとのペアリングができません。

〜注意事項〜
１．動作保証はできません。自己責任でお願いします。
２．ド素人がスケッチ書いてます。可読性は低いですがご了承ください。
３．RCB-3HVには対応していません。
４．PSボタンを押すとコントローラのLEDの色が変わりますが、とくに意味はありません。
*/

#include <PS4Controller.h>  //PS4コントローラをESP32で使うためのライブラリをインクルード
#include "esp_bt_main.h"
#include "esp_bt_device.h"
#include "esp_gap_bt_api.h"
#include "esp_err.h"

//送信用変数の宣言と初期化
byte rx[18] = { 0x3F, 0x7F, 0x00, 0x07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
//受信カウントと通信速度のための変数と宣言
int count;

bool color_change_flag = false;
bool first_connect = false;
int color_flag = -1;

void setup() {
  //PS4コントローラと通信開始
  PS4.begin();

  //正常にPS4コントローラが接続されるようにESP32のペアリング情報をいったん全削除する
  uint8_t pairedDeviceBtAddr[20][6];
  int count_bt = esp_bt_gap_get_bond_device_num();
  esp_bt_gap_get_bond_device_list(&count_bt, pairedDeviceBtAddr);
  for (int i = 0; i < count_bt; i++) {
    esp_bt_gap_remove_bond_device(pairedDeviceBtAddr[i]);
  }

  //ピンの設定
  pinMode(22, INPUT);      //使わないピンをINPUTに設定
  pinMode(32, OUTPUT);     //3ステートバッファのイネーブルピンをOUTPUTに設定（現在はバッファ未使用）
  digitalWrite(32, HIGH);  //3ステートバッファのイネーブルピンをHIGHに設定（現在はバッファ未使用）
  pinMode(25, OUTPUT);     //GNDの代わりとして使うピンをOUTPUTに設定
  digitalWrite(25, LOW);   //GNDの代わりとして使うピンをLOWに設定
  pinMode(27, OUTPUT);     //UART1のTXピンをOUTPUTに設定

  //ハードウェアシリアルの設定をとりあえず1.25Mbpsに設定
  Serial1.begin(1250000, SERIAL_8E1, 26, 27, true, 1, 1);   //通信速度、パリティ設定、受信ピン、送信ピン、信号反転、タイムアウト、バッファ設定
  Serial2.begin(1250000, SERIAL_8E1, 16, 17, false, 1, 1);  //通信速度、パリティ設定、受信ピン、送信ピン、信号反転、タイムアウト、バッファ設定
  Serial2.setTimeout(1);                                    //タイムアウトを1ミリ秒に設定
  pinMode(2, OUTPUT);                                       //D1miniのLEDピンをOUTPUTに設定
  digitalWrite(2, HIGH);                                    //D1miniのLEDピンをHIGHに設定

  //通信速度を判定する処理
  delay(1000);                           //RCB-4HVから信号が出力し始めるのを待つ
  count = 0;                             //カウンタをリセットする
  while (count <= 9000) {                //とりあえず9000バイト読み込む
    count++;                             //1カウント増やす
    if (Serial2.read() == 0xBF) {        //0xBFが読み込めたら
      delayMicroseconds(20);             //受信待ちする
      if (Serial2.read() == 0x7F) {      //0x7Fが読み込めたら
        delayMicroseconds(20);           //受信待ちする
        if (Serial2.read() == 0x00) {    //0x00が読み込めたら
          delayMicroseconds(20);         //受信待ちする
          if (Serial2.read() == 0x07) {  //0x07が読み込めたら
            count = 10000;               //現状の通信速度でRCB-4HVからデータを正しく受信出来たらループを抜ける
          }
        }
      }
    }
  }

  //もし9000バイト読み込んでも正しく受信できなければハードウェアシリアルの設定を115200bpsに設定
  if (count <= 9999) {                                       //もし9000バイト読み込んでも正しくデータが読み込めなかったら
    Serial1.end();                                           //ハードウェアシリアルをいったん終了する
    Serial2.end();                                           //ハードウェアシリアルをいったん終了する
    delay(1000);                                             //フリーズ防止で1秒待つ
    Serial1.begin(115200, SERIAL_8E1, 26, 27, true, 1, 1);   //通信速度、パリティ設定、受信ピン、送信ピン、信号反転、タイムアウト、バッファ設定
    Serial2.begin(115200, SERIAL_8E1, 22, 17, false, 1, 1);  //通信速度、パリティ設定、受信ピン、送信ピン、信号反転、タイムアウト、バッファ設定
    Serial2.setTimeout(1);                                   //タイムアウトを1ミリ秒に設定
    digitalWrite(2, LOW);                                    //D1miniのLEDピンをLOWに設定
  }
}

void loop() {
  if (Serial2.read() == 0xBF) {    //もし0xBFを受信したら
    if (Serial2.read() == 0x7F) {  //もし0x7Fを受信したら
      byte button_data_1 = 0;      //ボタンデータ１格納用変数の初期化
      byte button_data_2 = 0;      //ボタンデータ２格納用変数の初期化
      byte analog_data_1 = 64;     //アナログデータ１格納用変数の初期化
      byte analog_data_2 = 64;     //アナログデータ２格納用変数の初期化
      byte analog_data_3 = 64;     //アナログデータ３格納用変数の初期化
      byte analog_data_4 = 64;     //アナログデータ４格納用変数の初期化
      byte check_sum = 0;          //チェックサム格納用変数の初期化
      if (PS4.isConnected()) {     //PS4コントローラが接続されていたら

        //ボタンデータ１の情報を成形する
        if (PS4.Square()) button_data_1 += 0x01;
        if (PS4.L1()) button_data_1 += 0x02;
        if (PS4.L2()) button_data_1 += 0x04;
        if (PS4.R1()) button_data_1 += 0x08;
        if (PS4.R2()) button_data_1 += 0x10;

        //ボタンデータ２の情報を成形する
        if (PS4.Up() || PS4.UpRight() || PS4.UpLeft() || PS4.Options()) button_data_2 += 0x01;        // L3は上下同時押し判定
        if (PS4.Down() || PS4.DownRight() || PS4.DownLeft() || PS4.Options()) button_data_2 += 0x02;  // L3は上下同時押し判定
        if (PS4.Right() || PS4.UpRight() || PS4.DownRight() || PS4.Share()) button_data_2 += 0x04;    // R3は左右同時押し判定
        if (PS4.Left() || PS4.UpLeft() || PS4.DownLeft() || PS4.Share()) button_data_2 += 0x08;       // R3は左右同時押し判定
        if (PS4.Triangle()) button_data_2 += 0x10;
        if (PS4.Cross()) button_data_2 += 0x20;
        if (PS4.Circle()) button_data_2 += 0x40;

        //アナログデータの情報を成形する
        if (abs(PS4.data.analog.stick.ly) > 9) analog_data_1 = (PS4.data.analog.stick.ly / 2) + 64;  //アナログスティックの０付近は不安定のため９未満は切り捨てる
        if (abs(PS4.data.analog.stick.lx) > 9) analog_data_2 = (PS4.data.analog.stick.lx / 2) + 64;  //アナログスティックの０付近は不安定のため９未満は切り捨てる
        if (abs(PS4.data.analog.stick.ry) > 9) analog_data_3 = (PS4.data.analog.stick.ry / 2) + 64;  //アナログスティックの０付近は不安定のため９未満は切り捨てる
        if (abs(PS4.data.analog.stick.rx) > 9) analog_data_4 = (PS4.data.analog.stick.rx / 2) + 64;  //アナログスティックの０付近は不安定のため９未満は切り捨てる
      }
      //チェックサムの情報を成形する
      check_sum = ((int)0x80 + button_data_1 + button_data_2 + analog_data_1 + analog_data_2 + analog_data_3 + analog_data_4) & 0x7F;

      //送信用の変数に成形する
      rx[4] = button_data_1 >> 4;
      rx[5] = button_data_1 & 0x0F;
      rx[6] = button_data_2 >> 4;
      rx[7] = button_data_2 & 0x0F;
      rx[8] = analog_data_1 >> 4;
      rx[9] = analog_data_1 & 0x0F;
      rx[10] = analog_data_2 >> 4;
      rx[11] = analog_data_2 & 0x0F;
      rx[12] = analog_data_3 >> 4;
      rx[13] = analog_data_3 & 0x0F;
      rx[14] = analog_data_4 >> 4;
      rx[15] = analog_data_4 & 0x0F;
      rx[16] = check_sum >> 4;
      rx[17] = check_sum & 0x0F;

      //送信する
      if (count >= 10000) digitalWrite(32, LOW);   //通信速度が1.25Mbpsなら3ステートバッファを有効にする（一定時間待つ）
      Serial1.write(rx, 18);                       //送信する
      Serial1.flush();                             //送信が完了するまで待つ
      if (count >= 10000) digitalWrite(32, HIGH);  //通信速度が1.25Mbpsなら3ステートバッファを無効にする（一定時間待つ）
    }
  }
  if (PS4.isConnected() && (PS4.PSButton() || first_connect == false)) {
    if (color_change_flag == false) {
      first_connect = true;
      color_change_flag = true;
      color_flag++;
      if (color_flag == 0) PS4.setLed(44, 255, 164);
      else if (color_flag == 1) PS4.setLed(255, 50, 50);
      else if (color_flag == 2) PS4.setLed(30, 30, 255);
      else if (color_flag == 3) PS4.setLed(255, 255, 50);
      else if (color_flag == 4) PS4.setLed(200, 200, 200);
      if (color_flag >= 4) color_flag = -1;
      PS4.sendToController();
    }
  } else {
    color_change_flag = false;
  }
}