//3DP_Sisaku_H_SPI_Slave
//Arduino nano ATmega328p

//これはMEGA SUMO Robot 3DP Sisaku Hのスレーブ側Arduino nanoボードのプログラムです。 // This is the program for the slave-side Arduino Nano board of the MEGA SUMO Robot 3DP Sisaku H.
//マスター側Arduino nanoボードとSPI通信で情報をやり取りしながら動作します。 // It operates while exchanging information via SPI communication with the master-side Arduino Nano board.
//読み取ったセンサー情報をSPI通信でマスター側に送信する機能と、SPI通信で受け取ったマスター側からの指令により、ブザーを鳴らす機能があります。 // It has functions to send read sensor information to the master via SPI communication and to sound the buzzer based on commands received from the master via SPI.

#include <SPI.h>
#include <PinChangeInterrupt.h> // ピンチェンジ割り込みライブラリをインクルード // Include PinChangeInterrupt library
#include <Arduino.h>  // min/maxは標準で使える // min/max are available by default

  //volatile bool debugMode = true; // デバッグフラグ （true　＞＞＞　シリアルON） // Debug flag (true >>> Serial ON)
  volatile bool debugMode = false; // デバッグフラグ （false　＞＞＞　シリアルOFF） // Debug flag (false >>> Serial OFF)

  //volatile bool Test_F2 = true; // checksum test用フラグ　スレーブ＞＞＞マスター // Checksum test flag Slave >>> Master
  volatile bool Test_F2 = false; // checksum test用フラグ　スレーブ＞＞＞マスター // Checksum test flag Slave >>> Master

// マスターから受信するデータ  // Data received from master
  struct SentData {
    byte header1_;  //receivedBytes[0]
    byte CC1_;      //receivedBytes[1] receivedData.CC1_
    byte CC2_;      //receivedBytes[2] receivedData.CC2_
    byte checksum_; //receivedBytes[3] receivedData.checksum_
    byte dummy_;    //receivedBytes[4]
    byte footer1_;  //receivedBytes[5]
    byte footer2_;  //receivedBytes[6]
  };

  SentData receivedData; // 受信データ  // Received data

// スレーブから返すデータ  // Data to send to master
  struct DataToSend {
    byte Sensor_Status;   //dataToSend.Sensor_Status
    byte SW_Status_Val;   //dataToSend.SW_Status_Val
    byte ToF_C_S;         //dataToSend.ToF_C_S
    byte PSD_L_S;         //dataToSend.PSD_L_S
    byte PSD_R_S;         //dataToSend.PSD_R_S
    byte checksum_slave;  //dataToSend.Sensor_Status ^ dataToSend.SW_Status_Val ^ dataToSend.ToF_C_S ^ dataToSend.PSD_L_S ^ dataToSend.PSD_R_S;
    byte footer1; // 0xEE
  };

  DataToSend dataToSend; // 返すデータ // Data to send

// 受信バッファとグローバルデータ // Receive buffer and global data
  volatile byte receivedBytes[7]; 
  volatile int byteIndex = 0;
  volatile bool dataReceived = false;
  volatile bool checksum_M_OK = false; //マスターから受信した値のチェックサム確認結果 // Checksum verification result of value received from master
  volatile bool checksum_M_OK_Record = true; //マスターから受信した値のチェックサム確認結果レコード　シリアル通信用 // Record of checksum verification result received from master (for serial communication)
  volatile byte checksum_check_A_Record = 0;//シリアル通信用データ保存 // Data storage for serial communication
  volatile byte checksum_check_B_Record = 0;//シリアル通信用データ保存 // Data storage for serial communication
  volatile unsigned long receiveDuration = 0; // 受信時間 // Reception duration
  volatile unsigned long startTime = 0; // 受信開始時間 // Reception start time
  byte CC1_debug = 0; //シリアルコマンドで使用するCC1_の入力用変数 // Variable for CC1_ input used in serial commands
  bool has_Serial_Command_ON = false; //シリアルコマンドの入力があればtrueになる // Becomes true if there is serial command input

// 移動平均用の定義 // Definitions for moving average
  #define TOF_AVG_COUNT 6  // 平均を取る回数（変更可能） // Number of times to average (changeable)
  volatile int ToF_C_Val_Buffer[TOF_AVG_COUNT];  // ToF_C_Valの過去データを保存 // Store past data of ToF_C_Val
  volatile uint8_t ToF_C_Val_Index = 0;          // 配列の書き込みインデックス // Array write index
  volatile uint8_t ToF_C_Val_Count = 0;          // 有効なデータ数（最大TOF_AVG_COUNT） // Number of valid data (max TOF_AVG_COUNT)

