'''
This demo show the communication interface of MR813 motion control board based on Lcm.
Dependency: 
- robot_control_cmd_lcmt.py
- robot_control_response_lcmt.py
'''
import lcm
import sys
import os
import time
import threading
from threading import Thread, Lock

from robot_control_cmd_lcmt import robot_control_cmd_lcmt
from robot_control_response_lcmt import robot_control_response_lcmt
from camera import MiroCameraROS2

from pyzbar import pyzbar
from PIL import Image
import rclpy

from qreader import QReader
import cv2

class Robot_Ctrl(object):
    def __init__(self):
        self.rec_thread = Thread(target=self.rec_responce)
        self.send_thread = Thread(target=self.send_publish)
        self.lc_r = lcm.LCM("udpm://239.255.76.67:7670?ttl=255")
        self.lc_s = lcm.LCM("udpm://239.255.76.67:7671?ttl=255")
        self.subscription = None
        self.cmd_msg = robot_control_cmd_lcmt()
        self.rec_msg = robot_control_response_lcmt()
        self.send_lock = Lock()
        self.delay_cnt = 0
        self.mode_ok = 0
        self.gait_ok = 0
        self.runing = 1

    def run(self):
        self.subscription = self.lc_r.subscribe("robot_control_response", self.msg_handler)
        self.send_thread.start()
        self.rec_thread.start()

    def msg_handler(self, channel, data):
        try:
            self.rec_msg = robot_control_response_lcmt().decode(data)
            if self.rec_msg.order_process_bar >= 95:
                self.mode_ok = self.rec_msg.mode
            else:
                self.mode_ok = 0
        except Exception as e:
            print(f"消息解析失败: {str(e)}")

    def rec_responce(self):
        while self.runing:
            try:
                self.lc_r.handle()
            except Exception as e:
                print(f"接收线程错误: {str(e)}")
            time.sleep(0.002)

    def Wait_finish(self, mode, gait_id):
        count = 0
        while self.runing and count < 2000:
            if self.mode_ok == mode and self.gait_ok == gait_id:
                return True
            time.sleep(0.005)
            count += 1
        return False

    def send_publish(self):
        while self.runing:
            try:
                self.send_lock.acquire()
                if self.delay_cnt > 20:
                    self.lc_s.publish("robot_control_cmd", self.cmd_msg.encode())
                    self.delay_cnt = 0
                self.delay_cnt += 1
                self.send_lock.release()
                time.sleep(0.005)
            except Exception as e:
                print(f"发送线程错误: {str(e)}")

    def Send_cmd(self, msg):
        try:
            self.send_lock.acquire()
            self.delay_cnt = 50
            self.cmd_msg = msg
        finally:
            self.send_lock.release()

    def quit(self):
        self.runing = 0
        if self.rec_thread.is_alive():
            self.rec_thread.join(timeout=1.0)
        if self.send_thread.is_alive():
            self.send_thread.join(timeout=1.0)
        self.lc_r.unsubscribe(self.subscription)
        reset_msg = robot_control_cmd_lcmt()
        reset_msg.mode = 0
        reset_msg.life_count = 0
        self.lc_s.publish("robot_control_cmd", reset_msg.encode())

class MiroCameraThread(threading.Thread):
    def __init__(self):
        super().__init__(daemon=True)
        self.ros_initialized = False
        self.camera = None
        self.image = None
    
    def run(self):
        rclpy.init()
        self.ros_initialized = True
        self.camera = MiroCameraROS2()
        self.image = self.camera.get_latest_image()
        rclpy.shutdown()


