# Number of towns
N=25

# Creating the random map & the distance matrix
#townz=matrix(rnorm(2*N)+runif(2*N)*6,ncol=2)
dist=sqrt(( townz[,1]%*%t(rep(1,N))-rep(1,N)%*%t(townz[,1]) )^2 +
      ( townz[,2]%*%t(rep(1,N))-rep(1,N)%*%t(townz[,2]) )^2)
dev.off()
X11(w=12,h=8)
plot(townz,pch=19,col="red")

# Cost of a path
coss=function(perm){
 
  co=dist[perm[N],perm[1]]
  for (t in 1:(N-1))
    co=co+dist[perm[t],perm[t+1]]

  co
  }

# Plot of a path
plath=function(perm,tranz){

  # browser()
  lines(townz[tranz,1],townz[tranz,2],col="white",lwd=2)
  lines(c(townz[tranz[N],1],townz[tranz[1],1]),
	c(townz[tranz[N],2],townz[tranz[1],2]),col="white",lwd=2)
  lines(townz[perm,1],townz[perm,2],col="sienna",lwd=2)
  lines(c(townz[perm[N],1],townz[perm[1],1]),
	c(townz[perm[N],2],townz[perm[1],2]),col="sienna",lwd=2)
  points(townz,pch=19,col="red")
  }

lastplath=function(perm){

  lines(townz[perm,1],townz[perm,2],col="steelblue",lty=2,lwd=2)
  lines(c(townz[perm[N],1],townz[perm[1],1]),
        c(townz[perm[N],2],townz[perm[1],2]),col="steelblue",lty=2,lwd=2)
  }

# Random start
#overperm=c(1,sample(2:N))
refperm=overperm
plath(refperm,refperm)
lastplath(overperm)
overcos=coss(overperm)
refcos=overcos
print(refcos)
#alpha=refcos

# SA
# under the provision perm[1]=1
for (i in 1:5000){

   perm=c(1,sample(2:N))
   newcos=coss(perm)

   if (newcos<overcos){
         overcos=newcos;overperm=perm
         }

   if (log(runif(1))<(refcos-newcos)/alpha){
     print(refcos-newcos)
     plath(perm,refperm)
     lastplath(overperm)
     refperm=perm;refcos=newcos;

   }

   # same thing with distance dependent probabilities
   newe=sample(2:N,1,pro=1/dist[1,-1]^2)
   perm=c(1,newe)

   for (w in 3:(N-1)){

    newe=sample((1:N)[-perm],1,pro=1/dist[newe,-perm]^2)
    perm=c(perm,newe)
    }

   perm=c(perm,(1:N)[-perm])
      if (newcos<overcos){
         overcos=newcos;overperm=perm
         }

   if (log(runif(1))<(refcos-newcos)/alpha){
     print(refcos-newcos)
     plath(perm,refperm)
     lastplath(overperm)
     refperm=perm;refcos=newcos;

   }

   # switching permutation neighbours
   midperm=refperm
   for (t in 1:sample(1:N,1,pro=1/(1:N)^5)){

      val=2*sample(1:trunc((N-1)/2),1)
      oval=sample(c(1,-1),1)+val

      perm=midperm
      perm[midperm==val]=oval
      perm[midperm==oval]=val
      midperm=perm
      }

   nnewcos=coss(perm)

   if (nnewcos<overcos){
         overcos=nnewcos;overperm=perm
         }

   if (log(runif(1))<(refcos-nnewcos)/alpha){
     print(refcos-nnewcos)
     plath(perm,refperm)
     lastplath(overperm)
     refperm=perm;refcos=nnewcos;
   }

   # switching branches
   midperm=refperm
   for (t in 1:sample(1:5,1,pro=1/(1:5)^5)){

     val=sample(seq(2,N,2),1);oval=val-1
     vol=sample(seq(2,N,2)[-(val/2)],1);avol=vol-1

     perm=midperm
     perm[midperm==val]=oval
     perm[midperm==oval]=val
     midperm=perm
     }

   nnewcos=coss(perm)

   if (nnewcos<overcos){
         overcos=nnewcos;overperm=perm
         }

   if (log(runif(1))<(refcos-nnewcos)/alpha){
     print(refcos-nnewcos)
     plath(perm,refperm)
     lastplath(overperm)
     refperm=perm;refcos=nnewcos;
   }


   # switching distance neighbours
   midperm=refperm
   for (t in 1:sample(1:N,1,pro=1/(1:N)^5)){
   #for (t in 1:sample(1:N,1)){

      val=sample(1:N,1)
      oval=sample((1:N)[-val],1,pro=1/dist[(1:N)[-val],val])

      perm=midperm
      perm[midperm==val]=oval
      perm[midperm==oval]=val
      midperm=perm
      }
   
   nnewcos=coss(perm)

   if (nnewcos<overcos){
         overcos=nnewcos;overperm=perm
         }

   if (log(runif(1))<(refcos-nnewcos)/alpha){
     print(refcos-nnewcos)
     plath(perm,refperm)
     lastplath(overperm)
     refperm=perm;refcos=nnewcos;
   }

   # switching non-neighbours
   midperm=refperm
   for (t in 1:sample(1:N,1,pro=1/(1:N)^5)){
   #for (t in 1:sample(1:N,1)){
   
      val=sample(1:N,1)
      oval=sample((1:N)[-val],1,pro=dist[(1:N)[-val],val])
   
      perm=midperm
      perm[midperm==val]=oval
      perm[midperm==oval]=val
      midperm=perm
      }

   nnnewcos=coss(perm)

   if (nnnewcos<overcos){
         overcos=nnnewcos;overperm=perm
         }

   if (log(runif(1))<(refcos-nnnewcos)/alpha){
     print(refcos-nnnewcos)
     plath(perm,refperm)
     lastplath(overperm)
     refperm=perm;refcos=nnnewcos;
   }

   alpha=alpha*.999
   }

plot(townz,pch=19,col="red")
plath(refperm,refperm)
lastplath(overperm)