//PIN No
  #define PIN_RX 0         //D0 Serial
  #define PIN_TX 1         //D1 Serial
  #define BUTTON_OB 2      //D2 Digital_IN INPUT_PULLUP
  #define Opt_L75 3        //D3 Digital_IN
  #define Opt_L55 4        //D4 Digital_IN
  #define Opt_L25 5        //D5 Digital_IN
  #define Opt_R25 6        //D6 Digital_IN
  #define Opt_R55 7        //D7 Digital_IN
  #define BUZZER 8         //D8 Tone
  #define Opt_R75 9        //D9 Digital_IN
  #define SPI_SS 10        //D10 SPI
  #define SPI_MOSI 11      //D11 SPI
  #define SPI_MISO 12      //D12 SPI
  #define SPI_SCK 13       //D13 SPI
  #define DIP_SW1 14       //A0(14) Digital_IN INPUT_PULLUP
  #define DIP_SW2 15       //A1(15) Digital_IN INPUT_PULLUP
  #define DIP_SW3 16       //A2(16) Digital_IN INPUT_PULLUP
  #define DIP_SW4 17       //A3(17) Digital_IN INPUT_PULLUP
  #define NC_1 18          //A4(18) NC
  #define ToF_C 19         //A5(19) Digital_IN
  #define PSD_L 20         //A6(20) Analog_IN
  #define PSD_R 21         //A7(21) Analog_IN

//Sensor threshold
//  #define ToF_C_threshold 30   //ToF_Cの閾値(30-230)V*100(無効にする　マスター側で判定) // ToF_C threshold (30-230)V*100 (disabled, judged on master side)
//  #define PSD_L_threshold 30   //PSD_Lの閾値(30-230)V*100(無効にする　マスター側で判定) // PSD_L threshold (30-230)V*100 (disabled, judged on master side)
//  #define PSD_R_threshold 30   //PSD_Rの閾値(30-230)V*100(無効にする　マスター側で判定) // PSD_R threshold (30-230)V*100 (disabled, judged on master side)

//BIN bit
  #define Opt_L75_B 7     //b10000000
  #define Opt_L55_B 6     //b01000000
  #define Opt_L25_B 5     //b00100000
  #define PSD_L10_B 4     //b00010000
  #define PSD_R10_B 3     //b00001000
  #define Opt_R25_B 2     //b00000100
  #define Opt_R55_B 1     //b00000010
  #define Opt_R75_B 0     //b00000001
//DIP SW 
  #define DIP_SW1_B 0     //b00000001
  #define DIP_SW2_B 1     //b00000010
  #define DIP_SW3_B 2     //b00000100
  #define DIP_SW4_B 3     //b00001000
  #define BUTTON_OB_B 6   //b01000000
  #define ToF_C_B 7       //b10000000

//グローバル変数 // Global variables
  volatile int ToF_C_Val = 0;  //ToF_C Val (0-2000) 時間データ(micro sec) // ToF_C value (0-2000) time data (micro sec)
  volatile int PSD_L_Val = 0;  //PSD_L Val (61-470) 電圧データ(V) // PSD_L value (61-470) voltage data (V)
  volatile int PSD_R_Val = 0;  //PSD_R Val; (61-470) 電圧データ(V) // PSD_R value (61-470) voltage data (V)
  volatile int ToF_C_Distance_O = 0;  //ToF_CのDistance (t - 1000us) * 2mm // ToF_C distance (t - 1000us) * 2mm
  volatile int PSD_L_Distance_O = 0;  //PSD_LのDistance V*100 // PSD_L distance V*100
  volatile int PSD_R_Distance_O = 0;  //PSD_R のDistance V*100 // PSD_R distance V*100
  volatile byte SW_Status_Val_O = 0;  //DIP_SW_Val & ToF_Front // DIP_SW value & ToF front
  volatile bool ToF_C_Pulse_H_L = true;  //ToF_Cのパルス立ち上がり、立下りの判定に使用（true:立ち上がり待ち、false:立ち下がり待ち） // Used for detecting rising/falling edge of ToF_C pulse (true: waiting for rising, false: waiting for falling)
  volatile unsigned long ToF_C_Pulse_last_timeA = 0; //ToF_Cのパルス立ち上がり時のmicros()の値を保存 // Save micros() value at rising edge of ToF_C pulse
  volatile unsigned long ToF_C_Pulse_last_timeB = 0; //ToF_Cのパルス立下り時のmicros()の値を保存 // Save micros() value at falling edge of ToF_C pulse

  volatile byte Opt_Val = 0;            //センサー値保管用 // For storing sensor values
  volatile byte Opt_Val_1 = 0;          //センサー値保管用 // For storing sensor values
  volatile byte Opt_Val_2 = 0;          //センサー値保管用 // For storing sensor values
  volatile byte Opt_Val_3 = 0;          //センサー値保管用 // For storing sensor values
  volatile byte Opt_Val_4 = 0;          //センサー値保管用 // For storing sensor values
  volatile byte Opt_Val_5 = 0;          //センサー値保管用 // For storing sensor values
  volatile byte Opt_Val_6 = 0;          //センサー値保管用 // For storing sensor values
  volatile byte Opt_Val_7 = 0;          //センサー値保管用 // For storing sensor values
  volatile byte Opt_Val_8 = 0;          //センサー値保管用 // For storing sensor values
  volatile byte Opt_Val_9 = 0;          //センサー値保管用 // For storing sensor values
  volatile byte Opt_Val_10 = 0;         //センサー値保管用 // For storing sensor values
  volatile byte Opt_Val_X = 0;          //センサー値 動作判定用(10ms分のON情報保持あり) & PSD_R10 & PSD_L10 // Sensor value for operation judgment (with 10ms ON information hold) & PSD_R10 & PSD_L10
  volatile bool Opt_Val_Hold = false;   //センサー値 10自体分のON情報保持の切り替えフラグ // Flag to switch 10ms ON information hold for sensor values

  volatile byte Opt_R25_Val = 0;        //HIGH or LOW
  volatile byte Opt_R55_Val = 0;        //HIGH or LOW
  volatile byte Opt_R75_Val = 0;        //HIGH or LOW
  volatile byte Opt_L25_Val = 0;        //HIGH or LOW
  volatile byte Opt_L55_Val = 0;        //HIGH or LOW
  volatile byte Opt_L75_Val = 0;        //HIGH or LOW

