clc;
clear;
close all;

%% Problem Definition

data=load('data.mat','-mat');

D=data.D;

CostFunction=@(tour) TourLength(tour,D);

nVar=size(D,1);

VarSize=[1 nVar];


%% SA Parameters

MaxIt=500;

MaxItPerTemp=10;

nPop=5;

nMove=10;

T0=10;
Tf=0.001;

alpha=(Tf/T0)^(1/MaxIt);

%% Initialization

individual.Position=[];
individual.Cost=[];

pop=repmat(individual,nPop,1);
for i=1:nPop
    pop(i).Position=randperm(nVar);
    pop(i).Cost=CostFunction(pop(i).Position);
end

Costs=[pop.Cost];
[Costs SortOrder]=sort(Costs);
pop=pop(SortOrder);

BestSol=pop(1);

BestCost=zeros(MaxIt,1);

T=T0;

%% SA Main Loop

for it=1:MaxIt
    
    for it2=1:MaxItPerTemp
        
        NewSol=repmat(individual,nPop,nMove);
        for i=1:nPop
            for m=1:nMove
                NewSol(i,m).Position=CreateNeighbor(pop(i).Position);
                NewSol(i,m).Cost=CostFunction(NewSol(i,m).Position);
            end
        end
        NewSol=NewSol(:);
        
        NewSolCosts=[NewSol.Cost];
        [NewSolCosts NewSolSortOrder]=sort(NewSolCosts);
        NewSol=NewSol(NewSolSortOrder);

        for i=1:nPop
        
            DELTA=(NewSol(i).Cost-pop(i).Cost)/pop(i).Cost;

            if DELTA<=0
                % NewSol is Better
                pop(i)=NewSol(i);
            else
                % NewSol is not Better
                p=exp(-DELTA/T);
                if rand<p
                    pop(i)=NewSol(i);
                end
            end

            if pop(i).Cost<BestSol.Cost
                BestSol=pop(i);
            end

        end
    end
    
    BestCost(it)=BestSol.Cost;
    
    disp(['Iteration ' num2str(it) ':  Best Cost = ' num2str(BestCost(it))]);
    
    T=alpha*T;
    
end

%% Results

figure;
plot(BestCost);