def main():
    try:
        ctrl = Robot_Ctrl()
        ctrl.run()
        qreader = QReader()

        msg = robot_control_cmd_lcmt()
        msg.mode = 12
        msg.gait_id = 0
        msg.duration = 5000
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        ctrl.Wait_finish(12,0)

        msg.mode = 11
        msg.gait_id = 27
        msg.vel_des = [0.0, 0.1, 0.0]
        msg.duration = 1000
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        time.sleep(2)
        

        msg.mode = 11
        msg.gait_id = 27
        msg.vel_des = [0.3, 0.0, 0.0]
        msg.duration = 1500
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        time.sleep(3)

        # 回字路段入库段代码
        msg.mode = 11
        msg.gait_id = 27
        msg.vel_des = [0.0, 0.0, 0.0]
        msg.duration = 3000
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        time.sleep(6.5)

        #16650
        msg.mode = 11  # 从坐标12.90，稍微偏左一点的位置走进库里面
        msg.gait_id = 27  # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26
        msg.vel_des = [0.3, 0.0, 0.0]  # 速度
        msg.rpy_des = [0, 0, 0]  # 身体仰角
        msg.step_height = [0.03, 0.03]
        msg.duration = 7000 
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        time.sleep(20)

        msg.mode = 21
        msg.gait_id = 0
        msg.rpy_des = [0, 0.2, 0]
        msg.duration = 9000
        msg.life_count += 1
        ctrl.Send_cmd(msg)

        while ctrl.mode_ok != 21: 
            time.sleep(0.01)
        camera_thread2 = MiroCameraThread()
        camera_thread2.start()
        time.sleep(10)

        #等待动作和拍照完成
        try:
        # 等待抬头动作完成（根据实际情况调整超时时间）
            # 等待相机线程完成
            camera_thread2.join(timeout=10)
            # 处理拍摄结果
            if camera_thread2.ros_initialized and camera_thread2.image is not None:
            # 保存或显示图像
                camera_thread2.camera.save_image(camera_thread2.image, "camera_image2.png")
            else:
                print("Camera Failed")
    
        except Exception as e:
            print(f"Fail: {e}")

        image = cv2.cvtColor(cv2.imread("camera_image2.png"), cv2.COLOR_BGR2RGB)
        decoded_text = qreader.detect_and_decode(image=image)
        print("解码结果：", decoded_text)

        msg.mode = 12
        msg.gait_id = 0
        msg.duration = 5000
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        ctrl.Wait_finish(12,0)

        # msg.mode = 11  # 起立转90°
        # msg.gait_id = 27  # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26
        # msg.vel_des = [0.0, 0.0, 0.2]  # 速度
        # msg.rpy_des = [0, 0, 0]  # 身体仰角
        # msg.step_height = [0.03, 0.03]
        # msg.duration = 1000
        # msg.life_count += 1
        # ctrl.Send_cmd(msg)
        # time.sleep(9)

        msg.mode = 11  # 从坐标12.90，稍微偏左一点的位置走进库里面
        msg.gait_id = 27  # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26
        msg.vel_des = [0.3, 0.0, 0.0]  # 速度
        msg.rpy_des = [0, 0, 0]  # 身体仰角
        msg.step_height = [0.03, 0.03]
        msg.duration = 9650 
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        time.sleep(20)


        msg.mode = 7  # 入库之后趴5s
        msg.gait_id = 1
        msg.duration = 5000
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        ctrl.Wait_finish(7, 0)

        msg.mode = 12  # 站起来
        msg.gait_id = 0
        msg.life_count += 1 
        ctrl.Send_cmd(msg)
        ctrl.Wait_finish(12, 0)

        msg.mode = 11  # 往后退车出库
        msg.gait_id = 27  # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26
        msg.vel_des = [-0.3, 0.0, 0.0]  # 速度
        msg.rpy_des = [0, 0, 0]  # 身体仰角
        msg.step_height = [0.03, 0.03]
        msg.duration = 3200 
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        time.sleep(6)

        msg.mode = 11  # 起立转90°
        msg.gait_id = 27  # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26
        msg.vel_des = [0.0, 0.0, 0.4]  # 速度
        msg.rpy_des = [0, 0, 0]  # 身体仰角
        msg.step_height = [0.03, 0.03]
        msg.duration = 5040
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        time.sleep(9)

        msg.mode = 11  # 直走去第二个仓库
        msg.gait_id = 27  # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26
        msg.vel_des = [0.3, 0.0, 0.0]  # 速度
        msg.rpy_des = [0, 0, 0]  # 身体仰角
        msg.step_height = [0.03, 0.03]
        msg.duration = 6500 
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        time.sleep(10)

        msg.mode = 11  # 转90°
        msg.gait_id = 27  # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26
        msg.vel_des = [0.0, 0.0, 0.4]  # 速度
        msg.rpy_des = [0, 0, 0]  # 身体仰角
        msg.step_height = [0.03, 0.03]
        msg.duration = 5040
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        time.sleep(9)

        msg.mode = 11  # 倒车进第二个仓库
        msg.gait_id = 27  # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26
        msg.vel_des = [-0.3, 0.0, 0.0]  # 速度
        msg.rpy_des = [0, 0, 0]  # 身体仰角
        msg.step_height = [0.03, 0.03]
        msg.duration = 2880 
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        time.sleep(6)

        msg.mode = 7  # 入库之后趴5s
        msg.gait_id = 1
        msg.duration = 5000
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        ctrl.Wait_finish(7, 0)

        msg.mode = 12  # 站起来
        msg.gait_id = 0
        msg.life_count += 1 
        ctrl.Send_cmd(msg)
        ctrl.Wait_finish(12, 0)

        msg.mode = 11  # 往前走
        msg.gait_id = 27  # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26
        msg.vel_des = [0.3, 0.0, 0.0]  # 速度
        msg.rpy_des = [0, 0, 0]  # 身体仰角
        msg.step_height = [0.03, 0.03]
        msg.duration = 9000 
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        time.sleep(15)

        msg.mode = 7 
        msg.gait_id = 1
        msg.duration = 5000
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        ctrl.Wait_finish(7, 0)

    except KeyboardInterrupt:
        print("\n用户中断")
    except Exception as e:
        print(f"严重错误: {str(e)}")
    finally:
        if ctrl:
            ctrl.quit()
        sys.exit()

if __name__ == '__main__':
    main()