//Sensor_Status_O
  volatile byte Sensor_Status_O = 0;  //
  //bit7:Opt_R75
  //bit6:Opt_R55
  //bit5:Opt_R25
  //bit4:PSD_R10 <<< 一旦やめにする（マスター側で判定） // bit4:PSD_R10 <<< Temporarily disabled (judged on master side)
  //bit3:PSD_L10 <<< 一旦やめにする（マスター側で判定） // bit3:PSD_L10 <<< Temporarily disabled (judged on master side)
  //bit2:Opt_L25
  //bit1:Opt_L55
  //bit0:Opt_L75
//DIP_SW_Status
  volatile int DIP_SW1_Val = 0;  //
  volatile int DIP_SW2_Val = 0;  //
  volatile int DIP_SW3_Val = 0;  //
  volatile int DIP_SW4_Val = 0;  //
  volatile int DIP_SW_Val = 0;  //
  //bit7:ToF_C <<< 一旦やめにする（マスター側で判定） // bit7:ToF_C <<< Temporarily disabled (judged on master side)
  //bit6:none
  //bit5:none
  //bit4:none
  //bit3:DIP_SW4
  //bit2:DIP_SW3
  //bit1:DIP_SW2
  //bit0:DIP_SW1

//Tone frequency
  #define Tone_C4 262    //
  #define Tone_D4 294    //
  #define Tone_E4 330    //
  #define Tone_F4 349    //
  #define Tone_G4 392    //
  #define Tone_A4 440    //
  #define Tone_B4 494    //
  #define Tone_C5 523    //
  #define Tone_D5 587    //
  #define Tone_E5 659    //
  #define Tone_F5 698    //
  #define Tone_G5 784    //
  #define Tone_A5 880    //
  #define Tone_B5 988    //
  #define Tone_C6 1047    //
  #define Tone_D6 1175    //
  #define Tone_E6 1319    //
  #define Tone_F6 1397    //
  #define Tone_G6 1568    //
  #define Tone_A6 1760    //
  #define Tone_B6 1976    //
  #define Tone_C7 2093    //
  #define Tone_D7 2349    //
  #define Tone_E7 2637    //
  #define Tone_F7 2794    //
  #define Tone_G7 3136    //
  #define Tone_A7 3520    //
  #define Tone_B7 3951    //
  #define Tone_C8 4186    //


void setup() {
  delay(1000); //
  pinMode(BUTTON_OB, INPUT_PULLUP); //
  pinMode(Opt_L75, INPUT); //
  pinMode(Opt_L55, INPUT); //
  pinMode(Opt_L25, INPUT); //
  pinMode(Opt_R25, INPUT); //
  pinMode(Opt_R55, INPUT); //
  pinMode(BUZZER, OUTPUT); //
  pinMode(Opt_R75, INPUT); //
  pinMode(DIP_SW1, INPUT_PULLUP); //
  pinMode(DIP_SW2, INPUT_PULLUP); //
  pinMode(DIP_SW3, INPUT_PULLUP); //
  pinMode(DIP_SW4, INPUT_PULLUP); //
  pinMode(SPI_SS, INPUT); //
  pinMode(SPI_MISO, OUTPUT); //
  //pinMode(ToF_C, INPUT); //

  // ピンチェンジ割り込みを設定 // Set up pin change interrupt
  // 第一引数：割り込みを検知するピン // First argument: Pin to detect interrupt
  // 第二引数：割り込みが発生したときに呼び出される関数 // Second argument: Function called when interrupt occurs
  // 第三引数：割り込みのトリガー条件（RISING, FALLING, CHANGEなど） // Third argument: Interrupt trigger condition (RISING, FALLING, CHANGE, etc.)
  attachPCINT(digitalPinToPCINT(SPI_SS), ssHandler, CHANGE);
  attachPCINT(digitalPinToPCINT(ToF_C), ToF_Pulse_Cnt, CHANGE);

  //anarogRead()の分周値を変更　111=128,110=64,101=32,100=16,011=8,010=4,001=2,000=1 // Change prescaler for analogRead() 111=128,110=64,101=32,100=16,011=8,010=4,001=2,000=1
  bitWrite(ADCSRA, 0, 1); //bit0
  bitWrite(ADCSRA, 1, 0); //bit1
  bitWrite(ADCSRA, 2, 1); //bit2
  Serial.begin(115200); //デバッグしないときはシリアル出力をコメントアウト // Comment out serial output when not debugging

  //SPI通信設定 // SPI communication settings
  pinMode(MISO, OUTPUT); // MISOを出力に設定 (スレーブ必須) // Set MISO as output (required for slave)
  pinMode(SPI_SS, INPUT); // SSを入力に (CHANGE割り込み用) // Set SS as input (for CHANGE interrupt)
  SPCR |= _BV(SPE);      // SPIスレーブモード有効 // Enable SPI slave mode
  SPI.attachInterrupt(); // SPI割り込み有効 // Enable SPI interrupt

  // 返すデータの初期値例 // Example initial values for data to send
  dataToSend.Sensor_Status = 0;
  dataToSend.SW_Status_Val = 0;
  dataToSend.ToF_C_S = 0xFF;
  dataToSend.PSD_L_S = 0x00;
  dataToSend.PSD_R_S = 0x99;
  dataToSend.checksum_slave = 0x00;
  dataToSend.footer1 = 0xDD;
  
  // ToF移動平均バッファの初期化 // Initialize ToF moving average buffer
  for (uint8_t i = 0; i < TOF_AVG_COUNT; i++) {
    ToF_C_Val_Buffer[i] = 0;
  }
  ToF_C_Val_Index = 0;
  ToF_C_Val_Count = 0;

  Serial.println("Slave ready."); // Slave ready.
}


