import math
import cmath
import numpy

def reponseFreq(b,nf):
    f = numpy.arange(0.0,0.5,0.5/nf)
    g = numpy.zeros(f.size)
    phi = numpy.zeros(f.size)
    for m in range(f.size):
        H = 0.0
        for k in range(b.size):
            H += b[k]*cmath.exp(-1j*2*math.pi*k*f[m])
        g[m] = abs(H)
        phi[m] = cmath.phase(H)
    return (f,g,numpy.unwrap(phi))
                


def signal(t):
    return 1.0*math.cos(2*math.pi*t) \
            +0.5*math.cos(3*2*math.pi*t+math.pi/3) \
            +0.2*math.cos(5*2*math.pi*t+math.pi/5) \
            +0.2*math.cos(10*2*math.pi*t)
                    

import numpy
import random
from matplotlib.pyplot import *

fe=500
T=2.0
N=int(fe*T)
te = T/N
x = numpy.zeros(N)
t = numpy.zeros(N)
sigma = 0.05
for k in range(N):
    t[k] = k*te
    x[k] = signal(t[k])+random.gauss(0,sigma)
figure(figsize=(10,4))
plot(t,x)
grid()
                

from numpy.fft import fft
tfd = fft(x)
freq = numpy.arange(x.size)*1.0/T
spectre = 10*numpy.log10(numpy.absolute(tfd))
spectre = spectre-spectre.max()
figure(figsize=(10,4))
plot(freq,spectre,'r')
axis([0,fe/2,spectre.min(),spectre.max()])
xlabel("f")
ylabel("AdB")
grid()
                

b = numpy.array([0.5,0.5])
(f,g,phi)=reponseFreq(b,100)
figure(figsize=(6,4))
plot(f,g)
xlabel('f/fe')
ylabel('|H|')
axis([0,0.5,0,1])
grid()
                    

figure(figsize=(6,4))
plot(f,phi)
xlabel('f/fe')
ylabel('Phase')
axis([0,0.5,-3,3])
grid()
                    

import scipy.signal
b = np.array([0.5,0.5])
y = scipy.signal.convolve(x,b,mode='valid')
ny = y.size
ty = te+np.arange(ny)*te

figure(figsize=(10,4))
plot(ty,y)
xlabel('t')
ylabel('y')
grid()
                

b = numpy.ones(10)/10.0
(f,g,phi)=reponseFreq(b,100)
figure(figsize=(6,4))
plot(f,g)
xlabel('f/fe')
ylabel('|H|')
axis([0,0.5,0,1])
grid()
                

P_gauss=10
b_gauss = numpy.zeros(2*P_gauss+1)
epsilon=0.01
sigma=P_gauss/math.sqrt(-2.0*math.log(epsilon))
som = 0.0
for k in range(2*P_gauss+1):
    b_gauss[k] = math.exp(-(k-P_gauss)**2/(2*sigma**2))
    som += b_gauss[k]
b_gauss = b_gauss/som
(f,g,phi)=reponseFreq(b_gauss,500)
figure(figsize=(6,4))
plot(f,g)
xlabel('f/fe')
ylabel('|H|')
axis([0,0.5,0,1])
grid()
                    

figure(figsize=(6,4))
plot(f,phi)
xlabel('f/fe')
ylabel('Phase')
grid()
                    

y = scipy.signal.convolve(x,b_gauss,mode='valid')
ny = y.size
ty = (np.arange(ny)+P_gauss)*te
figure(figsize=(10,4))
plot(t,x,'b')
plot(ty,y,'r')
xlabel('t')
ylabel('y')
grid()
                    

P=10
b = numpy.zeros(2*P+1)
def sinc(u):
    if u==0:
        return 1.0
    else:
        return math.sin(u)/u
a=0.15
for k in range(2*P+1):
    b[k] = 2*a*sinc(2*math.pi*(k-P)*a)
(f,g,phi)=reponseFreq(b,500)
figure(figsize=(6,4))
plot(f,g)
xlabel('f/fe')
ylabel('|H|')
axis([0,0.5,0,1.2])
grid()
                    

figure(figsize=(6,4))
plot(f,phi)
xlabel('f/fe')
ylabel('Phase')
grid()
                    

y = scipy.signal.convolve(x,b,mode='valid')
ny = y.size
ty = (np.arange(ny)+P)*te
figure(figsize=(10,4))
plot(t,x,'b')
plot(ty,y,'r')
xlabel('t')
ylabel('y')
grid()
                    

b = numpy.array([1.0,-1.0])
(f,g,phi)=reponseFreq(b,500)
figure(figsize=(6,4))
plot(f,g)
xlabel('f/fe')
ylabel('|H|')
grid()
                    

figure(figsize=(6,4))
plot(f,phi)
xlabel('f/fe')
ylabel('Phase')
grid()
                    

y_deriv = scipy.signal.convolve(x,b,mode='valid')
ny = y_deriv.size
ty = te+np.arange(ny)
figure(figsize=(10,4))
plot(ty,y_deriv,'r')
xlabel('t')
ylabel('y')
grid()
                    

y = scipy.signal.convolve(x,b_gauss,mode='valid')
y_deriv = scipy.signal.convolve(y,b,mode='valid')
ny = y_deriv.size
ty = np.arange(ny)+(P_gauss+1)*te
figure(figsize=(10,4))
plot(ty,y_deriv,'r')
xlabel('t')
ylabel('y')
grid()
                     

b=[0.5,0.5]
a=[1,-1] 
[w,h] = scipy.signal.freqz(b,a)
figure(figsize=(6,4))
plot(w/(2*math.pi),numpy.abs(h))
xlabel('f/fe')
ylabel('|H|')
grid()
                

figure(figsize=(6,4))
plot(numpy.log10(w/(2*math.pi)),20*numpy.log10(numpy.abs(h)))
xlabel('log(f/fe)')
ylabel('GdB')
grid()
                

y = scipy.signal.lfilter(b,a,x)
figure(figsize=(10,4))
plot(t,y)
xlabel('t')
ylabel('y')
grid()