/* ***  SS:=fopen("/home/mc/Desktop/SunnySpell/SS_PSO.txt",Text): read(SS) */

/* SunnySpell
Written for MuPAD+Scilab
Maurice.Clerc@WriteMe.com
First version: 2005-08-21
Last update:2005-08-27

N points Pi in the unit D-square S. 
Add a point Xm so that the distribution {P1, ..., PN,Xm} is
as uniform/regular as possible.

Method (explanation for dimension D=2
------
For any point X(x1,x2) in S, let s(X) be the distance of X to S, i.e.
s(X)=min(x1,1-x1,x2,1-x2)

We define the "isolation" ("isolement" in French) function
I(X)=min(min_i(norm(X-Pi))+ s(X))

We are looking for the point Xm where this function reaches its maximum.

Note:
You could think a Voronoï/Delaunay tesselation does the job.
It is not the case for such a method don't take into account the infinite 
number of points on the "frontier" of the search space, so the result may be
a point to near of this "frontier".


Why "SunnySpell"?
----------------
Let's suppose each point on the frontier of S, and each Pi, is the center 
of a disc of radius r. Such a disc can be seen as a "cloud", and S is the "sky".

A point X is "covered" if it belongs to at least one "cloud". 

As long as r>I(Xm), all points are covered (the sky is overcast)
As soon as r<I(Xm), there is a "sunny spell" ("éclaircie" in French):
 some points are not covered (at least Xm).

What is for?
------------
In an adaptive Particle Swarm Optimiser like Tribes, some particles are
added from time to time. Doing that just purely at random may be not the best way.
It could be better to add a particle that is as "isolated" as possible
in order to explore some "no man's lands" in the search space.
It means, in particular, that we are not looking for a very precise result,
but just a "good enough" one. 

*/
Pref::maxMem(NIL): // Use all the memory
//========================================================== P2X
// Compute the components of x for the point of rank p
// (useful for "exhaustive" search and plot)
p2x:=proc(Dim,delta,p)
begin
x:=map(numlib::g_adic(p-1,floor(1/delta+1)),_mult,delta):
while nops(x)<Dim do x:=x.[0]: end_while:
end_proc:
//--------------
/*
// In a more "classical" way, this can be written as follows
local p1, d, m_d, k;
begin
	m:=1/delta+1:
	p1:=p-1:
	for d from Dim downto 1 do
		m_d:=m^(d-1):
		k[d]:=floor(p1/m_d):
		p1:=p1-m_d*k[d]:
		x[d]:=k[d]*delta:
	end_for:
x:
end_proc
*/
//========================================================== REPULS
// Evaluate a "repulsion" value depending on the distance
repuls:=proc(distance,F)
local r;
begin
	if iszero(distance) then return(infinity): end_if:
	case F
	of 3 do
	r:=float(ln(1/distance)):
	break:
	of 4 do
	r:=1/distance:
	break:
	otherwise r:=1/distance^2:
	end_case:
	return(r):
end_proc:
//========================================================== ISOL
// Evaluate the "isolation" min_i of the position
Isol:=proc(position)
local Y,Q,i,Norm,s;
// Global input: Dim,P,N,F
// Global output: list_x,min_i,X
begin
	list_x:=position[i] $ i=1..Dim:
	X:=matrix([list_x]); /* current point */
	Y:=X: for i from 2 to N do Y:=Y.X end_for: /* concatenation to define a matrix */
	Q:=P-Y:

	for i from 1 to N do Norm[i]:=norm(linalg::col(Q,i),2) end_for: /* norm(Pi-X); */

	s:= min(X[i] $ i=1..Dim,1-X[i] $ i=1..Dim): // Distance to the square
			
	case F
	of 1 do // As it is
		min_i:=min(s,Norm[i] $ i=1..N):// Compute the "isolation"
	break:
	
	of 2 do  // On a "sphere"
		circ:=float(sqrt(Dim)):
		min_i:=min(s,Norm[i] $ i=1..N,circ-Norm[i] $ i=1..N ):	
	break:
	
	of 3 do // Repulsion
		r:=0:
		for i from 1 to N do 
			r:=r+repuls(Norm[i],F):
		end_for:
		for i from 1 to Dim do
			r:=r+repuls(X[i],F)+repuls(1-X[i],F):
		end_for:
		min_i:=1/r:	
	break:
	end_case:
end_proc:
//========================================================  MAIN program

//---------------------------------------------- DATA
DIGITS:=30:
rand_type:=-1: /* >0 => the same "randomness" for each run
		(but depending on the value of rand_type)
		WARNING: as DIGITS is necessarily limited, the float 
		pseudo random numbers are still a bit different for each run
		-1 => different for each run
		*/
		
F:=2: /* Kind of function to maximize
	1 =>  min_distances(S, Pi)
	2 =>  min_distances( Pi, sqrt(Dim) - to Pi) (virtual sphere)
	3 =>  use  ln(1/norm) repulsion function 
	4 =>  use 1/norm repulsion function
	*/
 
