---

import socket
import json
import threading
import math

import cv2
import numpy as np
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
SONAR_PORT = 5003
CAMERA_TOPIC = '/camera/image_raw'

SONAR_SIZE = 500
SONAR_RANGE = 3.0


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.last_scan = None

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

        self.sweep_angle = 0.0

    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)
        self.last_scan = msg

    def render_sonar(self):
        center = SONAR_SIZE // 2
        frame = np.zeros((SONAR_SIZE, SONAR_SIZE, 3), dtype=np.uint8)

        for ring in (1, 2, 3):
            radius = int(center * ring / 3)
            cv2.circle(frame, (center, center), radius, (0, 70, 0), 1)
        cv2.line(frame, (center, 0), (center, SONAR_SIZE), (0, 70, 0), 1)
        cv2.line(frame, (0, center), (SONAR_SIZE, center), (0, 70, 0), 1)

        if self.last_scan is not None:
            msg = self.last_scan
            angle = msg.angle_min
            for r in msg.ranges:
                if 0.0 < r < msg.range_max and r <= SONAR_RANGE:
                    px = int(center + r * math.cos(angle) * center / SONAR_RANGE)
                    py = int(center - r * math.sin(angle) * center / SONAR_RANGE)
                    if 0 <= px < SONAR_SIZE and 0 <= py < SONAR_SIZE:
                        cv2.circle(frame, (px, py), 2, (0, 255, 0), -1)
                angle += msg.angle_increment

        sweep_x = int(center + center * math.cos(self.sweep_angle))
        sweep_y = int(center - center * math.sin(self.sweep_angle))
        cv2.line(frame, (center, center), (sweep_x, sweep_y), (0, 255, 0), 2)

        cv2.circle(frame, (center, center), 5, (0, 255, 0), -1)

        self.sweep_angle += 0.12
        if self.sweep_angle > 2 * math.pi:
            self.sweep_angle -= 2 * math.pi

        return frame


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 sonar_send_loop(node):
    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    sock.connect((OPERATOR_IP, SONAR_PORT))
    print(f"подключено к оператору {OPERATOR_IP}:{SONAR_PORT} (сонар)")

    try:
        while True:
            frame = node.render_sonar()
            ok, buffer = cv2.imencode('.jpg', frame, [cv2.IMWRITE_JPEG_QUALITY, 80])
            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()

    map_thread = threading.Thread(target=sonar_send_loop, args=(node,), daemon=True)
    map_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()