/*
RCB-4HV用Bluetooth通信用スケッチ(2023/03/05 release1 by 666)
RCB-4HVとパソコンのCOMポート通信を無線化し、同時にPS4コントローラでも操縦できるようにします。

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

〜使用方法〜
１．PS4コントローラのMACアドレスをこの使用するESP32と同じにしてください。
    【設定０】をtrueにするとMacアドレスを確認できます。MACアドレスを調べるスケッチは以下のものを参照しました。
　　　https://qiita.com/Geek493/items/8402ad875b88822e75ab
２．【設定１】でこのESP32がパソコンに接続される時のデバイス名を設定してください。なんでもいいです。
３．PS4コントローラを使わない方は【設定２】でPS4コントローラとの接続を無効にしてください。
４．ESP32にこのスケッチを書き込んでください。
　　ESP32-WROOM-32、ESP32-WROOM-32D、ESP32-WROOM-32Eで動作確認しています。
５．ESP32のTXとRCB4のRX、ESP32のRXとRCB4のTX、両方のGNDを接続してください。
６．ESP32に電源を供給してください。開発ボードを使用する場合はESP32開発ボードのVccとRCB-4の5v(PIOポートの真ん中)を接続すればOKです。

〜注意事項〜
１．動作保証はできません。自己責任でお願いします。
２．ド素人がスケッチ書いてます。可読性は低いですがご了承ください。

〜オプション：ボタン割り込み機能〜
今発動しているモーションをキャンセルして強制的に次のモーションに行く機能が使えます。
ボタンデータ割り込み機能の有効無効そのものの指定は【設定３】の部分を変更してください。
ボタンデータの割り込みの条件は【設定４】周辺のスケッチを変更してください。

*/

#include <BluetoothSerial.h>
#include <PS4Controller.h>

//【設定０】MACアドレスを調べる関数を呼び出す。使用後はfalseにすること。
#define MAC_ADDRESS_CHECK_EN false
//【設定１】ESP32がパソコンに接続される時のデバイス名
#define ESP_DEVICE_NAME "ESP32-RCB_CONNECT"
//【設定２】PS4コントローラを使う（使う：true，使わない：false)
#define PS4_CONTROLLER_CONNECT true
//【設定３】ボタン割り込みの有効無効を制御するピンの指定（GPIOのピン番号を指定(GPIO21を指定するなら21にする。1で割り込み常に有効。0で割り込み常に無効。）
#define BUTTON_INTERRUPT_EN_PIN 21

//Bluetoothシリアル通信のインスタンス宣言
BluetoothSerial SerialBT;
//コントローラ信号送信周期管理変数
unsigned long controller_count = 0;
//ラストボタンデータ格納配列の宣言と初期化
byte last_button_data[13] = { 0x0D, 0x00, 0x02, 0x50, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x62 };
//RCB4モーション停止データ格納配列の宣言と初期化
byte rcb_flag_reset_data[17] = { 0x11, 0x00, 0x02, 0x02, 0x00, 0x00, 0x4B, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64 };

void setup() {
  //MACアドレスチェックを行う関数を呼び出す
  if (MAC_ADDRESS_CHECK_EN == true) mac_address_check();

  //ハードウェアシリアルの開始とバッファの設定
  Serial.begin(115200, SERIAL_8E1, 3, 1, true);  //（通信速度、偶数パリティ、送信ピン、受信ピン、信号反転）
  Serial.setTxBufferSize(4096);
  Serial.setRxBufferSize(4096);

  //PS4コントローラと通信を開始
  if (PS4_CONTROLLER_CONNECT == true) PS4.begin();

  //Bluetoothシリアルでパソコンと通信を開始
  SerialBT.begin(ESP_DEVICE_NAME);

  //ボタン割り込み機能を判定するピンを設定
  if (BUTTON_INTERRUPT_EN_PIN >= 2) pinMode(BUTTON_INTERRUPT_EN_PIN, INPUT_PULLDOWN);
}

void loop() {
  //もしパソコンからシリアル通信のデータが届いていたら
  if (SerialBT.available()) {
    while (SerialBT.available() >= 1) Serial.write(SerialBT.read());  //届いているデータすべてをRCB4にそのまま送信
  }
  //もしRCB4からシリアル通信のデータが届いていたら
  if (Serial.available()) {
    while (Serial.available() >= 1) SerialBT.write(Serial.read());  //届いているデータすべてをパソコンにそのまま送信
  }
  //もし最後のコントローラ情報更新から一定時間以上たっていたら
  if (millis() - controller_count >= 50 && PS4_CONTROLLER_CONNECT == true) {
    button_data_send();           //ボタンデータを管理する関数を呼び出し
    controller_count = millis();  //最後にボタンデータを送った時間を記録
  }
}

