---

import socket
import json
import threading

import cv2
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TwistStamped
from sensor_msgs.msg import LaserScan, Image
from cv_bridge import CvBridge

OPERATOR_IP = '192.168.1.100'
CMD_PORT = 5001
SCAN_PORT = 5002
VIDEO_PORT = 5000
CAMERA_TOPIC = '/camera/image_raw'


def recv_exact(sock, n):
    buf = b''
    while len(buf) < n:
        chunk = sock.recv(n - len(buf))
        if not chunk:
            return None
        buf += chunk
    return buf


class CmdVelBridge(Node):

    def __init__(self):
        super().__init__('socket_cmd_bridge')
        self.pub = self.create_publisher(TwistStamped, '/cmd_vel', 10)
        self.create_subscription(LaserScan, '/scan', self.scan_cb, 10)
        self.min_distance = None

        self.bridge = CvBridge()
        self.last_frame = None
        self.create_subscription(Image, CAMERA_TOPIC, self.image_cb, 10)

    def image_cb(self, msg):
        self.last_frame = self.bridge.imgmsg_to_cv2(msg, 'bgr8')

    def send(self, linear_x, angular_z):
        msg = TwistStamped()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'base_link'
        msg.twist.linear.x = float(linear_x)
        msg.twist.angular.z = float(angular_z)
        self.pub.publish(msg)

    def stop(self):
        self.send(0.0, 0.0)

    def scan_cb(self, msg):
        valid = [r for r in msg.ranges if r > 0.0 and r < float('inf')]
        if valid:
            self.min_distance = min(valid)


def scan_reply_loop(node):
    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    sock.connect((OPERATOR_IP, SCAN_PORT))
    print(f"подключено к оператору {OPERATOR_IP}:{SCAN_PORT} (лидар)")

    try:
        while True:
            request = sock.recv(16)
            if not request:
                break
            distance = node.min_distance if node.min_distance is not None else -1.0
            payload = json.dumps({"distance": distance}).encode('utf-8')
            sock.sendall(len(payload).to_bytes(4, 'big') + payload)
    except (ConnectionResetError, OSError) as e:
        print(f"канал лидара оборван: {e}")
    finally:
        sock.close()


def video_send_loop(node):
    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    sock.connect((OPERATOR_IP, VIDEO_PORT))
    print(f"подключено к оператору {OPERATOR_IP}:{VIDEO_PORT} (видео)")

    try:
        while True:
            if node.last_frame is None:
                continue
            ok, buffer = cv2.imencode('.jpg', node.last_frame, [cv2.IMWRITE_JPEG_QUALITY, 70])
            if not ok:
                continue
            data = buffer.tobytes()
            sock.sendall(len(data).to_bytes(4, 'big') + data)
    except (ConnectionResetError, BrokenPipeError, OSError) as e:
        print(f"канал видео оборван: {e}")
    finally:
        sock.close()


def main():
    rclpy.init()
    node = CmdVelBridge()

    ros_thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True)
    ros_thread.start()

    scan_thread = threading.Thread(target=scan_reply_loop, args=(node,), daemon=True)
    scan_thread.start()

    video_thread = threading.Thread(target=video_send_loop, args=(node,), daemon=True)
    video_thread.start()

    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    sock.connect((OPERATOR_IP, CMD_PORT))
    print(f"подключено к оператору {OPERATOR_IP}:{CMD_PORT}")

    try:
        while True:
            size_bytes = recv_exact(sock, 4)
            if size_bytes is None:
                break
            msg_len = int.from_bytes(size_bytes, 'big')

            payload = recv_exact(sock, msg_len)
            if payload is None:
                break

            command = json.loads(payload.decode('utf-8'))
            node.send(command.get("linear_x", 0.0), command.get("angular_z", 0.0))
    except (ConnectionResetError, OSError) as e:
        print(f"соединение оборвано: {e}")
    except KeyboardInterrupt:
        pass
    finally:
        node.stop()
        sock.close()
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()