% SECOND ORDER DESIGN PROBLEM implementation
% Author: Sergio Baselga, Universidad Politécnica de Valencia, Spain
% September 21, 2010



A=load('A.txt');	    % design matrix
p=load('p.txt');            % column vector containing diagonal elements of P

initialsigma=2;             % factor relative to the initial weight: i.e. if p=200; initialsigma(in weight units)=2*p=2*200
finalsigma=0.2;             % factor relative to the initial weight (stop criterion)
coolingfactor=0.9999;       % sigma(i)=sigma(i-1)*coolingfactor;
smallprob=0.001;            % probab. of accepting worse solutions (prevents the algorithm from falling into local optima)

GoalElement_Values= [2 2 9e-6;         % [row column desiredValueforQxx]
                    4 4 9e-6;
                    6 6 9e-6;
                    8 8 9e-6;
                    10 10 9e-6];

WeightsToOptimize =[16 17 18 19 20 21 22 23 24 25 26 27 28 29 30];	% 1st option
%WeightsToOptimize ='all';       					% 2nd Option

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc
disp('Computation started...');
[m,n]=size(A);
if ischar(WeightsToOptimize),   % reason: WeightsToOptimize =='all'
    WeightsToOptimize=[];
    for j=1:m, WeightsToOptimize(j)=j;end;
end;
[mWTO,nWTO]=size(WeightsToOptimize);
for i=1:m,
    P(i,i)=p(i,1);  % m x m weight matrix
end;
P0=P;               % we store the initial weight matrix as P0
                
Qx=inv(A'*P*A);
Qx0=Qx;
[NoValues,col]=size(GoalElement_Values);
dis2=0;
for j=1:NoValues,
    dis2=dis2+(Qx(GoalElement_Values(j,1),GoalElement_Values(j,2))-GoalElement_Values(j,3))^2;
end;
fbest=dis2;         %"distance" to the desired solution as measured by the square of Frobenius norm
fprev=fbest;
bestQx=Qx;
previousP=P;


sigma=initialsigma;
iter=0;

while sigma>finalsigma,
   P2=previousP;
   iter=iter+1;     % No. of iterations counter
   %% Generation of every new random weight increment
   for i=1:m
       for jj=1:nWTO,
           if WeightsToOptimize(jj)==i,
               rightvalue = 0;
               while rightvalue == 0
                   NewP = normrnd(P(i,i),sigma*P0(i,i));   %% Generates a random number from the normal distribution: N(mu,sigma)
                   if NewP>=0,
                       rightvalue=1;
                       P2(i,i)=NewP;
                   end;
               end;
           end;
       end;
   end;
   
   %%Computation of objective function
   Qx=inv(A'*P2*A);
   dis2=0;
   for j=1:NoValues,
       dis2=dis2+(Qx(GoalElement_Values(j,1),GoalElement_Values(j,2))-GoalElement_Values(j,3))^2;
   end;
   fcomputed=dis2;
   
   %% Acceptance criterion
   if fcomputed<fprev
      previousP=P2;
      fprev=fcomputed;
      if fcomputed<fbest
         bestP = P2;
         fbest = fcomputed;
         bestQx=Qx;
      end;
   else
      randomnumber = rand(1);		%% This draws out a random number in the range [0,1]
      if randomnumber<smallprob
      	previousP=P2;
      	fprev=fcomputed;
      end;
   end;
   
   sigma=sigma*coolingfactor;
   if rem(iter,1000)==0,
      fprintf('%6.0f iterations, sigma=%6.4f (to reach finalsigma=%6.4f)\n',iter,sigma,finalsigma);
   end; 
end;

fprintf('\nIterations completed! The final value for the objective function is %6.4e\n',sqrt(fbest));

for i=1:NoValues,
    message=['The final value for the Qx element ',num2str(GoalElement_Values(i,1)),',',num2str(GoalElement_Values(i,2)),' is ',num2str(bestQx(GoalElement_Values(i,1),GoalElement_Values(i,2))),' (initially ',num2str(Qx0(GoalElement_Values(i,1),GoalElement_Values(i,2))),') as compared to the expected value ',num2str(GoalElement_Values(i,3))];
    disp(message);    
end;
initialweights=diag(P0);
newweights=diag(bestP);
fprintf(1,'Obs#  Initial Weight    Final Weight\n');
for i=1:m
    fprintf('%4.0f %14.4f %14.4f\n',i,initialweights(i,1),newweights(i,1));
end;