void button_data_send() {
  //PS4コントローラ接続フラグの宣言と初期化
  bool ps4_connect = false;
  //RCB4ボタンデータ格納変数の宣言と初期化
  byte button_data[13] = { 0x0D, 0x00, 0x02, 0x50, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x62 };

  //もしPS4コントローラが接続状態になっていたら
  if (PS4.isConnected()) {
    //PS4コントローラ接続フラグをTrueにする
    ps4_connect = true;

    //PS4のボタンデータからRCB4のボタンデータを作成
    if (PS4.data.button.square) button_data[6] += 0x01;
    if (PS4.data.button.l1) button_data[6] += 0x02;
    if (PS4.data.button.l2) button_data[6] += 0x04;
    if (PS4.data.button.r1) button_data[6] += 0x08;
    if (PS4.data.button.r2) button_data[6] += 0x10;

    if (PS4.data.button.up || PS4.data.button.l3) button_data[7] += 0x01;     // L3は上下同時押し判定
    if (PS4.data.button.down || PS4.data.button.l3) button_data[7] += 0x02;   // L3は上下同時押し判定
    if (PS4.data.button.right || PS4.data.button.r3) button_data[7] += 0x04;  // R3は左右同時押し判定
    if (PS4.data.button.left || PS4.data.button.r3) button_data[7] += 0x08;   // R3は左右同時押し判定
    if (PS4.data.button.triangle) button_data[7] += 0x10;
    if (PS4.data.button.cross) button_data[7] += 0x20;
    if (PS4.data.button.circle) button_data[7] += 0x40;

    if (abs(PS4.data.analog.stick.ly) > 9) button_data[8] = PS4.data.analog.stick.ly / 2;   //アナログスティックの０付近は不安定のため９未満は切り捨てる
    if (abs(PS4.data.analog.stick.lx) > 9) button_data[9] = PS4.data.analog.stick.lx / 2;   //アナログスティックの０付近は不安定のため９未満は切り捨てる
    if (abs(PS4.data.analog.stick.ry) > 9) button_data[10] = PS4.data.analog.stick.ry / 2;  //アナログスティックの０付近は不安定のため９未満は切り捨てる
    if (abs(PS4.data.analog.stick.rx) > 9) button_data[11] = PS4.data.analog.stick.rx / 2;  //アナログスティックの０付近は不安定のため９未満は切り捨てる

    bool change_flag = false;  //ボタンデータ更新フラグを初期化

    //今組み立てたボタンデータが1回前に組み立てたボタンデータ（ラストボタンデータ）と異なる場合は更新フラグをTrueにする
    if (button_data[6] != last_button_data[6]) change_flag = true;
    if (button_data[7] != last_button_data[7]) change_flag = true;
    if (button_data[8] != last_button_data[8]) change_flag = true;
    if (button_data[9] != last_button_data[9]) change_flag = true;
    if (button_data[10] != last_button_data[10]) change_flag = true;
    if (button_data[11] != last_button_data[11]) change_flag = true;

    //更新フラグがTrueならRCB4にデータを送信する
    if (change_flag == true) {

      //チェックサムの計算
      int sum = 0;  //チェックサム計算用変数の宣言と初期化
      for (byte i = 0; i < 12; i++) {
        sum += (int)button_data[i];            //チェックサム計算用に送信するデータの総和を求める
        last_button_data[i] = button_data[i];  //ラストボタンデータを今のボタンデータに更新
      }
      button_data[12] = sum & 0b11111111;  //チェックサムを配列の末尾に格納

      //割り込みを行うかどうかのチェック
      bool button_interrupt_en = false;  //割り込み許可用フラグ用の変数の宣言と初期化
      if (BUTTON_INTERRUPT_EN_PIN == 0) button_interrupt_en = false;
      else if (BUTTON_INTERRUPT_EN_PIN == 1) button_interrupt_en = true;
      else button_interrupt_en = digitalRead(BUTTON_INTERRUPT_EN_PIN);

      if (button_interrupt_en == true) {
        //割り込みの処理
        bool button_interrupt = false;                                    //割り込みフラグ用の変数の宣言と初期化
        if ((button_data[7] & 0b01111100) >= 1) button_interrupt = true;  //【設定３】条件を満たすと割り込みフラグをオンにする
        if ((button_data[6] & 0b00001011) >= 1) button_interrupt = true;  //【設定３】条件を満たすと割り込みフラグをオンにする
        if (button_interrupt == true) {                                   //割り込みフラグがTrueなら
          Serial.write(rcb_flag_reset_data, 17);                          //モーション停止をRCB4に送信
          while (Serial.available() >= 1) Serial.read();                  //RCB4から帰ってきたデータが帰ってきていたらデータを読み込む（実際はいらないので読み込むことでバッファをクリアしている）
          delay(25);                                                      //ボタンデータを送信するために少し待つ
        }
      }

      //RCB4へボタンデータを送信する
      Serial.write(button_data, 13);                  //RCB4へボタンデータを送信する
      while (Serial.available() >= 1) Serial.read();  //RCB4から帰ってきたデータが帰ってきていたらデータを読み込む（実際はいらないので読み込むことでバッファをクリアしている）
    }
  } else {
    //PS4コントローラが接続されていなかったら
    if (ps4_connect == true) {                        //直前までコントローラが接続されていたなら
      Serial.write(button_data, 13);                  //初期状態のボタンデータを送信
      while (Serial.available() >= 1) Serial.read();  //RCB4から帰ってきたデータが帰ってきていたらデータを読み込む（実際はいらないので読み込むことでバッファをクリアしている）
    }
    ps4_connect = false;  //PS4コントローラ接続フラグをfalseにする
  }
}

void mac_address_check() {
  Serial.begin(115200);
  uint8_t bt_mac[6];
  esp_read_mac(bt_mac, ESP_MAC_BT);
  Serial.printf("Bluetooth Mac Address => %02X:%02X:%02X:%02X:%02X:%02X\r\n", bt_mac[0], bt_mac[1], bt_mac[2], bt_mac[3], bt_mac[4], bt_mac[5]);
  while (1) {};
}