%**************************************************************************
%This function calculates the fields from Andy's Radia simulation
%when the EPU is in circular polarization right hand mode.
%
%Input:
%num_per, number of periods in the center of the undulator, even
%xc, x-position of the magnetic center (m)
%yc, y-position of the magnetic center (m)
%zc, z-position of the magnetic center (m)
%x, x-position to calculate the field at (m)
%y, y-position to calculate the field at (m)
%z, z-position to calculate the field at (m)
%
%Output:
%bx, x-component of the field strength (T)
%by, y-component of the field strength (T)
%bz, z-component of the field strength (T)
%
%Zachary Wolf
%4/2/13

function [bx, by, bz] = cp_rh_get_field(num_per, xc, yc, zc, x, y, z)

%Check that num_per is even
if mod(num_per, 2) > 0
    error('The number of periods must be even');
end

%Make sure x, y and z have the same length
if length(x) > 1 && length(x) ~= length(z)
    error('The x and z arrays have different lengths.');
end
if length(y) > 1 && length(y) ~= length(z)
    error('The y and z arrays have different lengths.');
end

%Read in Andy's array
z_vals = xlsread('C:\SSRL\EPU Measurements\Radia Model\bl05_EPU140_BvsY&Z.xlsx', 'BvsY&Z_longcoil_check', 'b158:bt158');
y_vals = xlsread('C:\SSRL\EPU Measurements\Radia Model\bl05_EPU140_BvsY&Z.xlsx', 'BvsY&Z_longcoil_check', 'a159:a167');
bx_vals = xlsread('C:\SSRL\EPU Measurements\Radia Model\bl05_EPU140_BvsY&Z.xlsx', 'BvsY&Z_longcoil_check', 'b159:bt167');
by_vals = xlsread('C:\SSRL\EPU Measurements\Radia Model\bl05_EPU140_BvsY&Z.xlsx', 'BvsY&Z_longcoil_check', 'b147:bt155');

%Convert from mm to m
z_vals = z_vals / 1000;
y_vals = y_vals / 1000;

%Calculate the period
lambda_u = z_vals(end) - z_vals(1);

%Calculate the undulator length
und_len = num_per * lambda_u;

%Shift z to values relative to a period
z_per_vals = z_vals - z_vals(1);

%Loop over locations to calculate the field at
for ii = 1 : length(z)

%Set the location to calculate the field at
zi = z(ii);
if length(x) > 1, xi = x(ii); else xi = x; end;
if length(y) > 1, yi = y(ii); else yi = y; end;

%See where in the period zi is located
z_per = mod((zi - zc + und_len / 2), lambda_u);

%Find the points in the period array nearest the point of interest
index_array = find(z_per >= z_per_vals);
iz1 = index_array(end);
index_array = find(z_per < z_per_vals);
iz2 = index_array(1);

%Find the y points in the array nearest the point of interest
index_array = find(yi - yc >= y_vals);
iy1 = index_array(1);
index_array = find(yi - yc < y_vals);
iy2 = index_array(end);

%Interpolate Bx
%f(y, z) = f(y0, z0) + dfdy (y - y0) + dfdz (z - z0) + ...
dbxdy = (bx_vals(iy1, iz1) - bx_vals(iy2, iz1)) / (y_vals(iy2) - y_vals(iy1));
dbxdz = (bx_vals(iy1, iz2) - bx_vals(iy1, iz1)) / (z_vals(iz2) - z_vals(iz1));
bx(ii) = bx_vals(iy1, iz1) + dbxdy * (yi - yc - y_vals(iy1)) + dbxdz * (z_per - z_per_vals(iz1));

%Interpolate By
%f(y, z) = f(y0, z0) + dfdy (y - y0) + dfdz (z - z0) + ...
dbydy = (by_vals(iy1, iz1) - by_vals(iy2, iz1)) / (y_vals(iy2) - y_vals(iy1));
dbydz = (by_vals(iy1, iz2) - by_vals(iy1, iz1)) / (z_vals(iz2) - z_vals(iz1));
by(ii) = by_vals(iy1, iz1) + dbydy * (yi - yc - y_vals(iy1)) + dbydz * (z_per - z_per_vals(iz1));

%Bz
bz(ii) = 0;

%See which half period zi is in
half_per_num = floor((zi - zc + und_len / 2) / (lambda_u / 2));

%Make the first half period at each end quarter strength
if half_per_num == 1 || half_per_num == 2 * num_per
    bx(ii) = bx(ii) * 1/4;
    by(ii) = by(ii) * 1/4;
end

%Make the second half period three-quarter strength
if half_per_num == 2 || half_per_num == 2 * num_per - 1
    bx(ii) = bx(ii) * 3/4;
    by(ii) = by(ii) * 3/4;
end

%Truncate fields outside the undulator
if half_per_num < 1 || half_per_num > 2 * num_per
    bx(ii) = 0;
    by(ii) = 0;
end

%End loop over field calculation locations
end

%Done