// SS割り込みハンドラ (CHANGEで呼び出し) // SS interrupt handler (called on CHANGE)
void ssHandler() {
  if (digitalRead(SPI_SS) == LOW) {
    // SS LOW: SPI通信開始、ToF_Cのピンチェンジ割り込みを無効化 // SS LOW: SPI communication start, disable ToF_C pin change interrupt
    //detachPCINT(digitalPinToPCINT(ToF_C)); // ToF_Cの割り込みを無効化 // Disable ToF_C interrupt
    PCMSK1 &= ~(1 << PCINT13); // ToF_Cの割り込み無効化 // Disable ToF_C interrupt
    // バッファリセット // Buffer reset
    byteIndex = 0;
    startTime = micros();
  } else {
    // SS HIGH: SPI通信終了、ToF_Cのピンチェンジ割り込みを再度有効化 // SS HIGH: SPI communication end, re-enable ToF_C pin change interrupt
    //attachPCINT(digitalPinToPCINT(ToF_C), ToF_Pulse_Cnt, CHANGE); // ToF_Cの割り込みを有効化 // Enable ToF_C interrupt
    PCMSK1 |= (1 << PCINT13); // ToF_Cの割り込み有効化 // Enable ToF_C interrupt
    unsigned long endTime = micros();
    receiveDuration = endTime - startTime;
    dataReceived = true; // 受信フラグ立て // Set reception flag

    //チェックサム確認 // Checksum verification
    byte CC1_check = receivedBytes[1]; //データ割り当て // Data assignment
    byte CC2_check = receivedBytes[2]; //データ割り当て // Data assignment
    byte checksum_check_A = receivedBytes[3]; //データ割り当て // Data assignment
    byte checksum_check_B = CC1_check ^ CC2_check; //チェックサム計算 // Checksum calculation
    if (checksum_check_A == checksum_check_B) {
      checksum_M_OK = true; //マスターから受信した値のチェックサム確認結果 // Checksum verification result of value received from master
    } else {
      checksum_M_OK = false; //マスターから受信した値のチェックサム確認結果 // Checksum verification result of value received from master
      checksum_M_OK_Record = false; //マスターから受信した値のチェックサム確認結果レコード　シリアル通信用 // Record of checksum verification result (for serial)
      checksum_check_A_Record = checksum_check_A;//シリアル通信用データ保存 // Data storage for serial
      checksum_check_B_Record = checksum_check_B;//シリアル通信用データ保存 // Data storage for serial
    }


    if (byteIndex < 7) {
      Serial.println("Warning: Incomplete reception, but flagged as complete.");
    }
  }
}

// ToF_Pulse割り込みハンドラ (CHANGEで呼び出し) // ToF_Pulse interrupt handler (called on CHANGE)
void ToF_Pulse_Cnt() {
  bool ToF_C_Pulse_Status = PINC & (1 << PC5); // ToF_C (ピン19、A5、PC5)の状態 // ToF_C state (pin 19, A5, PC5)
  if (ToF_C_Pulse_H_L) {
    // 立ち上がりエッジ待ち // Waiting for rising edge
    if (ToF_C_Pulse_Status) {
      ToF_C_Pulse_last_timeA = micros();
      ToF_C_Pulse_H_L = false;
    }
  } else {
    // 立下りエッジ待ち // Waiting for falling edge
    if (!ToF_C_Pulse_Status) {
      ToF_C_Pulse_last_timeB = micros();
      ToF_C_Pulse_H_L = true;
      ToF_C_Val = ToF_C_Pulse_last_timeB - ToF_C_Pulse_last_timeA;
      if (ToF_C_Val >= 0 && ToF_C_Val <= 2200) {
        // 移動平均用のバッファに追加 // Add to moving average buffer
        ToF_C_Val_Buffer[ToF_C_Val_Index] = ToF_C_Val;
        ToF_C_Val_Index = (ToF_C_Val_Index + 1) % TOF_AVG_COUNT; // リングバッファのインデックス更新 // Update ring buffer index
        if (ToF_C_Val_Count < TOF_AVG_COUNT) {
          ToF_C_Val_Count++; // 有効データ数をインクリメント // Increment valid data count
        }
        
        // 平均計算 // Average calculation
        int32_t sum = 0;
        for (uint8_t i = 0; i < ToF_C_Val_Count; i++) {
          sum += ToF_C_Val_Buffer[i];
        }
        int avg_ToF_C_Val = sum / ToF_C_Val_Count;
        ToF_C_Distance_O = (avg_ToF_C_Val - 1000) * 2; // 平均値に基づく距離計算 // Distance calculation based on average
      }
    }
  }
}


