ESP IDF+ESP32S3小智AI+4舵机=手搓一个桌面智能小狗狗 简单

#ifndef _BOARD_CONFIG_H_
#define _BOARD_CONFIG_H_

#include <driver/gpio.h>

#define AUDIO_INPUT_SAMPLE_RATE  16000
#define AUDIO_OUTPUT_SAMPLE_RATE 24000

// 如果使用 Duplex I2S 模式，请注释下面一行
#define AUDIO_I2S_METHOD_SIMPLEX

#ifdef AUDIO_I2S_METHOD_SIMPLEX

#define AUDIO_I2S_MIC_GPIO_WS   GPIO_NUM_4
#define AUDIO_I2S_MIC_GPIO_SCK  GPIO_NUM_5
#define AUDIO_I2S_MIC_GPIO_DIN  GPIO_NUM_6
#define AUDIO_I2S_SPK_GPIO_DOUT GPIO_NUM_7
#define AUDIO_I2S_SPK_GPIO_BCLK GPIO_NUM_15
#define AUDIO_I2S_SPK_GPIO_LRCK GPIO_NUM_16

#else

#define AUDIO_I2S_GPIO_WS GPIO_NUM_4
#define AUDIO_I2S_GPIO_BCLK GPIO_NUM_5
#define AUDIO_I2S_GPIO_DIN  GPIO_NUM_6
#define AUDIO_I2S_GPIO_DOUT GPIO_NUM_7

#endif


#define BUILTIN_LED_GPIO        GPIO_NUM_48
#define BOOT_BUTTON_GPIO        GPIO_NUM_0
#define TOUCH_BUTTON_GPIO       GPIO_NUM_47
#define VOLUME_UP_BUTTON_GPIO   GPIO_NUM_40
#define VOLUME_DOWN_BUTTON_GPIO GPIO_NUM_39

#define DISPLAY_SDA_PIN GPIO_NUM_41
#define DISPLAY_SCL_PIN GPIO_NUM_42
#define DISPLAY_WIDTH   128

#if CONFIG_OLED_SSD1306_128X32
#define DISPLAY_HEIGHT  32
#elif CONFIG_OLED_SSD1306_128X64
#define DISPLAY_HEIGHT  64
#elif CONFIG_OLED_SH1106_128X64
#define DISPLAY_HEIGHT  64
#define SH1106
#else
#error "未选择 OLED 屏幕类型"
#endif

#define DISPLAY_MIRROR_X true
#define DISPLAY_MIRROR_Y true

// 四舵机机器狗引脚定义
#define SERVO_LF_GPIO GPIO_NUM_9    // 左前腿
#define SERVO_RF_GPIO GPIO_NUM_10   // 右前腿  
#define SERVO_LB_GPIO GPIO_NUM_11   // 左后退
#define SERVO_RB_GPIO GPIO_NUM_12   // 右后退

// 舵机参数配置
#define SERVO_MIN_PULSEWIDTH_US 500           // 最小脉宽（微秒）对应0度
#define SERVO_MAX_PULSEWIDTH_US 2500          // 最大脉宽（微秒）对应180度
#define SERVO_MIN_DEGREE 0                    // 最小角度
#define SERVO_MAX_DEGREE 180                  // 最大角度
#define SERVO_TIMEBASE_RESOLUTION_HZ 1000000  // 1MHz, 1us per tick
#define SERVO_TIMEBASE_PERIOD 20000           // 20000 ticks, 20ms (50Hz)

// 舵机默认位置和限制
#define SERVO_DEFAULT_ANGLE 90                // 默认中心位置
#define SERVO_MAX_SPEED_DEGREE_PER_SEC 180    // 最大转速限制

// 板子版本信息
#define BREAD_COMPACT_WIFI_WITH_SERVO_VERSION "1.0.0"

#endif // _BOARD_CONFIG_H_

#ifndef __SERVO_CONTROLLER_H__
#define __SERVO_CONTROLLER_H__

#include <driver/ledc.h>
#include <driver/gpio.h>
#include <esp_log.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <freertos/queue.h>
#include <functional>
#include "config.h"
#include "mcp_server.h"

class ServoController {
public:
    ServoController(gpio_num_t servo_pin, ledc_channel_t ledc_channel = LEDC_CHANNEL_0, ledc_timer_t ledc_timer = LEDC_TIMER_0);
    ServoController(gpio_num_t servo_pin);
    ~ServoController();

    // 基本控制方法
    bool Initialize();
    void InitializeTools();  // 初始化MCP工具
    void SetAngle(int angle);
    int GetCurrentAngle() const { return current_angle_; }
    
    // 运动控制方法
    void RotateClockwise(int degrees);
    void RotateCounterclockwise(int degrees);
    void SweepBetween(int min_angle, int max_angle, int speed_ms = 1000);
    void Stop();
    void Reset(); // 回到中心位置（90度）
    
    // 状态查询
    bool IsMoving() const { return is_moving_; }
    bool IsSweeping() const { return is_sweeping_; }
    
    // 设置回调函数（当运动完成时调用）
    void SetOnMoveCompleteCallback(std::function<void()> callback) {
        on_move_complete_callback_ = callback;
    }

private:
    // 硬件相关
    gpio_num_t servo_pin_;
    ledc_channel_t ledc_channel_;
    ledc_timer_t ledc_timer_;
    
    // 状态变量
    int current_angle_;
    int target_angle_;
    bool is_moving_;
    bool is_sweeping_;
    bool stop_requested_;
    
    // 任务和队列
    TaskHandle_t servo_task_handle_;
    QueueHandle_t command_queue_;
    
    // 回调函数
    std::function<void()> on_move_complete_callback_;
    
    // 命令类型
    enum CommandType {
        CMD_SET_ANGLE,
        CMD_ROTATE_CW,
        CMD_ROTATE_CCW,
        CMD_SWEEP,
        CMD_STOP,
        CMD_RESET
    };
    
    // 命令结构
    struct ServoCommand {
        CommandType type;
        int param1;  // 角度或度数
        int param2;  // 最大角度（用于扫描）或速度
        int param3;  // 速度参数
    };
    
    // 私有方法
    void WriteAngle(int angle);
    uint32_t AngleToCompare(int angle);
    bool IsValidAngle(int angle) const;
    int ConstrainAngle(int angle) const;
    
    // 任务函数
    static void ServoTask(void* parameter);
    void ProcessCommands();
    void ExecuteSetAngle(int angle);
    void ExecuteRotate(int degrees, bool clockwise);
    void ExecuteSweep(int min_angle, int max_angle, int speed_ms);
    void SmoothMoveTo(int target_angle, int speed_ms = 500);
};

#endif // __SERVO_CONTROLLER_H__

#include "servo_controller.h"
#include <esp_log.h>
#include <cmath>

#define TAG "ServoController"

ServoController::ServoController(gpio_num_t servo_pin, ledc_channel_t ledc_channel, ledc_timer_t ledc_timer)
    : servo_pin_(servo_pin)
    , ledc_channel_(ledc_channel)
    , ledc_timer_(ledc_timer)
    , current_angle_(SERVO_DEFAULT_ANGLE)
    , target_angle_(SERVO_DEFAULT_ANGLE)
    , is_moving_(false)
    , is_sweeping_(false)
    , stop_requested_(false)
    , servo_task_handle_(nullptr)
    , command_queue_(nullptr)
    , on_move_complete_callback_(nullptr) {
}

