The Waving Penguin: Your Cheerful Entrance Companion

#ifndef SOUNDS_H
#define SOUNDS_H

#include <cstddef>

// --- 「いってらっしゃい」の音声データ（宣言） ---
extern const unsigned char itterassyai_wav_0[];
extern const unsigned char itterassyai_wav_1[];
extern const unsigned char itterassyai_wav_2[];
extern const unsigned char itterassyai_wav_3[];

// --- 「おかえり」の音声データ（宣言） ---
extern const unsigned char okaeri_wav_0[];
extern const unsigned char okaeri_wav_1[];
extern const unsigned char okaeri_wav_2[];
extern const unsigned char okaeri_wav_3[];

// ★★★ 追加: 自己紹介の音声データ（宣言） ★★★
extern const unsigned char explanation_wav[];
extern const size_t explanation_wav_len;

// --- 音声データを管理しやすくするための配列（宣言） ---

// 「いってらっしゃい」バリエーションの数
const int GO_OUT_SOUND_COUNT = 4;

// 各音声データへのポインタ配列
extern const unsigned char* go_out_sounds[GO_OUT_SOUND_COUNT];
// 各音声データのサイズ配列
extern const size_t go_out_sound_lens[GO_OUT_SOUND_COUNT];


// --- 「おかえり」も同様に設定 ---
const int COME_HOME_SOUND_COUNT = 4;

extern const unsigned char* come_home_sounds[COME_HOME_SOUND_COUNT];
extern const size_t come_home_sound_lens[COME_HOME_SOUND_COUNT];


#endif // SOUNDS_H

#include "RobotActionController.h"
#include "sounds.h" // 音声データヘッダーをインクルード

// コンストラクタで再生インデックスを初期化
RobotActionController::RobotActionController() 
    : _motion(nullptr), 
      _echo(nullptr), 
      _currentState(State::IDLE),
      _goOutSoundIndex(0), 
      _comeHomeSoundIndex(0) 
{}

void RobotActionController::begin(M5AtomicMotion* motion, M5EchoBase* echo) {
    _motion = motion;
    _echo = echo;
    Serial.begin(115200);
    M5.Log.println("RobotActionController initialized. Waiting for commands...");
    setAllServosToHome();
    delay(500);
}

void RobotActionController::update() {
    if (_currentState == State::IDLE) {
        processSerialCommand();
    }
}

void RobotActionController::processSerialCommand() {
    if (Serial.available() > 0) {
        String command = Serial.readStringUntil('\n');
        command.trim();

        M5.Log.printf("Received command: %s\n", command.c_str());
        _currentState = State::PERFORMING_ACTION;

        if (command == "GO_OUT") {
            performGoOutSequence();
        } else if (command == "COME_HOME") {
            performComeHomeSequence();
        } else if (command == "RESET") {
            resetToHomePosition();
        } else if (command == "TEST_GO_OUT") {
            performGoOutSequence();
            delay(3000);
            resetToHomePosition();
        } else if (command == "TEST_COME_HOME") {
            performComeHomeSequence();
            delay(3000);
            resetToHomePosition();
        }
        // ★★★ 追加: EXPLAINコマンドの処理 ★★★
        else if (command == "EXPLAIN") {
            performExplanationSequence();
        }
        else {
            M5.Log.printf("Unknown command: %s\n", command.c_str());
        }

        _currentState = State::IDLE;
        M5.Log.println("Action finished. Ready for next command.");
    }
}

void RobotActionController::performGoOutSequence() {
    M5.Log.println("Executing GO_OUT sequence...");
    _echo->setMute(false);
    _echo->play((uint8_t*)go_out_sounds[_goOutSoundIndex], go_out_sound_lens[_goOutSoundIndex]);
    _goOutSoundIndex = (_goOutSoundIndex + 1) % GO_OUT_SOUND_COUNT;
    _motion->setServoAngle(NECK_SERVO_CH, NECK_HOME_ANGLE + 10 + NECK_OFFSET);
    waveHand(LEFT_ARM_SERVO_CH, LEFT_ARM_HOME_ANGLE, 60, 5, true);
    M5.Log.println("Waiting for door to close...");
}