Dim:=2: // Dimension of the search space S. 
	//  
N:=10: // Number of random points in S
delta:=0.1: /* Step to discretize S (mesh), for exhaustive search,
		 and for plot.
		 WARNING: Plots are just for Dim=2 or 3 */
/*
//--- TEST DATA (Abhi's example)
// WARNING: don't forget to "remove" the random generation of known points
// (see below)
Dim:=2: N:=3:
Pt[1,1]:=0.4:Pt[2,1]:=0.4:
Pt[1,2]:=0.5:Pt[2,2]:=0.4:
Pt[1,3]:=0.5:Pt[2,3]:=0.5:
//--- end TEST DATA 
*/

//---------------------------------------------- END OF DATA
// ------------Define pseudo randomness
  case rand_type  
  of -1 do // Different for each run
  	SEED:=rtime(): 
  	rt:=random(1..1000):// Procedure to generate a pseudo random integer
  	rand_t:=rt():
  	for i from 1 to rand_t do wait(10): end_for:
  	seed_:=rtime():
  break: 
  otherwise // The same for each run
  	seed_:=rand_type: SEED:=rand_type:
  break:
  end_case:
 //-------------------------
alea:=frandom(seed_):

// Random generation of the known points
// WARNING: of course, you have to "remove" this line if you use
// predefined data (as Abhi's example)
for i from 1 to Dim do for j from 1 to N do Pt[i,j]:=alea(): end_for:end_for:

P:=matrix(Dim,N,Pt):

//repeat
//print("N",N);print("P",P);

//--------------------EXHAUSTIVE SEARCH
I_max:=0:
I_list:=[]: // All points of the mesh
pmax:=(round(1/delta)+1)^Dim:
 
for p from 1 to pmax do
	p2x(Dim,delta,p): // "row" vector x
	Isol(x):// Compute isolation min_i
	
	I_list:=I_list.[[list_x,min_i]]: // Just for Isolation surface plotting
	// Exhaustive search on the mesh (choose the max)
	if min_i>I_max then Xm:=X: I_max:=min_i: end_if:
end_for: 

print("Exhaustive search on mesh:",float((1/delta)^Dim)," evaluations"):
print("Xm",Xm, "Value",I_max); // The "best" new point ...

//=============================================== PSO SEARCH
/*  Just a very basic PSO version */
print("PSO search"):

/* Search space = unit D-cube
but in general it is a D-parallelepid
WARNING: the fitness evaluation (ISOL) is written for the unit D-cube
*/

for d from 1 to Dim do
	xmin[d]:=0 : xmax[d]:=1:
	dx[d]:=xmax[d]-xmin[d]:
end_for:

swarm_size:=round(9.5+0.124*(Dim-9)): // Cf. Annexe of "Binary PSO" study
c1:=float(1/(2*ln(2))): cmax:=float(c1+2*ln(2)-1): // Cf. "Stagnation Analysis" study
h_size:=3: // Nb of random links/particle

alea_rank:=random(1..swarm_size): // Procedure to generate a random integer 
				// in [1, swarm_size]. Depends on SEED

stop_eval:=max(2*swarm_size,300*Dim-500): /* Purely empirical.
		Note that we just are actually looking for a position that is
		"enough isolated".
		*/

// INITIALISATION
pos:=matrix(swarm_size,Dim):
vel:=matrix(swarm_size,Dim):
link:=matrix(swarm_size,swarm_size):

for n from 1 to swarm_size do
for d from 1 to Dim do
	pos[n,d]:=xmin[d]+alea()*dx[d]:
	vel[n,d]:=dx[d]*(-0.5+alea()):	
end_for:
pbest:=pos:
end_for:

// Fitness evaluation
z:=coerce(pos,DOM_LIST):
for n from 1 to swarm_size do
	Isol(z[n]): // Compute Isolation
	f[n]:=float(min_i): // Store fitness
end_for:

// Find the (rank of the) best in the swarm
best_swarm:=1: 
for n from 2 to swarm_size do
	if f[n]>f[best_swarm] then best_swarm:=n: end_if:
end_for:
f_best_swarm:=f[best_swarm]:
line_best:=coerce(linalg::row(pos,best_swarm),DOM_LIST): // Just for plotting

// At the beginning, best=currentb
f_best_swarm_prev:=f_best_swarm:
p_best:=pos:
f_best:=f:
nb_eval:=0:
 
while nb_eval<stop_eval do
nb_eval:=nb_eval+swarm_size:
// LINKS
// If no improvement, define random information links
if f_best_swarm<=f_best_swarm_prev then
	for n from 1 to swarm_size do
		for m from 1 to swarm_size do
			link[n,m]:=0: // Set all links to zero
		end_for:
		link[n,n]:=1: // Each particle informs itself 
	end_for:	

	for n from 1 to swarm_size do
		for k from 1 to h_size do
			link[n,alea_rank()]:=1:
		end_for:
	end_for:
end_if:

