import threading
import time
import zmq
import numpy as np
from loguru import logger
from simple_pid import PID
from utils import PidWrap

bycmd = None
move = None

def import_obj(_bycmd):
    global bycmd
    global move

    bycmd = _bycmd
    move = move_cls()

class lane_cls:
    def __init__(self, _speed, _time, _pid_argv = {"kp" : 1.2, "ki" : 0, "kd" : 0}) -> None:
        self.context = zmq.Context()
        self.socket = self.context.socket(zmq.REQ)
        self.socket.connect("tcp://localhost:6666")
        self.speed = _speed
        self.time = _time
        # 每 20ms 控制一次，故次数乘以 50
        self.count = self.time * 50
        logger.info(f"count = {self.count}")
        self.busy = False
        self. pid = PidWrap(_pid_argv["kp"], _pid_argv["ki"], _pid_argv["kd"], output_limits=50)
        pass
    def loop(self):
        while self.count >= 0:
            self.count -= 1

            self.socket.send_string("")
            resp = self.socket.recv_pyobj()
            # 检查该值是否有效
            ck_val = resp.get('data')
            if isinstance(ck_val, np.ndarray):  
                x = ck_val[0]
                y = ck_val[1]
            else:
                continue
            err = x - 160

            pid_out = self.pid.get(err)
            bycmd.send_speed_omega(pid_out)

            # logger.debug(f"[任务中巡线模式]#  error:{err} out:{pid_out}")
            time.sleep(0.012) # 时间补偿，保证控制周期 (虽然还是挺不准的)

        for _ in range(3):
            bycmd.send_speed_x(0)
            bycmd.send_speed_omega(0)
        self.busy = False
        logger.info("[任务中巡线模式] #结束")
        return

    def start(self):
        logger.info(f"[任务中巡线模式] #开启，速度：{self.speed} 时间：{self.time}")
        bycmd.send_speed_x(self.speed)
        self.busy = True
        thread = threading.Thread(target=self.loop)
        thread.start()
        pass
    def inquire(self):
        # logger.debug(f"[任务中巡线模式]# 查询 当前 busy 状态 {self.busy}")
        return self.busy

class move_cls:
    # def x(self, _speed):
    #     bycmd.send_speed_x(_speed)
    #     pass
    # def y(self, _speed):
    #     bycmd.send_speed_y(_speed)
    #     pass
    # def z(self, _speed):
    #     bycmd.send_speed_omega(_speed)
    #     pass
    def lane(self, _speed, _time):
        lane_obj = lane_cls(_speed, _time)
        lane_obj.start()
        # 此处为阻塞实现，非阻塞调用可直接从 lane_cls 类构造对象，然后使用查询方法
        # time.sleep(0.5) # 等待子线程启动
        while lane_obj.inquire() is True:
            pass

# TODO 新运动指令类，指令的发送和完成查询功能，发送时开启新线程

#  TODO 轴控制指令类，增加指令后加入动作队列，非阻塞

class axis_cls():
    def axis_z(self, _distance):
        pass
    def axis_z2(self, _position):
        pass
    def exec(self):
        pass