import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile

from std_msgs.msg import Float32

import socket
import struct

class NetRobot(Node):
    def __init__(self):
        super().__init__('net_robot')

        qos = QoSProfile(depth=10)
        self.create_subscription(Float32, '~/drives/j1', self._on_j1, qos)
        self.create_subscription(Float32, '~/drives/j2', self._on_j2, qos)
        self.create_subscription(Float32, '~/drives/j3', self._on_j3, qos)
        self.create_subscription(Float32, '~/drives/j4', self._on_j4, qos)
        self.create_subscription(Float32, '~/drives/j5', self._on_j5, qos)
        self.create_subscription(Float32, '~/drives/j6', self._on_j6, qos)

        self._sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        # self._sock.connect(('tmuller.xyz', 9999))
        self._sock.connect(('127.0.0.1', 9999))

        self._joints = [0.] * 6
        self._prev_joints = [None] * 6

    def _send(self):
        if self._joints != self._prev_joints:
            print('pub')
            self._sock.send(struct.pack('hhhhhh', *[int(j) for j in self._joints]) + b'\n')
            self._prev_joints = self._joints.copy()

    def _on_j1(self, msg):
        self._joints[0] = msg.data
        self._send()

    def _on_j2(self, msg):
        self._joints[1] = msg.data
        self._send()

    def _on_j3(self, msg):
        self._joints[2] = msg.data
        self._send()

    def _on_j4(self, msg):
        self._joints[3] = msg.data
        self._send()

    def _on_j5(self, msg):
        self._joints[4] = msg.data
        self._send()

    def _on_j6(self, msg):
        self._joints[5] = msg.data
        self._send()


def main(args=None):
    rclpy.init(args=args)

    node = NetRobot()

    rclpy.spin(node)

    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