// 原有的单参数构造函数调用新的三参数构造函数
ServoController::ServoController(gpio_num_t servo_pin)
    : ServoController(servo_pin, LEDC_CHANNEL_0, LEDC_TIMER_0) {
}

ServoController::~ServoController() {
    Stop();
    if (servo_task_handle_ != nullptr) {
        vTaskDelete(servo_task_handle_);
    }
    if (command_queue_ != nullptr) {
        vQueueDelete(command_queue_);
    }
}

bool ServoController::Initialize() {
    ESP_LOGI(TAG, "初始化SG90舵机控制器，引脚: %d", servo_pin_);
    
    // 配置LEDC定时器 (ESP32-S3最大支持14位分辨率)
    ledc_timer_config_t timer_config = {
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .duty_resolution = LEDC_TIMER_14_BIT,
        .timer_num = ledc_timer_,
        .freq_hz = 50, // 50Hz for servo
        .clk_cfg = LEDC_AUTO_CLK
    };
    
    esp_err_t ret = ledc_timer_config(&timer_config);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "LEDC定时器配置失败: %s", esp_err_to_name(ret));
        return false;
    }
    
    // 配置LEDC通道
    ledc_channel_config_t channel_config = {
        .gpio_num = servo_pin_,
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .channel = ledc_channel_,
        .intr_type = LEDC_INTR_DISABLE,
        .timer_sel = ledc_timer_,
        .duty = 0,
        .hpoint = 0
    };
    
    ret = ledc_channel_config(&channel_config);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "LEDC通道配置失败: %s", esp_err_to_name(ret));
        return false;
    }
    
    // 创建命令队列
    command_queue_ = xQueueCreate(10, sizeof(ServoCommand));
    if (command_queue_ == nullptr) {
        ESP_LOGE(TAG, "创建命令队列失败");
        return false;
    }
    
    // 创建舵机控制任务
    BaseType_t task_ret = xTaskCreate(
        ServoTask,
        "servo_task",
        4096,
        this,
        5,
        &servo_task_handle_
    );
    
    if (task_ret != pdPASS) {
        ESP_LOGE(TAG, "创建舵机任务失败");
        return false;
    }
    
    // 设置初始位置
    WriteAngle(current_angle_);
    
    ESP_LOGI(TAG, "SG90舵机控制器初始化成功");
    return true;
}

void ServoController::InitializeTools() {
    auto& mcp_server = McpServer::GetInstance();
    ESP_LOGI(TAG, "开始注册舵机MCP工具...");

    // 设置舵机角度
    mcp_server.AddTool("self.servo.set_angle",
                       "设置SG90舵机到指定角度。angle: 目标角度(0-180度)",
                       PropertyList({Property("angle", kPropertyTypeInteger, 90, 0, 180)}),
                       [this](const PropertyList& properties) -> ReturnValue {
                           int angle = properties["angle"].value<int>();
                           SetAngle(angle);
                           return "舵机设置到 " + std::to_string(angle) + " 度";
                       });

    // 顺时针旋转
    mcp_server.AddTool("self.servo.rotate_clockwise",
                       "顺时针旋转SG90舵机指定角度。degrees: 旋转角度(1-180度)",
                       PropertyList({Property("degrees", kPropertyTypeInteger, 30, 1, 180)}),
                       [this](const PropertyList& properties) -> ReturnValue {
                           int degrees = properties["degrees"].value<int>();
                           RotateClockwise(degrees);
                           return "舵机顺时针旋转 " + std::to_string(degrees) + " 度";
                       });

    // 逆时针旋转
    mcp_server.AddTool("self.servo.rotate_counterclockwise",
                       "逆时针旋转SG90舵机指定角度。degrees: 旋转角度(1-180度)",
                       PropertyList({Property("degrees", kPropertyTypeInteger, 30, 1, 180)}),
                       [this](const PropertyList& properties) -> ReturnValue {
                           int degrees = properties["degrees"].value<int>();
                           RotateCounterclockwise(degrees);
                           return "舵机逆时针旋转 " + std::to_string(degrees) + " 度";
                       });

    // 获取当前位置
    mcp_server.AddTool("self.servo.get_position",
                       "获取SG90舵机当前角度位置",
                       PropertyList(),
                       [this](const PropertyList& properties) -> ReturnValue {
                           int angle = GetCurrentAngle();
                           return "当前舵机角度: " + std::to_string(angle) + " 度";
                       });

    // 扫描模式
    mcp_server.AddTool("self.servo.sweep",
                       "SG90舵机扫描模式，在指定角度范围内来回摆动。"
                       "min_angle: 最小角度(0-179度); max_angle: 最大角度(1-180度); "
                       "speed: 摆动速度，毫秒(100-5000ms)",
                       PropertyList({Property("min_angle", kPropertyTypeInteger, 0, 0, 179),
                                     Property("max_angle", kPropertyTypeInteger, 180, 1, 180),
                                     Property("speed", kPropertyTypeInteger, 1000, 100, 5000)}),
                       [this](const PropertyList& properties) -> ReturnValue {
                           int min_angle = properties["min_angle"].value<int>();
                           int max_angle = properties["max_angle"].value<int>();
                           int speed = properties["speed"].value<int>();
                           SweepBetween(min_angle, max_angle, speed);
                           return "开始扫描模式: " + std::to_string(min_angle) + "° - " +
                                  std::to_string(max_angle) + "°";
                       });

    // 停止舵机
    mcp_server.AddTool("self.servo.stop",
                       "立即停止SG90舵机运动",
                       PropertyList(),
                       [this](const PropertyList& properties) -> ReturnValue {
                           Stop();
                           return "舵机已停止";
                       });

    // 复位到中心位置
    mcp_server.AddTool("self.servo.reset",
                       "将SG90舵机复位到中心位置(90度)",
                       PropertyList(),
                       [this](const PropertyList& properties) -> ReturnValue {
                           Reset();
                           return "舵机已复位到中心位置(90度)";
                       });

    // 获取舵机状态
    mcp_server.AddTool("self.servo.get_status",
                       "获取SG90舵机当前状态",
                       PropertyList(),
                       [this](const PropertyList& properties) -> ReturnValue {
                           int angle = GetCurrentAngle();
                           bool moving = IsMoving();
                           bool sweeping = IsSweeping();

                           std::string status = "{\"angle\":" + std::to_string(angle) +
                                              ",\"moving\":" + (moving ? "true" : "false") +
                                              ",\"sweeping\":" + (sweeping ? "true" : "false") + "}";
                           return status;
                       });

    ESP_LOGI(TAG, "舵机MCP工具注册完成");
}

void ServoController::SetAngle(int angle) {
    if (!IsValidAngle(angle)) {
        ESP_LOGW(TAG, "无效角度: %d，将限制在有效范围内", angle);
        angle = ConstrainAngle(angle);
    }
    
    ServoCommand cmd = {CMD_SET_ANGLE, angle, 0, 0};
    xQueueSend(command_queue_, &cmd, portMAX_DELAY);
}

