#! /usr/bin/env python3
import rospy
import os
import hanqiangpose
import command
import subprocess
import hjsx
import threading
import numpy as np
import multiprocessing
from moveit_msgs.msg import PlanningScene
from dynamic_reconfigure.client import Client
from rospy.exceptions import ROSException
from std_srvs.srv import Empty
from rospy.service import ServiceException

import actionlib_msgs.msg._GoalStatusArray
import sys
import time
import termios
from redis_publisher import Publisher

def wait_for_topic(topic_name, message_type):
    try:
        message = rospy.wait_for_message(topic_name, message_type, timeout=None)
        return message
    except rospy.ROSException as e:
        rospy.logerr(f"等待话题 {topic_name} 时程序被中断: {e}")
        return None
  
def clear_octomap():
    try:
        clear_octomap_service = rospy.ServiceProxy('/clear_octomap', Empty)  # 创建服务代理
        clear_octomap_service()  # 尝试调用服务
        rospy.loginfo("Octomap has been cleared.")
    except rospy.ServiceException as e:
        rospy.logerr(f"Failed to call /clear_octomap service or it is not available: {e}. Skipping operation.")

def check_file():
    file_path = rospy.get_param("folder_path")
    file_path_points = os.path.join(file_path, 'points.txt')
    if os.path.exists(file_path_points):
        result = True
    else:
        rospy.loginfo("焊缝数据不存在，请选择焊缝...")
        result = False
    return result

waited_once = False
tim_list=[]
num = 0

#publisher = Publisher()

if __name__ == "__main__":
    command.load_rviz()                 #启动rviz并在terminal输出相关信息
    #command.launch_rviz_background()   #启动rviz不在terminal输出信息
    rospy.init_node("set_update_paramter_p")

    # 初始化各种类型参数
    rospy.set_param("cishu",0)
    rospy.set_param("time",0)

    rospy.set_param("sign_control",0)
    rospy.set_param("sign_pointcloud",0)
    rospy.set_param("sign_traj_accepted",0)

    rospy.set_param("yaw",np.pi/6)  #内收角（偏航角）
    rospy.set_param("yaw_rate",100) #偏航角过渡速度（角度变化45度/100mm）
    rospy.set_param("pitch_of_Horizontalweld",np.pi/4)  #和底面夹角（俯仰角）
    rospy.set_param("pitch_of_Verticalweld",np.pi/5)
    rospy.set_param("culling_radius",0) #焊缝剔除半径
    rospy.set_param("folder_path","/home/tong/catkin_ws/data/t_xing")       #T型板(1焊缝)   
    #rospy.set_param("folder_path","/home/tong/moveir_ws/data/6_Welds")     #法兰座(6焊缝)

    # 显示参数当前值
    rospy.loginfo("当前参数值:")
    rospy.loginfo("sign_pointcloud = %s", rospy.get_param("sign_pointcloud"))
    rospy.loginfo("yaw = %s",rospy.get_param("yaw")/np.pi*180)
    rospy.loginfo("pitch_of_Horizontalweld = %s",rospy.get_param("pitch_of_Horizontalweld")/np.pi*180)
    rospy.loginfo("pitch_of_Verticalweld = %s",rospy.get_param("pitch_of_Verticalweld")/np.pi*180)
    rospy.loginfo("等待rviz启动")
    wait_for_topic("/execute_trajectory/status",actionlib_msgs.msg.GoalStatusArray)
    
    
    while not rospy.is_shutdown():
        sign_control = str(rospy.get_param("sign_control"))
        if sign_control == "0": 
            if not waited_once:
                print("等待点云数据准备完成...")
                # termios.tcflush(sys.stdin, termios.TCIFLUSH) #清空键盘缓冲区
                # print("---1.加载点云数据并规划---\n---2.仅重新规划路径---\n---按任意键退出程序---")
                
                # user_input=input("请选择1-2(默认enter为2): ")
                # if user_input=="":
                #     user_input='2'

                # rospy.set_param("cishu",rospy.get_param("cishu")+1)
                # if(user_input =='1' or user_input=='2'):
                #     rospy.set_param("sign_control", user_input)
                # else:
                #     command.close_rviz("/usr/bin/python3 /opt/ros/noetic/bin/roslaunch lstrobot_moveit_config_0605 demo.launch")
                #     sys.exit(0)
                waited_once = True
        elif sign_control == "1":
            flag = check_file()
            if flag:
                # 重置参数
                #rospy.set_param("sign_control",0)    #在轨迹规划里重置 防止重复调用
                rospy.set_param("sign_traj_accepted",0)
                # 清除场景
                clear_octomap()
                # 点云计算并发布
                process = multiprocessing.Process(target=command.launch_publish_pointcloud_background)
                process.start()
                #计算焊接顺序和焊接姿态
                hjsx.run()
                hanqiangpose.run()
                command.load_visual()
                # 等待 /move_group/monitored_planning_scene 话题发布消息
                rospy.loginfo("等待场景地图加载完毕...")
                wait_for_topic('/move_group/monitored_planning_scene', PlanningScene)
                rospy.loginfo("场景地图已加载完毕")
                rospy.set_param("sign_pointcloud",1)
                #rospy.loginfo("终止点云发布，关闭发布窗口")
                #input("按任意键继续...")
                
                rospy.loginfo("运行moveitserver2.py")
                process = multiprocessing.Process(target=command.launch_moveit_control_server_background)
                process.start()

                while rospy.get_param("sign_control"):
                    pass
                #运行结束，重置参数
                waited_once = False
            else:
                rospy.set_param("sign_control",0)
                waited_once = False
 
        elif sign_control == "2":
            flag = check_file()
            if flag:
                tim=time.time()
                rospy.set_param("time",0)
                # 重置参数
                #rospy.set_param("sign_control",0)
                rospy.set_param("sign_traj_accepted",0)
                hjsx.run()
                hanqiangpose.run()
                command.load_visual()
                process = multiprocessing.Process(target=command.launch_moveit_control_server_background)
                process.start()

                while rospy.get_param("sign_control"):
                    pass

                tim=time.time()-tim
                tim_list.append(tim)
                num += 1
                print("-------------------------------------")
                print(f'第{num}次重规划：')
                print(tim_list)
                print("")
                #运行结束，重置参数
                waited_once = False
            else:
                rospy.set_param("sign_control",0)
                waited_once = False

        else:
            rospy.loginfo("正在关闭规划程序...")
            command.close_rviz("/usr/bin/python3 /opt/ros/noetic/bin/roslaunch lstrobot_moveit_config_0605 demo.launch")
            exit(0)