void RobotActionController::performComeHomeSequence() {
    M5.Log.println("Executing COME_HOME sequence...");
    _echo->setMute(false);
    _echo->play((uint8_t*)come_home_sounds[_comeHomeSoundIndex], come_home_sound_lens[_comeHomeSoundIndex]);
    _comeHomeSoundIndex = (_comeHomeSoundIndex + 1) % COME_HOME_SOUND_COUNT;
    _motion->setServoAngle(NECK_SERVO_CH, NECK_HOME_ANGLE + NECK_OFFSET);

    xTaskCreate([](void* pvParameters){
        ((RobotActionController*)pvParameters)->waveHand(RIGHT_ARM_SERVO_CH, RIGHT_ARM_HOME_ANGLE, 60, 5, false);
        vTaskDelete(NULL);
    }, "waveRight", 2048, this, 1, NULL);

    xTaskCreate([](void* pvParameters){
        ((RobotActionController*)pvParameters)->waveHand(LEFT_ARM_SERVO_CH, LEFT_ARM_HOME_ANGLE, 60, 5, true);
        vTaskDelete(NULL);
    }, "waveLeft", 2048, this, 1, NULL);

    delay(2500);
    M5.Log.println("Waving finished. Waiting for reset command.");
}

// ★★★ 追加: 自己紹介シーケンスの実装 ★★★
void RobotActionController::performExplanationSequence() {
    M5.Log.println("Executing EXPLANATION sequence...");
    _echo->setMute(false);
    _echo->play((uint8_t*)explanation_wav, explanation_wav_len);

    // 自己紹介中の簡単な動き
    delay(500); // 音声開始を少し待つ
    // 左を見る
    _motion->setServoAngle(NECK_SERVO_CH, NECK_HOME_ANGLE + 20 + NECK_OFFSET);
    delay(1500);
    // 右を見る
    _motion->setServoAngle(NECK_SERVO_CH, NECK_HOME_ANGLE - 20 + NECK_OFFSET);
    delay(1500);
    // 正面に戻る
    _motion->setServoAngle(NECK_SERVO_CH, NECK_HOME_ANGLE + NECK_OFFSET);
    delay(1000);
    // 両手を少し広げるポーズ
    _motion->setServoAngle(RIGHT_ARM_SERVO_CH, RIGHT_ARM_HOME_ANGLE + 45 + RIGHT_ARM_OFFSET);
    _motion->setServoAngle(LEFT_ARM_SERVO_CH, LEFT_ARM_HOME_ANGLE - 45 - LEFT_ARM_OFFSET);
    
    // 音声の長さに合わせて待機（音声が5秒なら、上記のdelay合計と合わせて調整）
    delay(2000); 

    // 最後にホームポジションに戻る
    setAllServosToHome();
}

void RobotActionController::resetToHomePosition() {
    M5.Log.println("Returning to home position...");
    setAllServosToHome();
}

void RobotActionController::waveHand(int channel, int homeAngle, int waveAngle, int times, bool isLeftHand) {
    int upAngle, downAngle;
    for (int i = 0; i < times; ++i) {
        upAngle = isLeftHand ? homeAngle - waveAngle - LEFT_ARM_OFFSET : homeAngle + waveAngle + RIGHT_ARM_OFFSET;
        downAngle = isLeftHand ? homeAngle - LEFT_ARM_OFFSET : homeAngle + RIGHT_ARM_OFFSET;
        _motion->setServoAngle(channel, upAngle);
        delay(200);
        _motion->setServoAngle(channel, downAngle);
        delay(200);
    }
    upAngle = isLeftHand ? homeAngle - waveAngle - LEFT_ARM_OFFSET : homeAngle + waveAngle + RIGHT_ARM_OFFSET;
    _motion->setServoAngle(channel, upAngle);
}

void RobotActionController::setAllServosToHome() {
    _motion->setServoAngle(NECK_SERVO_CH, NECK_HOME_ANGLE + NECK_OFFSET);
    _motion->setServoAngle(RIGHT_ARM_SERVO_CH, RIGHT_ARM_HOME_ANGLE + RIGHT_ARM_OFFSET);
    _motion->setServoAngle(LEFT_ARM_SERVO_CH, LEFT_ARM_HOME_ANGLE - LEFT_ARM_OFFSET);
}

#ifndef ROBOT_ACTION_CONTROLLER_H
#define ROBOT_ACTION_CONTROLLER_H

#include <M5Unified.h>
#include "M5AtomicMotion.h"
#include <M5EchoBase.h>
#include "sounds.h"

// サーボのチャンネル定義
#define NECK_SERVO_CH      0
#define RIGHT_ARM_SERVO_CH 1
#define LEFT_ARM_SERVO_CH  2

// サーボの角度オフセット
#define NECK_OFFSET        (5)
#define RIGHT_ARM_OFFSET   (0)
#define LEFT_ARM_OFFSET    (2)