void ServoController::RotateClockwise(int degrees) {
    if (degrees <= 0) {
        ESP_LOGW(TAG, "旋转角度必须大于0");
        return;
    }
    
    ServoCommand cmd = {CMD_ROTATE_CW, degrees, 0, 0};
    xQueueSend(command_queue_, &cmd, portMAX_DELAY);
}

void ServoController::RotateCounterclockwise(int degrees) {
    if (degrees <= 0) {
        ESP_LOGW(TAG, "旋转角度必须大于0");
        return;
    }
    
    ServoCommand cmd = {CMD_ROTATE_CCW, degrees, 0, 0};
    xQueueSend(command_queue_, &cmd, portMAX_DELAY);
}

void ServoController::SweepBetween(int min_angle, int max_angle, int speed_ms) {
    if (!IsValidAngle(min_angle) || !IsValidAngle(max_angle)) {
        ESP_LOGW(TAG, "扫描角度范围无效: %d - %d", min_angle, max_angle);
        return;
    }
    
    if (min_angle >= max_angle) {
        ESP_LOGW(TAG, "最小角度必须小于最大角度");
        return;
    }
    
    ServoCommand cmd = {CMD_SWEEP, min_angle, max_angle, speed_ms};
    xQueueSend(command_queue_, &cmd, portMAX_DELAY);
}

void ServoController::Stop() {
    stop_requested_ = true;
    ServoCommand cmd = {CMD_STOP, 0, 0, 0};
    xQueueSend(command_queue_, &cmd, 0); // 不等待，立即发送停止命令
}

void ServoController::Reset() {
    ServoCommand cmd = {CMD_RESET, SERVO_DEFAULT_ANGLE, 0, 0};
    xQueueSend(command_queue_, &cmd, portMAX_DELAY);
}

void ServoController::WriteAngle(int angle) {
    angle = ConstrainAngle(angle);
    uint32_t compare_value = AngleToCompare(angle);
    ledc_set_duty(LEDC_LOW_SPEED_MODE, ledc_channel_, compare_value);
    ledc_update_duty(LEDC_LOW_SPEED_MODE, ledc_channel_);
    current_angle_ = angle;
}

uint32_t ServoController::AngleToCompare(int angle) {
    // 将角度转换为PWM占空比
    // SG90: 0.5ms-2.5ms 对应 0-180度
    // 50Hz周期 = 20ms
    // 占空比 = (脉宽 / 周期) * 2^14 (ESP32-S3使用14位分辨率)

    float pulse_width_ms = 0.5f + (angle / 180.0f) * 2.0f; // 0.5ms to 2.5ms
    float duty_cycle = pulse_width_ms / 20.0f; // 20ms period
    uint32_t compare_value = (uint32_t)(duty_cycle * 16383); // 14-bit resolution (2^14 - 1)

    return compare_value;
}

bool ServoController::IsValidAngle(int angle) const {
    return angle >= SERVO_MIN_DEGREE && angle <= SERVO_MAX_DEGREE;
}

int ServoController::ConstrainAngle(int angle) const {
    if (angle < SERVO_MIN_DEGREE) return SERVO_MIN_DEGREE;
    if (angle > SERVO_MAX_DEGREE) return SERVO_MAX_DEGREE;
    return angle;
}

void ServoController::ServoTask(void* parameter) {
    ServoController* controller = static_cast<ServoController*>(parameter);
    controller->ProcessCommands();
}

void ServoController::ProcessCommands() {
    ServoCommand cmd;
    
    while (true) {
        if (xQueueReceive(command_queue_, &cmd, pdMS_TO_TICKS(100)) == pdTRUE) {
            if (stop_requested_ && cmd.type != CMD_STOP) {
                continue; // 忽略非停止命令
            }
            
            switch (cmd.type) {
                case CMD_SET_ANGLE:
                    ExecuteSetAngle(cmd.param1);
                    break;
                    
                case CMD_ROTATE_CW:
                    ExecuteRotate(cmd.param1, true);
                    break;
                    
                case CMD_ROTATE_CCW:
                    ExecuteRotate(cmd.param1, false);
                    break;
                    
                case CMD_SWEEP:
                    ExecuteSweep(cmd.param1, cmd.param2, cmd.param3);
                    break;
                    
                case CMD_STOP:
                    is_moving_ = false;
                    is_sweeping_ = false;
                    stop_requested_ = false;
                    ESP_LOGI(TAG, "舵机停止");
                    break;
                    
                case CMD_RESET:
                    ExecuteSetAngle(cmd.param1);
                    break;
            }
        }
    }
}

void ServoController::ExecuteSetAngle(int angle) {
    ESP_LOGI(TAG, "设置舵机角度: %d度", angle);
    is_moving_ = true;
    SmoothMoveTo(angle, 500);
    is_moving_ = false;

    if (on_move_complete_callback_) {
        on_move_complete_callback_();
    }
}

void ServoController::ExecuteRotate(int degrees, bool clockwise) {
    int target = current_angle_ + (clockwise ? degrees : -degrees);
    target = ConstrainAngle(target);

    ESP_LOGI(TAG, "%s旋转 %d度，从 %d度 到 %d度",
             clockwise ? "顺时针" : "逆时针", degrees, current_angle_, target);

    is_moving_ = true;
    SmoothMoveTo(target, 500);
    is_moving_ = false;

    if (on_move_complete_callback_) {
        on_move_complete_callback_();
    }
}

void ServoController::ExecuteSweep(int min_angle, int max_angle, int speed_ms) {
    ESP_LOGI(TAG, "开始扫描模式: %d度 - %d度，速度: %dms", min_angle, max_angle, speed_ms);

    is_sweeping_ = true;
    is_moving_ = true;

    bool direction = true; // true = 向最大角度，false = 向最小角度

    while (is_sweeping_ && !stop_requested_) {
        int target = direction ? max_angle : min_angle;
        SmoothMoveTo(target, speed_ms);

        if (stop_requested_) break;

        direction = !direction;
        vTaskDelay(pdMS_TO_TICKS(100)); // 短暂停顿
    }

    is_sweeping_ = false;
    is_moving_ = false;

    ESP_LOGI(TAG, "扫描模式结束");

    if (on_move_complete_callback_) {
        on_move_complete_callback_();
    }
}

void ServoController::SmoothMoveTo(int target_angle, int speed_ms) {
    target_angle = ConstrainAngle(target_angle);

    if (target_angle == current_angle_) {
        return; // 已经在目标位置
    }

    int start_angle = current_angle_;
    int angle_diff = target_angle - start_angle;
    int steps = abs(angle_diff);

    if (steps == 0) return;

    int delay_per_step = speed_ms / steps;
    if (delay_per_step < 10) delay_per_step = 10; // 最小延迟

    for (int i = 1; i <= steps && !stop_requested_; i++) {
        int current_step_angle = start_angle + (angle_diff * i) / steps;
        WriteAngle(current_step_angle);
        vTaskDelay(pdMS_TO_TICKS(delay_per_step));
    }

    // 确保到达精确位置
    if (!stop_requested_) {
        WriteAngle(target_angle);
    }
}

#ifndef __DOG_CONTROLLER_H__
#define __DOG_CONTROLLER_H__

#include "servo_controller.h"
#include "config.h"
#include <vector>
#include <map>

// 腿的类型定义
enum LegType {
    LEG_LEFT_FRONT = 0,
    LEG_RIGHT_FRONT = 1,
    LEG_LEFT_BACK = 2,
    LEG_RIGHT_BACK = 3
};

