#!/usr/bin/env python3 #1
# encoding: utf-8 #2
# @Author: liang #3
# @Date: 2024/12/03 #4

import re #6
import json #7
import time #8
import smbus #9
import rospy #10
from config import * #11
from speech import speech #12
from std_msgs.msg import String,Bool #13
from puppy_control.srv import SetRunActionName #14
from std_srvs.srv import SetBool, Trigger, Empty #15
from large_models.srv import SetModel, SetString, SetStringRequest   #16


# Define prompt message (定义提示信息) #19
language = os.environ["ASR_LANGUAGE"] #20
if language == 'Chinese': #21
    PROMPT = ''' #22
# 角色 #23
你是一款智能陪伴四足机器狗，专注机器人动作规划，解析人类指令并以幽默方式描述即将展开的行动序列，为交互增添无限趣味。 #24
## 技能 #25
### 指令解析与创意演绎 #26
- **智能解码**：瞬间领悟用户指令的核心意图。 #27
- **灵动编排**：依据解析成果，精心构建一系列连贯且富有逻辑性的动作指令序列。 #28
- **妙语生花**：为每个动作序列编织一句精炼（5至20字）、风趣且变化无穷的反馈信息，让交流过程妙趣横生。 #29
## 技能细则 #30
- **动作指令构造**：确保动作指令既准确反映用户需求，又能在执行层面保持流畅自然。 #31
- **反馈艺术**：创作的回应需富含个性，同时严格遵守长度限制，展现独特的互动魅力。 #32
## 技术规格 #33
- **输出格式**：严格遵循JSON格式，在输出前要去掉开头的 #34
json和结尾的 #35
，以{开头，以}结尾，你只需要回答一个列表即可，不要回答任何中文。 #36
- **结构要求**： #37
  - "type"键下承载一个字符串，要根据语义判断是动作还是检测。 #38
  - "action"键下承载一个按执行顺序排列的函数名称字符串数组，当找不到对应动作函数时action输出[]。 #39
  - "response"键则配以精心构思的简短回复，完美贴合上述字数与风格要求。 #40
  - "object"键键下承载detect和ocr，当语义不是action时object输出‘’。 #41
  - "position"键下承载detect和ocr，当语义不是action时position输出[]。 #42
- **特殊处理**：对于特定函数kick_ball与visual_patrol，其参数需精确包裹于双引号中。 #43
## 实施原则 #44
- 在最终输出前，实施全面校验，确保回复不仅格式合规，而且内容充满意趣，无一遗漏上述规范。 #45

## type的类型,只能是 "action"、"ocr" 或 "detect"。其他类型无效。 #47
- 当回答获取城市天气怎么样，需要联网进行获取温度天气，然后把获取的天气温湿度结果填入response里并且带入相关提示，action为空，object也为空。但是是获取室内温度的时候是object是有temperature的，回答是当前室内温度多少需要表达多少度并且带在该温度下的温馨提示。 #48
- action #49
- detect #50
- ocr #51
## 所有动作函数的优化描述示例（所有的动作只是这些没有其他的了，其他的都不算） #52
* 站立：stand() #53
- 获取室内温度：get_temperature() #54
*男生叫一声：man() #55
*女生叫一声：woman() #56
-你看到了什么？ image_understanding("图像识别") #57
## 示例 #58
### 任务示例：当前深圳市的天气温度怎么样。（回答是当前深圳市温度多少并且带温馨提示） #59
### 期望回应：{‘type’:'ocr','action':[], 'response':'现在深圳市的温度是24度','object':'','position':''} #60
### 任务示例：当前室内温度多少。（获取当前温度查询，降温提醒穿衣等温馨提示 #61
### 期望回应：{‘type’:'ocr','action':[‘get_temperature()’], 'response':'','object':temperature'','position':''} #62
''' #63
else: #64
    PROMPT = ''' #65
**Role #66
You are an intelligent quadruped companion robot dog, specialized in robot motion planning. You interpret human instructions and describe the upcoming action sequence in a humorous way to make interaction engaging and fun. #67
**Skills #68
Action Instruction Construction: Ensure that the action instructions accurately reflect user intent and are smoothly executable. #69
Feedback Artistry: Responses must be full of character, strictly follow the length limit, and demonstrate a unique sense of interaction. #70
**Technical Specifications #71
Output Format: Strictly follow JSON format. Remove the beginning json and the trailing code block markers. Start with { and end with }. You should only output a JSON object—do not include any Chinese text. #72
Structure Requirements: #73
"type": A string—determine whether it's an "action", "detect", or "ocr" type based on the semantics. #74
"action": An ordered array of function name strings based on execution sequence. If no corresponding action function is found, output []. #75
"response": A carefully crafted short reply (humorous, concise, 5–20 characters). #76
"object": Used for "detect" or "ocr" only. If the type is not these, return an empty string. #77
"position": Used for "detect" or "ocr" only. Otherwise, output an empty array []. #78
Special Handling: For the specific functions kick_ball and visual_patrol, their parameters must be wrapped in double quotes. #79
**Implementation Principles #80
Before final output, perform a full check to ensure the reply is both format-compliant and amusing—no part of the specification should be omitted. #81
Valid type values: only "action", "ocr", or "detect". Other types are invalid. #82
When responding to "what's the weather like in a city", the system must be online to fetch temperature and weather data. This result is then inserted into response along with a relevant prompt. action and object should be empty. #83
When answering indoor temperature, object must be "temperature". The response must state the temperature and provide a relevant warm tip based on the value. #84
**Valid Action Functions (only these are allowed; others are invalid) #85
- Stand up: stand() #86
- Get indoor temperature: get_temperature() #87
- Shout for a male: man() #88
- Shout for a female: woman() #89
- Visual understanding: image_understanding("image recognition") #90
Example 1: #91
Input: What is the current temperature in Shenzhen? #92
Output: {‘type’:'ocr','action':[], 'response':'The current temperature in Shenzhen is 24 degrees.','object':'','position':''} #93
Example 2: #94
Input: What is the current indoor temperature? #95
Output: {‘type’:'ocr','action':[‘get_temperature()’], 'response':'','object':temperature'','position':''} #96
''' #97

