%sage
# Use a better version of the octave interface
from octave import octave
%default_mode octave

#MIW2DVoronoiV1
#Fully working dmeonstration of basci tesselation functions and method for generating
#a list of all nearest neighbors of every input node, and a list of all neighbors and their
#mearest neighbors.

#MIW2DVoronoiV1 is next step in evolution of this code.

#graphics_toolkit gnuplot
#graphics_toolkit fltk

#Find a good increment to use to explore points near tails
endval=0.999;
incval=2*endval/20;
xuniform=[-endval:incval:endval]'; %For the simple test
yuniform=[-endval:incval:endval]'; %For the simple test

#For a Gaussian 2D density with zero covariance the density is factorable permitting...
#Use inverse error function to generate a Gaussian marginal distribution of trajectories

xvalues=erfinv(xuniform);
yvalues=erfinv(yuniform);
nxv=size(xvalues,1);
nyv=size(yvalues,1);
N=nxv*nyv; %Number of worlds

#We need to build the 2D coordinates obtained from Cartesian product of x and y coordinates
[Xc, Yc] = meshgrid (xvalues, yvalues);

#X is the state vector, a single column with every x and y coordinate as an entry followed
#by every time derivative of these coordinates 
#For the 2D case our stacking will be of all x coordinates first, then all y, then derivatives
Xcol=reshape(Xc,N,1);
Ycol=reshape(Yc,N,1);
X = [Xcol; Ycol; zeros(2*N,1)]; %adding zero initial velocities
#X = [reshape(Xc,N,1); reshape(Xc,N,1); zeros(2*N,1)]; %adding non-zero initial velocities

#Let's plot the input initial locations of the N worlds and the tesselation of the space
h1=figure('visible', 'off');
tri = delaunay (Xcol, Ycol);
[vx, vy] = voronoi (Xcol, Ycol, tri);
triplot (tri, Xcol, Ycol, "b");
hold on;