// 动作类型定义
enum DogAction {
    ACTION_STAND = 0,
    ACTION_SIT,
    ACTION_LIE_DOWN,
    ACTION_STRETCH,
    ACTION_WAVE,
    ACTION_WALK_FORWARD,
    ACTION_WALK_BACKWARD,  // 新增：后退
    ACTION_SHAKE_BODY,     // 新增：摇摆身体
    ACTION_DANCE,          // 新增：跳舞
    ACTION_BOW,            // 新增：鞠躬
    ACTION_SHAKE_HEAD      // 新增：摇头
};

class DogController {
public:
    DogController();
    ~DogController();

    bool Initialize();
    void InitializeTools();
    
    // 基本动作控制
    void Stand();
    void Sit();
    void LieDown();
    void Stretch();
    void Wave();
    void WalkForward(int steps = 1);
    // 新增动作方法
    void WalkBackward(int steps = 1);
    void ShakeBody();
    void Dance();
    void Bow();
    void ShakeHead();
    
    // 停止所有动作
    void StopAll();
    
    // 状态查询
    bool IsMoving() const;
    DogAction GetCurrentAction() const { return current_action_; }

private:
    std::vector<ServoController*> servos_;
    DogAction current_action_;
    bool is_moving_;
    
    // 舵机校准参数（根据您的描述调整）
    struct LegCalibration {
        int stand_angle;    // 站立角度
        int sit_angle;      // 坐下角度  
        int lie_angle;      // 趴下角度
        int forward_offset; // 向前摆动的偏移量
    };
    
    std::map<LegType, LegCalibration> leg_calibration_;
    
    // 初始化舵机校准参数
    void InitializeCalibration();
    
    // 设置单个舵机角度（带校准）
    void SetLegAngle(LegType leg, int angle, int speed_ms = 500);
    
    // 设置所有舵机角度
    void SetAllLegs(int lf_angle, int rf_angle, int lb_angle, int rb_angle, int speed_ms = 500);
    
    // 动作执行函数
    void ExecuteStand();
    void ExecuteSit();
    void ExecuteLieDown();
    void ExecuteStretch();
    void ExecuteWave();
    void ExecuteWalkForward(int steps);
    // 新增动作执行函数
    void ExecuteWalkBackward(int steps);
    void ExecuteShakeBody();
    void ExecuteDance();
    void ExecuteBow();
    void ExecuteShakeHead();
    
    // 检查所有舵机是否就绪
    bool AllServosReady() const;
};

#endif // __DOG_CONTROLLER_H__

#include "dog_controller.h"
#include "mcp_server.h"
#include <esp_log.h>

#define TAG "DogController"

DogController::DogController() 
    : current_action_(ACTION_STAND)
    , is_moving_(false) {
    
    // 初始化四个舵机控制器，使用不同的LEDC通道
    servos_.push_back(new ServoController(SERVO_LF_GPIO, LEDC_CHANNEL_0, LEDC_TIMER_0));
    servos_.push_back(new ServoController(SERVO_RF_GPIO, LEDC_CHANNEL_1, LEDC_TIMER_0));  
    servos_.push_back(new ServoController(SERVO_LB_GPIO, LEDC_CHANNEL_2, LEDC_TIMER_0));
    servos_.push_back(new ServoController(SERVO_RB_GPIO, LEDC_CHANNEL_3, LEDC_TIMER_0));
    
    InitializeCalibration();
}

DogController::~DogController() {
    StopAll();  
    for (auto servo : servos_) {
        if (servo != nullptr) {
            delete servo;
        }
    }
    servos_.clear();
}

bool DogController::Initialize() {
    ESP_LOGI(TAG, "初始化四舵机机器狗控制器");
    
    // 初始化所有舵机
    for (int i = 0; i < servos_.size(); i++) {
        if (servos_[i] != nullptr && !servos_[i]->Initialize()) {
            ESP_LOGE(TAG, "舵机 %d 初始化失败", i);
            return false;
        }
    }
    
    // 设置初始站立位置
    vTaskDelay(pdMS_TO_TICKS(1000)); // 等待所有舵机就绪
    Stand();
    
    ESP_LOGI(TAG, "机器狗控制器初始化成功");
    return true;
}

void DogController::InitializeCalibration() {
    // 根据您的描述：左侧腿舵机小角度向前摆，右侧腿大角度向前摆
    // 这里需要根据实际机械结构调整这些参数
    
    // 左前腿校准
    leg_calibration_[LEG_LEFT_FRONT] = {
        .stand_angle = 90,    // 站立中间位置
        .sit_angle = 60,      // 坐下角度
        .lie_angle = 20,      // 趴下角度
        .forward_offset = -30 // 向前摆动偏移（小角度）
    };
    
    // 右前腿校准  
    leg_calibration_[LEG_RIGHT_FRONT] = {
        .stand_angle = 90,
        .sit_angle = 120,     // 对称角度
        .lie_angle = 160,     // 对称角度
        .forward_offset = 30  // 向前摆动偏移（大角度）
    };
    
    // 左后腿校准
    leg_calibration_[LEG_LEFT_BACK] = {
        .stand_angle = 90,
        .sit_angle = 45,     // 对称角度
        .lie_angle = 30,     // 对称角度
        .forward_offset = -30 // 向前摆动偏移（小角度）
    };
    
    // 右后腿校准
    leg_calibration_[LEG_RIGHT_BACK] = {
        .stand_angle = 90,
        .sit_angle = 135,      // 对称角度
        .lie_angle = 150,      // 对称角度
        .forward_offset = 30  // 向前摆动偏移（大角度）
    };
}

void DogController::Stand() {
    if (is_moving_) return;
    
    ESP_LOGI(TAG, "执行起立站立动作");
    current_action_ = ACTION_STAND;
    is_moving_ = true;
    
    ExecuteStand();
    is_moving_ = false;
}

void DogController::Sit() {
    if (is_moving_) return;
    
    ESP_LOGI(TAG, "执行坐下动作");
    current_action_ = ACTION_SIT;
    is_moving_ = true;
    
    ExecuteSit();
    is_moving_ = false;
}

void DogController::LieDown() {
    if (is_moving_) return;
    
    ESP_LOGI(TAG, "执行趴下动作");
    current_action_ = ACTION_LIE_DOWN;
    is_moving_ = true;
    
    ExecuteLieDown();
    is_moving_ = false;
}

void DogController::Stretch() {
    if (is_moving_) return;
    
    ESP_LOGI(TAG, "执行伸懒腰动作");
    current_action_ = ACTION_STRETCH;
    is_moving_ = true;
    
    ExecuteStretch();
    is_moving_ = false;
}

void DogController::Wave() {
    if (is_moving_) return;
    
    ESP_LOGI(TAG, "执行挥手招手打招呼动作");
    current_action_ = ACTION_WAVE;
    is_moving_ = true;
    
    ExecuteWave();
    is_moving_ = false;
}

void DogController::WalkForward(int steps) {
    if (is_moving_) return;
    
    ESP_LOGI(TAG, "执行前进动作，步数: %d", steps);
    current_action_ = ACTION_WALK_FORWARD;
    is_moving_ = true;
    
    ExecuteWalkForward(steps);
    is_moving_ = false;
}

