import numpy
from numpy.random import normal
from matplotlib.pyplot import *
from scipy.signal import freqz
from numpy.fft import fft,ifft

def sinus(periode,N):
    return 0.5+0.4*numpy.sin(2*numpy.pi/periode*numpy.arange(N,dtype=numpy.float32))

def creneau(periode,N):
    x = numpy.zeros(N,dtype=numpy.float32)
    y = numpy.sin(2*numpy.pi/periode*numpy.arange(N,dtype=numpy.float32))
    for i in range(N):
        if y[i]>0:
            x[i] = 0.9
        else:
            x[i] = 0.1
    return x

N=1000
periode = 100
V1 = sinus(periode,N)
V2 = creneau(periode,N)

figure()
plot(V1)
plot(V2)
ylim(0,1)
grid()

sigma = 0.005
V1b = V1+normal(0,sigma,N)
V2b = V2+normal(0,sigma,N)
figure()
plot(V1b)
plot(V2b)
ylim(0,1)
grid()

V1_int8 = numpy.array(V1*255,dtype=numpy.uint8)
V2_int8 = numpy.array(V2*255,dtype=numpy.uint8)
V1b_int8 = numpy.array(V1b*255,dtype=numpy.uint8)
V2b_int8 = numpy.array(V2b*255,dtype=numpy.uint8)

figure()
plot(V1_int8)
plot(V2_int8)
ylim(0,255)
grid()

figure()
plot(V1b_int8)
plot(V2b_int8)
ylim(0,255)
grid()

def convolution(V,N,B,P):
    Vp = numpy.zeros(N,dtype=numpy.float32)
    for i in range(N):
        for k in range(-P,P+1):
            if i+k>=0 and i+k<N:
                Vp[i] += V[i+k]*B[P+k]
    return Vp

P=5
B=numpy.ones(2*P+1,dtype=numpy.float32)/(2*P+1)

figure()
w,H = freqz(B)
plot(w/(2*numpy.pi),numpy.absolute(H))
grid()
xlabel("f")


V2bp = convolution(V2b,N,B,P)
V1bp = convolution(V1b,N,B,P)

figure()
plot(V1bp)
plot(V2bp)
ylim(0,1)
grid()

def passeBas(P,epsilon):
    sigma = P/numpy.sqrt(-2*numpy.log(epsilon))
    B = numpy.zeros(2*P+1,dtype=numpy.float32)
    somme = 0
    for k in range(-P,P+1):
        B[k+P] = numpy.exp(-k**2/(2*sigma**2))
        somme += B[k+P]
    for i in range(2*P+1):
        B[i] /= somme
    return B

P=6
B=passeBas(P,0.05)

figure()
w,H = freqz(B)
plot(w/(2*numpy.pi),numpy.absolute(H))
grid()
xlabel("f")

V2bp = convolution(V2b,N,B,P)
V1bp = convolution(V1b,N,B,P)

figure()
plot(V1bp)
plot(V2bp)
ylim(0,1)
grid()

P=1
B=[-1/2,0,1/2]
figure()
w,H = freqz(B)
plot(w/(2*numpy.pi),numpy.absolute(H))
grid()
xlabel("f")

V2bp = convolution(V2b,N,B,P)
V1bp = convolution(V1b,N,B,P)

figure()
plot(V1bp)
plot(V2bp)
ylim(-0.5,0.5)
grid()

def derivateurPasseBas(P,epsilon):
    sigma = P/numpy.sqrt(2*(numpy.log(P)-numpy.log(epsilon)))
    B = numpy.zeros(2*P+1,dtype=numpy.float32)
    somme = 0
    for k in range(-P,P+1):
        B[k+P] = k*numpy.exp(-k**2/(2*sigma**2))
        if k>0:
            somme += B[k+P]
    for i in range(2*P+1):
        B[i] /= somme
    return B

P=5
B=derivateurPasseBas(P,0.05)
V2bp = convolution(V2b,N,B,P)
V1bp = convolution(V1b,N,B,P)

figure()
plot(V1bp)
plot(V2bp)
ylim(-0.5,0.5)
grid()

tfd_V1b = fft(V1b)*2/N
tfd_V2b = fft(V2b)*2/N
frequences = numpy.arange(N)/N

figure()
subplot(211)
plot(frequences,numpy.absolute(tfd_V1b))
grid()
xlabel("f")
subplot(212)
plot(frequences,numpy.absolute(tfd_V2b))
grid()

def filtrageFourierPasseBas(tfd,N,fc):
    tfd_filtre = numpy.array(tfd)
    for i in range(int(N/2)):
        f = i/N
        tfd_filtre[i] = tfd[i]*numpy.exp(-(f/fc)**2*numpy.log(2))
    for i in range(int(N/2),N):
        f = i/N
        tfd_filtre[i] = tfd[i]*numpy.exp(-((1-f)/fc)**2*numpy.log(2))
    return tfd_filtre

fc=0.03
tfd_V1b_filtre = filtrageFourierPasseBas(tfd_V1b,N,fc)
tfd_V2b_filtre = filtrageFourierPasseBas(tfd_V2b,N,fc)
    
figure()
subplot(211)
plot(frequences,numpy.absolute(tfd_V1b_filtre))
grid()
xlabel("f")
subplot(212)
plot(frequences,numpy.absolute(tfd_V2b_filtre))
grid()

V2b_filtre = ifft(tfd_V2b_filtre)*N/2

figure()
plot(V2b_filtre)
grid()

P=15
B=passeBas(P,0.05)

figure()
w,H = freqz(B)
plot(w/(2*numpy.pi),numpy.absolute(H))
grid()
xlabel("f")

def calculVariance(V,N,P):
    B=numpy.ones(2*P+1,dtype=numpy.float32)/(2*P+1)
    A = convolution(V*V,N,B,P)
    B = convolution(V,N,B,P)
    return A-B*B

P=5
variance_V2b = calculVariance(V2b,N,P)

figure()
plot(variance_V2b)
grid()

def minimumVariance(variance,N,P,i):
    kmin = -P
    vmin = 1e10
    for k in range(-P,P+1):
        if i+k>=0 and i+k<N and variance[i+k]<vmin:
            kmin = k
            vmin = variance[i+k]
    return i+kmin
            

def filtrageAdaptatifPasseBas(V,N,B,P):
    variance = calculVariance(V,N,P)
    Vp = numpy.zeros(N,dtype=numpy.float32)
    for i in range(N):
        j = minimumVariance(variance,N,P,i)
        for k in range(-P,P+1):
            if j+k>=0 and j+k<N:
                Vp[i] += V[j+k]*B[P+k]
    return Vp

P=10
B=passeBas(P,0.05)
V2bp = convolution(V2b,N,B,P)
V2bp_adapt = filtrageAdaptatifPasseBas(V2b,N,B,P)

figure()
plot(V2bp_adapt)
plot(V2bp)
grid()
ylim(0,1)

        

show()