// MOVE
for n from 1 to swarm_size do
	// Look for the best neighbour
	g_best:=1:
	for n2 from 1 to swarm_size do
	 	if link[n,n2]=1 and f_best[n2]>f_best[g_best] then
			g_best:=n2:	
		end_if:
	end_for:

	//move
	for d from 1 to Dim do
		vel[n,d]:=c1*vel[n,d]+cmax*alea()*(pbest[n,d]-pos[n,d])+cmax*alea()*(p_best[g_best,d]-pos[n,d]):
		pos[n,d]:=pos[n,d]+vel[n,d]:
	end_for:

	// Keep in the box
	for d from 1 to Dim do
		if pos[n,d]<xmin[d] then pos[n,d]:=xmin[d]: vel[n,d]:=0:
		else if pos[n,d]>xmax[d] then pos[n,d]:=xmax[d]: vel[n,d]:=0:end_if:
		end_if:
	end_for:

	//Evaluation of the new position
	z:=coerce(linalg::row(pos,n),DOM_LIST):
	Isol(z[1]): // Compute Isolation
	f[n]:=min_i: // Store fitness

	// If improvement, change pbest, 
	// and possibly best_swarm and f_best_swarm
	if f[n]>f_best[n] then
		f_best[n]:=f[n]:
		for d from 1 to Dim do
			pbest[n,d]:=pos[n,d]:
		end_for:
		if f_best[n]>f_best_swarm then
			best_swarm:=n:
			f_best_swarm:=f_best[best_swarm]:
			line_best:=line_best.coerce(linalg::row(pos,best_swarm),DOM_LIST):	
		end_if:
	end_if:
end_for:

f_best_swarm_prev:=f_best[best_swarm]:
//print("Iter",iter," Best",f_best_swarm_prev):
end_while:

best_pos:=linalg::row(pbest,best_swarm):
print("PSO search:",nb_eval," evaluations"):
print("Best position",best_pos):
print("Best value",f_best_swarm_prev):

//Just for plotting
Xm_pso:=linalg::transpose(best_pos):

//===================================== PLOTTING
case Dim
of 2 do
// The square
A1:=[0,0]:
A2:=[1,0]:
A3:=[1,1]:
A4:=[0,1]:

S:=plot::Polygon2d([A1,A2,A3,A4]):

// The Pi points
pi:=plot::PointList2d([[P[1,i],P[2,i]] $ i=1..N]):

// The "improvement line"
line:=plot::Polygon2d(line_best):
// The "best" new point
xm:=plot::Point2d(Xm[1],Xm[2],PointSize=3*unit::mm,PointColor=RGB::Green):
xm_pso:=plot::Point2d(Xm_pso[1],Xm_pso[2],PointSize=3*unit::mm,PointColor=RGB::Red):


// Plots
plot(S,pi,xm,line,xm_pso): // Square, Pi, Xm
  
// Isolation surface
// (OK, it is not the fastest method, but it is easy to code...)
grid:=[]:
mesh:=round(1/delta):

// Normalize the "height" of the surface
max_z:=0:
for k from 1 to nops(I_list) do
 	max_z:=max(max_z,I_list[k][3]):
end_for:

for k from 1 to nops(I_list) do
 	I_list[k][3]:= I_list[k][3]/max_z:
end_for:
 

for k1 from 1 to mesh do
for k2 from 1 to mesh do
	k:=round((mesh+1)*(k1-1) + k2):
	square:=I_list[k].I_list[k+1].I_list[k+2+mesh].I_list[k+1+mesh]:
	grid:=grid.[].square:
end_for:
end_for:

Xm3:=plot::Sphere(0.02,[Xm[1],Xm[2],I_max],Color=RGB::Green):
I_surface:=plot::SurfaceSet(grid,MeshListType=Quads):
plot(I_surface,Xm3):
break:

of 3 do
A[1]:=[0,0,0]:A[2]:=[1,0,0]:
A[3]:=[0,1,0]:A[4]:=[0,0,1]:
A[5]:=[1,1,0]:A[6]:=[0,1,1]:
A[7]:=[1,0,1]:A[8]:=[1,1,1]:

Cube:=plot::PointList3d([A[i] $ i=1..8],PointSize=0.01):

// The Pi points
pi:=plot::PointList3d([[P[1,i],P[2,i],P[3,i]] $ i=1..N]):

// The "improvement line"
line:=plot::Polygon3d(line_best):

// The "best" new point
Xm3:=plot::Point3d(Xm[1],Xm[2],Xm[3],PointSize=3*unit::mm,PointColor=RGB::Green):
xm_pso:=plot::Point3d(Xm_pso[1],Xm_pso[2],Xm_pso[3],PointSize=3*unit::mm,PointColor=RGB::Red):
//Xm3:=plot::Sphere(0.02,[Xm[1],Xm[2],Xm[3]],Color=RGB::Green):

// Plots
plot(Cube,pi,Xm3,line,xm_pso): // Square, Pi, Xm
break:

otherwise print("No plots for Dim=",Dim):
break:
end_case: