function [energy_output,absoluteBias] = energy_function_v6(theta_vector,centroid_vector,h,omega,numStrides,targetabsoluteBias)
% use this to match to target calibration from bias-mu curve

% theta_vector = vector of angles formed over simulation
% centroid_vector = vector of (x;y) positions
% h = timestep
% omega = step frequency

% Every 1/(omega*h) timesteps is one full stride - take the average of the positions at the last stride. Use
% floor to get the closest index (has to be integer) without going over the
% total number of frames. NOTE: stabilizes after ~3 strides, so make sure
% numStrides > 3.

if numStrides <= 3
    disp('Increase the number of strides: better stability after 3 strides!')
end

angleDiffBetweenStrides = diff(theta_vector(floor(1/(omega*h)*(1:numStrides))));
initPosition = [0;0];
stridePosition = [initPosition centroid_vector(:,floor(1/(omega*h)*(1:numStrides)))];
stridePosition = [diff(stridePosition(1,:));diff(stridePosition(2,:))];
distMovedBetweenStrides = sqrt(sum(abs(stridePosition).^2,1)); % distance moved per stride in body lengths

absoluteBias = abs(angleDiffBetweenStrides(end)/distMovedBetweenStrides(end));
    % take it for last stride. This is now in units of radians/body length,
    % like in simulated paths. NOTE: ALWAYS TREATED AS POSITIVE! WHEN
    % OPPOSITE SIGN MU SCORE IS REQUIRED, SWITCH SIGN BY SYMMETRY! BUT DO
    % THAT IN R!


energy_output = abs(abs(absoluteBias)-abs(targetabsoluteBias));
    % absolute difference between |bias| and |target bias| (can be absolute
    % values by symmetry - this will take care of the case where e.g.
    % muScore > 0 (e.g. for nan flies preamp)
    % HAS TO BE POSITIVE SINCE WE'RE DOING ENERGY MINIMIZATION