\ Simulated Annealing; Minimization and Maximization of Functions.

\ anneal ( 'x 'y 'iorder ncity -- )
\ This algorithm finds the shortest round-trip path to ncity cities whose 
\ coordinates are in the arrays x[0..ncity-1],y[0..ncity-1]. The array
\ iorder[0..ncity-1] specifies the order in which the cities are visited. 
\ On input, the elements of iorder may be set to any permutation of the 
\ numbers 0 to ncity-1. This routine will return the best alternative path 
\ it can find.

\ This is an ANS Forth program requiring:
\	1. The Floating-Point word sets
\	2. Uses FSL words from fsl_util.xxx 
\	3. Uses : F>    ( -- bool ) ( F: r1 r2 -- ) FSWAP F< ;
\		: F+!   ( addr -- ) ( F: r -- )  DUP F@ F+ F! ;
\		: FSQR  ( F: r -- r^2 ) FDUP F* ;
\		: FNIP  ( r1 r2 -- r2 ) FSWAP FDROP ;
\       4. The word irbit2 assumes at least a 32 bit Forth.
\       5. }iprint and }icopy are assumed to be available in fsl_util.xxx

\ Note: the code uses 5 fp stack cells (iForth vsn 1.05) when executing
\       the test words with #p = 20.

\ See: 'Numerical Recipes in Pascal, The Art of Scientific Computing',
\ William H. Press, Brian P. Flannery, Saul A. Teukolsky and William
\ T. Vetterling, Chapter 10 (10.9) Combinatorial Minimization: Method 
\ of Simulated Annealing; 1989; Cambridge University Press, Cambridge, 
\ ISBN 0-521-37516-9

\ (c) Copyright 1995 Marcel Hendrix.  Permission is granted by the
\ author to use this software for any application provided this
\ copyright notice is preserved.


TEST-CODE? FALSE TO TEST-CODE?
	S" r250.frt" INCLUDED  1. r250d_init   
TO TEST-CODE?


CR .( ANNEALING         V1.0           21 May   1995     MH )

Public:

FVARIABLE lambda	0e lambda F!	\ handicap for crossing x=5 line

Private:

0.9e FCONSTANT tfactr	\ annealing schedule -- t is reduced by this 
			\ factor on each step.

