lens_set = [.075, .203, .05, .03];
lens_set = [.075,.203];
lens_permutations = pick(lens_set,3,'or');
n_perms = size(lens_permutations,1);
n_shuffles=20; %number of random placements of lenses


% ########################################## 
% Sample Solution
lambda= 1.064E-6 ;
Ltot= 1.010675025828971 ;
r0= 1.0E+100 ;
w0= 2.563E-5 ;
x0= 0 ;
% focal_length1 = .075;
% focal_length2 = .075;
% focal_length3 = .203;
% 
% lns1.abcd=abcd_lens( focal_length1 ) ;
% lns1.x= 0.21358727296049 ;
% lns2.abcd=abcd_lens( focal_length2 ) ;
% lns2.x= 0.40361319425309 ;
% lns3.abcd=abcd_lens( focal_length3 ) ;
% lns3.x= 0.80361319425309 ;
wf= 3.709E-5 ;
rf= 1.0E+100 ;
xf= Ltot;

q0=wr2q(w0,r0,lambda);
x0=0;
qf=wr2q(wf,rf,lambda);
xf=Ltot;
% 
% optics={lns1,lns2,lns3};
% figure(1)
% w_final_handmade = solution_visualization(q0,x0, qf, xf, optics, lambda);
% title('Hand made');
%% ########################################## 


%Initialize sample arrays

N = n_perms * n_shuffles;
possible_lens_placement = zeros(N,3);
possible_lens_set = zeros(N,3);
possible_sample_energy = zeros(N,1);
initial_rand_lens_placement=zeros(N,3);

lens_size = .03; % physical size of the lens

for ip = 1:n_perms
    f3=lens_permutations(ip,3);
    x3=xf-f3; % last lense transfer collimated region to focused spot
    for is = 1:n_shuffles
        possible_lens_set((ip-1)*n_shuffles + is,:) = lens_permutations(ip,:);

        initial_rand_lens_placement_tmp = sort(lens_size+(x3-2*lens_size)*rand(1,2));
        initial_rand_lens_placement((ip-1)*n_shuffles + is,:) = [initial_rand_lens_placement_tmp, x3];
    end
end

parfor i = 1:N
      
        fitness_simplified=@(x) fitness(q0, qf, Ltot, x, possible_lens_set(i,:), lambda );
        [x_sol, energy]=fminsearch(fitness_simplified, initial_rand_lens_placement(i,:), optimset('TolX',1e-8,'TolFun',1e-8,'MaxFunEvals',1e8,'MaxIter',200));
        
        possible_lens_placement(i,:) =x_sol;
        possible_sample_energy(i) = energy;

        %Visualize solution
%         figure(2)
%         solution_visualization(q0,x0, qf, xf, optics_placer(x_sol, lens_permutations(ip,:)), lambda);
%         title('Testing Points');
%         drawnow;       

end



%Sorting possible solution according to energy
[possible_sample_energy, index] = sort(possible_sample_energy);
possible_lens_placement = possible_lens_placement(index,:);
possible_lens_set = possible_lens_set(index,:);

%Truncate other possible solutions to an accuracy of n decimal places
n=4; 
possible_lens_placement_trunc = round(possible_lens_placement*10^n)./10^n;
[possible_lens_placement_uniq, index] = unique(possible_lens_placement_trunc,'rows','stable'); %Unique solutions only

%Visualize five best solutions
n_possible_lens_placement = min(5,size(possible_lens_placement_uniq,1));
for n_graph = 1:n_possible_lens_placement
    figure(n_graph+1)
    w_final_trial = solution_visualization(q0,x0, qf, xf, optics_placer(possible_lens_placement(index(n_graph),:), possible_lens_set(index(n_graph),:)), lambda);
    title('Other Solutions');
end

possible_lens_placement(index(1:n_graph),:)
possible_lens_set(index(1:n_graph),:)
possible_sample_energy(index(1:n_graph),:)