import numpy as np
from numpy import *
import matplotlib
matplotlib.use('TkAgg')
from matplotlib import pyplot as plt
from matplotlib import animation
%matplotlib inline
# import scipy
# from scipy import *
# from scipy.optimize import leastsq

h2r = 2.*3.1415926

w = np.multiply([80, 30, 50],h2r)
a = [1.0, 0.4, 0.6]

t = linspace(0,0.25,num=2000)

x = zeros([len(w),len(t)])
for i in range(len(w)):
	x[i]=a[i]*sin(w[i]*t)

sig = sum(x,0)

ts = sort(np.random.rand(60,1)*max(t),axis=0)
xs = np.interp(ts,t,sig)

fig = plt.figure()
ax = plt.axes()
# ax.plot(t,x[0,:],t,x[1,:],t,x[2,:],t,sum(x,0))
ax.plot(t,sig)
ax.plot(ts,xs,'ko')
# ax.plot(t,xfit,'r.')
plt.show()

\[Ax=b\] \[b = \phi f\] \[A = \phi D^{-1}\] \[\phi D^{-1}x-\phi f=0\]

import math
 
def dft(x, inverse = False) :
    N = len(x)
    inv = -1 if not inverse else 1
    X =[0] * N
    for k in xrange(N) :
        for n in xrange(N) :
            X[k] += x[n] * math.e**(inv * 2j * math.pi * k * n / N)
        if inverse :
            X[k] /= N
    return X

fig2 = plt.figure()
ax2 = plt.axes()
fft = dft(sig)
# ax2.plot(arange(len(fft)),fft)
# ax2.hist(,50)
t0 = t[-1]/len(t)
ax2.plot(arange(len(fft))/t0,real(fft))
plt.show()
# fft

# A*x - b = 0
# guess = [1.0,440*3.1415926/180.0,0.4,100*3.1415926/180.0,0.6,2000*3.1415926/180.0]
guess = [0.6,400*h2r,0.5,90*h2r,0.5,700*h2r]
# yy = xx[0]*np.sin(ts*xx[1])+xx[2]*np.sin(ts*xx[3])+xx[4]*np.sin(ts*xx[5])
# print yy
# def func(xx):
# 	return (xx[0]*np.sin(ts*xx[1])+xx[2]*np.sin(ts*xx[3])+xx[4]*np.sin(ts*xx[5])[:,0]

optimize_func = lambda xx: (xx[0]*np.sin(ts*xx[1])+xx[2]*np.sin(ts*xx[3])+xx[4]*np.sin(ts*xx[5])-xs)[:,0]
est = leastsq(optimize_func, guess)[0]
xfit = est[0]*np.sin(t*est[1])+est[2]*np.sin(t*est[3])+est[4]*np.sin(t*est[5])
# Calculate minimum L2 norm of x from SVD
print [a[0],w[0],a[1],w[1],a[2],w[2]]
print est
# Calculate minimum L1 norm of x

# approx L1 norm, minimize exact penalty, increase alpha
# abs(x) ~= 1/alpha*(log(1+e^(-alpha*x) + log(1+e^(alpha*x)) 

# A = np.matrix(np.append(ones([N,1]), x.reshape(N,1), 1))
# U, s, V = np.linalg.svd(A)
# w = np.matrix(np.append(np.diag(one_over(s)), zeros([M, N-M]),1))
# y = np.matrix(y).T
# xx = V.T*w*U.T*y
# b = xx[0,0]
# a = xx[1,0]