//SPI通信の割り込み // SPI communication interrupt
ISR(SPI_STC_vect) {
  receivedBytes[byteIndex] = SPDR; // 受信保存 // Save received data

  // 次に返すデータをSPDRにセット (dataToSendから1バイトずつ) // Set next data to send to SPDR (one byte at a time from dataToSend)
  byte nextByteToSend = ((byte*)&dataToSend)[byteIndex];
 // byte nextByteToSend = ((byte*)&dataToSend)[byteIndex];
  SPDR = nextByteToSend;

  byteIndex++;
  // byteIndex上限チェック (オーバーフロー防ぐ) // Upper limit check for byteIndex (prevent overflow)
  if (byteIndex > 7) {
    byteIndex = 7;
  }
}


//メインプログラム // Main program
void loop() {
  if (dataReceived) {
    PCMSK1 &= ~(1 << PCINT13); // ToF_Cの割り込み無効化 // Disable ToF_C interrupt
    // チェックサムOKなら受信データを構造体に戻す // If checksum OK, restore received data to struct
    if(checksum_M_OK){
      receivedData.CC1_ = receivedBytes[1]; //データ割り当て // Data assignment
      receivedData.CC2_ = receivedBytes[2]; //データ割り当て // Data assignment
      receivedData.checksum_ = receivedBytes[3]; //データ割り当て // Data assignment
    }
    //計測 // Measurement
    Sensor_Status_O = Opt_Checker();//敵センサー値更新 // Update opponent sensor values
    SW_Status_Val_O = DIP_SW_Checker(); //DIP_SWの値を読む & ToF_Front // Read DIP_SW values & ToF front
    // 返すデータの更新 // Update data to send
    dataToSend.Sensor_Status = Sensor_Status_O;
    dataToSend.SW_Status_Val = SW_Status_Val_O;
    if(ToF_C_Distance_O > 1500){
      dataToSend.ToF_C_S = 0;  // 検出範囲外の場合は値を0にする // Set to 0 if out of detection range
    } else {
      dataToSend.ToF_C_S = (byte)max(0, min(ToF_C_Distance_O, 255));  // 0〜255の範囲にクランプしてからキャスト // Clamp to 0-255 range then cast
    }
    dataToSend.PSD_L_S = (byte)max(0, min(PSD_L_Distance_O, 255));  // 0〜255の範囲にクランプしてからキャスト // Clamp to 0-255 range then cast
    dataToSend.PSD_R_S = (byte)max(0, min(PSD_R_Distance_O, 255));  // 0〜255の範囲にクランプしてからキャスト // Clamp to 0-255 range then cast
    dataToSend.checksum_slave = dataToSend.Sensor_Status ^ dataToSend.SW_Status_Val ^ dataToSend.ToF_C_S ^ dataToSend.PSD_L_S ^ dataToSend.PSD_R_S;
    dataToSend.footer1 = 0xDD;

    //Checksum Test Mode S >>> M　の処理 // Processing for Checksum Test Mode S >>> M
    if (Test_F2) {
      dataToSend.checksum_slave++; //返すデータのチェックサムに+1してマスター側で不一致にする // Increment checksum of data to send by 1 to cause mismatch on master side
    }
    //デバッグモード中のみシリアル有効 // Serial output only in debug mode
    if (debugMode) {
      STATUS_Serial_OUT(); //センサー値をシリアル出力 // Serial output of sensor values
    }
    dataReceived = false;
    PCMSK1 |= (1 << PCINT13); // ToF_Cの割り込み有効化 // Enable ToF_C interrupt
  }

  // PCからのシリアル入力で動作切り替え（toneなど、デバッグ用） // Switch operation with serial input from PC (tone etc., for debugging)
  if (Serial.available() > 0) {
    Serial_Commands();
  }


  //CC1_の値によってTone操作 // Tone operation based on CC1_ value
  if (receivedData.CC1_ > 0 || has_Serial_Command_ON) {
    byte CC1_ = receivedData.CC1_;
    if(has_Serial_Command_ON){
      CC1_ = CC1_debug; //シリアルコマンド時はCC1_を上書き // Overwrite CC1_ during serial command
    }
    //スイッチ文の方がいいかも。値による分岐 // Switch statement might be better. Branch by value
    if (CC1_ == B11111111) {
      has_Serial_Command_ON = false; //シリアルコマンドのフラグをクリア // Clear serial command flag
      Opt_Val_Hold = true;   //センサー値 10ms分の情報保持の切り替えフラグON // Turn ON flag to hold 10ms sensor ON information
    }
    if (CC1_ == B00000000) {
      has_Serial_Command_ON = false; //シリアルコマンドのフラグをクリア // Clear serial command flag
      Opt_Val_Hold = false;   //センサー値 10ms分の情報保持の切り替えフラグOFF // Turn OFF flag to hold 10ms sensor ON information
    }
    if (CC1_ == B10000001) {
      Sound_GET_COINS(); //マリオ　コインGET // Mario coin GET
    }
    if (CC1_ == B10000010) {
      Sound_1UP(); //マリオ1UP // Mario 1UP
    }
    if (CC1_ == B10000011) {
      KILL_MODE(); //マリオスーパースター // Mario Super Star
    }
    if (CC1_ == B10000100) {
      Sound_THE_Plumber(); //マリオのテーマ // Mario theme
    }
    if (CC1_ == B10000101) {
      Sound_Warning_1(); //警告音1 // Warning sound 1
      Opt_Val_Hold = false;   //センサー値 10ms分のON情報保持の切り替えフラグOFF // Turn OFF flag to hold 10ms sensor ON information
    }
    if (CC1_ == B01000000) {
      Test_F2 = true; // checksum test用フラグをセット // Set checksum test flag
    }
  } else {
    noTone(BUZZER); //Tone停止 // Stop tone
    Test_F2 = false; // checksum test用フラグをクリア // Clear checksum test flag
    Opt_Val_Hold = false;   //センサー値 10ms分のON情報保持の切り替えフラグOFF // Turn OFF flag to hold 10ms sensor ON information
  }

}

