%Initialize
clear;

%Measured data
measDK_K_vs_y_SN09 = [ -300, -200,  -100,      0,   100,   200 ;...
                          13.874, 5.144, 0.635, 0.057, 2.512, 7.848 ];
y_meas = [ -300, -200,  -100,      0,   100,   200];  %(microns)
y_meas = y_meas * 1.e-6;  %(m)
%y_offset = -25.e-6;
y_offset = 0;
y_corr = y_meas - y_offset;
dk_k_meas = [13.874, 5.144, 0.635, 0.057, 2.512, 7.848 ];  %(x10^-4)
dk_k_meas = dk_k_meas * 1.e-4;

%Nominal dependence
y_nom = -350.e-6 : 10.e-6 : 350.e-6;
dk_k_nom = cosh((2 * pi / 0.03) * y_nom) - 1;

%Calculate fields at different y-positions from the expansion parameters----------

%Constants
q = -1.60217733e-19;    %Electron charge (C)
m = 9.1093897e-31;      %Electron mass (kg)
c = 2.99792458e8;       %Speed of light (m/s)
gamma = 1;  %Drops out of K calculation

%Put in known expansion coefficients from SN09
bn(1) = 12475.7e-4;
bn(2) = 0;
bn(3) = -410.0e-4;
bn(4) = 0;
bn(5) = -106.6e-4;
bn(6) = 0;
bn(7) = 25.5e-4;
bn(8) = 0;
bn(9) = -1.6e-4;
bn(10) = 0;

%Set the range of z-values and y-values
l_u = .030;  %undulator period (m)
z = - l_u / 2 : .000001 : l_u / 2;
y = -340.e-6 : 10.e-6 : 340.e-6;

%Calculate By as a function of y and z
%by = Sum bn sin(nkz) cosh(nky)
%bz = Sum bn cos(nkz) sinh(nky)
for yy = 1 : length(y)
    %Add pitch
    %    y_pitch = y(yy) + 3000.e-6 * (z - z(1));
    %Add pitch
    by(yy, :) = zeros(1, length(z));
    for nn = 1 : length(bn)
        by(yy, :) = by(yy, :) + bn(nn) * sin(nn * (2 * pi / l_u) * z) * cosh(nn * (2 * pi / l_u) * y(yy));
        %by(yy, :) = by(yy, :) + bn(nn) * sin(nn * (2 * pi / l_u) * z) .* cosh(nn * (2 * pi / l_u) * y_pitch);
    end
end

%Calculate K as a function of y-position
for yy = 1 : length(y)
    x_prime = (- q / (gamma * m * c)) * cumtrapz(z, by(yy, :));
    x_prime = x_prime - mean(x_prime);
    slip = trapz(z, (1 / (2 * gamma^2)) + (1 / 2) * x_prime.^2);
    dsdz = slip / (z(end) - z(1));
    k(yy) = sqrt(2 * (2 * gamma^2 * dsdz - 1));
end

% %Alternate calculation of K as a function of y-position
% sum_n_sq = 0;
% for nn = 1 : length(bn)
%     sum_n_sq = sum_n_sq + (1 / nn)^2 * bn(nn)^2 * cosh(nn * (2 * pi / l_u) * y).^2;
% end
% sum_n = sqrt(sum_n_sq);
% k = (- q / (m * c)) * (2 * pi / l_u)^-1 * sum_n;

%Calculate dK/K as a function of y
dk_k_bn = (k - min(k)) / min(k);

%Plot the data
figure;
hold all;
plot(y_corr, dk_k_meas, 'b*');
plot(y_nom, dk_k_nom, 'r-');
plot(y, dk_k_bn, 'k-');
title('dK/K As A Function Of y-Position');
xlabel('y (m)');
ylabel('dK/K');
%legend('dK/K Measured, Y Corr', 'Cosh(ky) - 1', 'dK/K From Bn');
legend('dK/K Measured', 'Cosh(ky) - 1', 'dK/K From Bn');
grid on;
zoom on;
hold off;
