CoCube Meets M5 CoreS3: An AI Chat Robot

#include <esp_log.h>

#include <sstream>
#include <string>
#include <vector>

#include "audio_codec.h"
#include "board.h"
#include "esp_wifi.h"
#include "freertos/event_groups.h"
#include "iot/thing.h"
#include "mqtt_client.h"
#include "esp_system.h"
#include <esp_mac.h>

#define TAG "Copilot"
//  MQTT 

// #define MQTT_PUB_TOPIC "/ailab/control"
// #define MQTT_SUB_TOPIC "/ailab/position"


// std::string generate_mac_prefix() {
//     uint8_t mac[6];
//     esp_read_mac(mac, ESP_MAC_WIFI_STA);  //  MAC 

//     //  4 
//     char prefix[5];  // 4 
//     snprintf(prefix, sizeof(prefix), "%02X%02X", mac[3], mac[4]);  //  MAC 

//     // 
//     for (int i = 0; i < 4; i++) {
//         prefix[i] = toupper(prefix[i]);
//     }
//     ESP_LOGI(TAG, "Generated MAC prefix: %s", prefix);
//     return std::string(prefix);
// }

std::string generate_mac_prefix() {
    uint8_t mac[6];
    esp_read_mac(mac, ESP_MAC_WIFI_SOFTAP);  //  SoftAP  MAC 

    //  GetSsid()  4 mac[4]  mac[5]
    char prefix[5];  // 4  + 1 
    snprintf(prefix, sizeof(prefix), "%02X%02X", mac[4], mac[5]);

    ESP_LOGI(TAG, "Generated MAC prefix: %s", prefix);
    return std::string(prefix);
}

// 
struct Position {
    int x;
    int y;
    int yaw;
}position;


#define WIFI_CONNECTED_BIT BIT0

static EventGroupHandle_t wifi_event_group;

esp_mqtt_client_handle_t mqtt_client_ = nullptr;

class CoCube {
   public:
    // Helper function to format parameters
    std::string format_params(const std::vector<std::string>& params) {
        std::ostringstream oss;
        for (size_t i = 0; i < params.size(); ++i) {
            if (i > 0) oss << ",";
            oss << params[i];
        }
        return oss.str();
    }

    // Generate data_to_send for each command

    std::string move(const std::string& direction = "forward", int speed = 40) {
        std::string modified_direction = "cocube;" + direction;
        std::vector<std::string> params = {modified_direction, std::to_string(speed)};
        return "CoCube move," + format_params(params);
    }

    std::string rotate(const std::string& direction = "left", int speed = 30) {
        std::string modified_direction = "cocube;" + direction;
        std::vector<std::string> params = {modified_direction, std::to_string(speed)};
        return "CoCube rotate," + format_params(params);
    }

    std::string move_msecs(const std::string& direction = "forward", int speed = 40, int duration = 1000) {
        std::string modified_direction = "cocube;" + direction;
        std::vector<std::string> params = {modified_direction, std::to_string(speed), std::to_string(duration)};
        return "CoCube move for msecs," + format_params(params);
    }

    std::string move_to_landmark(int landmark = 1) {
        std::vector<std::string> params = {std::to_string(landmark)};
        return "move_to_landmark," + format_params(params);
    }

    std::string rotate_msecs(const std::string& direction = "left", int speed = 40, int duration = 1000) {
        std::string modified_direction = "cocube;" + direction;
        std::vector<std::string> params = {modified_direction, std::to_string(speed), std::to_string(duration)};
        return "CoCube rotate for msecs," + format_params(params);
    }

    std::string move_by_steps(const std::string& direction = "forward", int speed = 40, int step = 50) {
        std::string modified_direction = "cocube;" + direction;
        std::vector<std::string> params = {modified_direction, std::to_string(speed), std::to_string(step)};
        return "CoCube move by step," + format_params(params);
    }

    std::string rotate_by_degree(const std::string& direction = "left", int speed = 40, int degree = 90) {
        std::string modified_direction = "cocube;" + direction;
        std::vector<std::string> params = {modified_direction, std::to_string(speed), std::to_string(degree)};
        return "CoCube rotate by degree," + format_params(params);
    }