//ステータスシリアル出力 // Status serial output
void STATUS_Serial_OUT(){
  if(! checksum_M_OK_Record){
      Serial.println("!!! checksum ERROR !!!");
      Serial.print("checksum master:");
      Serial.println(checksum_check_A_Record,HEX);
      Serial.print("checksum slave:");
      Serial.println(checksum_check_B_Record,HEX);
      checksum_M_OK_Record = true; //
  }
  // 受信8バイトをHEXで横並び出力 (不足分"XX"埋め) // Output received 8 bytes in HEX side by side (fill missing with "XX")
  Serial.print("Received HEX: ");
  for (int i = 0; i < 6; i++) {
    if (i < byteIndex) {
      Serial.print(receivedBytes[i], HEX);
    } else {
      Serial.print("XX");
    }
    if (i < 5) Serial.print(", ");
  }
  Serial.println();
  Serial.print("Sensor_Status_O:");
  Serial.println(Sensor_Status_O,BIN);
  Serial.print("SW_Status_Val_O:");
  Serial.println(SW_Status_Val_O,BIN);
  Serial.print("ToF_C_Distance_O:");
  Serial.println(ToF_C_Distance_O,DEC);
  Serial.print("PSD_L_Distance_O:");
  Serial.println(PSD_L_Distance_O,DEC);
  Serial.print("PSD_R_Distance_O:");
  Serial.println(PSD_R_Distance_O,DEC);
  Serial.print("checksum_slave:");
  Serial.println(dataToSend.checksum_slave,HEX);
  Serial.print("CC1_:");
  Serial.println(receivedData.CC1_,BIN);
  Serial.print("CC2_:");
  Serial.println(receivedData.CC2_,BIN);//receiveDuration
  Serial.print("checksum_:");
  Serial.println(receivedData.checksum_,HEX);//receiveDuration
  Serial.print("receiveDuration:");
  Serial.print(receiveDuration); //receiveDuration
  Serial.println("micro sec:");
}

