import numpy as np
from numpy import *
import matplotlib
matplotlib.use('TkAgg')
from matplotlib import pyplot as plt
from matplotlib import animation
from mpl_toolkits.mplot3d import Axes3D
from matplotlib import cm
import sys
import os
import random

N = 100
M = 2

def gen_x():
	global N
	x = zeros(N)
	for i in range(N):
		x[i] = random.random()
	return x

def func_y(x):
	global N
	y = zeros(N)
	for i in range(N):
		gamma = random.gauss(0, 0.1)
		y[i] = sin(3*x[i]+2) # gamma
	return y

def y_est(x,a):
	y = sin(a[0]*x+a[1])
	return y

def jacob(x,a):
	j = zeros(2)
	j[0] = cos(a[0]*x+a[1])
	j[1] = x*cos(a[0]*x+a[1])
	return j

# def hess(x,a):
# 	h = zeros([2,2])
# 	h[0][0] = -sin(a[0]*x+a[1])
# 	h[1][0] = -x*sin(a[0]*x+a[1])
# 	h[0][1] = -x*sin(a[0]*x+a[1])
# 	h[1][1] = -x**2*sin(a[0]*x+a[1])
# 	return h

x = gen_x()
y = func_y(x)

# fit to y = sin(a+bx)
# dydx = b*cos(b*x + a)
# ddydx = -b^2*sin(b*x + a)
# 
# jacobian = [cos(b*x+a)	x*cos(b*x+a)]
# 
# hessian = [-sin(b*x+a)	-x*sin(b*x+a)]
# 			[-x*sin(b*x+a)	-x^2*sin(b*x+a)]
# 
# 
nsteps = 2000
# fixed lambda
# a0 = b, a1 = a
a = zeros([nsteps, M])
J = zeros([nsteps, N, M])
H = zeros([nsteps, M, M])

a[0] = [4, 1]
err = 0
alpha = 0.1
lam = 0.01
da = 0

for j in range(nsteps-1):
	JJ = 0
	for i in range(N):
		J[j,i] = jacob(x[i], a[j])
		JJ += J[j,i]/N
		# J[j,i] = np.dot((y_est(x, a[j])-y),x)
	# JJ = sum(J[j,i])
	# J[j] = np.dot((y_est(x, a[j])-y),x)

	# res = sum((y-y_est(x, a[j]))**2)/(2*N)
	e = y_est(x, a[j])-y
	res = np.dot(e.T,e)/(2*N)
	# da = -alpha*np.dot(J[j].T,res)
	da = -alpha*res*JJ
	a[j+1] = a[j] + da
	# H[j] = np.dot(J[j].T,J[j])

	# d = y-y_est(x, a[j])

	# H_lm = H[j] + (lam*eye(M,M))
	# da = -np.dot(np.linalg.inv(H_lm),np.dot(J[j].T,d))
	# # m = diag(0.5*H[j]*(1+lam))
	# # da = -np.inv(m)*np.dot(d,J[j])
	# a[j+1] = a[j]+da

	print res
	# Neil:
	# grad[0] = -2*sum(y-f(x))*df0(x)
	# grad[1] = -2*sum(y-f(x))*df1(x)
	# M = 2x2
	# a = a -np.dot(Minv,grad)

		# err += (y[i]-yxa(x[i],a[j]))/((yxa(x[i],a[j])-y[i])**2)*jacob(x[i],a[j])
	# err *= -2

	# del_chisq = -2*sum((y-yxa(x,a[j]))/((yxa(x,a[j])-y)**2)*jacob(x,a))
	
	# a = a - np.invert(H)*del_x2(a)
	

# j = jacob(1,[1, 1])


# adaptive lambda