function [Energy, Waist, Penalty] = fitness( q_0, q_final, x_final, optics_positions, optics_focal_length, lambda )
%FITNESS Summary of this function goes here
%   Detailed explanation goes here
    x0 = 0;
    q_f_trial = gbeam_propagation(x_final,q_0,x0,optics_placer(optics_positions, optics_focal_length));
    [Waist, Radius] = q2wr(q_f_trial, lambda);
        
    Energy = abs(q_final-q_f_trial);
    
    % penalty calculation
    % do not put lenses too close to each other and end positions
    lens_size=0.03;
    
    x1=optics_positions(1);
    x2=optics_positions(2);
    x3=optics_positions(3);
    
    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;
    
    penalty_lenses_outside_optical_path = coef * sum(1 + tanh(10*d));
    
    Energy = Energy + penalty_lenses_outside_optical_path;
    
    % make collimated region between 2nd and 3rd lens   
    %intialize intermediate points between lenses
    intermediate_positions = linspace(optics_positions(2), optics_positions(3),10);
    f_q_x = @(x) gbeam_propagation(x,q_0,x0,optics_placer(optics_positions, optics_focal_length));
    q_intermediate = arrayfun(f_q_x,intermediate_positions);
    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;
    
    Penalty = [ penalty_lenses_too_closely_spaced; penalty_lenses_too_closely_spaced; penalty_not_collimated_beam];
end