void DogController::WalkBackward(int steps) {
    if (is_moving_) return;
    
    ESP_LOGI(TAG, "执行后退动作，步数: %d", steps);
    current_action_ = ACTION_WALK_BACKWARD;
    is_moving_ = true;
    
    ExecuteWalkBackward(steps);
    is_moving_ = false;
}

void DogController::ShakeBody() {
    if (is_moving_) return;
    
    ESP_LOGI(TAG, "执行摇摆身体动作");
    current_action_ = ACTION_SHAKE_BODY;
    is_moving_ = true;
    
    ExecuteShakeBody();
    is_moving_ = false;
}

void DogController::Dance() {
    if (is_moving_) return;
    
    ESP_LOGI(TAG, "执行跳舞动作");
    current_action_ = ACTION_DANCE;
    is_moving_ = true;
    
    ExecuteDance();
    is_moving_ = false;
}

void DogController::Bow() {
    if (is_moving_) return;
    
    ESP_LOGI(TAG, "执行鞠躬动作");
    current_action_ = ACTION_BOW;
    is_moving_ = true;
    
    ExecuteBow();
    is_moving_ = false;
}

void DogController::ShakeHead() {
    if (is_moving_) return;
    
    ESP_LOGI(TAG, "执行摇头动作");
    current_action_ = ACTION_SHAKE_HEAD;
    is_moving_ = true;
    
    ExecuteShakeHead();
    is_moving_ = false;
}

void DogController::StopAll() {
    ESP_LOGI(TAG, "停止所有动作");
    for (auto servo : servos_) {
        if (servo != nullptr) {
            servo->Stop();
        }
    }
    is_moving_ = false;
}

bool DogController::IsMoving() const {
    return is_moving_;
}

void DogController::SetLegAngle(LegType leg, int angle, int speed_ms) {
    if (leg < servos_.size() && servos_[leg] != nullptr) {
        servos_[leg]->SetAngle(angle);
        // 注意：这里需要等待动作完成，可以使用回调或延时
        vTaskDelay(pdMS_TO_TICKS(speed_ms));
    }
}

void DogController::SetAllLegs(int lf_angle, int rf_angle, int lb_angle, int rb_angle, int speed_ms) {
    if (servos_[LEG_LEFT_FRONT] != nullptr) {
        servos_[LEG_LEFT_FRONT]->SetAngle(lf_angle);
    }
    if (servos_[LEG_RIGHT_FRONT] != nullptr) {
        servos_[LEG_RIGHT_FRONT]->SetAngle(rf_angle);
    }
    if (servos_[LEG_LEFT_BACK] != nullptr) {
        servos_[LEG_LEFT_BACK]->SetAngle(lb_angle);
    }
    if (servos_[LEG_RIGHT_BACK] != nullptr) {
        servos_[LEG_RIGHT_BACK]->SetAngle(rb_angle);
    }
    
    vTaskDelay(pdMS_TO_TICKS(speed_ms));
}

bool DogController::AllServosReady() const {
    for (auto servo : servos_) {
        if (servo != nullptr && servo->IsMoving()) {
            return false;
        }
    }
    return true;
}

void DogController::ExecuteStand() {
    // 站立姿势：所有腿在中间位置
    SetAllLegs(
        leg_calibration_[LEG_LEFT_FRONT].stand_angle ,
        leg_calibration_[LEG_RIGHT_FRONT].stand_angle ,
        leg_calibration_[LEG_LEFT_BACK].stand_angle ,
        leg_calibration_[LEG_RIGHT_BACK].stand_angle ,
        400
    );
}

void DogController::ExecuteSit() {
    // 坐下姿势：前腿稍微弯曲，后腿大幅度弯曲
    SetAllLegs(
        leg_calibration_[LEG_LEFT_FRONT].stand_angle,
        leg_calibration_[LEG_RIGHT_FRONT].stand_angle,
        leg_calibration_[LEG_LEFT_BACK].sit_angle,
        leg_calibration_[LEG_RIGHT_BACK].sit_angle,
        400
    );
}

void DogController::ExecuteLieDown() {
    // 趴下姿势：所有腿都贴近身体
    SetAllLegs(
        leg_calibration_[LEG_LEFT_FRONT].lie_angle,
        leg_calibration_[LEG_RIGHT_FRONT].lie_angle,
        leg_calibration_[LEG_LEFT_BACK].lie_angle,
        leg_calibration_[LEG_RIGHT_BACK].lie_angle,
        600
    );
}

void DogController::ExecuteStretch() {
    // 伸懒腰：前腿向前伸展，后腿向后伸展
    SetAllLegs(
        leg_calibration_[LEG_LEFT_FRONT].stand_angle - 60 ,
        leg_calibration_[LEG_RIGHT_FRONT].stand_angle + 60,
        leg_calibration_[LEG_LEFT_BACK].stand_angle + 60,
        leg_calibration_[LEG_RIGHT_BACK].stand_angle - 60,
        400
    );
    
    vTaskDelay(pdMS_TO_TICKS(1000));
    
    // 回到站立姿势
    ExecuteStand();
}

void DogController::ExecuteWave() {
    // 回到坐姿
    ExecuteSit();

    // 挥手：右前腿 waving 动作
    for (int i = 0; i < 3; i++) {
        SetLegAngle(LEG_RIGHT_FRONT, 
                   leg_calibration_[LEG_RIGHT_FRONT].stand_angle + 90, 150);
        vTaskDelay(pdMS_TO_TICKS(150));
        SetLegAngle(LEG_RIGHT_FRONT, 
                   leg_calibration_[LEG_RIGHT_FRONT].stand_angle + 60, 150);
        vTaskDelay(pdMS_TO_TICKS(150));
    }
    
    // 回到站立姿势
    ExecuteStand();
    
}

void DogController::ExecuteWalkForward(int steps) {
    for (int step = 0; step < steps ; step++) {
        // 步态1：对角线腿抬起
        SetAllLegs(
            leg_calibration_[LEG_LEFT_FRONT].stand_angle + leg_calibration_[LEG_LEFT_FRONT].forward_offset,
            leg_calibration_[LEG_RIGHT_FRONT].stand_angle ,
            leg_calibration_[LEG_LEFT_BACK].stand_angle ,
            leg_calibration_[LEG_RIGHT_BACK].stand_angle + leg_calibration_[LEG_RIGHT_BACK].forward_offset,
            300
        );
        
        vTaskDelay(pdMS_TO_TICKS(100));
        
        // 步态2：另一对角线腿抬起
        SetAllLegs(
            leg_calibration_[LEG_LEFT_FRONT].stand_angle ,
            leg_calibration_[LEG_RIGHT_FRONT].stand_angle - leg_calibration_[LEG_RIGHT_FRONT].forward_offset,
            leg_calibration_[LEG_LEFT_BACK].stand_angle - leg_calibration_[LEG_LEFT_BACK].forward_offset,
            leg_calibration_[LEG_RIGHT_BACK].stand_angle ,
            300
        );
        
        vTaskDelay(pdMS_TO_TICKS(100));

        // 步态3：对角线腿抬起
        SetAllLegs(
            leg_calibration_[LEG_LEFT_FRONT].stand_angle ,
            leg_calibration_[LEG_RIGHT_FRONT].stand_angle + leg_calibration_[LEG_RIGHT_FRONT].forward_offset,
            leg_calibration_[LEG_LEFT_BACK].stand_angle + leg_calibration_[LEG_LEFT_BACK].forward_offset,
            leg_calibration_[LEG_RIGHT_BACK].stand_angle ,
            300
        );
        
        vTaskDelay(pdMS_TO_TICKS(100));
        
        // 步态4：另一对角线腿抬起
        SetAllLegs(
            leg_calibration_[LEG_LEFT_FRONT].stand_angle - leg_calibration_[LEG_LEFT_FRONT].forward_offset,
            leg_calibration_[LEG_RIGHT_FRONT].stand_angle ,
            leg_calibration_[LEG_LEFT_BACK].stand_angle ,
            leg_calibration_[LEG_RIGHT_BACK].stand_angle - leg_calibration_[LEG_RIGHT_BACK].forward_offset,
            300
        );
        
        vTaskDelay(pdMS_TO_TICKS(100));
    }
    
    // 回到站立姿势
    ExecuteStand();
}