//敵センサーの値読み込み // Read opponent sensor values
volatile int Opt_Checker(){
  //センサー値処理する // Process sensor values
  Opt_R25_Val = digitalRead(Opt_R25);
  Opt_R55_Val = digitalRead(Opt_R55);
  Opt_R75_Val = digitalRead(Opt_R75);
  Opt_L25_Val = digitalRead(Opt_L25);
  Opt_L55_Val = digitalRead(Opt_L55);
  Opt_L75_Val = digitalRead(Opt_L75);
  bitWrite(Opt_Val,Opt_R25_B,Opt_R25_Val);
  bitWrite(Opt_Val,Opt_R55_B,Opt_R55_Val);
  bitWrite(Opt_Val,Opt_R75_B,Opt_R75_Val);
  bitWrite(Opt_Val,Opt_L25_B,Opt_L25_Val);
  bitWrite(Opt_Val,Opt_L55_B,Opt_L55_Val);
  bitWrite(Opt_Val,Opt_L75_B,Opt_L75_Val);
  Opt_Val = ~Opt_Val;

  //センサー値 10ms分のON情報保持の切り替え // Switch to hold 10ms ON information for sensor values
  if(Opt_Val_Hold){
    //Opt_Sens_OR_12345678910 敵センサーON情報を過去10ms分保持する // Hold opponent sensor ON information for past 10ms
    Opt_Val_10 = Opt_Val_9;       //保管センサー値シフト // Shift stored sensor values
    Opt_Val_9 = Opt_Val_8;       //保管センサー値シフト // Shift stored sensor values
    Opt_Val_8 = Opt_Val_7;       //保管センサー値シフト // Shift stored sensor values
    Opt_Val_7 = Opt_Val_6;       //保管センサー値シフト // Shift stored sensor values
    Opt_Val_6 = Opt_Val_5;       //保管センサー値シフト // Shift stored sensor values
    Opt_Val_5 = Opt_Val_4;       //保管センサー値シフト // Shift stored sensor values
    Opt_Val_4 = Opt_Val_3;       //保管センサー値シフト // Shift stored sensor values
    Opt_Val_3 = Opt_Val_2;       //保管センサー値シフト // Shift stored sensor values
    Opt_Val_2 = Opt_Val_1;       //保管センサー値シフト // Shift stored sensor values
    Opt_Val_1 = Opt_Val;         //保管センサー値シフト // Shift stored sensor values
    //OR
    Opt_Val = Opt_Val | Opt_Val_2;    //保管センサー値OR // OR stored sensor values
    Opt_Val = Opt_Val | Opt_Val_3;    //保管センサー値OR // OR stored sensor values
    Opt_Val = Opt_Val | Opt_Val_4;    //保管センサー値OR // OR stored sensor values
    Opt_Val = Opt_Val | Opt_Val_5;    //保管センサー値OR // OR stored sensor values
    Opt_Val = Opt_Val | Opt_Val_6;    //保管センサー値OR // OR stored sensor values
    Opt_Val = Opt_Val | Opt_Val_7;    //保管センサー値OR // OR stored sensor values
    Opt_Val = Opt_Val | Opt_Val_8;    //保管センサー値OR // OR stored sensor values
    Opt_Val = Opt_Val | Opt_Val_9;    //保管センサー値OR // OR stored sensor values
    Opt_Val = Opt_Val | Opt_Val_10;    //保管センサー値OR // OR stored sensor values
  } else {
    //バッファーをクリア // Clear buffer
    Opt_Val_10 = 0;      //
    Opt_Val_9 = 0;       //
    Opt_Val_8 = 0;       //
    Opt_Val_7 = 0;       //
    Opt_Val_6 = 0;       //
    Opt_Val_5 = 0;       //
    Opt_Val_4 = 0;       //
    Opt_Val_3 = 0;       //
    Opt_Val_2 = 0;       //
    Opt_Val_1 = 0;       //
  }
  //PSDを反映 // Reflect PSD
  PSD_L_Val = analogRead(PSD_L);
  float voltage1 = PSD_L_Val * (5.0 / 1023.0);
  PSD_L_Distance_O = (int)round(voltage1*100); //実際の電圧に直す // Convert to actual voltage
  PSD_R_Val = analogRead(PSD_R);
  float voltage2 = PSD_R_Val * (5.0 / 1023.0);
  PSD_R_Distance_O = (int)round(voltage2*100); //実際の電圧に直す // Convert to actual voltage
 
  bitWrite(Opt_Val,PSD_L10_B,0);
  bitWrite(Opt_Val,PSD_R10_B,0);

  Opt_Val_X = Opt_Val;
  //処理したセンサー値を反映 // Reflect processed sensor values
  return Opt_Val_X;
}

//DIP SW 読み取り // DIP SW reading
volatile int DIP_SW_Checker(){
  //DIP SW READ // Read DIP SW
  DIP_SW1_Val = digitalRead(DIP_SW1);
  DIP_SW2_Val = digitalRead(DIP_SW2);
  DIP_SW3_Val = digitalRead(DIP_SW3);
  DIP_SW4_Val = digitalRead(DIP_SW4);
  bool BUTTON_OB_High = digitalRead(BUTTON_OB);
  //DIP SW Val Set // Set DIP SW values
  if(DIP_SW1_Val == 1){
    bitWrite(DIP_SW_Val,DIP_SW1_B,0);
  } else{
    bitWrite(DIP_SW_Val,DIP_SW1_B,1);
  }
  if(DIP_SW2_Val == 1){
    bitWrite(DIP_SW_Val,DIP_SW2_B,0);
  } else{
    bitWrite(DIP_SW_Val,DIP_SW2_B,1);
  }
  if(DIP_SW3_Val == 1){
    bitWrite(DIP_SW_Val,DIP_SW3_B,0);
  } else{
    bitWrite(DIP_SW_Val,DIP_SW3_B,1);
  }
  if(DIP_SW4_Val == 1){
    bitWrite(DIP_SW_Val,DIP_SW4_B,0);
  } else{
    bitWrite(DIP_SW_Val,DIP_SW4_B,1);
  }
  if(BUTTON_OB_High == 1){
    bitWrite(DIP_SW_Val,BUTTON_OB_B,0);
  } else{
    bitWrite(DIP_SW_Val,BUTTON_OB_B,1);
  }
  //ToF_Cの計測 // ToF_C measurement
  bitWrite(DIP_SW_Val,ToF_C_B,0);

  return DIP_SW_Val;}

//効果音：警告音1 // Sound effect: Warning sound 1
void Sound_Warning_1(){  

  tone(8,Tone_G6,50);               //
  delay(200);
}


//効果音：マリオのテーマ // Sound effect: Mario theme
void Sound_THE_Plumber(){  
  //マリオのテーマ // Mario theme
  tone(8,Tone_E5,75);               //
  delay(150);
  tone(8,Tone_E5,75);               //
  delay(300);
  tone(8,Tone_E5,75);               //
  delay(300);
  tone(8,Tone_C5,75);               //
  delay(150);
  tone(8,Tone_E5,75);               //
  delay(300);
  tone(8,Tone_G5,75);               //
  delay(600);
  tone(8,Tone_G4,75);               //
  delay(150);
  delay(500);
}