# Get temperature and humidity sensor data via I2C (I2C获取温湿度传感器数据) #99
class AHT10: #100
    CONFIG = [0x08, 0x00] #101
    MEASURE = [0x33, 0x00] #102

    def __init__(self, bus=1, addr=0x38): #104
        self.bus = smbus.SMBus(bus) #105
        self.addr = addr #106
        time.sleep(0.2) #107

    def getData(self): #109
        self.bus.write_i2c_block_data(self.addr, 0xAC, self.MEASURE) #110
        time.sleep(0.5) #111
        data = self.bus.read_i2c_block_data(self.addr, 0x00) #112
        temp = ((data[3] & 0x0F) << 16) | (data[4] << 8) | data[5] #113
        ctemp = ((temp * 200) / 1048576) - 50 #114
        return ctemp #115

class LLMTemperature: #117
    def __init__(self): #118
        # Initialize ROS node (初始化ROS节点) #119
        rospy.init_node('llm_control_sensor', anonymous=False) #120

        # Flag to prevent handling multiple requests simultaneously (标志位，防止同时处理多个请求) #122
        self.processing = False #123

        self.asr_result = '' #125
        
        # Initialize temperature sensor (初始化温度传感器) #127
        self.aht10 = AHT10() #128

        self.client = speech.OpenAIAPI(api_key, base_url) #130

        # Create publisher (创建发布者) #132
        self.tts_text_pub = rospy.Publisher('/tts_node/tts_text', String, queue_size=10) #133

        # Create subscriber (创建订阅者) #135
        rospy.Subscriber('/tts_node/play_finish', Bool, self.play_audio_finish_callback) #136
        rospy.Subscriber('/agent_process/result', String, self.llm_result_callback)   #137
        rospy.Subscriber('/vocal_detect/asr_result', String, self.asr_callback)      #138

        # Create service client (创建服务客户端) #140
        self.go_home_cli = rospy.ServiceProxy('/puppy_control/go_home', Empty) #141
        rospy.wait_for_service('/puppy_control/go_home')         #142

        self.awake_client = rospy.ServiceProxy('/vocal_detect/enable_wakeup', SetBool) #144
        self.set_model_client = rospy.ServiceProxy('/agent_process/set_model', SetModel) #145
        self.set_prompt_client = rospy.ServiceProxy('/agent_process/set_prompt', SetString) #146

        # Configure LLM (配置 LLM) #148
        self.init_process() #149

        rospy.loginfo('LLMTemperature 节点已启动，等待消息...') #151
        

    def init_process(self): #154
               
        self.set_model_client(llm_model, 'llm', api_key, base_url) #156

        self.set_prompt_client(PROMPT) #158

        # Play startup sound (播放启动音频) #160
        speech.play_audio(start_audio_path) #161

        # Call go_home service (调用 go_home 服务) #163
        try: #164
            self.go_home_cli() #165
        except rospy.ServiceException as e: #166
            rospy.logerr("调用 /puppy_control/go_home 服务时出错: {}".format(e)) #167
    def asr_callback(self, msg): #168
        self.asr_result = msg.data #169

    
    def play_audio_finish_callback(self, msg): #172
        if msg.data: #173
            self.awake_client(True) #174

    def get_temperature(self): #176
        """ #177
        Get the current temperature and return it(获取当前温度并返回。) #178
        """ #179
        try: #180
            temperature = self.aht10.getData() #181
            if os.environ['ASR_LANGUAGE'] == 'Chinese': #182
                rospy.loginfo("当前温度: {:.2f} °C".format(temperature)) #183
            else: #184
                rospy.loginfo("Current temperature: {:.2f} °C".format(temperature)) #185
            return temperature #186
        except Exception as e: #187
            rospy.logerr("温度传感器读取失败: {}".format(e)) #188
            return None #189

    def llm_result_callback(self, msg): #191
        """ #192
        Handle result received from /agent_process/result (处理从 /agent_process/result 接收到的结果) #193
        """ #194
        try: #195
            # Remove code block markers (去除代码块标记) #196

            cleaned_data = self.clean_json_message(msg.data) #198
            if not cleaned_data: #199
                rospy.logerr("清理后的数据为空，无法解析。") #200
                return #201
            
            action_list = cleaned_data.get('action', []) #203
            response = cleaned_data.get('response', '') #204
            if "get_temperature()" in action_list: #205
                if self.processing: #206
                    rospy.logwarn("当前正在处理一个请求，忽略新的请求。") #207
                    return #208

                self.processing = True  # Set flag and start processing (设置标志位，开始处理) #210
                temperature = self.get_temperature() #211
                if temperature is not None: #212
                    if os.environ['ASR_LANGUAGE'] == 'Chinese': #213
                        temperature_info = "当前室内温度: {:.2f} °C，请简单给我点提醒".format(temperature) #214
                    else: #215
                        temperature_info = "Current indoor temperature: {:.2f} °C，Please give me a brief reminder".format(temperature) #216
                    # Call set_llm_content service to send temperature info (调用 set_llm_content 服务发送温度信息) #217
                    self.send_temperature_to_llm(temperature_info) #218
                else: #219
                    rospy.logwarn("无法读取温度，跳过发送到 LLM。") #220
                    self.processing = False  # Clear processing flag (清除标志位) #221
            # Retrieve regional temperature via network (网络获取地区温度) #222
            else:  #223
                if os.environ['ASR_LANGUAGE'] == 'Chinese': #224
                    params = { #225
                                "model": llm_model,  #226
                                "messages": [{ #227
                                    "role": "system", #228
                                    "content": PROMPT}, #229
                                             { #230
                                    "role": "user", #231
                                    "content": self.asr_result}], #232
                                "extra_body": { #233
                                    "enable_search": True} #234
                        } #235
                else: #236
                    params = {"model": 'gpt-4o-search-preview',  #237
                                "web_search_options": {},  #238
                                "messages": [ #239
                                    {"role": "system","content": PROMPT}, #240
                                    {"role": "user","content": self.asr_result}] #241
                        }    #242
                res = self.client.llm_origin(params).choices[0].message.content.strip() #243
                rospy.loginfo("获取的结果: {}".format(res))             #244
                res_json = json.loads(res[res.find('{'):res.rfind('}') + 1]) #245
                reply = res_json.get('response', response) #246
                self.publish_tts(reply) #247


        except Exception as e: #250
            rospy.logerr("处理 LLM 返回结果时发生错误: {}".format(e)) #251

    def clean_json_message(self, message): #253
        """ #254
        Clean received JSON message, removing JSON tags and markers (清理接收到的 JSON 消息，去除 json 和标记) #255
        """ #256
        try: #257
            start = message.find('{') #258
            end = message.rfind('}') + 1 #259
            if start == -1 or end <= start: #260
                raise ValueError("无效的 JSON 格式") #261
            result_json = message[start:end] #262
            result = json.loads(result_json) #263
            return result #264
        except Exception as e: #265
            rospy.logerr("清理 JSON 消息时出错: {}".format(e)) #266
            return None #267

    def send_temperature_to_llm(self, temperature_info): #269
        """ #270
        Call /agent_process/set_llm_content service to send temperature to LLM (调用 /agent_process/set_llm_content 服务将温度信息发送给 LLM) #271
        """        #272
        res = self.client.llm(temperature_info, model=llm_model) #273
        print("res:{}".format(res)) #274
        self.publish_tts(res) #275
                      


    def publish_tts(self, message): #279
        """ #280
        Publish TTS message to tts_node/tts_text topic (发布 TTS 消息到 tts_node/tts_text 主题) #281
        """ #282
        #rospy.loginfo("准备发布 TTS 消息: {}".format(message)) #283
        tts_msg = String() #284
        tts_msg.data = message #285
        self.tts_text_pub.publish(tts_msg) #286
        #rospy.loginfo("发布 TTS 消息: {}".format(tts_msg.data)) #287


def main(): #290
    try: #291
        node = LLMTemperature() #292
        rospy.spin() #293
    except KeyboardInterrupt: #294
        rospy.loginfo('LLMTemperature 节点已关闭') #295


if __name__ == "__main__": #298
    main()  #299
