import socket
import time

class VisionClient:
    def __init__(self, host='192.168.30.200', port=9527):
        self.host = host
        self.port = port
        self.timeout = 10  # 10秒超时
    
    def connect(self):
        """连接视觉服务端"""
        try:
            self.client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            self.client_socket.settimeout(self.timeout)
            self.client_socket.connect((self.host, self.port))
            print(f"成功连接到视觉服务端 {self.host}:{self.port}")
            return True
        except Exception as e:
            print(f"连接视觉服务端失败: {e}")
            return False
    
    def capture_image(self):
        """拍照功能"""
        try:
            # 发送拍照指令
            command = "capture"
            self.client_socket.send(command.encode('utf-8'))
            
            # 接收响应
            response = self.client_socket.recv(1024).decode('utf-8').strip()
            
            if response == "NG":
                print("拍照失败: 相机无法拍摄")
                return None
            else:
                try:
                    image_id = int(response)
                    image_url = f"http://{self.host}:8080/imageFile/{image_id}.jpg"
                    print(f"拍照成功! 图片ID: {image_id}")
                    print(f"图片查看地址: {image_url}")
                    return image_id
                except ValueError:
                    print(f"无效的响应: {response}")
                    return None
                    
        except Exception as e:
            print(f"拍照过程中发生错误: {e}")
            return None
    
    def capture_and_recognize(self, camera_pose, hand_eye):
        """
        拍照并识别功能
        
        参数:
        camera_pose: 相机姿态 [x, y, z, rx, ry, rz] (单位: mm, 度)
        hand_eye: 相机手眼参数 [x, y, z, rx, ry, rz] (单位: mm, 度)
        """
        try:
            # 组合请求参数
            params = list(camera_pose) + list(hand_eye)
            request_str = " ".join(str(x) for x in params)
            
            # 发送请求
            self.client_socket.send(request_str.encode('utf-8'))
            
            # 接收响应
            response = self.client_socket.recv(1024).decode('utf-8').strip()
            
            if response == "NG":
                print("拍照识别失败")
                return None
            else:
                try:
                    # 解析返回的姿态数据
                    pose_data = [float(x) for x in response.split()]
                    print(f"识别成功! Mark板姿态: {pose_data}")
                    
                    # 提取推荐的x, y, rz参数
                    if len(pose_data) >= 6:
                        x, y, z, rx, ry, rz = pose_data
                        print(f"推荐使用参数 - X: {x:.3f}m, Y: {y:.3f}m, Rz: {rz:.3f}rad")
                    
                    return pose_data
                    
                except ValueError as e:
                    print(f"解析响应数据失败: {e}, 原始响应: {response}")
                    return None
                    
        except Exception as e:
            print(f"拍照识别过程中发生错误: {e}")
            return None
    
    def close(self):
        """关闭连接"""
        if hasattr(self, 'client_socket'):
            self.client_socket.close()
            print("连接已关闭")

# 使用示例
if __name__ == "__main__":
    # 创建视觉客户端实例
    vision_client = VisionClient()
    
    try:
        # 连接服务端
        if vision_client.connect():
            # 示例1: 简单拍照
            print("\n=== 示例1: 简单拍照 ===")
            image_id = vision_client.capture_image()
            
            # 示例2: 拍照并识别
            print("\n=== 示例2: 拍照并识别 ===")
            # 相机姿态: [x, y, z, rx, ry, rz] (mm, 度)
            camera_pose = [0.5, 0.5, 0.3, 180, 0, 0]  # 注意高度设置为300mm
            # 手眼参数: [x, y, z, rx, ry, rz] (mm, 度)
            hand_eye = [0, 0, 0.1, 0, 0, -90]
            
            result = vision_client.capture_and_recognize(camera_pose, hand_eye)
            
    except Exception as e:
        print(f"示例运行出错: {e}")
    finally:
        vision_client.close()