import matplotlib
import numpy as np
import matplotlib.pyplot as plt


plt.rcParams['font.sans-serif']=['SimHei']
plt.rcParams['axes.unicode_minus']=False


x=np.arange(0,np.pi,0.0025)
y=np.sin(x)*0.5


# 添加方波
def add_line(x):
    ret=[]
    length=len(x)
    num=length//40
    index=0
    t_old=0
    for i in x:
        t=(index//num)
        if(t_old!=t):
            h=np.random.random_sample()
            t_old=t
        if((t&1)!=0):
            t=index%num
            if(t<(num//2)):
                ret.append(((0.15+h)/num)*(index%num))
            else:
                ret.append(((0.15+h)/num)*(num-index%num))
            # ret.append(0.15+((h)))
        else:
            ret.append(0)
        index+=1
    return ret
y=add_line(x)+y

# 添加噪声
def add_random(x):
    ret=[]
    length=len(x)
    index_table=[]
    for i in range(20):
        index_table.append(int(np.random.random_sample()*10000%length))
    # print(index_table)
    for i in range(length):
        if(i in index_table):
            ret.append(x[i]+np.random.random_sample()-0.5)
        else:
            ret.append(x[i])
    return ret


# 卡尔曼
class kalman:
    LastP=0.02 #上次估算的协方差
    Now_P=0# 当前估算的协方差
    out=0# 输出值
    Kg=0#卡尔曼增益
    Q=0.001# 过程噪声协方差
    # R=.0543# 观测噪声协方差
    R=.00543# 观测噪声协方差
    def calc(self,value:int):
        self.Now_P=self.LastP+self.Q
        self.Kg=self.Now_P/(self.Now_P+self.R)
        self.out=self.out+self.Kg*(value-self.out)
        self.LastP=(1-self.Kg)*self.Now_P
        return self.out
def my_kalman(x):
    ret=[]
    k=kalman()
    for i in x:
        ret.append(k.calc(i))
    return ret


# 1阶低通滤波
def rc_low(x):
    a=0.6
    old=0
    ret=[]
    for i in x:
        t=a*old+(1-a)*i
        old=t
        ret.append(t)
    return ret

# 求导
def der(x):
    old=0
    ret=[]
    for i in x:
        t=i-old
        old=i
        ret.append(t)
    return ret


# 滤波,过滤毛刺
def my_filter(x):
    ret=[]
    sub=[]
    last=0
    index=0
    for i in x:
        cat=x[index:index+10]
        index+=1
        lenth=len(cat)
        if(lenth<10):
            ret.append(0)
            sub.append(0)
            continue
        temp=np.sort(cat)
        head=np.average(cat[:lenth//2])
        tail=np.average(cat[lenth//2:])
        if((temp[lenth//2+1]-temp[lenth//2-1])>0.1):
            if(head>tail):
                last=0
            else:
                last=0.3
        ret.append(last)
        sub.append(0)
    return ret,sub


# 滤波,三角变形
def my_filter2(x):
    def limit(x):
        x*=10
        if(x>0.5):
            return 0.5
        elif(x<0):
            return 0
        else:
            return x
    ret=[]
    sub=[]
    last=0
    index=0
    old=0
    for i in x:
        index+=1
        t=i-old
        old=i
        if(t>0.001):
            last=0.3
        elif(t>0.001):
            last=0
        ret.append(last)
        sub.append(limit(t))
    return ret,sub



# 只滤除毛刺
def my_burr(x):
    ret=[]
    last=0
    index=0
    for i in x:
        cat=x[index:index+10]
        index+=1
        lenth=len(cat)
        if(lenth<10):
            ret.append(0)
            continue
        temp=np.sort(cat)
        ret.append(temp[5])
    return ret





# 去除基线漂移
def my_filter3(x):
    x_burr=my_burr(x)
    x_burr=rc_low(x_burr)
    # ker=my_kalman(x_burr)
    x2=der(x_burr)
    x3=my_burr(x2)
    ret=[]
    last=0
    index=0
    signal=False
    for i in x_burr:
        if((x2[index]>=0.004)):
            signal=True
        elif((x2[index]<=-0.1)):
            signal=False
        if(signal==False):
            last=i
        index+=1
        ret.append(i-last)
    return np.multiply(x3,10),np.add(ret,0.3)



y2=add_random(y)
y3,y4=my_filter3(y2)

def show_xy(xy_list:list):
    figure = plt.figure(figsize=(14,7))
    ax = figure.add_axes([0.1,0.1,0.8,0.8])
    index=0
    for xy in xy_list:
        line,=ax.plot(xy[0],xy[1],)
        line.set_label("index:{d}".format(d=index))
        ax.legend()
        index+=1
    plt.show()
# show_xy([(x,y2),(x,y3),(x,y4)])


# def calc_bytes(a,b,c,i,j,k):
#     ta=((a&((1<<i)-1))<<(16-i))
#     tb=((b&((1<<j)-1))<<(16-i-j))
#     tc=((c&((1<<k)-1))<<(16-i-j-k))
#     t=ta|tb|tc
#     return bytearray([t>>8,t&0xff])

# print(calc_bytes(20,15,13,5,7,4).hex(' '))