
from class_sensorcmd import *
from sensorPara import *
import struct


# 电容数据存储结构
class capData:
    def __init__(self):
        self.sensorIndex = 0            # 电容序号，与iic addr相同
        self.channelCapData = list()    # 原始通道数值
        self.tf = list()                # 切向力数组
        self.tfDir = list()             # 切向力方向数组
        self.nf = list()                # 法向力数组
        self.sProxCapData = list()      # 接近(自电容)数组
        self.mProxCapData = list()      # 接近（互电容）数组
        self.cnt = 0                                    # 计数，测试用

    def init(self, addr, yddsNum, sProxNum, mProxNum, capChannelNum):
        self.sensorIndex = addr                         # 电容序号，与iic addr相同
        self.channelCapData = list(range(capChannelNum))# 原始通道数值
        self.tf = list(range(yddsNum))                  # 切向力数组
        self.tfDir = list(range(yddsNum))               # 切向力方向数组
        self.nf = list(range(yddsNum))                  # 法向力数组
        self.sProxCapData = list(range(sProxNum))       # 接近(自电容)数组
        self.mProxCapData = list(range(mProxNum))       # 接近（互电容）数组
        self.cnt = 0                                    # 计数，测试用

    def deinit(self):
        self.channelCapData = None
        self.tf = None
        self.tfDir = None
        self.nf = None
        self.sProxCapData = None
        self.mProxCapData = None
        