void DogController::ExecuteWalkBackward(int steps) {
    for (int step = 0; step < steps ; step++) {
        // 后退步态1：与前进相反的对角线运动
        SetAllLegs(
            leg_calibration_[LEG_LEFT_FRONT].stand_angle - leg_calibration_[LEG_LEFT_FRONT].forward_offset,
            leg_calibration_[LEG_RIGHT_FRONT].stand_angle,
            leg_calibration_[LEG_LEFT_BACK].stand_angle,
            leg_calibration_[LEG_RIGHT_BACK].stand_angle - leg_calibration_[LEG_RIGHT_BACK].forward_offset,
            300
        );
        
        vTaskDelay(pdMS_TO_TICKS(100));
        
        // 后退步态2：另一对角线
        SetAllLegs(
            leg_calibration_[LEG_LEFT_FRONT].stand_angle,
            leg_calibration_[LEG_RIGHT_FRONT].stand_angle + leg_calibration_[LEG_RIGHT_FRONT].forward_offset,
            leg_calibration_[LEG_LEFT_BACK].stand_angle + leg_calibration_[LEG_LEFT_BACK].forward_offset,
            leg_calibration_[LEG_RIGHT_BACK].stand_angle,
            300
        );
        
        vTaskDelay(pdMS_TO_TICKS(100));

        // 后退步态3
        SetAllLegs(
            leg_calibration_[LEG_LEFT_FRONT].stand_angle,
            leg_calibration_[LEG_RIGHT_FRONT].stand_angle - leg_calibration_[LEG_RIGHT_FRONT].forward_offset,
            leg_calibration_[LEG_LEFT_BACK].stand_angle - leg_calibration_[LEG_LEFT_BACK].forward_offset,
            leg_calibration_[LEG_RIGHT_BACK].stand_angle,
            300
        );
        
        vTaskDelay(pdMS_TO_TICKS(100));
        
        // 后退步态4
        SetAllLegs(
            leg_calibration_[LEG_LEFT_FRONT].stand_angle + leg_calibration_[LEG_LEFT_FRONT].forward_offset,
            leg_calibration_[LEG_RIGHT_FRONT].stand_angle,
            leg_calibration_[LEG_LEFT_BACK].stand_angle,
            leg_calibration_[LEG_RIGHT_BACK].stand_angle + leg_calibration_[LEG_RIGHT_BACK].forward_offset,
            300
        );
        
        vTaskDelay(pdMS_TO_TICKS(100));
    }
    
    // 回到站立姿势
    ExecuteStand();
}

void DogController::ExecuteShakeBody() {
    // 身体左右摇摆：左右腿交替倾斜
    for (int i = 0; i < 4; i++) {
        // 向左倾斜
        SetAllLegs(
            leg_calibration_[LEG_LEFT_FRONT].stand_angle - 20,
            leg_calibration_[LEG_RIGHT_FRONT].stand_angle + 20,
            leg_calibration_[LEG_LEFT_BACK].stand_angle - 20,
            leg_calibration_[LEG_RIGHT_BACK].stand_angle + 20,
            200
        );
        
        vTaskDelay(pdMS_TO_TICKS(100));
        
        // 向右倾斜
        SetAllLegs(
            leg_calibration_[LEG_LEFT_FRONT].stand_angle + 20,
            leg_calibration_[LEG_RIGHT_FRONT].stand_angle - 20,
            leg_calibration_[LEG_LEFT_BACK].stand_angle + 20,
            leg_calibration_[LEG_RIGHT_BACK].stand_angle - 20,
            200
        );
        
        vTaskDelay(pdMS_TO_TICKS(100));
    }
    
    // 回到站立姿势
    ExecuteStand();
}

void DogController::ExecuteDance() {
    // 跳舞动作：一系列有趣的动作组合
    
    // 1. 抬起前腿
    SetAllLegs(
        leg_calibration_[LEG_LEFT_FRONT].stand_angle + 60,
        leg_calibration_[LEG_RIGHT_FRONT].stand_angle + 60,
        leg_calibration_[LEG_LEFT_BACK].stand_angle,
        leg_calibration_[LEG_RIGHT_BACK].stand_angle,
        200
    );
    vTaskDelay(pdMS_TO_TICKS(100));
    
    // 2. 放下前腿，抬起后腿
    SetAllLegs(
        leg_calibration_[LEG_LEFT_FRONT].stand_angle,
        leg_calibration_[LEG_RIGHT_FRONT].stand_angle,
        leg_calibration_[LEG_LEFT_BACK].stand_angle - 60,
        leg_calibration_[LEG_RIGHT_BACK].stand_angle - 60,
        200
    );
    vTaskDelay(pdMS_TO_TICKS(100));
    
    // 3. 快速左右摇摆
    for (int i = 0; i < 3; i++) {
        SetAllLegs(
            leg_calibration_[LEG_LEFT_FRONT].stand_angle - 30,
            leg_calibration_[LEG_RIGHT_FRONT].stand_angle + 30,
            leg_calibration_[LEG_LEFT_BACK].stand_angle - 30,
            leg_calibration_[LEG_RIGHT_BACK].stand_angle + 30,
            200
        );
        vTaskDelay(pdMS_TO_TICKS(100));
        
        SetAllLegs(
            leg_calibration_[LEG_LEFT_FRONT].stand_angle + 30,
            leg_calibration_[LEG_RIGHT_FRONT].stand_angle - 30,
            leg_calibration_[LEG_LEFT_BACK].stand_angle + 30,
            leg_calibration_[LEG_RIGHT_BACK].stand_angle - 30,
            200
        );
        vTaskDelay(pdMS_TO_TICKS(100));
    }
    
    // 回到站立姿势
    ExecuteStand();
}

void DogController::ExecuteBow() {
    // 鞠躬动作：前腿弯曲，身体前倾
    SetAllLegs(
        leg_calibration_[LEG_LEFT_FRONT].sit_angle,
        leg_calibration_[LEG_RIGHT_FRONT].sit_angle,
        leg_calibration_[LEG_LEFT_BACK].stand_angle,
        leg_calibration_[LEG_RIGHT_BACK].stand_angle,
        500
    );
    
    vTaskDelay(pdMS_TO_TICKS(1000));
    
    // 回到站立姿势
    ExecuteStand();
}

