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;
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 );
    
    coef = 10;
    d_object = abs(optics_positions(end) - w_pos(end - 1));
    d_lens = optics_positions(end) - optics_positions(end - 1);
    penalty_not_collimated_beam = coef * exp(-(d_object/d_lens)^2);
    
    Energy = Energy + penalty_not_collimated_beam;
    
    % 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];

end