%  function [ results ] = deflection_scan( x0, y0, xp0, yp0, gamma, Bmap1D, alpha )
%  %deflection_scan deflection for a given field map
%  

c = 2.998e8; % speed of light (m/s)

%   x0, y0 is initial transverse position
%   gamma0 is beam's lorentz factor
%   Bmap1D is the fieldmap data
%   alpha is the field rolloff fraction at 1 mm: alpha = 1 - B(x=1mm) / B(x=0)
%   outray is list of [z, x, y, xp, yp] at output position z

x0 = 565.5e-6;
y0 = 0;
xp0 = 0;
yp0 = 0;
gamma = 4000. / 0.511;
Bmap = readfieldmap( 'xleap2-wiggler-data/xLEAP-II-onaxis/xLEAP-II-1.dat' );
alpha = 0.003;
%  input_offsets = 644e-6 + 144e-6 * linspace(-1,0,2); % for 3.49 GeV
input_offsets = 0e-6 + 50e-6 * linspace(-2,2,5); % for 4 GeV

%  alpha = 0.00;
%  %  input_offsets = 0.e-6;% + 50e-6 * linspace(-3,3,7); % for 4 GeV & ideal map
%  ideal_wiggler_map;

%  x0 = 565.5e-6;
%  y0 = 0;
%  xp0 = 0;
%  yp0 = 0;
%  gamma = 4000. / 0.511;
%  Bmap = read2Dfieldmap( 'xleap2-wiggler-data/xLEAP-II-1/' );
%  alpha = 0.00;
%  %  input_offsets = 644e-6 + 144e-6 * linspace(-1,0,2); % for 3.49 GeV
%  input_offsets = 565.5e-6 + 50e-6 * linspace(-2,2,5); % for 4 GeV

%  x0 = 0.5e-3;
%  y0 = 0;
%  xp0 = 0;
%  yp0 = 0;
%  gamma = 4000. / 0.511;
%  Bmap = readfieldmap( 'xleap2-wiggler-data/xLEAP-II-onaxis/xLEAP-II-2.dat' );
%  alpha = 0.003;
%  input_offsets = -500e-6 + 1e-6 * [-50,-10,0,10,50];

output_offsets = [];
output_angles = [];
trajectories = {};

for i = 1:numel(input_offsets)
    xp = input_offsets(i);
    fprintf(['step ' num2str(i) ' of ' num2str(numel(input_offsets)) ' - input offset: ' num2str(round(xp*1e6)) ' urad\n']);
    results = trajectory( x0, y0, xp, yp0, gamma, Bmap, alpha );
    trajectories{i} = results;
    outray = [results(end,3), results(end,1), results(end,2), results(end,4)/c, results(end,5)/c]; 
    output_offsets = [output_offsets, outray(2)];
    output_angles = [output_angles, outray(4)];
    
    figure;set(gcf,'color','w');
    c1 = [0, 0.4470, 0.7410];
    c2 = [0.8500, 0.3250, 0.0980];
    plot(results(:,3), 1e3*results(:,1), 'color', c1);
    hold on;
    plot(results(:,3), 1e3*results(:,2), 'color', c2);
    xlabel('z position (m)');
    ylabel('Transverse position (mm)');
    legend('x','y');
    title([num2str(round(xp*1e6)) ' urad input offset']);
end

% colors
c1 = [0, 0.4470, 0.7410];
c2 = [0.8500, 0.3250, 0.0980];

%  % offset
%  figure;set(gcf,'color','w');
%  plot(input_offsets * 1e6, 1e6*output_offsets, 'color', c1);
%  xlabel('Input angle (urad)');
%  ylabel('Output offset (um)');

% displacement
figure;set(gcf,'color','w');
plot(input_offsets * 1e6, 1e6*(output_offsets-x0), 'color', c1);
xlabel('Input angle (urad)');
ylabel('Output displacement (um)');

% kick angle
figure;set(gcf,'color','w');
plot(input_offsets * 1e6, 1e6*(output_angles-input_offsets), 'color', c1);
xlabel('Input angle (urad)');
ylabel('Output kick displacement (urad)');

%  end