void DogController::ExecuteShakeHead() {
    // 摇头动作：通过前腿的交替运动模拟摇头
    for (int i = 0; i < 3; i++) {
        SetAllLegs(
            leg_calibration_[LEG_LEFT_FRONT].stand_angle - 40,
            leg_calibration_[LEG_RIGHT_FRONT].stand_angle + 40,
            leg_calibration_[LEG_LEFT_BACK].stand_angle,
            leg_calibration_[LEG_RIGHT_BACK].stand_angle,
            200
        );
        vTaskDelay(pdMS_TO_TICKS(100));
        
        SetAllLegs(
            leg_calibration_[LEG_LEFT_FRONT].stand_angle + 40,
            leg_calibration_[LEG_RIGHT_FRONT].stand_angle - 40,
            leg_calibration_[LEG_LEFT_BACK].stand_angle,
            leg_calibration_[LEG_RIGHT_BACK].stand_angle,
            200
        );
        vTaskDelay(pdMS_TO_TICKS(100));
    }
    
    // 回到站立姿势
    ExecuteStand();
}

void DogController::InitializeTools() {
    auto& mcp_server = McpServer::GetInstance();
    ESP_LOGI(TAG, "开始注册机器狗MCP工具...");

    // 站立
    mcp_server.AddTool("self.dog.stand",
                       "机器狗站立姿势",
                       PropertyList(),
                       [this](const PropertyList& properties) -> ReturnValue {
                           Stand();
                           return "机器狗已站立";
                       });

    // 坐下
    mcp_server.AddTool("self.dog.sit",
                       "机器狗坐下",
                       PropertyList(),
                       [this](const PropertyList& properties) -> ReturnValue {
                           Sit();
                           return "机器狗已坐下";
                       });

    // 趴下
    mcp_server.AddTool("self.dog.lie_down",
                       "机器狗趴下",
                       PropertyList(),
                       [this](const PropertyList& properties) -> ReturnValue {
                           LieDown();
                           return "机器狗已趴下";
                       });

    // 伸懒腰
    mcp_server.AddTool("self.dog.stretch",
                       "机器狗伸懒腰",
                       PropertyList(),
                       [this](const PropertyList& properties) -> ReturnValue {
                           Stretch();
                           return "机器狗在伸懒腰";
                       });

    // 挥手打招呼
    mcp_server.AddTool("self.dog.wave",
                       "机器狗挥手打招呼",
                       PropertyList(),
                       [this](const PropertyList& properties) -> ReturnValue {
                           Wave();
                           return "机器狗在挥手打招呼";
                       });

    // 前进
    mcp_server.AddTool("self.dog.walk_forward",
                       "机器狗前进。steps: 步数(1-10)",
                       PropertyList({Property("steps", kPropertyTypeInteger, 8, 1, 10)}),
                       [this](const PropertyList& properties) -> ReturnValue {
                           int steps = properties["steps"].value<int>();
                           WalkForward(steps);
                           return "机器狗前进 " + std::to_string(steps) + " 步";
                       });

    // 后退
    mcp_server.AddTool("self.dog.walk_backward",
                       "机器狗后退。steps: 步数(1-10)",
                       PropertyList({Property("steps", kPropertyTypeInteger, 8, 1, 10)}),
                       [this](const PropertyList& properties) -> ReturnValue {
                           int steps = properties["steps"].value<int>();
                           WalkBackward(steps);
                           return "机器狗后退 " + std::to_string(steps) + " 步";
                       });

    // 摇摆身体
    mcp_server.AddTool("self.dog.shake_body",
                       "机器狗摇摆身体",
                       PropertyList(),
                       [this](const PropertyList& properties) -> ReturnValue {
                           ShakeBody();
                           return "机器狗在摇摆身体";
                       });

    // 跳舞
    mcp_server.AddTool("self.dog.dance",
                       "机器狗跳舞",
                       PropertyList(),
                       [this](const PropertyList& properties) -> ReturnValue {
                           Dance();
                           return "机器狗在跳舞";
                       });

    // 鞠躬
    mcp_server.AddTool("self.dog.bow",
                       "机器狗鞠躬",
                       PropertyList(),
                       [this](const PropertyList& properties) -> ReturnValue {
                           Bow();
                           return "机器狗在鞠躬";
                       });

    // 摇头
    mcp_server.AddTool("self.dog.shake_head",
                       "机器狗摇头",
                       PropertyList(),
                       [this](const PropertyList& properties) -> ReturnValue {
                           ShakeHead();
                           return "机器狗在摇头";
                       });

    // 停止所有动作
    mcp_server.AddTool("self.dog.stop",
                       "停止机器狗所有动作",
                       PropertyList(),
                       [this](const PropertyList& properties) -> ReturnValue {
                           StopAll();
                           return "机器狗已停止";
                       });

    // 获取状态
    mcp_server.AddTool("self.dog.get_status",
                       "获取机器狗当前状态",
                       PropertyList(),
                       [this](const PropertyList& properties) -> ReturnValue {
                           std::string action_name;
                           switch (GetCurrentAction()) {
                               case ACTION_STAND: action_name = "站立"; break;
                               case ACTION_SIT: action_name = "坐下"; break;
                               case ACTION_LIE_DOWN: action_name = "趴下"; break;
                               case ACTION_STRETCH: action_name = "伸懒腰"; break;
                               case ACTION_WAVE: action_name = "挥手"; break;
                               case ACTION_WALK_FORWARD: action_name = "前进"; break;
                               case ACTION_WALK_BACKWARD: action_name = "后退"; break;
                               case ACTION_SHAKE_BODY: action_name = "摇摆身体"; break;
                               case ACTION_DANCE: action_name = "跳舞"; break;
                               case ACTION_BOW: action_name = "鞠躬"; break;
                               case ACTION_SHAKE_HEAD: action_name = "摇头"; break;
                               default: action_name = "未知";
                           }
                           
                           return "当前状态: " + action_name + 
                                  ", 是否运动中: " + (IsMoving() ? "是" : "否");
                       });
                       
    ESP_LOGI(TAG, "机器狗MCP工具注册完成");
}

#include "wifi_board.h"
#include "codecs/no_audio_codec.h"
#include "display/oled_display.h"
#include "system_reset.h"
#include "application.h"
#include "button.h"
#include "config.h"
#include "mcp_server.h"
#include "servo_controller.h"
#include "led/single_led.h"
#include "assets/lang_config.h"

#include <wifi_station.h>
#include <esp_log.h>
#include <driver/i2c_master.h>
#include <esp_lcd_panel_ops.h>
#include <esp_lcd_panel_vendor.h>
#include "dog_controller.h"

#ifdef SH1106
#include <esp_lcd_panel_sh1106.h>
#endif

#define TAG "BreadCompactWifiWithServo"

LV_FONT_DECLARE(font_puhui_14_1);
LV_FONT_DECLARE(font_awesome_14_1);

class BreadCompactWifiWithServoBoard : public WifiBoard {
private:
    i2c_master_bus_handle_t display_i2c_bus_;
    esp_lcd_panel_io_handle_t panel_io_ = nullptr;
    esp_lcd_panel_handle_t panel_ = nullptr;
    Display *display_ = nullptr;
    Button boot_button_;
    Button touch_button_;
    Button volume_up_button_;
    Button volume_down_button_;
    DogController *dog_controller_;