// サーボの基本角度
#define NECK_HOME_ANGLE      (90)
#define RIGHT_ARM_HOME_ANGLE (0)
#define LEFT_ARM_HOME_ANGLE  (180)

class RobotActionController {
public:
    RobotActionController();
    void begin(M5AtomicMotion* motion, M5EchoBase* echo);
    void update();

private:
    M5AtomicMotion* _motion;
    M5EchoBase* _echo;
    
    // 音声再生のインデックスを管理するメンバー変数
    int _goOutSoundIndex;
    int _comeHomeSoundIndex;
    
    // 動作シーケンス
    void performGoOutSequence();
    void performComeHomeSequence();
    void resetToHomePosition();
    void performExplanationSequence(); // ★★★ 追加 ★★★

    // 基本動作
    void waveHand(int channel, int homeAngle, int waveAngle, int times, bool isLeftHand);
    void setAllServosToHome();

    // 状態管理
    enum class State {
        IDLE,
        PERFORMING_ACTION
    };
    State _currentState;
    void processSerialCommand();
};

#endif // ROBOT_ACTION_CONTROLLER_H

#ifndef CALIBRATOR_H
#define CALIBRATOR_H

#include <M5Unified.h>

// キャリブレーション機能をまとめたクラス
class Calibrator {
public:
    // コンストラクタ
    Calibrator();

    // 初期化処理
    void begin();

    // キャリブレーションのメインループを実行
    void run();

private:
    // データバッファや状態変数はクラス内に保持します
    double vReal[1024]; // FFT_SAMPLES
    double vImag[1024]; // FFT_SAMPLES
    int16_t i2s_buffer[1024]; // FFT_SAMPLES
    int bass_freq_start_index;
    int bass_freq_end_index;

    // ★★★ 修正点: SERVO_INIT モードを追加 ★★★
    enum Mode {
        MEASURE_NOISE,
        OBSERVE_BEAT,
        SERVO_INIT
    };
    Mode current_mode;

    double max_noise;
    double total_noise;
    int noise_sample_count;
    unsigned long measurement_start_time;
    bool measurement_done;
    
    double bass_history_array[16]; // BEAT_HISTORY_SIZE
    int bass_history_index;
    bool bass_history_filled;

    // ★★★ 追加: サーボ初期化が完了したかを管理するフラグ ★★★
    bool servo_init_done;

    // 内部で使う関数
    void initI2S_Driver();
    void switchMode();
    void runMeasureNoiseMode(double current_magnitude);
    void runObserveBeatMode(double current_magnitude);
    // ★★★ 追加: サーボ初期化モード用の関数 ★★★
    void runServoInitMode();
};

#endif // CALIBRATOR_H

#include "Calibrator.h"
#include <M5EchoBase.h>
#include <ArduinoFFT.h>
#include "M5AtomicMotion.h"

extern M5EchoBase echobase;
extern ArduinoFFT<double> FFT;
extern LGFX_Sprite lcdSprite;
extern M5AtomicMotion AtomicMotion;

// ===============================================================
//  設定項目
// ===============================================================
#define I2S_BCLK_PIN   8
#define I2S_LRCK_PIN   6
#define I2S_DATA_PIN   7
#define I2S_DOUT_PIN   5
#define I2C_SDA_PIN 38
#define I2C_SCL_PIN 39
#define SAMPLING_FREQUENCY 16000
#define FFT_SAMPLES        1024
#define BASS_FREQ_MIN 60
#define BASS_FREQ_MAX 120
#define BEAT_HISTORY_SIZE 16
const int NOISE_MEASUREMENT_DURATION_MS = 15000;

// ===============================================================

Calibrator::Calibrator() {
    current_mode = MEASURE_NOISE;
    max_noise = 0.0;
    total_noise = 0.0;
    noise_sample_count = 0;
    measurement_done = false;
    bass_history_index = 0;
    bass_history_filled = false;
    servo_init_done = false; // ★★★ 追加: フラグを初期化
}

// 初期化処理
void Calibrator::begin() {
    M5.Log.println("Calibration Program Started.");
    M5.Log.println("Mode 1: Measuring Ambient Noise...");

    echobase.init(SAMPLING_FREQUENCY, I2C_SDA_PIN, I2C_SCL_PIN, I2S_DATA_PIN, I2S_LRCK_PIN, I2S_DOUT_PIN, I2S_BCLK_PIN, Wire);
    echobase.setMicGain(ES8311_MIC_GAIN_24DB);
    initI2S_Driver();
    M5.Log.println("Microphone initialized.");

    lcdSprite.createSprite(M5.Display.width(), M5.Display.height());
    lcdSprite.setTextSize(2);
    lcdSprite.setTextColor(TFT_WHITE, TFT_BLACK);
    lcdSprite.setTextDatum(MC_DATUM);

    double freq_resolution = (double)SAMPLING_FREQUENCY / FFT_SAMPLES;
    bass_freq_start_index = (int)ceil(BASS_FREQ_MIN / freq_resolution);
    bass_freq_end_index = (int)floor(BASS_FREQ_MAX / freq_resolution);

    measurement_start_time = millis(); // 測定タイマーを開始
}