FLOAT   DARRAY x{ 
FLOAT   DARRAY y{ 
INTEGER DARRAY iorder{ 
INTEGER DARRAY jorder{

6 FLOAT   ARRAY  xx{
6 FLOAT   ARRAY  yy{
6 INTEGER ARRAY  n{

FVARIABLE lenpath
FVARIABLE atime

0 VALUE iseed
0 VALUE nn
0 VALUE nsucc
0 VALUE ncity
0 VALUE nover
0 VALUE nlimit

\ Random bit generator. Press et al, 7.4, 'Generation of Random Bits'.
\ Useful on its own?
: irbit2 ( -- 0|1 ) \ Primitive Polynomial (32, 7, 5, 3, 2, 1, 0)
	iseed 0< IF [ 64 16 + 4 + 2+ 1+ ] LITERAL iseed XOR 
		    1 LSHIFT 1 OR  TO iseed  1
	       ELSE iseed 1 LSHIFT TO iseed  0 
	       THEN ;
\ This is the handicap suggested by Press et al. Crossing the line x=5
\ costs lambda extra.
: handicap ( p1 p2 -- ) ( F: -- penalty )
	xx{ SWAP } F@ 5e F<  
	xx{ ROT  } F@ 5e F< 
	XOR IF lambda F@ ELSE 0e THEN ;

\ Compute the distance between the points p1 and p2 (coordinates in xx{ yy{).
: alength ( p1 p2 -- ) 
	2DUP handicap
	xx{ 2 PICK } F@ xx{ OVER } F@ F- FSQR
	yy{ SWAP   } F@ yy{ SWAP } F@ F- FSQR  F+ FSQRT 
	F+ ;

\ Compute the distance between the points p1 and p2 (coordinates in x{ y{).
: blength ( p1 p2 -- ) 
	2DUP handicap
	x{ 2 PICK } F@ x{ OVER } F@ F- FSQR
	y{ SWAP   } F@ y{ SWAP } F@ F- FSQR  F+ FSQRT 
	F+ ;

: compute-path ( F: -- r )
	0e 				\ initial path length
	ncity 1- 0 DO  iorder{ I } @  iorder{ I 1+ } @  blength F+  LOOP
	iorder{ ncity 1- } @ iorder{ 0 } @ blength F+ ;

\ Return the value of the cost function for a proposed path reversal. ncity
\ is the number of cities, and arrays x[0..ncity-1], y[0..ncity-1] give the 
\ coordinates of these cities. iorder[0..ncity-1] holds the present itinerary.
\ The first two values n[0] and n[1] of array n give the starting and 
\ ending cities along the path segment which is to be reversed. On output, de
\ is the cost of making the reversal. The actual reversal is not performed by
\ this word.
: revcost ( F: -- de )
	n{ 0 } @  1- ncity +  ncity MOD  n{ 2 } !	\ city before n[0]
	n{ 1 } @  1+          ncity MOD  n{ 3 } !	\ city after n[1]
	4 0 DO  iorder{  n{ I } @ } @ ( index)
		x{ OVER } F@  xx{ I } F!		\ find coordinates
		y{ SWAP } F@  yy{ I } F!		\ of the 4 cities
	  LOOP
	\ calculate cost of disconnecting the segment and reconnecting it 
	\ in reverse order.
	0e   0 2 alength F-  1 3 alength F-   0 3 alength F+  1 2 alength F+ ;
	
\ This word performs a path segment reversal, iorder[0..ncity-1] is an input 
\ array giving the present itinerary. The vector n has as its first four 
\ elements the first and last cities n[0], n[1] of the path segment to be 
\ reversed, and the two cities n[2] and n[3] which immediately precede and 
\ follow this segment. n[2] and n[3] are found by revcost. On output, iorder
\ contains the segment from n[0] to n[1] in reversed order.
: reverse ( -- )
	0 LOCALS| k |
	n{ 1 } @  n{ 0 } @ -  ncity + ncity MOD 1+ 2/  
	\ This many cities must be swapped to effect the reversal
	0 ?DO iorder{ n{ 0 } @ I +          ncity MOD } TO k
	      iorder{ n{ 1 } @ I - ncity +  ncity MOD }
	      DUP @  k @  SWAP k !  SWAP !
	 LOOP ;

\ Return the value of the cost function for a proposed path segment transport. 
\ ncity is the number of cities, and arrays x[0..ncity-1], y[0..ncity-1] give
\ the coordinates of these cities. iorder[0..ncity-1] holds the present 
\ itinerary.
\ The first three values of array n give the starting and ending cities along
\ the path to be transported, and the point among the remaining cities after 
\ which it is to be inserted. On output, de is the cost of making the change. 
\ The actual transport is not performed by this word.
: transportcost ( F: -- de )
	n{ 2 } @  1+          ncity MOD  n{ 3 } !	\ city following n[2]
	n{ 0 } @  1- ncity +  ncity MOD  n{ 4 } !	\ city preceding n[0]
	n{ 1 } @  1+          ncity MOD  n{ 5 } !	\ city following n[1]
	6 0 DO  iorder{ n{ I } @ } @ ( index)
		x{ OVER } F@  xx{ I } F!		\ find coordinates
		y{ SWAP } F@  yy{ I } F!		\ of the 6 cities
	  LOOP
	\ calculate cost of disconnecting the path segment from n[0] to n[1],
	\ opening a space between n[2] and n[3], connecting the segment in the 
	\ space, and connecting n[4] to n[5].
	0e   1 5 alength F-  0 4 alength F-   2 3 alength F-
	     0 2 alength F+  1 3 alength F+   4 5 alength F+ ;

\ This word performs the actual path transport, if metrop has approved.
\ iorder[0..ncity-1] is an input array giving the present itinerary. 
\ The vector n has as its six elements the first and last cities n[0], n[1] of
\ the path segment to be transported, the adjacent cities n[2] and n[3] between
\ which the path is to be placed, and the cities n[4] and n[5] which precede 
\ and follow the path. n[3], n[4] and n[5] are found by transportcost. On 
\ output, iorder is modified to reflect the movement of the path segment.
: transport ( -- )
	0 0 0 0 LOCALS| nn m1 m2 m3 |
	& jorder{ ncity }malloc malloc-fail? ABORT" transport :: out of memory"
	n{ 1 } @ n{ 0 } @ - ncity +  ncity MOD 1+ TO m1	\ cities from 0 to 1
	n{ 4 } @ n{ 3 } @ - ncity +  ncity MOD 1+ TO m2	\ cities from 3 to 4 
	n{ 2 } @ n{ 5 } @ - ncity +  ncity MOD 1+ TO m3	\ cities from 5 to 0 
	m1 0 DO  iorder{ n{ 0 } @ I +   ncity MOD } @
		 jorder{ nn } !  nn 1+ TO nn		\ copy chosen segment
	   LOOP
	m2 0 DO  iorder{ n{ 3 } @ I +   ncity MOD } @
		 jorder{ nn } !  nn 1+ TO nn
	   LOOP						\ copy seg n[3] to n[4]
	m3 0 DO  iorder{ n{ 5 } @ I +   ncity MOD } @ 
		 jorder{ nn } !  nn 1+ TO nn
	   LOOP						\ copy seg n[5] to n[2]
	jorder{ iorder{ ncity }icopy			\ jorder back to iorder
	& jorder{ }free	;

\ Metropolis algorithm, metrop? returns a boolean variable which issues a 
\ verdict on whether to accept a reconfiguration which leads to a change de
\ in the objective function e. If de < 0, ans = true, while if de > 0, ans is 
\ only true with probability exp(-de/t), where t is a temperature determined 
\ by the annealing schedule.
: metrop? ( -- ans ) ( F: de t -- )
	FOVER F0< IF F2DROP TRUE EXIT THEN
	F/ FNEGATE FEXP  ranf  F> ;
		
: modify ( -- )		\ note: nn = #cities NOT on the segment ( >= 3 )
	  irbit2	\ choose between segment reversal or transport
	    IF		\ do a transport to a location not on the segment
		nn 2- ABS S>F  ranf F* F>S  n{ 1 } @ + 1+  ncity MOD n{ 2 } !
		transportcost
		FDUP atime F@ metrop?
	 	  IF lenpath F+!
		     nsucc 1+ TO nsucc
		     transport
		ELSE FDROP
		THEN
	  ELSE 	revcost
		FDUP atime F@ metrop? 
		  IF lenpath F+!
		     nsucc 1+ TO nsucc
		     reverse
		ELSE FDROP
		THEN
	  THEN ;

: init-se ( -- )
	  BEGIN
	    ranf  ncity     S>F F* F>S  n{ 0 } ! \ choose start ..
	    ranf  ncity 1-  S>F F* F>S  n{ 1 } ! \ choose end
	    n{ 1 } @  n{ 0 } @ >= 1 AND n{ 1 } +! 
	    n{ 0 } @  n{ 1 } @ - 1- ncity +  
		ncity MOD 1+ TO nn	\ number of cities NOT on the segment
	    nn 3 >=
	  UNTIL ;

Public:

v: showpath ( nsucc iter -- ) ( F: lenpath atime -- )

:NONAME	( nsucc iter -- ) ( F: lenpath atime -- )
	2DROP F2DROP ; defines showpath

\ This algorithm finds the shortest round-trip path to ncity cities whose 
\ coordinates are in the arrays x[0..ncity-1],y[0..ncity-1]. The array
\ iorder[0..ncity-1] specifies the order in which the cities are visited. 
\ On input, the elements of iorder may be set to any permutation of the 
\ numbers 0 to ncity-1. This routine will return the best alternative path 
\ it can find.
: anneal ( 'x 'y 'iorder ncity -- )
	TO ncity  & iorder{ &!  & y{ &!  & x{ &!
	0 LOCALS| k |

	0.5e atime F!  
	iseed 0= IF 111 TO iseed THEN
	ncity 100 * TO nover
	ncity  10 * TO nlimit

	compute-path  lenpath F!
	100 0 DO 
		0 TO nsucc  0 TO k
		BEGIN
		  k 1+ TO k
		  init-se 
		  modify
		  nsucc nlimit >=   k nover >=  OR
		UNTIL
		nsucc I  lenpath F@  atime F@  showpath
		atime F@ tfactr F*  atime F!
		nsucc 0= IF LEAVE THEN
	    LOOP ;

Reset_Search_Order


TEST-CODE? [IF]

1000 CONSTANT #pmax
20 VALUE #p

#pmax FLOAT   ARRAY px{
#pmax FLOAT   ARRAY py{
#pmax INTEGER ARRAY order{
#pmax INTEGER ARRAY old{


: point@ ( ix -- ) ( F: -- x y )
	order{ OVER } @ px{ SWAP } F@
	order{ SWAP } @ py{ SWAP } F@ ;

: opoint@ ( ix -- ) ( F: -- x y )
	old{ OVER } @ px{ SWAP } F@
	old{ SWAP } @ py{ SWAP } F@ ;

TRUE [IF]  ( Forths without graphics use this path...)

: init-graph ( -- ) ;

: .path ( -- )
	#p order{ CR }iprint 
	CR ." #" 0 .R  ." , changes = " 0 .R  
	." , T = " F. ." , Path Length = "  F. ;

[ELSE]

: init-graph ( -- )
	GRAPHICS
	40 40  Xmax 40 - Ymax 40 -  0e 0e 10e 10e  SET-GWINDOW 
	PAGE ;

: .path ( -- )
	0 opoint@  TO PenY TO PenX
	#p 1 DO I opoint@ black DRAW-SCALED-LINE  
	   LOOP 
	0 opoint@ black DRAW-SCALED-LINE 
	0 point@  TO PenY TO PenX
	#p 1 DO I point@ yellow DRAW-SCALED-LINE  
	   LOOP 
	0 point@ yellow DRAW-SCALED-LINE
	#p 0 DO 
		I opoint@ TO PenY TO PenX  
		0.1e white DRAW-SCALED-CIRCLE 
	   LOOP 
	order{ old{ #pmax }icopy 
	HOME ." #" 0 .R  ." , changes = " 0 .R  
	     ." , T = " F. ." , Path Length = "  F. ;

[THEN]
	& .path defines showpath


: TEST-PATTERN
	100 TO #p init-graph
	#p 0 DO 
		I order{ I } !
		10e ranf F*  px{ I } F!
		10e ranf F*  py{ I } F!
	   LOOP
	order{ old{ #p }icopy
	px{ py{ order{ #p anneal ;


\ ----------------------------------------------------------------------------
\ Original program by H. Ritter in the German magazine 'mc' 9/87
\ Neural-net view of the annealing algorithm.
\ The net exists of #p neurons, neuron i is at px[i], py[i].
\ The arrays px, py are organized as a two sequential lists of #rows * #columns
\ x and y neuron coordinates.

FVARIABLE h0       FVARIABLE h1	\ these 6 set up the cooling-down parameters
FVARIABLE b0       FVARIABLE b1
FVARIABLE a0	   FVARIABLE c0

0 VALUE maxtime			\ simulation end time
0 VALUE time			\ current time

FVARIABLE xr 			\ position of the random point hitting the net
FVARIABLE yr
0 VALUE row0			\ position of the neuron nearest to it
0 VALUE col0

0 VALUE rows			\ number of neuron rows
0 VALUE columns			\ number of neuron columns

v: randomvector ( -- )

\ The best generator for a nice-looking net.
: randomvector1 ( -- )
	10e ranf F*  xr F! 
	10e ranf F*  yr F! ;  

: randomvector2 ( -- )
	ranf #p S>F F* F>S columns /MOD 
	S>F 10e rows    S>F F/ F* yr F!
	S>F 10e columns S>F F/ F* xr F! ;

: randomvector3 ( -- )
	5e ranf F*  5e F+ xr F! 
	5e ranf F*  5e F+ yr F! ;  

	& randomvector1 defines randomvector


: initialize ( rows columns maxtime -- ) 
	TO maxtime 0 TO time  TO columns TO rows 
	rows columns * DUP #pmax > ABORT" too many neurons" TO #p

	0.9e h0 F!  0.02e h1 F!
	5.0e b0 F!  1e    b1 F!

	h0 F@ h1 F@ F/ FLN  maxtime S>F F/  a0 F!
	b0 F@ b1 F@ F/ FLN  maxtime S>F F/  c0 F!

	#p 0 DO  randomvector1  xr F@ px{ I } F!  yr F@ py{ I } F!  LOOP 
	#p 0 DO  I order{ I } !  LOOP 

	init-graph ;


TRUE [IF]   ( Forths without graphics use this path...)

: draw-net ( -- ) 
	#p 0 DO ." (" I point@ FSWAP F. F. ." ) " 
	   LOOP ;

[ELSE]  \ Only works for GNUGRAFxx.frt drivers!

	0 [IF]
		1000000 ALLOCATE ?ALLOCATE VALUE 'shadow

: draw-net ( -- )
	'shadow TO 'Screen
	rows    0 DO  I columns * point@ TO PenY TO PenX
		      columns 1
		      ?DO  J columns * I + point@ yellow DRAW-SCALED-LINE LOOP
	        LOOP
	columns 0 DO  I point@ TO PenY TO PenX
		      rows 1
		      ?DO  I columns * J + point@ yellow DRAW-SCALED-LINE LOOP
	        LOOP 
	$D0000000 TO 'Screen  'shadow 'Screen Xmax Ymax * >GSCREEN 
	'shadow Xmax Ymax * ERASE ;

	[ELSE]

: draw-net ( -- )
        1 >GSCREEN#
        rows    0 DO  I columns * point@ TO PenY TO PenX
                      columns 1
                      ?DO  J columns * I + point@ yellow DRAW-SCALED-LINE LOOP
                LOOP
        columns 0 DO  I point@ TO PenY TO PenX
                      rows 1
                      ?DO  I columns * J + point@ yellow DRAW-SCALED-LINE LOOP
                LOOP 
        1 0 COPYSCREEN  GCLEAR  0 >GSCREEN# ;
  
	[THEN]

[THEN]

\ All neurons in the vicinity of (row0 col0) "crawl" towards the intruder 
\ point at (xr,yr). 'Vicinity' is defined as a square around (row0,col0) with
\ a width and height of 2*radius. Radius depends on the simulation time and 
\ decreases strongly with time. Neurons near to the intruder move much more
\ than distant neurons. Towards the end of the simulation the neurons hardly
\ move at all, no matter how near the intruder is.
: adjust-net ( -- ) 
	0e 0e FRAME| a b |
	0 0 0 0 0 LOCALS| radius col1 col2 row1 row2 |

	c0 F@ time S>F F*  FNEGATE  FEXP  b0 F@ F*  &a F!
	a F2* F1+ F>S TO radius

	col0 radius >= IF col0 radius - TO col1 ELSE 0 TO col1 THEN
	col0 radius +  columns MIN TO col2

	row0 radius >= IF row0 radius - TO row1 ELSE 0 TO row1 THEN
	row0 radius +  rows MIN TO row2

	col2 col1	\ move the neurons inside the square closer to (xr,yr)
	 ?DO 
	     row2 row1
		 ?DO  	\ nearby neurons move much more, but they grow tired 
			\ with time
		    col0 J - DUP *  row0 I - DUP *  + S>F   a FSQR F/
		    a0 F@  time S>F F*   F+ FNEGATE FEXP  h0 F@ F*  &b F!
		    px{ I columns * J + } DUP F@  1e b F- F*
		    	xr F@ b F* F+  F!
		    py{ I columns * J + } DUP F@  1e b F- F*
		   	yr F@ b F* F+  F!
		LOOP
	LOOP 
	|FRAME ;

\ row0, col0 return the neuron closest to where vector xr,yr points.
\ When the closest neuron is found all neurons in its vicinity "crawl"
\ towards the intruder point.
: match-to ( -- )
	1e100 -1 	\ max distance^2 and non-existing point
	#p 0 DO  I point@  yr F@ F- FSQR  FSWAP xr F@ F- FSQR  F+
		 F2DUP F< IF FDROP 
			ELSE DROP I FNIP
			THEN
	   LOOP 
	FDROP
	columns /MOD TO row0 TO col0 
	adjust-net ;

\ This builds a 'fishnet' of (rows-1)*(columns-1) mazes that stretches across
\ as wide an area as possible. In the course of the action the net will unfold,
\ unless a gordian knot develops at a moment the kinetic energy of the neurons
\ is becoming insufficient (This is likely to happen for larger nets, eg. 30 30
\ 3000 organize may not fully unfold).
\ If the word randomvector does NOT produce random points you may see some
\ clustering of the net.
: organize ( rows columns maxtime -- ) 
	initialize
	BEGIN
	   time 1+ TO time
	    draw-net
	    randomvector match-to
	   time maxtime > EKEY? OR
	UNTIL 
	CR time maxtime > IF ." Cooled down." 
		        ELSE EKEY DROP ." User interrupt." 
			THEN ;

[THEN]

				( * End of File * )