import threading
import time
import json
import math


class ControlServer(threading.Thread):
    def __init__(self, client, USERLIST, MSLIST, PI):
        threading.Thread.__init__(self)
        self.setDaemon(True)
        self.client = client
        self.userList = USERLIST
        self.mslist = MSLIST
        self.pi = PI

    def Calibration(self):
        # 手动动校准
        speed = 0
        while True:
            speed += 1
            for s in self.mslist:
                for d in s['pinList']:
                    if s['name'] == 'MOTOR':
                        # Set ESC speed via PWM
                        self.pi.set_servo_pulsewidth(
                            d, speed * 1000 / 7 + 1000)
                    else:
                        self.pi.set_servo_pulsewidth(d, 1500)
            time.sleep(2)
            print(speed)
            if speed > 3:
                break

    def changePWM(self, params):
        # 改变占空比
        try:
            # 传来的数据格式同配置
            data = json.loads(params)
            num = data['data']
            for j, s in enumerate(self.mslist):
                for i, d in enumerate(s['pinList']):
                    if s['name'] == 'MOTOR':
                        # Set ESC speed via PWM
                        dd = math.floor(self.ratioPwm(num[j]['value'][i]))
                        self.pi.set_servo_pulsewidth(
                            d, round(dd * 1000 / 7 + 1000))
                        print(dd)
                    else:
                        dds = math.floor(self.ratioSPwm(num[j]['value'][i]))
                        self.pi.set_servo_pulsewidth(d, dds)
                        print(dds)
        except:
            print('参数错误')
        pass

    def ratioPwm(self, v):
        # 换算比例：摇杆范围0-256
        num = 2
        if v >= 133 and v <= 256:
            num = math.floor(((2 / 61) * v - (144 / 61)) * 10) / 10
        elif v >= 0 and v < 123:
            num = math.floor(((4 / 132 * v) - 2) * 10) / 10
        else:
            num = 2
        return num

    def ratioSPwm(self, v):
        #  换算比例：摇杆范围0-256
        num = math.floor((256 / 2000) * 1000) / 1000
        a = (v / num) + 500
        if a > 2500:
            a = 2500
        return a

    def __del__(self):
        if self.client in self.userList:
            self.userList.remove(self.client)
        self.client.close()
        if self.pi is not None:
            self.pi.stop()

    def run(self):
        while True:
            try:
                msg = self.client.recv(1024)
                if not msg:
                    self.userList.remove(self.client)
                    self.client.close()
                    print('clinet exit1:', len(self.userList))
                    break
                self.changePWM(msg)
                # print('客户信息', msg, time.ctime())
            except:
                self.userList.remove(self.client)
                self.client.close()
                print('client exit2:', len(self.userList))
                break

# 校准 ESC
# ESC_GPIO  =  13
# pi.set_servo_pulsewidth(ESC_GPIO,  2000 )  # 最大油门。
# sleep( 2 )
# pi.set_servo_pulsewidth(ESC_GPIO,  1000 )  # 最小油门。
# 睡觉( 2 )