%matplotlib inline

import numpy as np
import numpy.linalg as linalg
import matplotlib as mpl
import matplotlib.pyplot as plt
from sklearn import manifold
from mpl_toolkits.mplot3d import Axes3D
from scipy.optimize import minimize
import scipy.optimize 
from scipy.spatial import Delaunay

# parameter points
alphas = np.array([[0,0],[1,0],[0,1]])

# dim = 1+#dim parameters = dim of embedding
dim = 3 

# for starters, define a parametrized family of functions explicitly and draw from it
f = lambda alpha: lambda x : np.exp(-0.5*(x-alpha[0])**2 / (0.5*alpha[1]+1)**2)/np.sqrt(2*np.pi) / (0.5*alpha[1]+1)
dists = []
for alpha in alphas:
    dists.append( f(alpha) )

q = lambda h: (lambda x: np.sqrt(h(x)))

def inner_product(q1,q2, xmin=-7, xmax=7):
    # this is a simple numeric integration
    num = 10000.
    xarray = np.linspace(xmin,xmax,num)
    return np.sum(q1(xarray) * q2(xarray))*(xmax-xmin)/num

(inner_product(q(dists[0]),q(dists[0])), inner_product(q(dists[1]),q(dists[1])),inner_product(q(dists[2]),q(dists[2])))

def getChordDistance(q1,q2):
    return 2.*np.sin( np.arccos( inner_product(q1, q2) ) /2. )

tempSim=[]
for f1 in dists:
    temp = []
    for f2 in dists:
        temp.append(getChordDistance(q(f1),q(f2)))
    tempSim.append(temp)
chordDistMatrix=np.array(tempSim)
chordDistMatrix #diagonals should be close to 0

seed = np.random.RandomState(seed=3)
mds = manifold.MDS(n_components=dim, metric=True, max_iter=3000, eps=1e-9, random_state=seed,dissimilarity="precomputed", n_jobs=1)
embeded = mds.fit(chordDistMatrix).embedding_
mds.fit(chordDistMatrix).stress_

def minimizeMe(x):
    ret = 0.
    for point in embeded:
        ret += (linalg.norm( point-x)-1.)**2
    return ret

center = [0,0,0]
result = minimize(minimizeMe,center, method='nelder-mead',
                    options={'xtol': 1e-8, 'disp': True})
center = result.x
for point in embeded:
    point -= center

u = np.zeros(dim)
u[-1]=-1
v = embeded[0] - embeded[0].dot(u)*u
v/=linalg.norm(v)
v.dot(u)
theta = -np.arccos(embeded[0].dot(u))
vuT=np.outer(v,u.T)
uvT=np.outer(u,v.T)
uuT=np.outer(u,u.T)
vvT=np.outer(v,v.T)
I = np.identity(dim)
A=I + np.sin(theta)*(vuT-uvT) + (np.cos(theta)-1.)*(uuT+vvT)
A.dot(embeded[0])

pointsOnSphere = embeded.copy()
for i, point in enumerate(embeded):
    #if i==0: continue
    pointsOnSphere[i] = A.dot(point)
    if pointsOnSphere[i,2]>0:
        print "error, shouldn't have points up there"
pointsOnSphere[0] = np.zeros(dim) #just to get rid of roundoff for this first point
pointsOnSphere[0,-1] = -1.
pointsOnSphere[:3]

gnomonicProjection = pointsOnSphere.copy()
for i, point in enumerate(pointsOnSphere):
    gnomonicProjection[i] = -1.*point/point[-1]

nSamples = 15 #samples along ray for visualization
rays = np.zeros((3*nSamples,dim))
for i, point in enumerate(gnomonicProjection):
    for j, c in enumerate(np.linspace(0,1,nSamples)):
        rays[i*nSamples+j] = c * point

fig = plt.figure(figsize=(5,5))
subpl = fig.add_subplot(111,projection='3d')
subpl.scatter(rays[:, 0], rays[:, 1], rays[:, 2],marker='.',c=pointsOnSphere[:,0]*0+.01)
subpl.scatter(pointsOnSphere[:, 0], pointsOnSphere[:, 1],pointsOnSphere[:, 2],
              marker='o',c=-pointsOnSphere[:,2])#,cmap=mpl.cm.gray)
subpl.scatter(gnomonicProjection[:, 0], gnomonicProjection[:, 1], gnomonicProjection[:, 2],
              c=-pointsOnSphere[:,2])#,cmap=mpl.cm.gray)
plt.show()

alphaSimplex = Delaunay(alphas)
gnomonicSimplex = Delaunay(gnomonicProjection[:3,:2])

normedVertices = gnomonicProjection[:3,:2].copy()
normedVertices[1] /= linalg.norm(normedVertices[1])
normedVertices[2] /= linalg.norm(normedVertices[2])
normedSimplex = Delaunay(normedVertices)

inner_product(q(dists[0]),q(dists[1])),inner_product(q(dists[0]),q(dists[2])),inner_product(q(dists[1]),q(dists[2]))

pointsOnSphere[0].dot(pointsOnSphere[1]),pointsOnSphere[0].dot(pointsOnSphere[2]),pointsOnSphere[1].dot(pointsOnSphere[2])