    void InitializeDisplayI2c() {
        i2c_master_bus_config_t bus_config = {
            .i2c_port = (i2c_port_t)0,
            .sda_io_num = DISPLAY_SDA_PIN,
            .scl_io_num = DISPLAY_SCL_PIN,
            .clk_source = I2C_CLK_SRC_DEFAULT,
            .glitch_ignore_cnt = 7,
            .intr_priority = 0,
            .trans_queue_depth = 0,
            .flags = {
                .enable_internal_pullup = 1,
            },
        };
        ESP_ERROR_CHECK(i2c_new_master_bus(&bus_config, &display_i2c_bus_));
    }

    void InitializeSsd1306Display() {
        // SSD1306 config
        esp_lcd_panel_io_i2c_config_t io_config = {
            .dev_addr = 0x3C,
            .on_color_trans_done = nullptr,
            .user_ctx = nullptr,
            .control_phase_bytes = 1,
            .dc_bit_offset = 6,
            .lcd_cmd_bits = 8,
            .lcd_param_bits = 8,
            .flags = {
                .dc_low_on_data = 0,
                .disable_control_phase = 0,
            },
            .scl_speed_hz = 400 * 1000,
        };

        ESP_ERROR_CHECK(esp_lcd_new_panel_io_i2c_v2(display_i2c_bus_, &io_config, &panel_io_));

        ESP_LOGI(TAG, "Install SSD1306 driver");
        esp_lcd_panel_dev_config_t panel_config = {};
        panel_config.reset_gpio_num = -1;
        panel_config.bits_per_pixel = 1;

        esp_lcd_panel_ssd1306_config_t ssd1306_config = {
            .height = static_cast<uint8_t>(DISPLAY_HEIGHT),
        };
        panel_config.vendor_config = &ssd1306_config;

#ifdef SH1106
        ESP_ERROR_CHECK(esp_lcd_new_panel_sh1106(panel_io_, &panel_config, &panel_));
#else
        ESP_ERROR_CHECK(esp_lcd_new_panel_ssd1306(panel_io_, &panel_config, &panel_));
#endif
        ESP_LOGI(TAG, "SSD1306 driver installed");

        // Reset the display
        ESP_ERROR_CHECK(esp_lcd_panel_reset(panel_));
        if (esp_lcd_panel_init(panel_) != ESP_OK) {
            ESP_LOGE(TAG, "Failed to initialize display");
            display_ = new NoDisplay();
            return;
        }
        ESP_ERROR_CHECK(esp_lcd_panel_invert_color(panel_, false));

        // Set the display to on
        ESP_LOGI(TAG, "Turning display on");
        ESP_ERROR_CHECK(esp_lcd_panel_disp_on_off(panel_, true));

        display_ = new OledDisplay(panel_io_, panel_, DISPLAY_WIDTH, DISPLAY_HEIGHT, DISPLAY_MIRROR_X, DISPLAY_MIRROR_Y,
            {&font_puhui_14_1, &font_awesome_14_1});
    }

    void InitializeButtons() {
        boot_button_.OnClick([this]() {
            auto& app = Application::GetInstance();
            if (app.GetDeviceState() == kDeviceStateStarting && !WifiStation::GetInstance().IsConnected()) {
                ResetWifiConfiguration();
            }
            app.ToggleChatState();
        });
        touch_button_.OnPressDown([this]() {
            Application::GetInstance().StartListening();
        });
        touch_button_.OnPressUp([this]() {
            Application::GetInstance().StopListening();
        });

        volume_up_button_.OnClick([this]() {
            auto codec = GetAudioCodec();
            auto volume = codec->output_volume() + 10;
            if (volume > 100) {
                volume = 100;
            }
            codec->SetOutputVolume(volume);
            GetDisplay()->ShowNotification(Lang::Strings::VOLUME + std::to_string(volume));
        });

        volume_up_button_.OnLongPress([this]() {
            GetAudioCodec()->SetOutputVolume(100);
            GetDisplay()->ShowNotification(Lang::Strings::MAX_VOLUME);
        });

        volume_down_button_.OnClick([this]() {
            auto codec = GetAudioCodec();
            auto volume = codec->output_volume() - 10;
            if (volume < 0) {
                volume = 0;
            }
            codec->SetOutputVolume(volume);
            GetDisplay()->ShowNotification(Lang::Strings::VOLUME + std::to_string(volume));
        });

        volume_down_button_.OnLongPress([this]() {
            GetAudioCodec()->SetOutputVolume(0);
            GetDisplay()->ShowNotification(Lang::Strings::MUTED);
        });
    }

    void InitializeDogController() {
        ESP_LOGI(TAG, "初始化机器狗控制器");
        dog_controller_ = new DogController();

        if (!dog_controller_->Initialize()) {
            ESP_LOGE(TAG, "机器狗控制器初始化失败");
            delete dog_controller_;
            dog_controller_ = nullptr;
            return;
        }

        ESP_LOGI(TAG, "机器狗控制器初始化完成");
    }

    // 物联网初始化，逐步迁移到 MCP 协议
    void InitializeTools() {
        if (dog_controller_ != nullptr) {
            dog_controller_->InitializeTools();
        }
    }

public:
    BreadCompactWifiWithServoBoard() :
        boot_button_(BOOT_BUTTON_GPIO),
        touch_button_(TOUCH_BUTTON_GPIO),
        volume_up_button_(VOLUME_UP_BUTTON_GPIO),
        volume_down_button_(VOLUME_DOWN_BUTTON_GPIO),
        dog_controller_(nullptr) {
    
        InitializeDisplayI2c();
        InitializeSsd1306Display();
        InitializeButtons();
        InitializeDogController();  
        InitializeTools();
    }

    virtual ~BreadCompactWifiWithServoBoard() {
        if (dog_controller_) {
            delete dog_controller_;
        }
    }

    virtual Led* GetLed() override {
        static SingleLed led(BUILTIN_LED_GPIO);
        return &led;
    }

    virtual AudioCodec* GetAudioCodec() override {
#ifdef AUDIO_I2S_METHOD_SIMPLEX
        static NoAudioCodecSimplex audio_codec(AUDIO_INPUT_SAMPLE_RATE, AUDIO_OUTPUT_SAMPLE_RATE,
            AUDIO_I2S_SPK_GPIO_BCLK, AUDIO_I2S_SPK_GPIO_LRCK, AUDIO_I2S_SPK_GPIO_DOUT, AUDIO_I2S_MIC_GPIO_SCK, AUDIO_I2S_MIC_GPIO_WS, AUDIO_I2S_MIC_GPIO_DIN);
#else
        static NoAudioCodecDuplex audio_codec(AUDIO_INPUT_SAMPLE_RATE, AUDIO_OUTPUT_SAMPLE_RATE,
            AUDIO_I2S_GPIO_BCLK, AUDIO_I2S_GPIO_WS, AUDIO_I2S_GPIO_DOUT, AUDIO_I2S_GPIO_DIN);
#endif
        return &audio_codec;
    }

    virtual Display* GetDisplay() override {
        return display_;
    }
};

DECLARE_BOARD(BreadCompactWifiWithServoBoard);