---

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 nav_msgs.msg import Odometry
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.x = 0.0
        self.y = 0.0
        self.yaw = 0.0
        self.create_subscription(Odometry, '/odom', self.odom_cb, 10)

        self.sweep_angle = 0.0
        self.sweep_speed = 0.12

    def odom_cb(self, msg):
        self.x = msg.pose.pose.position.x
        self.y = msg.pose.pose.position.y
        q = msg.pose.pose.orientation
        siny = 2 * (q.w * q.z + q.x * q.y)
        cosy = 1 - 2 * (q.y * q.y + q.z * q.z)
        self.yaw = math.atan2(siny, cosy)

    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)

        heading_len = 35
        hx = center
        hy = center - heading_len
        cv2.arrowedLine(frame, (center, center), (hx, hy), (0, 165, 255), 3, tipLength=0.4)

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

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

        return frame

    def render_camera(self):
        if self.last_frame is None:
            return None
        frame = self.last_frame.copy()
        text = f"x={self.x:.2f}  y={self.y:.2f}  yaw={math.degrees(self.yaw):.0f} deg"
        cv2.putText(frame, text, (10, 25), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 0), 3)
        cv2.putText(frame, text, (10, 25), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 1)
        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:
            frame = node.render_camera()
            if frame is None:
                continue
            ok, buffer = cv2.imencode('.jpg', 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))
            if "sweep_speed" in command:
                node.sweep_speed = command["sweep_speed"]
    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()