%  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

%______________________
% wiggler 1

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

%  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/' );  % 2D field map
%  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

%______________________
% wiggler 2

%  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];

%______________________
% wiggler 3

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

%______________________
% wiggler 4

%  x0 = -0.52e-3;
%  y0 = 0;
%  xp0 = 0;
%  yp0 = 0;
%  gamma = 4000. / 0.511;
%  Bmap = readfieldmap( 'xleap2-wiggler-data/xLEAP-II-onaxis/xLEAP-II-4.dat' );
%  alpha = 0.003;
%  input_offsets = -520e-6 + 50e-6 * linspace(-2,2,5);

%______________________
% wigglers 1 and 2 stacked end to end

%  x0 = 0e-6;
%  y0 = 0;
%  xp0 = 0;
%  yp0 = 0;
%  Bmap1 = readfieldmap( 'xleap2-wiggler-data/xLEAP-II-onaxis/xLEAP-II-1.dat' );
%  Bmap2 = readfieldmap( 'xleap2-wiggler-data/xLEAP-II-onaxis/xLEAP-II-2.dat' );
%  Bmap2(:,1) = Bmap2(:,1) - Bmap2(1,1) + Bmap1(end,1);% + (Bmap1(2,1) - Bmap1(1,1));
%  Bmap = {{Bmap1,[Bmap1(1,1),Bmap1(end,1)],-565.5e-6*[1,1]}, {Bmap2,[Bmap2(1,1),Bmap2(end,1)],500e-6*[1,1]}};
%  alpha = 0.003;
%  gamma = 4000. / 0.511;
%  input_offsets = 0. + 100e-6 * linspace(-2,2,7); % for 4 GeV

%______________________
% wigglers 1 and 3 stacked end to end

%  x0 = 0e-6;
%  y0 = 0;
%  xp0 = 0;
%  yp0 = 0;
%  Bmap1 = readfieldmap( 'xleap2-wiggler-data/xLEAP-II-onaxis/xLEAP-II-1.dat' );
%  Bmap2 = readfieldmap( 'xleap2-wiggler-data/xLEAP-II-onaxis/xLEAP-II-3.dat', 34 );
%  Bmap2(:,1) = Bmap2(:,1) - Bmap2(1,1) + Bmap1(end,1);% + (Bmap1(2,1) - Bmap1(1,1));
%  Bmap = {{Bmap1,[Bmap1(1,1),Bmap1(end,1)],-565.5e-6*[1,1]}, {Bmap2,[Bmap2(1,1),Bmap2(end,1)],-515.5e-6*[1,1]}};
%  alpha = 0.003;
%  gamma = 4000. / 0.511;
%  input_offsets = 0. + 100e-6 * linspace(-2,2,7); % for 4 GeV

%______________________
% OLD WIGGLER

x0 = 0;
y0 = 0;
xp0 = 0;
yp0 = 0;
Bmap = dlmread('~/xleap/xLEAP_wiggler_measurement_files/ByvsZ.162',',',6);
Bmap(:,1) = Bmap(:,1) / 1000.;
Bmap(:,3) = Bmap(:,2);
Bmap(:,2) = 0;
alpha = 0.006;
%  gamma = 4000. / 0.511;
gamma = 3460. / 0.511;
input_offsets = 0. + 100e-6 * linspace(-2,2,7); % for 4 GeV

%______________________

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

for i = 1:numel(input_offsets)
    x = input_offsets(i);
    fprintf(['step ' num2str(i) ' of ' num2str(numel(input_offsets)) ' - input offset: ' num2str(round(x*1e6)) ' um\n']);
%      results = trajectory( x, y0, xp0, yp0, gamma, Bmap, alpha );
    results = trajectory( x, y0, xp0, 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);
    hx = xlabel('z position (m)');
    ylabel('Transverse position (mm)');
    legend('x','y','location','best');
    title([num2str(round(x*1e6)) ' um input offset']);
    enhance_plot('arial',20,2,8)
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 offset (um)');
ylabel('Output offset (um)');
enhance_plot('arial',20,2,8)

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

% kick angle
figure;set(gcf,'color','w');
plot(input_offsets * 1e6, 1e6*(output_angles), 'color', c1);
xlabel('Input offset (um)');
ylabel('Output angle (urad)');
enhance_plot('arial',20,2,8)

%  end