import socket
import struct
import math
import time

class ModbusRobot:
    def __init__(self, ip="192.168.18.201", port=502):
        self.ip = ip
        self.port = port

    def send_modbus_tcp(self, register, value):
        transaction_id = 0x01
        protocol_id = 0x00
        length = 0x06
        unit_id = 0x01
        function_code = 0x06
        reg_address = register
        reg_value = value
        
        packet = struct.pack('>HHHBBHH', 
                             transaction_id, 
                             protocol_id, 
                             length, 
                             unit_id, 
                             function_code, 
                             reg_address, 
                             reg_value)
        try:
            with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
                s.settimeout(2.0)
                s.connect((self.ip, self.port))
                print(f"Sending: {packet.hex(' ')}")
                s.sendall(packet)
                response = s.recv(1024)
                return response
        except Exception as e:
            print(f"Error setting: {e}")

    def send_modbus_tcp_32bit(self, start_register, value):
        transaction_id = 0x01
        protocol_id = 0x00
        unit_id = 0x01
        
        low_word = (value >> 16) & 0xFFFF
        high_word = value & 0xFFFF
        function_code = 0x10
        quantity_of_registers = 2
        byte_count = 4
        length = 11

        packet = struct.pack('>HHHBBHHBHH', 
                             transaction_id, 
                             protocol_id, 
                             length, 
                             unit_id, 
                             function_code, 
                             start_register * 2, 
                             quantity_of_registers, 
                             byte_count,
                             high_word, 
                             low_word)
        try:
            with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
                s.settimeout(2.0)
                s.connect((self.ip, self.port))
                print(f"Send 32-bit ({value}): {packet.hex(' ')}")
                s.sendall(packet)
                response = s.recv(1024)
                print(f"Answer: {response.hex(' ')}")
                return response
        except Exception as e:
            print(f"Error setting: {e}")

    def read_modbus_tcp_registers(self, register, count):
        transaction_id = 0x02
        protocol_id = 0x00
        length = 0x06
        unit_id = 0x01
        function_code = 0x04
        
        packet = struct.pack('>HHHBBHH', 
                             transaction_id, 
                             protocol_id, 
                             length, 
                             unit_id, 
                             function_code, 
                             register, 
                             count)
        try:
            with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
                s.settimeout(2.0)
                s.connect((self.ip, self.port))
                
                print(f"send request (FC04): {packet.hex(' ')}")
                s.sendall(packet)
                response = s.recv(1024)
                
                if len(response) < 9:
                    print("Error: short message")
                    return None
                
                byte_count = response[8]
                register_data = response[9:]
                num_registers = byte_count // 2
                values = struct.unpack('>' + 'H' * num_registers, register_data)
                
                print(f"answer success: {response.hex(' ')}")
                print(f"Values кregistr (с {register}): {values}")
                return values
        except Exception as e:
            print(f"Error while reading: {e}")



    def _get_circle_points(self, X0, Y0, R):
        num_segments = 100
        points = []    
        for K in range(num_segments + 1):
            angle = (2 * math.pi * K) / num_segments
            Xk = X0 + R * math.cos(angle)
            Yk = Y0 + R * math.sin(angle)
            points.append((Xk, Yk))
        return points

    def run_circle_XY_motion(self, X0, Y0, Z0, R, delay_ms, repeat_count=0):
        points = self._get_circle_points(X0, Y0, R)
        z_scaled = int(Z0 * 1000)
        iteration = 0
        while True:
            if repeat_count > 0 and iteration >= repeat_count:
                break
            for x, y in points:
                x_scaled = int(x * 1000)
                y_scaled = int(y * 1000)
                self.send_modbus_tcp_32bit(0x02, x_scaled)        
                self.send_modbus_tcp_32bit(0x03, y_scaled)
                self.send_modbus_tcp_32bit(0x04, z_scaled)
                self.send_modbus_tcp_32bit(0x08, 0)
                time.sleep(delay_ms / 1000.0)
            iteration += 1
            print(f"Completed circle {iteration}")



    def _get_circle_points_in_plane(self, X0, Y0, Z0, R, vector_u, vector_v):
        #vector_u, vector_v - два ортогональных вектора (длиной 1), задающих ориентацию плоскости
        num_segments = 100
        points = []
        # Нормализация векторов
        mag_u = math.sqrt(sum(i**2 for i in vector_u))
        mag_v = math.sqrt(sum(i**2 for i in vector_v))
        u = [i/mag_u for i in vector_u]
        v = [i/mag_v for i in vector_v]
        
        for K in range(num_segments + 1):
            angle = (2 * math.pi * K) / num_segments
            cos_a = math.cos(angle)
            sin_a = math.sin(angle)
            
            # P = P0 + R*cos(a)*u + R*sin(a)*v
            xk = X0 + R * cos_a * u[0] + R * sin_a * v[0]
            yk = Y0 + R * cos_a * u[1] + R * sin_a * v[1]
            zk = Z0 + R * cos_a * u[2] + R * sin_a * v[2]
            points.append((xk, yk, zk))
        return points

    def run_circle_motion(self, X0, Y0, Z0, R, delay_ms, vector_u, vector_v, repeat_count=0):
        points = self._get_circle_points_in_plane(X0, Y0, Z0, R, vector_u, vector_v)
        iteration = 0
        while True:
            if repeat_count > 0 and iteration >= repeat_count:
                break
            for x, y, z in points:
                self.send_modbus_tcp_32bit(0x02, int(x * 1000))
                self.send_modbus_tcp_32bit(0x03, int(y * 1000))
                self.send_modbus_tcp_32bit(0x04, int(z * 1000))
                self.send_modbus_tcp_32bit(0x08, 0)
                time.sleep(delay_ms / 1000.0)
            iteration += 1