alpha1 = np.array([0.9,0.0]) #for testing
alpha2 = np.array([0.0,.9]) #for testing
alpha = alpha2 #switch for testing
alpha = np.array([0.5,0.3])
baryCoords = alphaSimplex.transform[0,:dim-1,:].dot(alpha)
gnomonicTarget = linalg.inv(gnomonicSimplex.transform[0,:dim-1,:]).dot(baryCoords)
normedBaryCoords = normedSimplex.transform[0,:dim-1,:].dot(gnomonicTarget)
t = np.arctan(linalg.norm(gnomonicTarget))
t, normedBaryCoords, linalg.norm(gnomonicTarget), linalg.norm(gnomonicProjection[1,:2]), linalg.norm(gnomonicProjection[2,:2])
gnomonicProjection[1:], gnomonicTarget

dot01 = inner_product(q(dists[0]),q(dists[1]))
dot02 = inner_product(q(dists[0]),q(dists[2]))
t1 = lambda x: q(dists[1])(x) - dot01*q(dists[0])(x)
t2 = lambda x: q(dists[2])(x) - dot02*q(dists[0])(x)
norm1 = inner_product(t1,t1)
norm2 = inner_product(t2,t2)
u1 = lambda x: t1(x)/np.sqrt(norm1)
u2 = lambda x: t2(x)/np.sqrt(norm2)
#unnorm_tan = lambda x: u1(x)*normedBaryCoords[1] + u2(x)*normedBaryCoords[0]
unnorm_tan = lambda x: u1(x)*normedBaryCoords[0]+u2(x)*normedBaryCoords[1]
norm_tan = inner_product(unnorm_tan,unnorm_tan)
tangent = lambda x: unnorm_tan(x) / np.sqrt(norm_tan)
#tangent = lambda x: (u1(x)*normedBaryCoords[1] + u2(x)*normedBaryCoords[0])/np.sqrt(norm_tan)
#tangent = lambda x: (u1(x)*normedBaryCoords[0] + u2(x)*normedBaryCoords[1])/np.sqrt(norm_tan)
print inner_product(q(dists[0]),tangent), inner_product(tangent,tangent)
q_interpolant = lambda x: np.cos(t)*q(dists[0])(x) + np.sin(t)*tangent(x) 
interpolant = lambda x: ( np.cos(t)*q(dists[0])(x) + np.sin(t)*tangent(x) )**2
#interpolant = lambda x: (q_interpolant(x)*q_interpolant(x))
#interpolant = lambda x: ( np.cos(t)*q(dists[0])(x) + np.sin(t)*u1(x) )**2
np.arccos(inner_product(q(interpolant),q(dists[0]))),inner_product(q(interpolant),q(interpolant))

np.arccos(dot02), t, np.arccos(dot01)

xarray = np.linspace(-5,5,100)
plt.plot(xarray,dists[0](xarray),c='black')
plt.plot(xarray,dists[1](xarray),c='r')
plt.plot(xarray,dists[2](xarray),c='g')
plt.plot(xarray,interpolant(xarray),c='b',ls='dashed')

plt.plot(xarray,u1(xarray),c='r')
plt.plot(xarray,u2(xarray),c='g')
#plt.plot(xarray,unnorm_tan(xarray),c='black')
plt.plot(xarray,tangent(xarray),c='b',ls='dashed')

targetOnSphere = gnomonicTarget3d/linalg.norm(gnomonicTarget3d)
targetOnSphere.dot(pointsOnSphere[0]), targetOnSphere.dot(pointsOnSphere[1]), targetOnSphere.dot(pointsOnSphere[2])

inner_product( q(dists[0]),q(interpolant)), inner_product( q(dists[1]),q(interpolant)), inner_product( q(dists[2]),q(interpolant) )

targetRays = np.zeros(15*3).reshape(15,3)
gnomonicTarget3d = np.zeros(3)
gnomonicTarget3d[:2] = gnomonicTarget
gnomonicTarget3d[2]=-1
for i,l in enumerate(np.linspace(0,1,15)):
    targetRays[i] = l*gnomonicTarget3d

fig = plt.figure(figsize=(5,5))
subpl = fig.add_subplot(111,projection='3d')
subpl.scatter(rays[:, 0], rays[:, 1], rays[:, 2],marker='.',c=pointsOnSphere[:,0]*0+.01)
subpl.scatter(targetRays[:, 0], targetRays[:, 1], targetRays[:, 2],marker='*',c=pointsOnSphere[:,0]*0+.01)
subpl.scatter(gnomonicTarget3d[0], gnomonicTarget3d[1], gnomonicTarget3d[2],marker='o',c=pointsOnSphere[:,0]*0+.01)
subpl.scatter(targetOnSphere[0], targetOnSphere[1], targetOnSphere[2],marker='o',c=pointsOnSphere[:,0]*0+.01)
subpl.scatter(pointsOnSphere[:, 0], pointsOnSphere[:, 1],pointsOnSphere[:, 2],
              marker='o',c=-pointsOnSphere[:,2])#,cmap=mpl.cm.gray)
subpl.scatter(gnomonicProjection[:, 0], gnomonicProjection[:, 1], gnomonicProjection[:, 2],
              c=-pointsOnSphere[:,2])#,cmap=mpl.cm.gray)
plt.show()