// I2Sドライバの初期化
void Calibrator::initI2S_Driver() {
  i2s_config_t i2s_config = {
      .mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_RX),
      .sample_rate = SAMPLING_FREQUENCY,
      .bits_per_sample = I2S_BITS_PER_SAMPLE_16BIT,
      .channel_format = I2S_CHANNEL_FMT_ONLY_LEFT,
      .communication_format = I2S_COMM_FORMAT_STAND_I2S,
      .intr_alloc_flags = ESP_INTR_FLAG_LEVEL1,
      .dma_buf_count = 4,
      .dma_buf_len = 512,
  };
  i2s_driver_install(I2S_NUM_0, &i2s_config, 0, NULL);
}

// モード切り替え
void Calibrator::switchMode() {
    // ★★★ 修正点: 3つのモードを順番に切り替える ★★★
    if (current_mode == MEASURE_NOISE) {
        current_mode = OBSERVE_BEAT;
        M5.Log.println("\nSwitched to Mode 2: Observe Beat Sensitivity");
        M5.Log.println("Play music and observe the 'Ratio' in the log.");
    } else if (current_mode == OBSERVE_BEAT) {
        current_mode = SERVO_INIT;
        servo_init_done = false; // サーボ初期化フラグをリセット
        M5.Log.println("\nSwitched to Mode 3: Servo Initialization");
    } else { // current_mode == SERVO_INIT
        current_mode = MEASURE_NOISE;
        M5.Log.println("\nSwitched to Mode 1: Measuring Ambient Noise...");
        max_noise = 0.0;
        total_noise = 0.0;
        noise_sample_count = 0;
        measurement_done = false;
        measurement_start_time = millis(); // モード切替時にタイマーをリセット
    }
}

// メインループ
void Calibrator::run() {
    while (true) {
        M5.update();

        if (M5.BtnA.wasPressed()) {
            switchMode();
        }

        // ★★★ 修正点: サーボ初期化モードではマイク処理をスキップ ★★★
        if (current_mode == SERVO_INIT) {
            runServoInitMode();
        } else {
            size_t bytes_read = 0;
            esp_err_t result = i2s_read(I2S_NUM_0, i2s_buffer, sizeof(i2s_buffer), &bytes_read, portMAX_DELAY);
            
            if (result != ESP_OK || bytes_read == 0) {
                continue;
            }

            long long buffer_sum = 0;
            for (int i = 0; i < FFT_SAMPLES; i++) {
                buffer_sum += abs(i2s_buffer[i]);
            }
            if (buffer_sum == 0) {
                M5.Log.println("Skipping empty buffer...");
                delay(10); 
                continue;
            }

            long total = 0;
            for (int i = 0; i < FFT_SAMPLES; i++) total += i2s_buffer[i];
            double average = (double)total / FFT_SAMPLES;
            for (int i = 0; i < FFT_SAMPLES; i++) {
                vReal[i] = (double)i2s_buffer[i] - average;
                vImag[i] = 0;
            }
            FFT.windowing(vReal, FFT_SAMPLES, FFT_WIN_TYP_HAMMING, FFT_FORWARD);
            FFT.compute(vReal, vImag, FFT_SAMPLES, FFT_FORWARD);
            FFT.complexToMagnitude(vReal, vImag, FFT_SAMPLES);
            
            double bass_magnitude = 0;
            int freq_range_count = bass_freq_end_index - bass_freq_start_index + 1;
            if (freq_range_count > 0) {
                for (int i = bass_freq_start_index; i <= bass_freq_end_index; i++) {
                    bass_magnitude += vReal[i];
                }
                bass_magnitude /= freq_range_count;
            }

            if (current_mode == MEASURE_NOISE) {
                runMeasureNoiseMode(bass_magnitude);
            } else { // OBSERVE_BEAT
                runObserveBeatMode(bass_magnitude);
            }
        }

        lcdSprite.pushSprite(0, 0);
        delay(1);
    }
}

