import socket
from time import sleep
import time
import threading
import logging
from logging.handlers import RotatingFileHandler

def setup_logger(log_file='app.log'):
    # 创建日志记录器
    logger = logging.getLogger('my_app')
    logger.setLevel(logging.DEBUG)
    # 创建格式化器
    formatter = logging.Formatter('%(asctime)s - %(name)s - %(levelname)s - %(message)s')
    # 创建旋转文件处理器
    # maxBytes=50MB (50 * 1024 * 1024), backupCount=5
    handler = RotatingFileHandler(
        log_file,
        maxBytes=50 * 1024 * 1024,  # 50MB
        backupCount=5,  # 保留5个备份文件
        encoding='utf-8'
    )
    handler.setFormatter(formatter)
    # 添加处理器到日志记录器
    logger.addHandler(handler)
    return logger
def ServoP(client, pt_floats):
    tcp_command = f"ServoP({pt_floats[0]},{pt_floats[1]},{pt_floats[2]},{pt_floats[3]},{pt_floats[4]},{pt_floats[5]}, t=0.004, aheadtime=80, gain=500)"
    tcp_command = tcp_command.encode()
    client.send(tcp_command)

def ServoJ(client, J1_angle):
    tcp_command = "servoj(0,%f,%f,0,0,0,t=0.008,aheadtime=20, gain=200)" % (J1_angle,J1_angle/2)
    tcp_command = tcp_command.encode()
    client.sendall(tcp_command)
    # buf = client.recv(200).decode()  # 接收反馈信息的长度
    # print(buf)

def servoj_2(client, J1_angle):
    tcp_command = "servoj(%f,0,0,0,0,0,t=2,aheadtime=50, gain=500)" % (J1_angle)
    tcp_command = tcp_command.encode()
    print(tcp_command)
    client.send(tcp_command)

def EnableRobot(client):
    tcp_command = "EnableRobot()"
    tcp_command = tcp_command.encode()
    client.send(tcp_command)
    buf = client.recv(200).decode()  # 接收反馈信息的长度
    index_str = buf[buf.find('{') + 1:buf.find('}')]
    if (index_str == ''):
        return -1
    return buf[buf.find('{') + 1:buf.find('}')]
stop = True
def enableStatus(client):
    global stop
    logger = setup_logger()
    servoj_2(client, 0)
    sleep(3)
    J1 = 0
    bias = 0.04
    pl = bias
    while stop:
        time_home = time.time()
        if J1 > 60 and bias == pl:
            bias = -pl
        elif J1 < -1 and bias == -pl:
            bias = pl
        J1 = J1 + bias
        ServoJ(client, J1)
        time.sleep(0.007)
        tie = time.time()-time_home
        logger.info(f"time: {tie}")
        print(tie)

SERVER3_ADDRESS = '192.168.5.1'
SERVER3_PORT = 29999
client_socket1 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)

# 设置TCP_NODELAY禁用Nagle算法
client_socket1.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
client_socket1.setsockopt(socket.SOL_SOCKET, socket.SO_SNDBUF, 100000)

client_socket1.connect((SERVER3_ADDRESS, SERVER3_PORT))
#client_socket1.setblocking(False)
print('成功连接到第1个服务端')
#enableStatus(client_socket1)
def worker():  #子线程接受模式
    global stop
    while True:
        try:
            data = client_socket1.recv(1024).decode()
            if data:
                print(data)
                index_str = data[data.find('{') + 1:data.find('}')]
                if (index_str == ''):
                    stop = False
                    print(11111111111)
                else:
                    stop = True
        except BlockingIOError:
            time.sleep(0.001)
time_h = time.time()
try:
    EnableRobot(client_socket1)
    feed_thread = threading.Thread(
        target=worker)  # 机器状态反馈线程
    feed_thread.daemon = True
    feed_thread.start()
    enableStatus(client_socket1)
except KeyboardInterrupt:
    print("用户中断操作。")
    print((time.time()-time_h)/60)
except Exception as e:
    print("发生异常：", e)
    print((time.time() - time_h) / 60)

print((time.time()-time_h)/60)