'''
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 utils.robot_control_cmd_lcmt import robot_control_cmd_lcmt
from utils.robot_control_response_lcmt import robot_control_response_lcmt

def main():
    Ctrl = Robot_Ctrl()
    Ctrl.run()
    msg = robot_control_cmd_lcmt()

    try:

#起立
        msg.mode = 12 
        msg.gait_id = 0
        msg.life_count += 1 # Command will take effect when life_count update
        Ctrl.Send_cmd(msg)
        Ctrl.Wait_finish(12, 0)

#横着走向库位1
        msg.mode = 11 
        msg.gait_id = 3
        msg.vel_des = [0.0,0.2,0.0]
        msg.duration = 4500
        msg.step_height = [0.06, 0.06]
        msg.life_count += 1
        Ctrl.Send_cmd(msg)
        time.sleep(5)

#直走进入库位1
        msg.mode = 11                # 旋转1
        msg.gait_id = 3             # 自变频Trot步态
        msg.vel_des = [0.3, 0.0, 0.0]   # 顺时针旋转（负Z轴速度）
        msg.duration = 3000          # 执行9.5秒后自动停止
        msg.step_height = [0.06, 0.06]
        msg.life_count += 1
        Ctrl.Send_cmd(msg)
        time.sleep(4)

#趴下卸货
        msg.mode = 7    # PureDamper
        msg.gait_id = 1
        msg.life_count += 1
        Ctrl.Send_cmd(msg)
        Ctrl.Wait_finish(7, 0)
        time.sleep(10)   


#起立
        msg.mode = 12 # Recovery stand
        msg.gait_id = 0
        msg.life_count += 1 # Command will take effect when life_count update
        Ctrl.Send_cmd(msg)
        Ctrl.Wait_finish(12, 0)


#退出库1
        msg.mode = 11                # 旋转1
        msg.gait_id = 3             # 自变频Trot步态
        msg.vel_des = [-0.3, 0.0, 0.0]   # 顺时针旋转（负Z轴速度）
        msg.duration = 3300          # 执行9.5秒后自动停止
        msg.step_height = [0.06, 0.06]
        msg.life_count += 1
        Ctrl.Send_cmd(msg)
        time.sleep(4)

#向库2走
        msg.mode = 11
        msg.gait_id = 3
        msg.vel_des = [0.0,-0.2,0.0]
        msg.duration = 10000
        msg.step_height = [0.06,0.06]
        msg.life_count += 1
        Ctrl.Send_cmd(msg)
        time.sleep(11)

#进入库2
        msg.mode = 11                # 旋转1
        msg.gait_id = 3             # 自变频Trot步态
        msg.vel_des = [0.3, 0.0, 0.0]   # 顺时针旋转（负Z轴速度）
        msg.duration = 3000          # 执行9.5秒后自动停止
        msg.step_height = [0.06, 0.06]
        msg.life_count += 1
        Ctrl.Send_cmd(msg)
        time.sleep(4)

#趴下
        msg.mode = 7    # PureDamper
        msg.gait_id = 1
        msg.life_count += 1
        Ctrl.Send_cmd(msg)
        Ctrl.Wait_finish(7, 0)
        time.sleep(10)

#起立
        msg.mode = 12 
        msg.gait_id = 0
        msg.life_count += 1 # Command will take effect when life_count update
        Ctrl.Send_cmd(msg)
        Ctrl.Wait_finish(12, 0)

#退出库2
        msg.mode = 11                # 旋转1
        msg.gait_id = 3             # 自变频Trot步态
        msg.vel_des = [-0.3, 0.0, 0.0]   # 顺时针旋转（负Z轴速度）
        msg.duration = 3300          # 执行9.5秒后自动停止
        msg.step_height = [0.06, 0.06]
        msg.life_count += 1
        Ctrl.Send_cmd(msg)
        time.sleep(4)

#走向限高杆赛段
        msg.mode = 11
        msg.gait_id = 3
        msg.vel_des = [0.0,0.2,0.0]
        msg.duration = 8700
        msg.step_height = [0.06,0.06]
        msg.life_count += 1
        Ctrl.Send_cmd(msg)
        time.sleep(10)

#旋转180度
        msg.mode = 11
        msg.gait_id = 3
        msg.vel_des = [0.0,0.0,0.4]
        msg.duration = 9050
        msg.step_height = [0.06,0.06]
        msg.life_count += 1
        Ctrl.Send_cmd(msg)
        time.sleep(10)
#下面接限高杆步态
       

    except KeyboardInterrupt:
        pass
    Ctrl.quit()
    sys.exit()


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.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.lc_r.subscribe("robot_control_response", self.msg_handler)
        self.send_thread.start()
        self.rec_thread.start()

    def msg_handler(self, channel, data):
        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

    def rec_responce(self):
        while self.runing:
            self.lc_r.handle()
            time.sleep( 0.002 )

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

    def send_publish(self):
        while self.runing:
            self.send_lock.acquire()
            if self.delay_cnt > 20: # Heartbeat signal 10HZ, It is used to maintain the heartbeat when life count is not updated
                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 )

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

    def quit(self):
        self.runing = 0
        self.rec_thread.join()
        self.send_thread.join()

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()


# Main function
if __name__ == '__main__':
    main()


#python3 src/cyberdog_simulator/cyberdog_gazebo/script/launchsim.py

#python3 src/cyberdog_simulator/cyberdog_gazebo/script/launchsim.py