# 传感器类：包换传感器相关参数
class ClassFinger:
    def __init__(self, pca_idx, ch341):
        self.snsCmd = ClassSensorCmd(ch341)
        self.pcaIdx = pca_idx   # iic使能芯片序号，从2开始
        self.readData = capData()
        self.disconnected()

    # 检查传感器是否连接，如果读写地址正确则认为连接正常
    def checkSensor(self):
        # 广播的方式读取当前传感器地址，并默认将iic地址配置为和pca相同的地址
        addrRead = self.snsCmd.getAddr(0) 
        if addrRead != self.pcaIdx:
            if self.pcaIdx != self.snsCmd.setAddr(addrRead, self.pcaIdx):
                print(f"set addr false, setaddr={self.pcaIdx}")
                return False
            else:
                addrRead = self.pcaIdx
                print(f"update iic addr, new addr ={addrRead}")
        
        # 设置发送数据类型为原始值
        if True != self.snsCmd.setSensorSendType(addrRead, 0):
            print(f"setSensorSendType err, addr = {addrRead}")

        # 设置电容采集序列，这里按照地址来分配采集时序，只要每个传感器不同即可
        if self.snsCmd.setSensorCapOffset(addrRead, addrRead) != True:
            print(f"setSensorCapOffset err, addr = {addrRead}")
        
        # 实际用户使用中只需要根据使用的传感器来定义参数即可，不需要读取项目号
        projectRead = self.snsCmd.getSensorProjectIdex(addrRead)
        print(f"project={projectRead}")
        findProjectFlg = False
        if projectRead > 0:
            for pro in finger_params:
                if pro.prg == projectRead:
                    self.projectPara = pro
                    print(f"finger connected, project = {self.projectPara.name}")
                    findProjectFlg = True

        if not findProjectFlg:
            raise RuntimeError(
                f"Unknown sensor project ID: {projectRead}. "
                f"Expected one of: {[p.prg for p in finger_params]}. "
                "Check sensorPara.py or update sensor firmware."
            )

        self.connected(addrRead)
        
        return True

    # 传感器连接，初始化参数
    def connected(self, addr):
        self.addr = addr
        self.connect = True
        self.connectTimer = time.time()
        self.packIdx = 0
        self.data = list()
        self.data.extend(range(self.projectPara.pack_len))#

        self.readData.init(addr, self.projectPara.ydds_num, self.projectPara.s_prox_num, self.projectPara.m_prox_num, self.projectPara.sensor_num)

        #print(f"datalen={len(self.data)}")

    # 传感器断开,复位参数
    def disconnected(self):
        self.addr = 0xFF   # iic地址
        self.connect = False   # 连接标志位
        self.packIdx = 0     # 采样序号
        self.connectTimer = 0   # 连接超时计时

        self.projectPara = finger_params[0]

        self.readData.deinit()

    def capRead(self):
        for retry in range(0, 3):
            if self.snsCmd.getSensorCapData(self.addr, self.data) == True:
                if self.data[5] != self.projectPara.sensor_num:
                    print(f"cap channel num err, read num = {self.data[5]}, expect num = {self.projectPara.sensor_num}")
                
                if self.data[4] != self.packIdx:
                    self.packIdx = self.data[4]
                    self.connectTimer = time.time()

                    # 根据通道值占用字节大小获取通道数据
                    if self.projectPara.cap_byte == 4:
                        for j in range(0, self.projectPara.sensor_num):
                            self.readData.channelCapData[j] = ((self.data[6 + j * self.projectPara.cap_byte] & 0xFF) +
                                                            ((self.data[6 + j * self.projectPara.cap_byte + 1] & 0xFF) << 8) +
                                                            ((self.data[6 + j * self.projectPara.cap_byte + 2] & 0xFF) << 16) +
                                                            ((self.data[6 + j * 4 + 3] & 0xFF) << 24))
                    else:
                        for j in range(0, self.projectPara.sensor_num):
                            self.readData.channelCapData[j] = ((self.data[6 + j * self.projectPara.cap_byte] & 0xFF) +
                                                            ((self.data[6 + j * self.projectPara.cap_byte + 1] & 0xFF) << 8) +
                                                            ((self.data[6 + j * self.projectPara.cap_byte + 2] & 0xFF) << 16))

                    yddsOffset = 6 + self.projectPara.sensor_num*self.projectPara.cap_byte

                    if self.projectPara.ydds_type == 2:
                        struct_size = sizeof(DynamicYddsComTs)
                        for i in range(self.projectPara.ydds_num):
                            offset = yddsOffset + i * struct_size
                            struct_data = self.data[offset : offset + struct_size]
                            struct_data = [value & 0xFF for value in struct_data]
                            struct_data = bytes(struct_data)  # 转换为 bytes 类型
                            instance = DynamicYddsComTs.from_buffer_copy(struct_data)
                            self.readData.nf[i] = instance.nf
                            self.readData.tf[i] = instance.tf
                            self.readData.tfDir[i] = instance.tfDir
                            self.readData.sProxCapData[i] = instance.prox
                    elif self.projectPara.ydds_type == 4:
                        struct_size = sizeof(DynamicYddsU16Ts)
                        for i in range(self.projectPara.ydds_num):
                            offset = yddsOffset + i * struct_size
                            struct_data = self.data[offset : offset + struct_size]
                            # print(f"struct={struct_data}")
                            struct_data = [value & 0xFF for value in struct_data]
                            struct_data = bytes(struct_data)  # 转换为 bytes 类型
                            instance = DynamicYddsU16Ts.from_buffer_copy(struct_data)
                            self.readData.nf[i] = instance.nf/1000.0
                            self.readData.tf[i] = instance.tf/1000.0
                            self.readData.tfDir[i] = instance.tfDir
                        sProxOffset = yddsOffset + self.projectPara.ydds_num*struct_size
                        for i in range(self.projectPara.s_prox_num):
                            self.readData.sProxCapData[i] = ((self.data[sProxOffset + i*self.projectPara.cap_byte] & 0xFF) +
                                                            ((self.data[sProxOffset + i*self.projectPara.cap_byte + 1] & 0xFF) << 8) +
                                                            ((self.data[sProxOffset + i*self.projectPara.cap_byte + 2] & 0xFF) << 16))
                        mProxOffset = yddsOffset + self.projectPara.ydds_num*struct_size
                        for i in range(self.projectPara.m_prox_num):
                            self.readData.mProxCapData[i] = ((self.data[mProxOffset + i*self.projectPara.cap_byte] & 0xFF) +
                                                            ((self.data[mProxOffset + i*self.projectPara.cap_byte + 1] & 0xFF) << 8) +
                                                            ((self.data[mProxOffset + i*self.projectPara.cap_byte + 2] & 0xFF) << 16))


                    # print(f"capChannelDat={self.readData.channelCapData}")
                    # for i in range(self.projectPara.ydds_num):
                    #     print(f"nf[{i}] = {self.readData.nf[i]}")
                    #     print(f"tf[{i}] = {self.readData.tf[i]}")
                    #     print(f"tfDir[{i}] = {self.readData.tfDir[i]}")
                    # for i in range(self.projectPara.s_prox_num):
                    #     print(f"sProxCapData[{i}] = {self.readData.sProxCapData[i]}")
                    # for i in range(self.projectPara.m_prox_num):
                    #     print(f"mProxCapData[{i}] = {self.readData.mProxCapData[i]}")

                break
            else:
                pass

        # 2S未接收到数据超时
        if (time.time() - self.connectTimer) > 2:
            self.disconnected()