    std::string set_wheel_speed(int left_speed = 40, int right_speed = 40) {
        std::vector<std::string> params = {std::to_string(left_speed), std::to_string(right_speed)};
        return "CoCube set wheel," + format_params(params);
    }

    std::string wheels_stop() { return "CoCube wheels stop"; }

    std::string wheels_break() { return "CoCube wheels break"; }

    std::string rotate_to_angle(int angle = 0, int speed = 40) {
        std::vector<std::string> params = {std::to_string(angle), std::to_string(speed)};
        return "CoCube rotate to angle," + format_params(params);
    }

    std::string rotate_to_target(int target_x = 0, int target_y = 0, int speed = 30) {
        std::vector<std::string> params = {std::to_string(target_x), std::to_string(target_y), std::to_string(speed)};
        return "CoCube point towards," + format_params(params);
    }

    std::string move_to_target(int target_x = 0, int target_y = 0, int speed = 50) {
        std::vector<std::string> params = {std::to_string(target_x), std::to_string(target_y), std::to_string(speed)};
        return "CoCube move to," + format_params(params);
    }

    // External Module Functions
    std::string gripper_open() { return "ccmodule_gripper open"; }

    std::string gripper_close() { return "ccmodule_gripper close"; }

    std::string change_color(int color = 65280) {
        std::vector<std::string> params = {std::to_string(color)};
        return "set display color," + format_params(params);
    }

    std::string change_image(int img_num = 2) {
        std::vector<std::string> params = {std::to_string(img_num)};
        return "drawBMPfile,wanxiang" + format_params(params) + ".bmp,0,0";
    }

};

bool is_wifi_connected() {
    wifi_ap_record_t ap_info;
    if (esp_wifi_sta_get_ap_info(&ap_info) == ESP_OK) {
        return true;  // Wi-Fi 
    } else {
        return false;  // Wi-Fi 
    }
}

//  MQTT 
static esp_err_t mqtt_message_handler_cb(esp_mqtt_event_handle_t event) {
    std::string mac_prefix = generate_mac_prefix();
    std::string mqtt_sub_topic = "/" + mac_prefix + "/position";
    
    //  topic  data
    ESP_LOGI(TAG, "Received message on topic: %.*s", event->topic_len, event->topic);
    ESP_LOGI(TAG, "Message data: %.*s", event->data_len, event->data);
    
    //  topic  strncmp 
    if (strncmp(event->topic, mqtt_sub_topic.c_str(), event->topic_len) == 0) {
        //  "87,74,191": "x,y,yaw"
        std::string message(event->data, event->data + event->data_len);

        //  (: "x,y,yaw")
        Position new_position;
        int ret = std::sscanf(message.c_str(), "%d,%d,%d", &new_position.x, &new_position.y, &new_position.yaw);

        if (ret == 3) {
            // 
            position = new_position;  // 

            // 
            ESP_LOGI(TAG, "Updated position: x=%d, y=%d, yaw=%d", position.x, position.y, position.yaw);
        
        } else {
            ESP_LOGE(TAG, "Failed to parse position data: %s", message.c_str());
        }
    }
    else {
        // 
        ESP_LOGE(TAG, "Received message on unexpected topic: %.*s", event->topic_len, event->topic);
    }

    return ESP_OK;
}

static void mqtt_event_handler(void* handler_args, esp_event_base_t base, int32_t event_id, void* event_data) {
    std::string mac_prefix = generate_mac_prefix();
    std::string mqtt_sub_topic = "/" + mac_prefix + "/position";
    
    esp_mqtt_event_handle_t event = (esp_mqtt_event_handle_t)event_data;

    //  MQTT 
    switch (event->event_id) {
    case MQTT_EVENT_CONNECTED:
        ESP_LOGI(TAG, "MQTT_EVENT_CONNECTED");
        // 
        esp_mqtt_client_subscribe(mqtt_client_, mqtt_sub_topic.c_str(), 0);
        break;
    case MQTT_EVENT_DISCONNECTED:
        ESP_LOGI(TAG, "MQTT_EVENT_DISCONNECTED");
        break;
    case MQTT_EVENT_DATA:
        mqtt_message_handler_cb(event);
        ESP_LOGI(TAG, "MQTT_EVENT_DATA");
        break;
    default:
        break;
    }
    // return ESP_OK;
}


