Added descriptions to each function.v3.0

15 files changed, 246 insertions, 158 deletions

diff --git a/energy_vs_stability.m b/energy_vs_stability.m
index 796c631..1915449 100644
--- a/energy_vs_stability.m
+++ b/energy_vs_stability.m
@@ -1,10 +1,22 @@
 function [] = energy_vs_stability( possible_energy, stability, index, stability_max )
-%ENERGY_VS_STABILITY Summary of this function goes here
-%   Detailed explanation goes here
+%Plots energy vs. stability
+%   Using data sent from mode_match, this function plots energy vs.
+%   stability of each solution for eyed comparisons.
 
     n = size(stability,2);
-    c = 1:n;
     
+    %Color code data points
+    if n == 3
+        c = [1, 0, 0; 0, 1, 0; 0, 0, 1];
+    elseif n == 2
+        c = [1, 0, 0; 0, 1, 0];
+    elseif n == 1
+        c = [1, 0, 0];
+    else
+         c = 1:n;
+    end   
+    
+    %Reorganize stability to coincide with energies
     for i=1:n
         fixed_stability(i,1) = stability(i);
     end
@@ -13,6 +25,7 @@ function [] = energy_vs_stability( possible_energy, stability, index, stability_
         energy(i,1) = possible_energy(index(i));
     end
     
+    %Graph scatter plot
     figure(n+1)
     textCell = arrayfun(@(x,y) sprintf('(%3.2f, %3.2f)',x,y),energy,fixed_stability,'un',0);
     scatter(energy,fixed_stability,25,c,'filled')
@@ -23,13 +36,11 @@ function [] = energy_vs_stability( possible_energy, stability, index, stability_
     dx =0.0000000005;
     dy =0.0000000005;
     
+    %Label data points relative to solution number
     for i = 1:n
         n_solution = num2str(i);
         text(energy(i)+dx, fixed_stability(i)+dy, n_solution, 'FontSize',10);
     end
     
-%     for i = 1:n
-%         text(energy(i), fixed_stability(i), textCell{i}, 'FontSize',8)
-%     end
-    
+   
 end
diff --git a/find_min.m b/find_min.m
index 2c3ae7d..a6962d2 100644
--- a/find_min.m
+++ b/find_min.m
@@ -1,7 +1,5 @@
 function [ x_sol, energy ] = find_min( index, lens_placement, fitness_simplified, MaxFunEvals, MaxIter )
-%FIND_MIN Summary of this function goes here
-%   Detailed explanation goes here
-
+%Minimizes energy for given solution
     [x_sol, energy] = fminsearch(fitness_simplified, lens_placement(index,:), optimset('TolFun',1e-5,'MaxFunEvals',MaxFunEvals,'MaxIter', MaxIter));
 
 end
diff --git a/fitness.m b/fitness.m
index 403d8d7..697d5fd 100644
--- a/fitness.m
+++ b/fitness.m
@@ -1,45 +1,16 @@
-function [Energy, Waist, Penalty] = fitness( q_0, q_final, x_final, optics_positions, optics_focal_length, lambda )

+function [Energy, Waist, Penalty] = fitness( q_0, q_final, x_final, optics_positions, optics_focal_length, lambda, self_flag )

 %Outputs fitness of suggested solution

-    x0 = 0;

-    Energy = 0;

-    

-    x1=optics_positions(1);

-    x2=optics_positions(2);

-    x3=optics_positions(3);

-    

-    % penalty calculation

-    % do not put lenses too close to each other and end positions

-    lens_size=0.03;

-    

-    d(1)=abs(x1-x2);

-    d(2)=abs(x2-x3);

-    d(3)=abs(x1-x3);

-    

-    d_from_start=abs(x0-optics_positions);

-    d_from_end=abs(x_final-optics_positions);

-    

-    

-    d=cat(2, d, d_from_start, d_from_end);

-    

-    coef = 1;

-   

-    penalty_lenses_too_closely_spaced =coef*sum( exp(-(d/(lens_size)).^12) );

-    

-    Energy = Energy + penalty_lenses_too_closely_spaced;

-    

-    % make sure that lenses are between ends

-    d_from_start=(x0-optics_positions);

-    d_from_end=(optics_positions-x_final);

-    

-    d = cat(2, d_from_start, d_from_end);

-    

-    coef = 1;

-    distance_scaling=100;

-    penalty_lenses_outside_optical_path = coef * sum(1 + tanh(distance_scaling*d));

-    

-    Energy = Energy + penalty_lenses_outside_optical_path;

-    

-    % make collimated region between 2nd and 3rd lens   

+x0 = 0;

+Np=20; % # of pts between start position and second lens

+N_collimated = 10; % # of pts between second lens and third lens to be collimated region

+Energy = 0;

+

+x1=optics_positions(1);

+x2=optics_positions(2);

+x3=optics_positions(3);

+

+if self_flag == 1

+    % make collimated region between 2nd and 3rd lens

     

     [w0, r0] = q2wr(q_0, lambda);

     [ w, w_pos ] = self_gbeam_propagation( w0, optics_positions, optics_focal_length, x0, lambda );

@@ -51,20 +22,84 @@ function [Energy, Waist, Penalty] = fitness( q_0, q_final, x_final, optics_posit
     

     Energy = Energy + penalty_not_collimated_beam;

     

-%     % waist at end matches desired waist

+    % waist at end matches desired waist

     coef = 10;

     [waist_desired, r_desired] = q2wr(q_final, lambda);

     

     penalty_waist = coef *((( waist_desired-w(end) )/waist_desired)^2);

     

     Energy = Energy + penalty_waist;

-

+    

     coef=10;

     dist_between_desired_and_final_waist_location =w_pos(end) - x_final;

     penalty_final_waist_position = coef*dist_between_desired_and_final_waist_location^2;

-

+    

     Energy = Energy + penalty_final_waist_position;

+    

+else

+    % check if waists match in forward and backward propagation

+    x_array1=linspace(x0,x2,Np);

+    x_array2=linspace(x2, x3, N_collimated);

+    x = cat(2,x_array1,x_array2);

+    

+    q_f_trial_forward = gbeam_propagation(x,q_0,x0,optics_placer(optics_positions, optics_focal_length));

+    [Waist_trial_forward, Radius_trial_forward] = q2wr(q_f_trial_forward, lambda);

+    q_f_trial_backward = gbeam_propagation(x,q_final,x_final,optics_placer(optics_positions, optics_focal_length));

+    [Waist_trial_backward, Radius_trial_backward] = q2wr(q_f_trial_backward, lambda);

+    

+    Penalty_waist_mismatch = sum(abs((Waist_trial_forward-Waist_trial_backward)./min(Waist_trial_forward, Waist_trial_backward)))/Np;

+    

+    Energy = 1e-2*Penalty_waist_mismatch;

+    

+    %intialize intermediate points between lenses

+    q_intermediate = q_f_trial_forward((x2<x) & (x<x3));

+    lambda_over_waist_sq = (-imag(1./q_intermediate)); %with numerical factor

+    

+    coef = 1e-3;

+    penalty_not_collimated_beam = coef * sum(exp((std(lambda_over_waist_sq)/mean(lambda_over_waist_sq)/abs(optics_positions(2) - optics_positions(3)))));

+    Energy = Energy + penalty_not_collimated_beam;

+    

+    

+end

+

+

+

+% penalty calculation

+% do not put lenses too close to each other and end positions

+lens_size=0.03;

+

+

+

+d(1)=abs(x1-x2);

+d(2)=abs(x2-x3);

+d(3)=abs(x1-x3);

+

+d_from_start=abs(x0-optics_positions);

+d_from_end=abs(x_final-optics_positions);

+

+

+

+d=cat(2, d, d_from_start, d_from_end);

+

+coef = 1;

+

+penalty_lenses_too_closely_spaced =coef*sum( exp(-(d/(lens_size)).^12) );

+

+Energy = Energy + penalty_lenses_too_closely_spaced;

+

+% make sure that lenses are between ends

+d_from_start=(x0-optics_positions);

+d_from_end=(optics_positions-x_final);

+

+d = cat(2, d_from_start, d_from_end);

+

+coef = 1e-2;

+distance_scaling=100;

+penalty_lenses_outside_optical_path = coef * sum(1 + tanh(distance_scaling*d));

+

+Energy = Energy + penalty_lenses_outside_optical_path;

+

+Penalty = [ penalty_lenses_too_closely_spaced; penalty_lenses_too_closely_spaced; penalty_not_collimated_beam];

 

-   Penalty = [ penalty_lenses_too_closely_spaced; penalty_lenses_outside_optical_path; penalty_not_collimated_beam; penalty_waist; penalty_final_waist_position];

 end

 

diff --git a/fitter_check.m b/fitter_check.m
index 9491295..b7866ae 100644
--- a/fitter_check.m
+++ b/fitter_check.m
@@ -1,39 +1,39 @@
 %Permute all possible lens combinations out of set of lenses
-lens_set = [.075, .203, .05, .03];
-lens_set = [.075,.203];
-lens_permutations = pick(lens_set,3,'or');
+lens_set = [.075, .203]; %Given lenses of unique focal lengths
+lens_permutations = pick(lens_set,3,'or'); %3 lens solutions
 
 %Pre-defined Constants
-lambda= 1.064E-6 ;
-Ltot= 1.010675025828971 ;
-r0= 1.0E+100 ;
-w0= 2.563E-5 ;
-x0= 0 ;
-wf= 3.709E-5 ;
-rf= 1.0E+100 ;
-xf= Ltot;
-q0=wr2q(w0,r0,lambda);
-qf=wr2q(wf,rf,lambda);
+lambda= 1.064E-6 ; %Wavelength of beam
+Ltot= 1.010675025828971 ; %Length of optical system
+r0= 1.0E+100 ; %Initial radius of curvature
+w0= 2.563E-5 ; %Initial waist
+x0= 0 ; %Starting position of beam
+wf= 3.709E-5 ; %Desired final waist
+rf= 1.0E+100 ; %Desired final radius
+lens_width = .03; %Lens width
+show_lens_width = 1; %Set to 1 to enable display of lens width on solution propagation plot
+show_lens_position = 1; %Set to 1 to enable display of position of center of lens on solution propagation plot
+display_prop = [show_lens_width, show_lens_position];
+n_truncate = 3; %number of digits in truncated solution
+n_visualizations = 2; %number of best solutions to visualize
+n_hist = 1000; %number of sample points in histogram
+stability_max = 1; %max stability (y-axis) shown on energy vs. stability graph
+self_flag = 0; %Set to 1 to use Self's gaussian beam propagation, otherwise set to 0
 %End list
 
+q0=wr2q(w0,r0,lambda); %Calculate intial q
+qf=wr2q(wf,rf,lambda); %Calculate final q
+
 %Mode match
-[ possible_lens_placement, possible_lens_set, possible_sample_energy] = mode_match( q0, qf, Ltot, lambda, lens_permutations );
+[ possible_lens_placement, initial_lens_placement, possible_lens_set, possible_sample_energy] = mode_match( q0, qf, Ltot, lambda, lens_permutations, lens_width, self_flag );
 
 %Remove similar solutions
-n_truncate = 3; %number of digits in truncated solution
 [ possible_lens_placement_uniq, possible_lens_placement, possible_sample_energy, possible_lens_set, index ] = remove_similar_soln( possible_sample_energy, possible_lens_placement, possible_lens_set, n_truncate );
 
-n_visualizations = 5; %number of best solutions to visualize
-pick_visualization( possible_sample_energy, possible_lens_placement_uniq, possible_lens_placement, possible_lens_set, index, n_visualizations, q0, qf, Ltot, lambda );
-
-%Visualize fitness function for fixed f2 and f3
-lens_set = [.075, .075, .203];
-f2= 0.40361319425309 ;
-f3= 0.80361319425309 ;
 
-%fitness_simplified=@(x) fitness(q0, qf, Ltot, [x, f2, f3], lens_set, lambda );
-%figure(6)
-%ezplot(fitness_simplified, [0,Ltot])
+%Visualize solutions
+pick_visualization( possible_sample_energy, possible_lens_placement_uniq, possible_lens_placement, possible_lens_set, index, n_visualizations, q0, qf, Ltot, lambda, lens_width, display_prop );
 
-[ w, w_pos ] = self_gbeam_propagation( w0, possible_lens_placement, possible_lens_set, x0, lambda )
-%solution_visualization(q0,x0,qf,xf, optics_placer(possible_lens_placement, possible_lens_set), lambda)
+%Plot energy vs. stability for each solution
+[stability] = stability_visualization( possible_lens_placement_uniq, q0, qf, Ltot, possible_lens_placement, possible_lens_set, lambda, n_visualizations, n_hist, index, self_flag );
+energy_vs_stability( possible_sample_energy, stability, index, stability_max)
diff --git a/gaussian_focus.m b/gaussian_focus.m
index ff444ee..cceeda0 100644
--- a/gaussian_focus.m
+++ b/gaussian_focus.m
@@ -1,9 +1,14 @@
 function [ w, s ] = gaussian_focus( w0, s0, f, lambda )
-%GAUSSIAN_FOCUS Summary of this function goes here
-%   Detailed explanation goes here
+%Focusing of Gaussian beams based on Sidney A. Self's "Focusing of
+%spherical Gaussian beams" Applied Optics, Vol. 22, Issue 5, pp. 658-661 (1983)
+% Input: lambda = wavelength;
+%         w0 = waist before the lens;
+%         s0 = distance of waist before lens (s>0 before lens)
+%         f = focal length of lens (f>0 for converging lens)
+% Output: w = new waist;
+%         s = position of waist from lens
 
-zR = pi*w0^2/lambda;
-s = f*(1+(s0/f-1)/((s0/f-1)^2+(zR/f)^2));
+zR = pi*w0^2/lambda; %Rayleigh Range
+s = f*(1+(s0/f-1)/((s0/f-1)^2+(zR/f)^2)); %Eq. (9b)
 w = w0/sqrt((1-s0/f)^2+(zR/f)^2);
-end
-
+end
\ No newline at end of file
diff --git a/gbeam_propagation.m b/gbeam_propagation.m
index d81f70e..585048d 100644
--- a/gbeam_propagation.m
+++ b/gbeam_propagation.m
@@ -1,4 +1,4 @@
-function q = gbeam_propagation(x_pos, q_in, x_in,  optics_elements)
+function [q] = gbeam_propagation(x_pos, q_in, x_in,  optics_elements)
 % calculate the 'q' parameter of the Gaussian beam propagating through optical
 % 'optics_elements' array along 'x' axis at points 'x_pos'
 %  takes the gaussian beam with initial q_in parameter at x_in
@@ -8,7 +8,7 @@ function q = gbeam_propagation(x_pos, q_in, x_in,  optics_elements)
 
 	if any(x_pos >= x_in)
 		% Forward propagation to the right of x_in
-		q(x_pos >= x_in) = gbeam_propagation_froward_only(x_pos(x_pos>=x_in), q_in, x_in,  optics_elements);
+		[q(x_pos >= x_in)] = gbeam_propagation_froward_only(x_pos(x_pos>=x_in), q_in, x_in,  optics_elements);
 	end
 
 	if any(x_pos < x_in)
@@ -45,8 +45,10 @@ function q = gbeam_propagation(x_pos, q_in, x_in,  optics_elements)
 		% final assignment of the backwards propagating beam 
 		% which we need to flip back
 		q(x_pos<x_in) = fliplr(q_backw);
-	end
+    end
 
+    
+    
 end
 
 %!test 
diff --git a/gbeam_propagation_froward_only.m b/gbeam_propagation_froward_only.m
index afc1316..2cdc1d8 100644
--- a/gbeam_propagation_froward_only.m
+++ b/gbeam_propagation_froward_only.m
@@ -1,4 +1,4 @@
-function q = gbeam_propagation_froward_only(x_pos, q_in, x_in,  optics_elements)
+function [q] = gbeam_propagation_froward_only(x_pos, q_in, x_in,  optics_elements)
 % calculate the 'q' parameter of the Gaussian beam propagating through optical
 % 'optics_elements' only in the positive direction  along 'x' axis at points 'x_pos'
 % takes the gaussian beam with initial q_in parameter at x_in
@@ -8,6 +8,10 @@ function q = gbeam_propagation_froward_only(x_pos, q_in, x_in,  optics_elements)
 %
 % all x_pos must be to the right of x_in
 % x_pos must be monotonic!
+    
+    %Initialize q_lens (q at position of lens)
+    q_lens = zeros(1,size(optics_elements,2));
+
 	if (any(x_pos < x_in))
 		error('all beam positions must be to the right of the x_in');
 	end
@@ -45,8 +49,7 @@ function q = gbeam_propagation_froward_only(x_pos, q_in, x_in,  optics_elements)
     index = (x_last_calc <= x_pos);
     x=x_pos(index);
     q(index)= q_after_free_space(q_last_calc,x-x_last_calc);
-       
-	
+      
 	
 end
 
diff --git a/mode_match.m b/mode_match.m
index 0a36332..f3f7408 100644
--- a/mode_match.m
+++ b/mode_match.m
@@ -1,4 +1,4 @@
-function [ possible_lens_placement, possible_lens_set, possible_sample_energy] = mode_match( q0, qf, Ltot, lambda, lens_permutations )
+function [ final_possible_lens_placement, initial_possible_lens_placement, possible_lens_set, possible_sample_energy] = mode_match( q0, qf, Ltot, lambda, lens_permutations, lens_width, self_flag )
 %Shuffles lenses into random positions and stores possible solutions
 %   Shuffles over entire lens permutation array for n_shuffles times.
 %   Afterwards, solutions are sorted by Energy and truncated to n_truncate
@@ -7,6 +7,11 @@ function [ possible_lens_placement, possible_lens_set, possible_sample_energy] =
 n_perms = size(lens_permutations,1);
 n_shuffles=20; %number of random placements of lenses
 
+initial_MaxFunEvals = 1e8;
+initial_MaxIter = 10;
+final_MaxFunEvals = 1e8;
+final_MaxIter = 100;
+
 %Initialize sample arrays
 N = n_perms * n_shuffles;
 possible_lens_placement = zeros(N,3);
@@ -14,25 +19,33 @@ 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=Ltot-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_tmp = sort(lens_width+(x3-2*lens_width)*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('TolFun',1e-5,'MaxFunEvals',1e8,'MaxIter',200));
+        fitness_simplified=@(x) fitness(q0, qf, Ltot, x, possible_lens_set(i,:), lambda, self_flag );
+        [ x_sol, energy ] = find_min(i, initial_rand_lens_placement, fitness_simplified, initial_MaxFunEvals, initial_MaxIter )
+                
+        initial_possible_lens_placement(i,:) =x_sol;
         
-        possible_lens_placement(i,:) =x_sol;
+end
+
+parfor i = 1:N
+        fitness_simplified=@(x) fitness(q0, qf, Ltot, x, possible_lens_set(i,:), lambda, self_flag );
+        [ x_sol, energy ] = find_min(i, initial_possible_lens_placement, fitness_simplified, final_MaxFunEvals, final_MaxIter )
+                
+        final_possible_lens_placement(i,:) =x_sol;
         possible_sample_energy(i) = energy;
 
 end
diff --git a/optics_placer.m b/optics_placer.m
index 247cc81..2407691 100644
--- a/optics_placer.m
+++ b/optics_placer.m
@@ -1,4 +1,5 @@
 function optics = optics_placer( x ,f )

+%Places optical elements within array optics for use in other functions

     x1=x(1);

     x2=x(2);

     x3=x(3);

diff --git a/pick_visualization.m b/pick_visualization.m
index 88f1989..5a11231 100644
--- a/pick_visualization.m
+++ b/pick_visualization.m
@@ -1,4 +1,4 @@
-function [ ] = pick_visualization( fitness_energy, possible_lens_placement_uniq, possible_lens_placement, possible_lens_set, index, n_visualizations, q0, qf, Ltot, lambda )
+function [ ] = pick_visualization( fitness_energy, possible_lens_placement_uniq, possible_lens_placement, possible_lens_set, index, n_visualizations, q0, qf, Ltot, lambda, lens_width, display_prop )
 %Picks n_visualizations of sets of data and graphs each 
 x0 = 0;
 
@@ -6,11 +6,11 @@ n_possible_lens_placement = min(n_visualizations,size(possible_lens_placement_un
 
 for n_graph = 1:n_possible_lens_placement
     figure(n_graph)
-    [w_final_trial, r_final_trial] = solution_visualization(q0, x0, qf, Ltot, optics_placer(possible_lens_placement(index(n_graph),:), possible_lens_set(index(n_graph),:)), lambda);
+    [w_final_trial, r_final_trial] = solution_visualization(q0, x0, qf, Ltot, optics_placer(possible_lens_placement(index(n_graph),:), possible_lens_set(index(n_graph),:)), lambda, lens_width, display_prop);
     
-    str1=sprintf('\n f_1 = %0.4f, x_1 = %0.4f\n',possible_lens_set(index(n_graph),1),possible_lens_placement(index(n_graph),1));
-    str2=sprintf('f_2 = %0.4f, x_2 = %0.4f\n',possible_lens_set(index(n_graph),2),possible_lens_placement(index(n_graph),2));
-    str3=sprintf('f_3 = %0.4f, x_3 = %0.4f\n',possible_lens_set(index(n_graph),3),possible_lens_placement(index(n_graph),3));
+    str1=sprintf('\n (red) f_1 = %0.4f, x_1 = %0.4f\n',possible_lens_set(index(n_graph),1),possible_lens_placement(index(n_graph),1));
+    str2=sprintf(' (green) f_2 = %0.4f, x_2 = %0.4f\n',possible_lens_set(index(n_graph),2),possible_lens_placement(index(n_graph),2));
+    str3=sprintf(' (blue) f_3 = %0.4f, x_3 = %0.4f\n',possible_lens_set(index(n_graph),3),possible_lens_placement(index(n_graph),3));
     tstr='Solution #';
     str_w = ['w_{final}= ',num2str(w_final_trial)];
     str_r = [', r_{final}= ',num2str(r_final_trial)];
diff --git a/remove_similar_soln.m b/remove_similar_soln.m
index 9bea732..a106aff 100644
--- a/remove_similar_soln.m
+++ b/remove_similar_soln.m
@@ -1,6 +1,6 @@
 function [ possible_lens_placement_uniq, possible_lens_placement, possible_sample_energy, possible_lens_set, index ] = remove_similar_soln( possible_sample_energy, possible_lens_placement, possible_lens_set, n_truncate )
-%REMOVE_SIMILAR_SOLN Summary of this function goes here
-%   Detailed explanation goes here
+%Removes similar solutions based on n_truncate (decimal tolerance to
+%determin uniqueness)
 
 %Sorting possible solution according to energy
 [possible_sample_energy, index] = sort(possible_sample_energy);
diff --git a/self_gbeam_propagation.m b/self_gbeam_propagation.m
index 10bcf05..824be5a 100644
--- a/self_gbeam_propagation.m
+++ b/self_gbeam_propagation.m
@@ -1,11 +1,11 @@
 function [ w, w_pos ] = self_gbeam_propagation( w0, x_lens, f, x0, lambda )
-%SELF_GBEAM_PROPAGATION Summary of this function goes here
-%   Detailed explanation goes here
+%Beam propagation based on Sidney A. Self's "Focusing of spherical Gaussian
+%beams". Applied Optics, Vol. 22, Issue 5, pp. 658-661 (1983)
 
 n_lens = 3;
 
-w = w0;
-w_pos = x0;
+w = w0; %Initial waist
+w_pos = x0; %Initial waist position
 
 for i = 1:n_lens
     x_from_next_lens = x_lens(i) - w_pos(i);
@@ -14,32 +14,4 @@ for i = 1:n_lens
     w_pos(i+1) = x_lens(i) + s_new;
 end
 
-% x_pos= x_lens;
-% x(1)=w_pos(1);
-% waist_prop(1)=w(1);
-% 
-% x_temp=linspace(x0 ,x_pos(1));
-% waist_prop=[waist_prop, w(1)*sqrt(1+((x_temp-w_pos(1))/(pi*w(1)^2/lambda)).^2)];
-% x=[x x_temp];
-% 
-% for i=1:n_lens
-%     x_temp=linspace(x_pos(i),x_pos(i+1));     
-%     waist_prop=[waist_prop, w(i)*sqrt(1+((x_temp-w_pos(i))/(pi*w(i)^2/lambda)).^2)];  
-%     x=[x x_temp];
-% end
-% 
-% x_temp=linspace(x_pos(end),x_pos(end)+abs(f(end)));  
-% waist_prop=[waist_prop, w(end)*sqrt(1+((x_temp-w_pos(end))/(pi*w(end)^2/lambda)).^2)];
-% x=[x x_temp];
-% 
-% 
-% figure(1)
-% plot(x,waist_prop,'r',x,-waist_prop,'r'); 
-% 
-
-
-
-
-
-end
-
+end
\ No newline at end of file
diff --git a/solution_stability.m b/solution_stability.m
index 324621f..359335c 100644
--- a/solution_stability.m
+++ b/solution_stability.m
@@ -1,12 +1,13 @@
-function [ hist_h, hist_x ] = solution_stability( q_0, q_final, x_final, optics_positions, optics_focal_length, lambda, n_hist  )
-%SOLUTION_STABILITY Summary of this function goes here
-%   Detailed explanation goes here
+function [ hist_h, hist_x ] = solution_stability( q_0, q_final, x_final, optics_positions, optics_focal_length, lambda, n_hist, self_flag  )
+%Plots histogram of n_hist number of sample points
+%Area under plot is equal to the stability of the solution.
+
 lens_displacement = 10^-3;
 x = lens_displacement*randn(n_hist,3);
 
 for i=1:n_hist
     xd= x(i,:);
-    fitness_array(i) = fitness( q_0, q_final, x_final, optics_positions + xd, optics_focal_length, lambda );
+    fitness_array(i) = fitness( q_0, q_final, x_final, optics_positions + xd, optics_focal_length, lambda, self_flag );
 end
 
 [hist_h, hist_x] = hist(fitness_array,100);
diff --git a/solution_visualization.m b/solution_visualization.m
index 2cf8ce7..2b4158c 100644
--- a/solution_visualization.m
+++ b/solution_visualization.m
@@ -1,11 +1,19 @@
-function [waste_at_the_end, radius_at_the_end] = solution_visualization(q0,x0, qf, xf, optics, lambda)
+function [waste_at_the_end, radius_at_the_end, waist_at_lens_position] = solution_visualization(q0,x0, qf, xf, optics, lambda, lens_width, display_prop)
 %Propagates beam with given input parameters
 %   Forward propagation and backward propagation are taken in order to
 %   visualize reasonable solutions.
 
+%Array of lens positions
+n_lens = size(optics,2);
+
+for i = 1:n_lens
+    lens_position(i) = optics{i}.x;
+end
+
 x=linspace(x0,xf,1000); % we will calculate beam profile between x0 and xf
+
 %Forward propagation
-q_forward=gbeam_propagation(x,q0,x0,optics);
+q_forward =gbeam_propagation(x,q0,x0,optics);
 [w_forward,r_forward]=q2wr(q_forward, lambda);
 
 %Backward propagation
@@ -13,13 +21,53 @@ q_backward=gbeam_propagation(x,qf,xf,optics);
 [w_backward,r_backward]=q2wr(q_backward, lambda);
 
 %Plot beam profile
-plot ( ...
-	x,w_forward, '-r',  ...
-	x,-w_forward, '-r', ...
-	x, w_backward, '-.b', ...
-	x, -w_backward, '-.b')
-legend({'forward propagation', '', 'backward propagation', ''})      
+subplot(2,1,1); plot ( ...
+    x,w_forward, '-r',  ...
+    x,-w_forward, '-r', ...
+    x, w_backward, '-.b', ...
+    x, -w_backward, '-.b')
+legend({'forward propagation', '', 'backward propagation', ''})
+
+%Find q and waist at lens positions
+waist_at_lens_position = zeros(1,n_lens);
+q_lens = zeros(1,n_lens);
+
+for i = 1:n_lens
+    q_lens(i) = gbeam_propagation(lens_position(i),q0,x0,optics);
+end
+
+
+for i = 1:n_lens
+    waist_at_lens_position(i) = q2wr(q_lens(i), lambda);
+end
+
+
+%Plot lenses
+color = ['m' 'g' 'c'];
+
+if display_prop(1) == 1
+    for i = 1:n_lens
+        half_width = lens_width/2;
+        corrected_lens_position = lens_position(i)-half_width;
+        corrected_waist = waist_at_lens_position(i)*2;
+        rectangle('Position', [corrected_lens_position,-waist_at_lens_position(i),lens_width,corrected_waist], 'EdgeColor', color(i), 'LineWidth',1);
+    end
+end
+
+if display_prop(2) == 1
+    for i = 1:n_lens
+        x1 = optics{i}.x;
+        y1 = waist_at_lens_position(i);
+        x2 = x1;
+        y2 = -waist_at_lens_position(i);
+        
+        line([x1;x2], [y1;y2], 'LineWidth', 3, 'Color', color(i));
+    end
+end
+
 
 [waste_at_the_end,radius_at_the_end]  = q2wr(q_forward(end), lambda);
 
 
+
+
diff --git a/stability_visualization.m b/stability_visualization.m
index ba0a8de..37e9aad 100644
--- a/stability_visualization.m
+++ b/stability_visualization.m
@@ -1,12 +1,11 @@
-function [stability] = stability_visualization( possible_lens_placement_uniq, q0, qf, xf, optics_position, optics_set, lambda, n, n_hist, index )
-%STABILITY_VISUALIZATION Summary of this function goes here
-%   Detailed explanation goes here
+function [stability] = stability_visualization( possible_lens_placement_uniq, q0, qf, xf, optics_position, optics_set, lambda, n, n_hist, index, self_flag )
+%Visualizes stability graph
 
 n_possible_lens_placement = min(n, size(possible_lens_placement_uniq,1));
 
 for i=1:n
     figure(i)
-    [hist_h, hist_x] = solution_stability( q0, qf, xf, optics_position(index(i),:), optics_set(index(i),:), lambda, n_hist  );
+    [hist_h, hist_x] = solution_stability( q0, qf, xf, optics_position(index(i),:), optics_set(index(i),:), lambda, n_hist, self_flag  );
     subplot(2,1,2); plot(hist_x, hist_h);  
     area = trapz(hist_x, hist_h);
     str_n = ['# of test points = ', num2str(n_hist)];