void Calibrator::runServoInitMode() {
    // このモードに入った最初の1回だけ実行
    if (!servo_init_done) {
        M5.Log.println("Setting servos to initial position...");
        // AtomicMotionの初期化（既に初期化済みでも問題ない）
        while (!AtomicMotion.begin(&Wire, M5_ATOMIC_MOTION_I2C_ADDR, I2C_SDA_PIN, I2C_SCL_PIN, 100000)) {
            M5.Log.println("Atomic Motion begin failed");
            lcdSprite.fillScreen(TFT_RED);
            lcdSprite.drawString("Motion FAIL", M5.Display.width() / 2, M5.Display.height() / 2);
            lcdSprite.pushSprite(0, 0);
            delay(1000);
        }
        
        // サーボを初期位置に設定
        AtomicMotion.setServoAngle(0, 90);  // 首: 90°
        AtomicMotion.setServoAngle(1, 0);   // 右手 (仮): 0°
        AtomicMotion.setServoAngle(2, 180); // 左手 (仮): 180°
        
        M5.Log.println("Servos set. Neck: 90, Right Arm: 0, Left Arm: 180");
        servo_init_done = true;
    }

    // 画面に情報を表示し続ける
    lcdSprite.fillScreen(TFT_DARKGREEN);
    lcdSprite.drawString("Mode 3", M5.Display.width() / 2, 20);
    lcdSprite.drawString("Servo Init", M5.Display.width() / 2, 50);
    lcdSprite.drawString("Neck: 90", M5.Display.width() / 2, 85);
    lcdSprite.drawString("L:180  R:0", M5.Display.width() / 2, 115);
}


// モード1: 環境ノイズ測定の処理
void Calibrator::runMeasureNoiseMode(double current_magnitude) {
    if (!measurement_done) {
        if (millis() - measurement_start_time < NOISE_MEASUREMENT_DURATION_MS) {
            lcdSprite.fillScreen(TFT_BLACK);
            lcdSprite.drawString("Mode 1", M5.Display.width() / 2, 20);
            lcdSprite.drawString("Measuring", M5.Display.width() / 2, 50);
            lcdSprite.drawString("Noise...", M5.Display.width() / 2, 80);
            
            M5.Log.printf("Measuring... Current Magnitude: %.0f\n", current_magnitude);

            if (current_magnitude > max_noise) max_noise = current_magnitude;
            total_noise += current_magnitude;
            noise_sample_count++;
        } else {
            measurement_done = true;
            double avg_noise = (noise_sample_count > 0) ? total_noise / noise_sample_count : 0.0;
            
            M5.Log.println("--- Ambient Noise Measurement Result ---");
            M5.Log.printf("Max Noise Magnitude: %.0f\n", max_noise);
            M5.Log.printf("Avg Noise Magnitude: %.0f\n", avg_noise);
            M5.Log.println("----------------------------------------");
            M5.Log.println("Recommendation: Set SILENCE_THRESHOLD slightly higher than Max Noise.");
            
            lcdSprite.fillScreen(TFT_BLUE);
            lcdSprite.drawString("Done!", M5.Display.width() / 2, 20);
            lcdSprite.drawString("Max:", M5.Display.width() / 2, 50);
            lcdSprite.drawFloat(max_noise, 0, M5.Display.width() / 2, 80);
        }
    }
}

// モード2: ビート感度観察の処理
void Calibrator::runObserveBeatMode(double current_magnitude) {
    bass_history_array[bass_history_index] = current_magnitude;
    bass_history_index = (bass_history_index + 1) % BEAT_HISTORY_SIZE;
    if (bass_history_index == 0) bass_history_filled = true;

    double avg_bass = 0.0;
    if (bass_history_filled) {
        double sum = 0.0;
        for(int i=0; i<BEAT_HISTORY_SIZE; i++) sum += bass_history_array[i];
        avg_bass = sum / BEAT_HISTORY_SIZE;
    }
    
    lcdSprite.fillScreen(TFT_BLACK);
    lcdSprite.drawString("Mode 2", M5.Display.width()/2, 20);
    lcdSprite.drawString("Current:", M5.Display.width()/2, 50);
    lcdSprite.drawFloat(current_magnitude, 0, M5.Display.width()/2, 70);
    lcdSprite.drawString("Avg:", M5.Display.width()/2, 100);
    lcdSprite.drawFloat(avg_bass, 0, M5.Display.width()/2, 120);

    if(avg_bass > 0) {
        M5.Log.printf("Current: %.0f, Avg: %.0f, Ratio: %.2f\n", current_magnitude, avg_bass, current_magnitude / avg_bass);
    }
}