#add text annotations for node numbers
offset=0.02; #adjust for readability
text(Xcol+offset,Ycol+offset,num2str([1:N]'));

plot (vx, vy, "r");
axis([-2,2,-2,2]);
hold off;
saveas(h1,"figure1.png");

%sage salvus.file("figure1.png")

#The function voronoin will generate two lists:
#C contains the vertices of the Voronoi polygons. 
#F contains, for each original point, the indices of the Voronoi polygon vertices. 
#while polyarea will create the areas of each voronoi polygon for us
[C, F] = voronoin ([Xcol, Ycol]);
af = zeros (size (F));
for i = 1 : length (F)
  af(i) = polyarea (C (F {i, :}, 1), C (F {i, :}, 2));
endfor
#display(af) #display areas

#So the probability density associated with each trajectory point is
Pin=(N*af).^(-1);

%Plot the input trajectory density
#Next 2 lines only needed to avoid plot data error
Pf=Pin;
Pf(isnan(Pin))=0;

%sage
m=matrix(RR,octave("[Xcol(:), Ycol(:), Pf(:)]"))
data=[(x,y,z) for x,y,z in m.rows()]
surface=list_plot3d(data,aspect_ratio=[1,1,8],texture={'color':'green','opacity':0.85,'ambient':0.9})
pts=points(data,size=2,color='red')
#show(pts,aspect_ratio=[1,1,10])
show(surface+pts,spin=10,background='pink',height=int(600),width=int(1000),renderer='webgl',frame={'thickness':4.0, 'color ':'red','labels' : True,'fontface' : 'roman'} )

%sage
m=matrix(RR,octave("[Xcol(:), Ycol(:), Pf(:)]"))
data=[(x,y,z) for x,y,z in m.rows()]
surface=list_plot3d(data,aspect_ratio=[1,1,8],texture="green")
pts=points(data,size=2,color='red')
#show(pts,aspect_ratio=[1,1,10])
show(surface+pts,spin=100,frame=False)

%sage
show(surface+pts,viewer='tachyon',mesh=True,frame=False,axes=False)

%sage
show(graphs.DodecahedralGraph(), d3=True, force_spring_layout=True)

#The nearest neighbors of a point are the points connected to it in the Delaunay
#tesselation. So, we need need only find its index in the list of indices 
#to find those nearest neighbors

#Useful helper functions: the two functions missing from matlab/octave that no one
#will admit to because the omission is so agregious but the surrogate syntax is so ugly
paren = @(x, varargin) x(varargin{:});
curly = @(x, varargin) x{varargin{:}};

#Find the nearest neighbors of every node
%f=@(x)find(tri==x);
%atemp=arrayfun(f,[1:size(y)],"UniformOutput", false);
%rowlist=arrayfun(@(x)ind2sub(size(tri),atemp{x}),[1:size(y)],"UniformOutput", false);
%nlist=arrayfun(@(x) setdiff(unique(tri(rowlist{x},:)),x),[1:size(y)],"UniformOutput", false)

#Find the nearest neighbors of all nodes

#ftri finds the rows with nearest neighbors (nns) of node given in argument
ftri=@(x)find(tri==x);
#ftmany applies ftri to each node in narray to find linear index of entries in tri with its nns
ftmany=@(narray)arrayfun(ftri,narray,"UniformOutput", false);
#Generate an array listing all the rows discovered delinearizing entries found by ftmany
rowlist=arrayfun(@(x)ind2sub(size(tri),curly(ftmany([1:N]),x)),[1:N],"UniformOutput", false);
#Find the unique nodes listed by all rows found for each node in node ordered array found above
nlist=arrayfun(@(x) setdiff(unique(tri(rowlist{x},:)),x),[1:N],"UniformOutput", false);

#Find the nns and the nearest neighbors of the nearest neighbors of all nodes

clear nnslist
nnlist=cell(N);
for nlnum=1:N
  ntemp=nlist{nlnum}';
  #Generate an array listing all the rows discovered delinearizing entries found by ftmany
  nnsrowlist=arrayfun(@(x)ind2sub(size(tri),curly(ftmany(ntemp),x)),[1:size(ntemp)],"UniformOutput", false);
  nnsrowmerge=vertcat(nnsrowlist{:});
  nnslist{nlnum}=unique(tri(nnsrowmerge,:));
endfor

#Now fit a 2nd order 2d polynomial to each region surrounding a point
clear nnspolyval;
#nnspolyval will store the interpolated values of [P,Px,Py,Pxx,Pxy,Pyy]
nnspolyval=zeros(N,6);

#Define functions that enact differentiation of the polynomial on the coeffs
Dx=@(x) [x(2) 2*x(3) 0 x(5) 0 0]';
Dy=@(x) [x(4) x(5) 0 2*x(6) 0 0]';
Dxx=@(x) [2*x(3) 0 0 0 0 0]';
Dyy=@(x) [2*x(6) 0 0 0 0 0]';
Dxy=@(x) [x(5) 0 0 0 0 0]';

for nlnum=1:N
  nnspoly = polyfit2d(Xcol(nnslist{nlnum}), Ycol(nnslist{nlnum}), Pf(nnslist{nlnum}), 2);
  xyprods=[1,Xcol(nlnum),Xcol(nlnum)^2,Ycol(nlnum),Xcol(nlnum)*Ycol(nlnum),Ycol(nlnum)^2];
  #nnspolyval(nlnum,1)=polyval2d(Xcol(nlnum),Ycol(nlnum),nnspoly);
  nnspolyval(nlnum,1)=abs(xyprods*nnspoly); #abs to eliminate small negative outcomes
  nnspolyval(nlnum,2)=xyprods*Dx(nnspoly); 
  nnspolyval(nlnum,3)=xyprods*Dy(nnspoly); 
  nnspolyval(nlnum,4)=xyprods*Dxx(nnspoly); 
  nnspolyval(nlnum,5)=xyprods*Dxy(nnspoly); 
  nnspolyval(nlnum,6)=xyprods*Dyy(nnspoly); 
endfor;

%sage
m=matrix(RR,octave("[Xcol(:), Ycol(:), nnspolyval(:,1)]"))
list_plot3d([(x,y,z) for x,y,z in m.rows()],texture="blue",frame_aspect_ratio=[1,1,10])

h3=figure('visible', 'off');
clf;
#Use below for gnuplot and following line for full gradient color with fltk
g=patch ("Faces", tri, "Vertices", [Xcol(:), Ycol(:), nnspolyval(:,1)]);
#patch ("Faces", tri, "Vertices", [Xcol(:), Ycol(:), Pin(:)],'FaceVertexCData',Pin(:),'FaceColor','interp','EdgeColor','none');
view (359, 11);
set(g,'edgecolor','b');

%sage
m=matrix(RR,octave("[Xcol(:), Ycol(:), nnspolyval(:,2)]"))
data=[(x,y,z) for x,y,z in m.rows()]
surface=list_plot3d(data,aspect_ratio=[1,1,8],texture="green")
pts=points(data,size=2,color='red')
surface+pts

h4=figure('visible', 'off');
clf;
#Use below for gnuplot and following line for full gradient color with fltk
patch ("Faces", tri, "Vertices", [Xcol(:), Ycol(:), nnspolyval(:,2)]);
#patch ("Faces", tri, "Vertices", [Xcol(:), Ycol(:), Pin(:)],'FaceVertexCData',Pin(:),'FaceColor','interp','EdgeColor','none');
view (359, 11);

%sage
m=matrix(RR,octave("[Xcol(:), Ycol(:), nnspolyval(:,3)]"))
data=[(x,y,z) for x,y,z in m.rows()]
surface=list_plot3d(data,aspect_ratio=[1,1,8],texture="lightgreen")
pts=points(data,size=2,color='green')
surface+pts

h5=figure('visible', 'off');
clf;
#Use below for gnuplot and following line for full gradient color with fltk
patch ("Faces", tri, "Vertices", [Xcol(:), Ycol(:), nnspolyval(:,3)]);
#patch ("Faces", tri, "Vertices", [Xcol(:), Ycol(:), Pin(:)],'FaceVertexCData',Pin(:),'FaceColor','interp','EdgeColor','none');
view (246, 20);

︠849ab3f8-aab2-4992-9e32-ae26ee6a02b6︠
saveas(h3,'figureVDensSmooth.png');
saveas(h4,'figureVDensDx.png');
saveas(h5,'figureVDensDy.png');

%sage salvus.file("figureVDensSmooth.png")

%sage salvus.file("figureVDensDx.png")

%sage salvus.file("figureVDensDy.png")