//効果音：コインGET // Sound effect: Coin GET
void Sound_GET_COINS(){  
  //マリオ　コインGET // Mario coin GET
  tone(BUZZER,Tone_B5,100); 
  delay(100);
  tone(BUZZER,Tone_E6,250); 
  delay(300);
}

//効果音：NOT START // Sound effect: NOT START
void Sound_NOT_START(){  
  //NOT START
  tone(BUZZER,Tone_B4, 75);
  delay(150); 
  tone(BUZZER,Tone_F5, 75);
  delay(225); 
  tone(BUZZER,Tone_F5, 75);
  delay(150); 
  tone(BUZZER,Tone_F5, 75);
  delay(150); 
  tone(BUZZER,Tone_E5, 75);
  delay(225); 
  tone(BUZZER,Tone_D5, 75);
  delay(150); 
  tone(BUZZER,Tone_C5, 75);
  delay(1000); 
}

//効果音：マリオ 1UP // Sound effect: Mario 1UP
void Sound_1UP(){    
  //マリオ 1UP // Mario 1UP
  tone(BUZZER,Tone_E5,100);
  delay(100);
  tone(BUZZER,Tone_G5,100);
  delay(100);
  tone(BUZZER,Tone_E6,100);
  delay(100);
  tone(BUZZER,Tone_C6,100);
  delay(100);
  tone(BUZZER,Tone_D6,100);
  delay(100);
  tone(BUZZER,Tone_G6,100);
  delay(100);
  tone(BUZZER,Tone_G7,10);
  delay(500);
}

//効果音：マリオ STAR // Sound effect: Mario STAR
void KILL_MODE(){ 
    //マリオ STAR // Mario STAR
    tone(BUZZER,Tone_C5,100);
    delay(200);
    tone(BUZZER,Tone_C5,100);
    delay(200);
    tone(BUZZER,Tone_C5,100);
    delay(200);
    tone(BUZZER,Tone_D4,50);
    delay(100);
    tone(BUZZER,Tone_C5,50);
    delay(100);
    tone(BUZZER,Tone_C5,50);
    delay(100);
    tone(BUZZER,Tone_C5,100);
    delay(200);
    delay(5);
    tone(BUZZER,Tone_D4,50);
    delay(100);
    tone(BUZZER,Tone_C5,100);
    delay(200);
    tone(BUZZER,Tone_C5,100);
    delay(200);
    tone(BUZZER,Tone_B4,100);
    delay(200);
    tone(BUZZER,Tone_B4,100);
    delay(200);
    tone(BUZZER,Tone_B4,100);
    delay(200);
    delay(5);
    tone(BUZZER,Tone_C4,50);
    delay(100);
    tone(BUZZER,Tone_B4,50);
    delay(100);
    tone(BUZZER,Tone_B4,50);
    delay(100);
    tone(BUZZER,Tone_B4,100);
    delay(200);
    tone(BUZZER,Tone_C4,50);
    delay(100);
    tone(BUZZER,Tone_B4,100);
    delay(200);
    tone(BUZZER,Tone_B4,100);
    delay(200);
}


//シリアルコマンド制御： PC >>> Slave のシリアルコマンドで動作切り替え（ブザー鳴らす等、デバッグ用） // Serial command control: Switch operation with serial commands from PC >>> Slave (sound buzzer etc., for debugging)
void Serial_Commands(){
  char input = Serial.read();
  const char* message = "Tone set:";  // デフォルトメッセージ（'a'〜'c'用） // Default message (for 'a'~'c')
  has_Serial_Command_ON = true; //シリアルコマンドのフラグをON // Turn ON serial command flag
  //コマンドによって動作分岐 // Branch by command
  switch (input) {
    case 'a':
      CC1_debug = B10000001; //コインGET  // Coin GET
      break;
    case 'b':
      CC1_debug = B10000010; //1UP // 1UP
      break;
    case 'c':
      CC1_debug = B10000011; //スーパースター // Super Star
      break;
    case 'd':
      CC1_debug = B00000000; //Reset (ブザーOFF,エラーテストOFF) // Reset (buzzer OFF, error test OFF)
      message = "Reset:";  //
      Test_F2 = false; // checksum test用フラグクリア // Clear checksum test flag
      break;
    case 'e':
      CC1_debug = B10000100; //マリオのテーマ // Mario theme
      break;
    case 'f':
      CC1_debug = B10000101; //警告音1 // Warning sound 1
      break;
    case 'g':
      CC1_debug = B11111111; //Reset (ブザーOFF,エラーテストOFF) // Reset (buzzer OFF, error test OFF)
      message = "Reset:";  //
      Test_F2 = false; // checksum test用フラグクリア // Clear checksum test flag
      break;
    default:
      // 他の入力の場合の処理 // Processing for other inputs
      message = "Invalid input:";  //
      return;  // 無効入力時は出力せず抜ける（オプション） // Exit without output on invalid input (optional)
  }
  // 共通の出力部分 // Common output part
  Serial.print("Input:");
  Serial.println(input);
  Serial.print(message);
  Serial.println(CC1_debug, BIN);
  //delay(2000);  //

}