% ########################################## 
clear;
lambda= 1.064E-6 ;
Ltot= 1.010675025828971 ;
r0= 1.0E+100 ;
w0= 2.563E-5 ;
x0= 0 ;
lns1.abcd=abcd_lens( 0.075 ) ;
lns1.x= 0.21358727296049 ;
lns2.abcd=abcd_lens( 0.075 ) ;
lns2.x= 0.40361319425309 ;
lns3.abcd=abcd_lens( 0.203 ) ;
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 = [];
lens_size = .03;

for iteration = 1:10
    optics_x_rand = sort(lens_size+(xf-2*lens_size)*rand(1,3));
   
    fitness_simplified=@(x) fitness(q0, qf, Ltot, x );
    [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];        
    
    figure(2)
    solution_visualization(q0,x0, qf, xf, optics_placer(x_sol), lambda);
end

[energy_min, index_of_energy_min] = min(sample_energy(:))
x_sol = sample_x(index_of_energy_min,:);

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

w_final_handmade;
x_sol