%Creating one set of test points
h = 10;
x = 100*randn(h,1); y = 100*randn(h,1); z = 100*randn(h,1);
d = dDistances(x,y,z);

n=5; k=1;
d = zeros(n,n);
teta = ones(n,n);
x1 = 100*randn(n,1);	x2 = 100*randn(n,1);
y1 = 100*randn(n,1);	y2 = 100*randn(n,1);
z1 = 100*randn(n,1);	z2 = 100*randn(n,1);
d1 = dDistances(x1,y1,z1); d2 = dDistances(x2,y2,z2); 
d = d2 + 10*randn(size(d2)); d = d - diag(diag(d));

%Checking first derivative value
[ e, gx,gy,gz, Gx,Gy,Gz] = dErrorF ( x1,y1,z1, d, teta);
i=1;
ni = 0.01; df = (dErrorF(x1+ni*((1:n)'==i),y1,z1,d,teta) - e)/ni; [df  gx(i) df/gx(i)]
% Checking the second derivative value
[ e, gx,gy,gz, Gx,Gy,Gz] = dErrorF ( x1,y1,z1, d, teta);
ni = 0.01; i=3; j=5;

for i=1:n; for j=1:n;
dxi = ni*((1:n)'==i); 	dxj = ni*((1:n)'==j); 
d2f = (dErrorF(x1+dxi+dxj,y1,z1,d,teta) - dErrorF(x1+dxj,y1,z1,d,teta) - dErrorF(x1+dxi,y1,z1,d,teta) + dErrorF(x1,y1,z1,d,teta) )/(ni^2); disp([d2f  Gx(i,j) d2f/Gx(i,j)])
end; end;

[ e2, gx2,gy2,gz2, Gx2,Gy2,Gz2] = dErrorF ( x1+ni*((1:n)'==i),y1,z1, d, teta); d2f = (gx2-gx)/ni; [ d2f(i) Gx(i,i) d2f(i)/Gx(i,i)] 




i=1; j=0;
j = j+1; plot(dSpikeEliminate(z( (0:k-1)*h+i, j ),10 ));
j = j+1; x=dExtract(dist,i,j);subplot(211);plot(x);subplot(212);plot(diff(x));


% The following part of the code creates random points that move as sine wave,
% from the points, distances are calculated and, finaly, minimization is 
% conducted.
dist = zeros(h*k,h);
teta = ones(size(dist));
x1 = 100*randn(h,1);	x2 = 100*randn(h,1);
y1 = 100*randn(h,1);	y2 = 100*randn(h,1);
z1 = 100*randn(h,1);	z2 = 100*randn(h,1);
d1 = dDistances(x1,y1,z1); d2 = dDistances(x2,y2,z2); 
for i=1:h, 
	for j=i+1:h, 
		dist((0:k-1)*h+i,j)=(d1(i,j)+d2(i,j))/2+(d1(i,j)-d2(i,j))*sin(2*pi*(1:k)/20+randn(1))'; 
		dist((0:k-1)*h+j,i)=dist((0:k-1)*h+i,j) + 10*randn(k,1); 
end; end;


k = 20; h=50; w = h;
I = sqrt(-1);
x0 = 100*rand(h,1)*ones(1,k) + 100*rand(h,1) * sin(2*pi*(1:k)/(k/1)) ;
y0 = 100*rand(h,1)*ones(1,k) + 100*rand(h,1) * sin(2*pi*(1:k)/(k/1)) ;
z0 = 100*rand(h,1)*ones(1,k) + 100*rand(h,1) * sin(2*pi*(1:k)/(k/1)) ;
for t = 1:k,
	d = dDistances(x0(:,t), y0(:,t), z0(:,t));
	dist((t-1)*h+(1:h),(1:h)) = d + (d/5).*randn(size(d));
end;
teta = ones(size(dist)) ./ (50+dist) .* round(0.2+rand(size(dist)));

%This is minimization. This is the CORE of the program
tic;
x1 = 100*randn(h,1); y1=100*randn(h,1); z1 = 100*randn(h,1);
x = zeros(h,k); y=x; z=x;
for t = 1:k,
        if (round(t/10)==t/10), disp(t); end;
	[x1,y1,z1] = dMinimize( x1,y1,z1, dPlane(dist,t), dPlane(teta,t) );
	x(:,t) = x1; y(:,t) = y1; z(:,t) = z1; 
end;
toc
readme = 'Minimization done on real data';
save loadthis dist k h w x y z readme

for t = 1:k,
	dist0((t-1)*h+(1:h),(1:h)) = dDistances(x0(:,t), y0(:,t), z0(:,t));
	dist2((t-1)*h+(1:h),(1:h)) = dDistances(x(:,t) , y(:,t) , z(:,t));
end;

% Plotting symetric time-lamina
j = j+1;plot([ dExtract( dist, i,j) dExtract( dist2, i,j)]);

% Plotting signal and its teta (goodness)
i=1; j=0;
j=j+1; subplot(211); plot([dExtract( dist, i,j) dExtract( dist, j,i) ]);  subplot(212); plot([dExtract( teta, i,j) dExtract( teta, j,i)]); axis([1 3000 -0.1 1.1]);
j=j+1; subplot(211); plot([dExtract(dist, i,j) dExtract( dist, j,i) dExtract(d2,i,j)]); title(sprintf('tx108(%d,%d)',i,j)); subplot(212); plot([dExtract( teta, i,j) dExtract( teta, j,i)]); xlabel('teta');
j=j+1; if(j>n), j=1; i=i+1; end; dShowSignal(i,j);

% Plotting surface of signal goodness
q = zeros(h,h);
for i=1:h, for j=1:h, q(i,j) = sum(dExtract(teta,i,j))/length(teta)*h; end; end
surf(q); view(0,90);

% Plotting one row of signals
i=3;
t = 1000:1500;
for j = 1:h, subplot(h,1,j); x=dExtract(dist,i,j); plot(x(t)); axis('off'); end;
% ... and one column of signals
for i = 1:h, subplot(h,1,i); x=dExtract(dist,i,j); plot(x(t)); axis('off'); end;



t = 451; 
d = dPlane(dist,t); tt = dPlane(teta,t);
x1 = x(:,t); y1 = y(:,t); z1 = z(:,t);
[x2,y2,z2,e,counter] = ...
		dMinimiz( dtrend(x1),dtrend(y1),dtrend(z1), d, tt );
newd = dDistances( x2,y2,z2);


% Applying spike elimination to the whole 'dist' matrix
n = size(dist,2);
for i=1:n; for j=i+1:n;
	dist( (0:k-1)*n+i, j ) = dSpikeElim( dExtract(dist,i,j), 10);
	dist( (0:k-1)*n+j, i ) = dSpikeElim( dExtract(dist,j,i), 10);
end;       end;