void InitializeMqtt() {
    //  4 
    std::string mac_prefix = generate_mac_prefix();
    
    //  MQTT 
    std::string mqtt_pub_topic = "/" + mac_prefix + "/control";
    std::string mqtt_sub_topic = "/" + mac_prefix + "/position";

    ESP_LOGI(TAG, "MQTT PUB Topic: %s", mqtt_pub_topic.c_str());
    ESP_LOGI(TAG, "MQTT SUB Topic: %s", mqtt_sub_topic.c_str());

    ESP_LOGI(TAG, "Waiting for Wi-Fi connection...");
    while (!is_wifi_connected()) {
        vTaskDelay(pdMS_TO_TICKS(1000));  //  1 
        ESP_LOGI(TAG, "Wi-Fi not connected yet...");
    }
    ESP_LOGI(TAG, "Wi-Fi connected!");
    
    wifi_ap_record_t ap_info;
    if (esp_wifi_sta_get_ap_info(&ap_info) != ESP_OK) {
        ESP_LOGE(TAG, "Wi-Fi  MQTT");
        return;
    }

    const esp_mqtt_client_config_t mqtt_cfg = {
        .broker =
            {
                .address = {.uri = "mqtt://broker.emqx.io", .port = 1883},
                // .address = {.uri = "mqtt://broker.hivemq.com", .port = 1883},
            },
        .credentials = {.username = "emqx", .authentication = {.password = "public"}}};

    mqtt_client_ = esp_mqtt_client_init(&mqtt_cfg);
    if (mqtt_client_ == nullptr) {
        ESP_LOGE(TAG, "Failed to initialize MQTT client");
        return;
    }

    ESP_LOGI(TAG, "MQTT client initialized");

    esp_err_t err = esp_mqtt_client_register_event(mqtt_client_, MQTT_EVENT_ANY, mqtt_event_handler, mqtt_client_);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "Failed to register MQTT event handler: %s", esp_err_to_name(err));
        return;
    }
    ESP_LOGI(TAG, "MQTT event handler registered");

    // 
    esp_mqtt_client_subscribe(mqtt_client_, mqtt_sub_topic.c_str(), 0);
    esp_mqtt_client_publish(mqtt_client_, mqtt_pub_topic.c_str(), "message", 0, 0, 0);

    err = esp_mqtt_client_start(mqtt_client_);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "Failed to start MQTT client: %s", esp_err_to_name(err));
        return;
    }

    ESP_LOGI(TAG, "MQTT client started");
}

static void wifi_event_handler(void* arg, esp_event_base_t event_base, int32_t event_id, void* event_data) {
    // ... Wi-Fi  ...
    if (event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP) {
        // ...  IP  ...
        xEventGroupSetBits(wifi_event_group, WIFI_CONNECTED_BIT);  // 
    }
}

void mqtt_task(void* pvParameters) {
    ESP_LOGI(TAG, "MQTT task started");

    //  Wi-Fi 
    ESP_LOGI(TAG, "Waiting for Wi-Fi connection...");
    xEventGroupWaitBits(wifi_event_group, WIFI_CONNECTED_BIT, false, true, portMAX_DELAY);
    ESP_LOGI(TAG, "Wi-Fi connected!");

    //  MQTT
    InitializeMqtt();

    vTaskDelete(NULL);  // 
}

namespace iot {

class Copilot : public Thing {
//    private:
//     Position position = {0, 0, 0};  // 
    
