% ########################################## 
% Sample Solution
clear;
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
sample_energy = [];
sample_x = [];
possible_soln = [];
possible_lens_pos = [];
lens_size = .03;

%Lens permutations
lens_permutations = perms( [ focal_length1, focal_length2, focal_length3 ]);
n_shuffles=10;

%Check if permutation has duplicates
lens_permutations = unique(lens_permutations,'rows');
n_perms = size(lens_permutations,1);

for i = 1:n_perms
    
    
    lenses_choice=lens_permutations(i,:)
      
    for iteration = 1:n_shuffles
        optics_x_rand = sort(lens_size+(xf-2*lens_size)*rand(1,3));
        
        fitness_simplified=@(x) fitness(q0, qf, Ltot, x, lenses_choice, lambda );
        [x_sol, energy]=fminsearch(fitness_simplified, optics_x_rand, optimset('TolX',1e-8,'TolFun',1e-8,'MaxFunEvals',1e8,'MaxIter',2000));
        
        sample_energy = [sample_energy; energy];
        sample_x = [sample_x; x_sol];
        
        %Return final Waist of trial
        q_f_trial = gbeam_propagation(Ltot,q0,x0,optics_placer(x_sol, lenses_choice));
        [waist, Radius] = q2wr(q_f_trial, lambda);
        
        %If it is a good solution, add to list of possible solutions
        waist_desired = wf;
        compare_waist = abs(waist - waist_desired);
        tolerance = 1E-6;
                
        if compare_waist < tolerance
            possible_soln = [possible_soln; x_sol];
            possible_lens_pos = [possible_lens_pos; lenses_choice];
        end
        
        %Visualize solution
        figure(2)
        solution_visualization(q0,x0, qf, xf, optics_placer(x_sol, lenses_choice), lambda);
        title('Testing Points');
        drawnow;       
      
    end
    
end

%Display solution with lowest energy
[energy_min, index_of_energy_min] = min(sample_energy(:));
x_sol = sample_x(index_of_energy_min,:);
lenses_choice=lens_permutations(ceil(index_of_energy_min/n_shuffles),:);

figure(2)
w_final_trial = solution_visualization(q0,x0, qf, xf, optics_placer(x_sol, lenses_choice), lambda);
title('Optimized made');


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

rounded_x_sol = round(x_sol*10^n)./10^n;
remove_index = find(ismember(possible_soln, rounded_x_sol,'rows'),1);
possible_soln(remove_index,:) = [];
index(remove_index,:) = [];

%Visualize other solutions
n_possible_soln = size(possible_soln,1);
for n_graph = 1:n_possible_soln
    figure(n_graph+2)
    w_final_trial = solution_visualization(q0,x0, qf, xf, optics_placer(possible_soln(n_graph,:), possible_lens_pos(index(n_graph),:)), lambda);
    title('Other Solutions');
end

w_final_handmade;
x_sol