   public:
    Copilot() : Thing("Copilot", "") {
        wifi_event_group = xEventGroupCreate();

        ESP_ERROR_CHECK(esp_event_handler_register(WIFI_EVENT, ESP_EVENT_ANY_ID, &wifi_event_handler, NULL));
        ESP_ERROR_CHECK(esp_event_handler_register(IP_EVENT, IP_EVENT_STA_GOT_IP, &wifi_event_handler, NULL));

        //  MQTT 
        xTaskCreate(mqtt_task, "MQTT_Task", 8192, NULL, 5, NULL);

        // // 
        // properties_.AddStringProperty("CarPosition", " (x, y, yaw)",
        //     [this]() -> std::string {
        //         //  position 
        //         std::ostringstream oss;
        //         oss << "x:" << position.x << ", y:" << position.y << ", yaw:" << position.yaw;
        //         return oss.str();
        //     });
        
        // x_positionX
        properties_.AddNumberProperty("x_position", "X", [this]() -> int {
            return position.x;  //  X 
        });

        // y_positionY
        properties_.AddNumberProperty("y_position", "Y", [this]() -> int {
            return position.y;  //  Y 
        });

        // yaw
        properties_.AddNumberProperty("yaw", "", [this]() -> int {
            return position.yaw;  //  Y 
        });


        CoCube cocube;

        //  Movemsecs 
        methods_.AddMethod(
            "Move", "",
            ParameterList({Parameter("direction", " (forward/backward),  forward", kValueTypeString, false),
                           Parameter("speed", " (0-50),  40", kValueTypeNumber, false),
                           Parameter("duration", " (),  1000", kValueTypeNumber, false)}),
            [&cocube](const ParameterList& parameters) {

                //  MAC  MQTT 
                std::string mac_prefix = generate_mac_prefix();
                std::string mqtt_pub_topic = "/" + mac_prefix + "/control";  // 

                std::string direction = "forward";
                int speed = 40;
                int duration = 1000;

                if (parameters["direction"].type() == kValueTypeString) {
                    direction = parameters["direction"].string();
                }
                if (parameters["speed"].type() == kValueTypeNumber) {
                    speed = parameters["speed"].number();
                }
                if (parameters["duration"].type() == kValueTypeNumber) {
                    duration = parameters["duration"].number();
                }

                std::string msg = cocube.move_msecs(direction, speed, duration);
                if (mqtt_client_ != nullptr) {
                    esp_mqtt_client_publish(mqtt_client_, mqtt_pub_topic.c_str(), msg.c_str(), msg.length(), 0, 0);
                    ESP_LOGI(TAG, "Move");
                }
            });

        //  Movemsecs 
        methods_.AddMethod(
            "Move to landmark", "",
            ParameterList({Parameter("landmark", "(1-4),  1", kValueTypeNumber, false)}),
            [&cocube](const ParameterList& parameters) {

                //  MAC  MQTT 
                std::string mac_prefix = generate_mac_prefix();
                std::string mqtt_pub_topic = "/" + mac_prefix + "/control";  // 

                int landmark = 1;


                if (parameters["landmark"].type() == kValueTypeNumber) {
                    landmark = parameters["landmark"].number();
                }


                std::string msg = cocube.move_to_landmark(landmark);
                if (mqtt_client_ != nullptr) {
                    esp_mqtt_client_publish(mqtt_client_, mqtt_pub_topic.c_str(), msg.c_str(), msg.length(), 0, 0);
                    ESP_LOGI(TAG, "Move to landmark");
                }
            });


        //  Rotatemsecs 
        methods_.AddMethod(
            "Rotate", "",
            ParameterList({Parameter("direction", " (left/right),  left", kValueTypeString, false),
                           Parameter("speed", " (0-50),  30", kValueTypeNumber, false),
                           Parameter("duration", " (),  1000", kValueTypeNumber, false)}),
            [&cocube](const ParameterList& parameters) {
                //  MAC  MQTT 
                std::string mac_prefix = generate_mac_prefix();
                std::string mqtt_pub_topic = "/" + mac_prefix + "/control";  // 
                std::string direction = "left";
                int speed = 30;
                int duration = 1000;

                if (parameters["direction"].type() == kValueTypeString) {
                    direction = parameters["direction"].string();
                }
                if (parameters["speed"].type() == kValueTypeNumber) {
                    speed = parameters["speed"].number();
                }
                if (parameters["duration"].type() == kValueTypeNumber) {
                    duration = parameters["duration"].number();
                }

                std::string msg = cocube.rotate_msecs(direction, speed, duration);
                if (mqtt_client_ != nullptr) {
                    esp_mqtt_client_publish(mqtt_client_, mqtt_pub_topic.c_str(), msg.c_str(), msg.length(), 0, 0);
                    ESP_LOGI(TAG, "Rotate");
                }
            });

        //  MoveBySteps 
        methods_.AddMethod(
            "MoveBySteps", "",
            ParameterList({Parameter("direction", " (forward/backward),  forward", kValueTypeString, false),
                           Parameter("speed", " (0-50),  40", kValueTypeNumber, false),
                           Parameter("step", ",  50", kValueTypeNumber, false)}),
            [&cocube](const ParameterList& parameters) {
                //  MAC  MQTT 
                std::string mac_prefix = generate_mac_prefix();
                std::string mqtt_pub_topic = "/" + mac_prefix + "/control";  // 
                std::string direction = "forward";
                int speed = 40;
                int step = 50;

                if (parameters["direction"].type() == kValueTypeString) {
                    direction = parameters["direction"].string();
                }
                if (parameters["speed"].type() == kValueTypeNumber) {
                    speed = parameters["speed"].number();
                }
                if (parameters["step"].type() == kValueTypeNumber) {
                    step = parameters["step"].number();
                }

                std::string msg = cocube.move_by_steps(direction, speed, step);
                if (mqtt_client_ != nullptr) {
                    esp_mqtt_client_publish(mqtt_client_, mqtt_pub_topic.c_str(), msg.c_str(), msg.length(), 0, 0);
                    ESP_LOGI(TAG, "MoveBySteps");
                }
            });
        //  RotateByDegree 
        methods_.AddMethod(
            "RotateByDegree", "",
            ParameterList({Parameter("direction", " (left/right),  left", kValueTypeString, false),
                           Parameter("speed", " (0-50),  40", kValueTypeNumber, false),
                           Parameter("degree", ",  90", kValueTypeNumber, false)}),
            [&cocube](const ParameterList& parameters) {
                //  MAC  MQTT 
                std::string mac_prefix = generate_mac_prefix();
                std::string mqtt_pub_topic = "/" + mac_prefix + "/control";  // 
                std::string direction = "left";
                int speed = 40;
                int degree = 90;

                if (parameters["direction"].type() == kValueTypeString) {
                    direction = parameters["direction"].string();
                }
                if (parameters["speed"].type() == kValueTypeNumber) {
                    speed = parameters["speed"].number();
                }
                if (parameters["degree"].type() == kValueTypeNumber) {
                    degree = parameters["degree"].number();
                }

                std::string msg = cocube.rotate_by_degree(direction, speed, degree);
                if (mqtt_client_ != nullptr) {
                    esp_mqtt_client_publish(mqtt_client_, mqtt_pub_topic.c_str(), msg.c_str(), msg.length(), 0, 0);
                    ESP_LOGI(TAG, "RotateByDegree");
                }
            });

        //  MoveToTarget 
        methods_.AddMethod("MoveToTarget", "",
                           ParameterList({Parameter("target_x", " X ,  0", kValueTypeNumber, false),
                                          Parameter("target_y", " Y ,  0", kValueTypeNumber, false),
                                          Parameter("speed", " (0-50),  50", kValueTypeNumber, false)}),
                           [&cocube](const ParameterList& parameters) {
                                //  MAC  MQTT 
                                std::string mac_prefix = generate_mac_prefix();
                                std::string mqtt_pub_topic = "/" + mac_prefix + "/control";  // 
                               int target_x = 0;
                               int target_y = 0;
                               int speed = 50;

                               if (parameters["target_x"].type() == kValueTypeNumber) {
                                   target_x = parameters["target_x"].number();
                               }
                               if (parameters["target_y"].type() == kValueTypeNumber) {
                                   target_y = parameters["target_y"].number();
                               }
                               if (parameters["speed"].type() == kValueTypeNumber) {
                                   speed = parameters["speed"].number();
                               }

                               std::string msg = cocube.move_to_target(target_x, target_y, speed);
                               if (mqtt_client_ != nullptr) {
                                   esp_mqtt_client_publish(mqtt_client_, mqtt_pub_topic.c_str(), msg.c_str(), msg.length(), 0, 0);
                                   ESP_LOGI(TAG, "MoveToTarget");
                               }
                           });

        // Register rotate_to_angle method
        methods_.AddMethod("RotateToAngle", "",
                           ParameterList({Parameter("angle", ",  0", kValueTypeNumber, false),
                                          Parameter("speed", " (0-50),  40", kValueTypeNumber, false)}),
                           [&cocube](const ParameterList& parameters) {
                            //  MAC  MQTT 
                                std::string mac_prefix = generate_mac_prefix();
                                std::string mqtt_pub_topic = "/" + mac_prefix + "/control";  // 
                               int angle = 0;
                               int speed = 40;

                               if (parameters["angle"].type() == kValueTypeNumber) {
                                   angle = parameters["angle"].number();
                               }
                               if (parameters["speed"].type() == kValueTypeNumber) {
                                   speed = parameters["speed"].number();
                               }

                               std::string msg = cocube.rotate_to_angle(angle, speed);
                               if (mqtt_client_ != nullptr) {
                                   esp_mqtt_client_publish(mqtt_client_, mqtt_pub_topic.c_str(), msg.c_str(), msg.length(), 0, 0);
                                   ESP_LOGI(TAG, "RotateToAngle");
                               }
                           });

        // Register rotate_to_target method
        methods_.AddMethod("RotateToTarget", "",
                           ParameterList({Parameter("target_x", " X ,  0", kValueTypeNumber, false),
                                          Parameter("target_y", " Y ,  0", kValueTypeNumber, false),
                                          Parameter("speed", " (0-50),  30", kValueTypeNumber, false)}),
                           [&cocube](const ParameterList& parameters) {
                            //  MAC  MQTT 
                std::string mac_prefix = generate_mac_prefix();
                std::string mqtt_pub_topic = "/" + mac_prefix + "/control";  // 
                               int target_x = 0;
                               int target_y = 0;
                               int speed = 30;

                               if (parameters["target_x"].type() == kValueTypeNumber) {
                                   target_x = parameters["target_x"].number();
                               }
                               if (parameters["target_y"].type() == kValueTypeNumber) {
                                   target_y = parameters["target_y"].number();
                               }
                               if (parameters["speed"].type() == kValueTypeNumber) {
                                   speed = parameters["speed"].number();
                               }

                               std::string msg = cocube.rotate_to_target(target_x, target_y, speed);
                               if (mqtt_client_ != nullptr) {
                                   esp_mqtt_client_publish(mqtt_client_, mqtt_pub_topic.c_str(), msg.c_str(), msg.length(), 0, 0);
                                   ESP_LOGI(TAG, "RotateToTarget");
                               }
                           });

        //   
        methods_.AddMethod("GripperOpen", "", ParameterList(),
                           [&cocube](const ParameterList& parameters) {
                            //  MAC  MQTT 
                std::string mac_prefix = generate_mac_prefix();
                std::string mqtt_pub_topic = "/" + mac_prefix + "/control";  // 
                               std::string msg = cocube.gripper_open();
                               if (mqtt_client_ != nullptr) {
                                   esp_mqtt_client_publish(mqtt_client_, mqtt_pub_topic.c_str(), msg.c_str(), msg.length(), 0, 0);
                                   ESP_LOGI(TAG, "GripperOpen");
                               }
                           });

        //   
        methods_.AddMethod("GripperClose", "", ParameterList(),
                           [&cocube](const ParameterList& parameters) {
                            //  MAC  MQTT 
                std::string mac_prefix = generate_mac_prefix();
                std::string mqtt_pub_topic = "/" + mac_prefix + "/control";  // 
                               std::string msg = cocube.gripper_close();
                               if (mqtt_client_ != nullptr) {
                                   esp_mqtt_client_publish(mqtt_client_, mqtt_pub_topic.c_str(), msg.c_str(), msg.length(), 0, 0);
                                   ESP_LOGI(TAG, "GripperClose");
                               }
                           });

        //   
        methods_.AddMethod("ChangeColor", "",
                           ParameterList({
                               Parameter("color_R", ", 0-255, 0", kValueTypeNumber, false),
                               Parameter("color_G", ", 0-255, 255", kValueTypeNumber, false),
                               Parameter("color_B", ", 0-255, 0", kValueTypeNumber, false)}),
                           [&cocube](const ParameterList& parameters) {
                            //  MAC  MQTT 
                std::string mac_prefix = generate_mac_prefix();
                std::string mqtt_pub_topic = "/" + mac_prefix + "/control";  // 
                               int color_r = 0;
                               int color_g = 255;
                               int color_b = 0;

                               if (parameters["color_R"].type() == kValueTypeNumber) {
                                   color_r = parameters["color_R"].number();
                               }

                               if (parameters["color_G"].type() == kValueTypeNumber) {
                                   color_g = parameters["color_G"].number();
                               }

                               if (parameters["color_B"].type() == kValueTypeNumber) {
                                   color_b = parameters["color_B"].number();
                               }

                               int color = (color_r << 16) | (color_g << 8) | color_b;  // RGB to int

                               std::string msg = cocube.change_color(color);
                               if (mqtt_client_ != nullptr) {
                                   esp_mqtt_client_publish(mqtt_client_, mqtt_pub_topic.c_str(), msg.c_str(), msg.length(), 0, 0);
                                   ESP_LOGI(TAG, "ChangeColor");
                               }
                           });

        //   
        methods_.AddMethod("ChangeImage", "",
                           ParameterList({
                               Parameter("img_num", ", 1-7, 2, 1, 2, 3, 4, 5, 6, 7", kValueTypeNumber, false),
                           }),
                           [&cocube](const ParameterList& parameters) {
                            //  MAC  MQTT 
                std::string mac_prefix = generate_mac_prefix();
                std::string mqtt_pub_topic = "/" + mac_prefix + "/control";  // 
                               int img_num = 2;

                               if (parameters["img_num"].type() == kValueTypeNumber) {
                                   img_num = parameters["img_num"].number();
                               }

                               std::string msg = cocube.change_image(img_num);
                               if (mqtt_client_ != nullptr) {
                                   esp_mqtt_client_publish(mqtt_client_, mqtt_pub_topic.c_str(), msg.c_str(), msg.length(), 0, 0);
                                   ESP_LOGI(TAG, "ChangeImage");
                               }
                           });

        //  WheelsStop 
        methods_.AddMethod("WheelsStop", "", ParameterList(), [&cocube](const ParameterList& parameters) {
            //  MAC  MQTT 
                std::string mac_prefix = generate_mac_prefix();
                std::string mqtt_pub_topic = "/" + mac_prefix + "/control";  // 
            std::string msg = cocube.wheels_stop();
            if (mqtt_client_ != nullptr) {
                esp_mqtt_client_publish(mqtt_client_, mqtt_pub_topic.c_str(), msg.c_str(), msg.length(), 0, 0);
                ESP_LOGI(TAG, "WheelsStop");
            }
        });


        //  shoot 
        methods_.AddMethod("Shoot", "", ParameterList(), [&cocube](const ParameterList& parameters) {
            //  MAC  MQTT 
                std::string mac_prefix = generate_mac_prefix();
                std::string mqtt_pub_topic = "/" + mac_prefix + "/control";  // 
            std::string msg = "CoCube shoot";
            if (mqtt_client_ != nullptr) {
                esp_mqtt_client_publish(mqtt_client_, mqtt_pub_topic.c_str(), msg.c_str(), msg.length(), 0, 0);
                ESP_LOGI(TAG, "CoCube shoot");
            }
        });
    }
};

}  // namespace iot

DECLARE_THING(Copilot);