%**************************************************************************
%This program takes calibrated field measurements from a 2-probe array.
%It calculates the position of probe 1 (on the probe center line) relative to
%the magnetic center.  It also calculates parameters describing the
%undulaltor field.
%
%Input:
%param, structure containing program parameters
%dat_file, file to write data to
%plt_file, file to send plots to
%field, structure containing peak field information
%
%Output:
%mag_ctr, structure containing magnetic center information
%
%Zachary Wolf
%7/3/13

function [mag_ctr] = hall2_anal_mag_ctr(param, dat_file, plt_file, field)

%Useful constants
lambda_u = field.lambda_u;
ku = 2 * pi / lambda_u;
y_delta_by = param.probe2_param.by_sensor.y - param.probe1_param.by_sensor.y;
y_delta_bx = param.probe2_param.bx_sensor.y - param.probe1_param.bx_sensor.y;
x_delta_by = param.probe2_param.by_sensor.x - param.probe1_param.by_sensor.x;
x_delta_bx = param.probe2_param.bx_sensor.x - param.probe1_param.bx_sensor.x;

%Local parameter
num_iter = 5;

%Initialize
x1_minus_xc = 0;
y1_minus_yc = 0;
kx = 0;
ky = 0;
kx_sq = 0;
ky_sq = 0;
delta_x_calc = 0;
delta_y_calc = 0;

%Z positions for plots
z_peak_ctr = field.z_peak_ctr;

%Linear polarization vertical field-----

%Field values needed for linear polarization vertical field calculations
by1_peak_ne = field.by1_peak_ne;
dbx_at_by1_peak_ne = field.dbx_at_by1_peak_ne;
dby_at_by1_peak_ne = field.dby_at_by1_peak_ne;
bz1_at_by1_zero_ne = field.bz1_at_by1_zero_ne;
bz2_at_by1_zero_ne = field.bz2_at_by1_zero_ne;

%Find the magnetic center relative position, linear polarization vertical field, probes offset vertically
if strcmp(param.und_pol_mode, 'LIN_POL_VERT_FIELD')
    if param.probe2_offset_dir == 'Y' || param.probe2_offset_dir == 'y'
        %Calculate magnetic center parameters
        y1_minus_yc = -(1 / ku) * (bz1_at_by1_zero_ne ./ by1_peak_ne);
        delta_y_calc = -(1 / ku) * (bz2_at_by1_zero_ne - bz1_at_by1_zero_ne) ./ by1_peak_ne;
        for ii = 1 : num_iter  %Iterative solution
            ky_sq = (dby_at_by1_peak_ne ./ by1_peak_ne) ./ (y1_minus_yc * y_delta_by + 0.5 * y_delta_by^2) - mean(kx_sq) .* (mean(x1_minus_xc) * x_delta_by + (1/2) * x_delta_by^2) ./ (y1_minus_yc * y_delta_by + (1/2) * y_delta_by^2);
            index_array = find(abs(y1_minus_yc + 0.5 * y_delta_by) < 50.e-6);
            ky_sq(index_array) = param.ky_nom^2;
            ky = sqrt(ky_sq);
            kx_sq = ku^2 - ky_sq;
            kx = sqrt(kx_sq);
            x1_minus_xc = (dbx_at_by1_peak_ne ./ by1_peak_ne) ./ (mean(kx_sq) * y_delta_bx) - (y1_minus_yc * x_delta_bx / y_delta_bx) - x_delta_bx;
        end
        %Plot results
        halln_mag_ctr_plot_y1minusyc_vs_z(plt_file, 'Lin Pol Vert Field, Vert Probe Offset', z_peak_ctr, y1_minus_yc);
        halln_mag_ctr_plot_deltay_vs_z(plt_file, 'Lin Pol Vert Field, Vert Probe Offset', z_peak_ctr, delta_y_calc);
        halln_mag_ctr_plot_ky_vs_z(plt_file, 'Lin Pol Vert Field, Vert Probe Offset', z_peak_ctr, ky);
        halln_mag_ctr_plot_kx_vs_z(plt_file, 'Lin Pol Vert Field, Vert Probe Offset, From Constraint', z_peak_ctr, kx);
        halln_mag_ctr_plot_x1minusxc_vs_z(plt_file, 'Lin Pol Vert Field, Vert Probe Offset', z_peak_ctr, x1_minus_xc);
    end
end

%Find the magnetic center relative position, linear polarization vertical field, probes offset horizontally
if strcmp(param.und_pol_mode, 'LIN_POL_VERT_FIELD')
    if param.probe2_offset_dir == 'X' || param.probe2_offset_dir == 'x'
        %Calculate magnetic center parameters
        y1_minus_yc = -(1 / ku) * (bz1_at_by1_zero_ne ./ by1_peak_ne);
        delta_x_calc = (1 / ku) * (bz2_at_by1_zero_ne - bz1_at_by1_zero_ne) ./ by1_peak_ne;
        for ii = 1 : num_iter  %Iterative solution
            kx_sq = (dbx_at_by1_peak_ne ./ by1_peak_ne) ./ ((y1_minus_yc * y_delta_bx) - (mean(x1_minus_xc) * x_delta_bx) - (x_delta_bx * y_delta_bx));
            kx = sqrt(kx_sq);
            ky_sq = ku^2 - kx_sq;
            ky = sqrt(ky_sq);
            x1_minus_xc = (dby_at_by1_peak_ne ./ by1_peak_ne) ./ (mean(kx_sq) * y_delta_by) - 0.5 * y_delta_by - (mean(ky_sq) ./ mean(kx_sq)) .* (-y1_minus_yc + 0.5 * x_delta_by) * (x_delta_by / y_delta_by);
        end
        %Plot results
        halln_mag_ctr_plot_y1minusyc_vs_z(plt_file, 'Lin Pol Vert Field, Horiz Probe Offset', z_peak_ctr, y1_minus_yc);
        halln_mag_ctr_plot_deltax_vs_z(plt_file, 'Lin Pol Vert Field, Horiz Probe Offset', z_peak_ctr, delta_x_calc);
        halln_mag_ctr_plot_kx_vs_z(plt_file, 'Lin Pol Vert Field, Horiz Probe Offset', z_peak_ctr, kx);
        halln_mag_ctr_plot_ky_vs_z(plt_file, 'Lin Pol Vert Field, Horiz Probe Offset, From Constraint', z_peak_ctr, ky);
        halln_mag_ctr_plot_x1minusxc_vs_z(plt_file, 'Lin Pol Vert Field, Horiz Probe Offset', z_peak_ctr, x1_minus_xc);
    end
end

%Linear polarization horizontal field-----

%Field values needed for linear polarization horizontal field calculations
bx1_peak_ne = field.bx1_peak_ne;
dby_at_bx1_peak_ne = field.dby_at_bx1_peak_ne;
dbx_at_bx1_peak_ne = field.dbx_at_bx1_peak_ne;
bz1_at_bx1_zero_ne = field.bz1_at_bx1_zero_ne;
bz2_at_bx1_zero_ne = field.bz2_at_bx1_zero_ne;

%Find the magnetic center relative position, linear polarization horizontal field, probes offset horizontally
if strcmp(param.und_pol_mode, 'LIN_POL_HORIZ_FIELD')
    if param.probe2_offset_dir == 'X' || param.probe2_offset_dir == 'x'
        %Calculate magnetic center parameters
        x1_minus_xc = -(1 / ku) * (bz1_at_bx1_zero_ne ./ bx1_peak_ne);
        delta_y_calc = -(1 / ku) * (bz2_at_bx1_zero_ne - bz1_at_bx1_zero_ne) ./ bx1_peak_ne;
        for ii = 1 : num_iter  %Iterative solution
            kx_sq = (dbx_at_bx1_peak_ne ./ bx1_peak_ne) ./ (x1_minus_xc * y_delta_bx + 0.5 * y_delta_bx^2) - mean(ky_sq) .* ((-mean(y1_minus_yc) * x_delta_bx) + (1/2) * x_delta_bx^2) ./ ((x1_minus_xc * y_delta_bx) + (1/2) * y_delta_bx^2);
            index_array = find(abs(x1_minus_xc + 0.5 * y_delta_bx) < 50.e-6);
            kx_sq(index_array) = param.kx_nom^2;
            kx = sqrt(kx_sq);
            ky_sq = ku^2 - kx_sq;
            ky = sqrt(ky_sq);
            y1_minus_yc = (dby_at_bx1_peak_ne ./ bx1_peak_ne) ./ (mean(ky_sq) * y_delta_by) + x1_minus_xc * x_delta_by / y_delta_by + x_delta_by;
        end
        %Plot results
        halln_mag_ctr_plot_x1minusxc_vs_z(plt_file, 'Lin Pol Horiz Field, Horiz Probe Offset', z_peak_ctr, x1_minus_xc);
        halln_mag_ctr_plot_deltay_vs_z(plt_file, 'Lin Pol Horiz Field, Horiz Probe Offset', z_peak_ctr, delta_y_calc);
        halln_mag_ctr_plot_kx_vs_z(plt_file, 'Lin Pol Horiz Field, Horiz Probe Offset', z_peak_ctr, kx);
        halln_mag_ctr_plot_ky_vs_z(plt_file, 'Lin Pol Vert Field, Horiz Probe Offset, From Constraint', z_peak_ctr, ky);
        halln_mag_ctr_plot_y1minusyc_vs_z(plt_file, 'Lin Pol Horiz Field, Horiz Probe Offset', z_peak_ctr, y1_minus_yc);
    end
end

%Find the magnetic center relative position, linear polarization horizontal field, probes offset vertically
if strcmp(param.und_pol_mode, 'LIN_POL_HORIZ_FIELD')
    if param.probe2_offset_dir == 'Y' || param.probe2_offset_dir == 'y'
        %Calculate magnetic center parameters
        x1_minus_xc = - (1 / ku) * (bz1_at_bx1_zero_ne ./ bx1_peak_ne);
        delta_x_calc = - (1 / ku) * ((bz2_at_bx1_zero_ne - bz1_at_bx1_zero_ne) ./ bx1_peak_ne);
        for ii = 1 : num_iter  %Iterative solution
            ky_sq = (dby_at_bx1_peak_ne ./ bx1_peak_ne) ./ ((x1_minus_xc * y_delta_by) + (mean(y1_minus_yc) * x_delta_by) + (x_delta_by * y_delta_by));
            ky = sqrt(ky_sq);
            kx_sq = ku^2 - ky_sq;
            kx = sqrt(kx_sq);
            y1_minus_yc = (dbx_at_bx1_peak_ne ./ bx1_peak_ne) ./ (mean(ky_sq) * y_delta_bx) - 0.5 * y_delta_bx - (mean(kx_sq) ./ mean(ky_sq)) .* (x1_minus_xc + (1/2) * x_delta_bx) * (x_delta_bx / y_delta_bx);
        end
        %Plot results
        halln_mag_ctr_plot_x1minusxc_vs_z(plt_file, 'Lin Pol Horiz Field, Vert Probe Offset', z_peak_ctr, x1_minus_xc);
        halln_mag_ctr_plot_deltax_vs_z(plt_file, 'Lin Pol Vert Field, Horiz Probe Offset', z_peak_ctr, delta_x_calc);
        halln_mag_ctr_plot_ky_vs_z(plt_file, 'Lin Pol Horiz Field, Vert Probe Offset', z_peak_ctr, ky);
        halln_mag_ctr_plot_kx_vs_z(plt_file, 'Lin Pol Vert Field, Vert Probe Offset, From Constraint', z_peak_ctr, kx);
        halln_mag_ctr_plot_y1minusyc_vs_z(plt_file, 'Lin Pol Horiz Field, Vert Probe Offset', z_peak_ctr, y1_minus_yc);
    end
end

%Circular polarization right hand-----

%Field values needed for the circular polarization calculations
bx1_peak_ne = field.bx1_peak_ne;
by1_peak_ne = field.by1_peak_ne;
bx2_peak_ne = field.bx2_peak_ne;
by2_peak_ne = field.by2_peak_ne;
bz1_at_bx1_peak_ne = field.bz1_at_bx1_peak_ne;
bz1_at_by1_peak_ne = field.bz1_at_by1_peak_ne;
bz2_at_bx2_peak_ne = field.bz2_at_bx2_peak_ne;
bz2_at_by2_peak_ne = field.bz2_at_by2_peak_ne;

%Find the magnetic center relative position, circular right handed polarization, probes offset vertically
if strcmp(param.und_pol_mode, 'CIRC_POL_RIGHT_HAND')
    if param.probe2_offset_dir == 'Y' || param.probe2_offset_dir == 'y'
        %Calculate magnetic center parameters
        x1_minus_xc = -(1 / ku) * (bz1_at_by1_peak_ne ./ by1_peak_ne);
        y1_minus_yc = (1 / ku) * (bz1_at_bx1_peak_ne ./ bx1_peak_ne);
        x2_minus_xc = -(1 / ku) * (bz2_at_by2_peak_ne ./ by2_peak_ne);
        y2_minus_yc = (1 / ku) * (bz2_at_bx2_peak_ne ./ bx2_peak_ne);
        delta_x_calc = x2_minus_xc - x1_minus_xc;
        delta_y_calc = y2_minus_yc - y1_minus_yc;
        %Plot results
        halln_mag_ctr_plot_x1minusxc_vs_z(plt_file, 'Circ Pol Right Hand, Vert Probe Offset', z_peak_ctr, x1_minus_xc);
        halln_mag_ctr_plot_y1minusyc_vs_z(plt_file, 'Circ Pol Right Hand, Vert Probe Offset', z_peak_ctr, y1_minus_yc);
        halln_mag_ctr_plot_deltax_vs_z(plt_file, 'Circ Pol Right Hand, Vert Probe Offset', z_peak_ctr, delta_x_calc);
        halln_mag_ctr_plot_deltay_vs_z(plt_file, 'Circ Pol Right Hand, Vert Probe Offset', z_peak_ctr, delta_y_calc);
    end
end

%Find the magnetic center relative position, circular right handed polarization, probes offset horizontally
if strcmp(param.und_pol_mode, 'CIRC_POL_RIGHT_HAND')
    if param.probe2_offset_dir == 'X' || param.probe2_offset_dir == 'x'
        %Calculate magnetic center parameters
        x1_minus_xc = -(1 / ku) * (bz1_at_by1_peak_ne ./ by1_peak_ne);
        y1_minus_yc = (1 / ku) * (bz1_at_bx1_peak_ne ./ bx1_peak_ne);
        x2_minus_xc = -(1 / ku) * (bz2_at_by2_peak_ne ./ by2_peak_ne);
        y2_minus_yc = (1 / ku) * (bz2_at_bx2_peak_ne ./ bx2_peak_ne);
        delta_x_calc = y1_minus_yc - y2_minus_yc;
        delta_y_calc = x2_minus_xc - x1_minus_xc;
        %Plot results
        halln_mag_ctr_plot_x1minusxc_vs_z(plt_file, 'Circ Pol Right Hand, Vert Probe Offset', z_peak_ctr, x1_minus_xc);
        halln_mag_ctr_plot_y1minusyc_vs_z(plt_file, 'Circ Pol Right Hand, Vert Probe Offset', z_peak_ctr, y1_minus_yc);
        halln_mag_ctr_plot_deltax_vs_z(plt_file, 'Circ Pol Right Hand, Vert Probe Offset', z_peak_ctr, delta_x_calc);
        halln_mag_ctr_plot_deltay_vs_z(plt_file, 'Circ Pol Right Hand, Vert Probe Offset', z_peak_ctr, delta_y_calc);
    end
end

%Circular polarization left hand-----

%Field values needed for the circular polarization calculations
bx1_peak_ne = field.bx1_peak_ne;
by1_peak_ne = field.by1_peak_ne;
bx2_peak_ne = field.bx2_peak_ne;
by2_peak_ne = field.by2_peak_ne;
bz1_at_bx1_peak_ne = field.bz1_at_bx1_peak_ne;
bz1_at_by1_peak_ne = field.bz1_at_by1_peak_ne;
bz2_at_bx2_peak_ne = field.bz2_at_bx2_peak_ne;
bz2_at_by2_peak_ne = field.bz2_at_by2_peak_ne;

%Find the magnetic center relative position, circular left handed polarization, probes offset vertically
if strcmp(param.und_pol_mode, 'CIRC_POL_LEFT_HAND')
    if param.probe2_offset_dir == 'Y' || param.probe2_offset_dir == 'y'
        %Calculate magnetic center parameters
        x1_minus_xc = (1 / ku) * (bz1_at_by1_peak_ne ./ by1_peak_ne);
        y1_minus_yc = -(1 / ku) * (bz1_at_bx1_peak_ne ./ bx1_peak_ne);
        x2_minus_xc = (1 / ku) * (bz2_at_by2_peak_ne ./ by2_peak_ne);
        y2_minus_yc = -(1 / ku) * (bz2_at_bx2_peak_ne ./ bx2_peak_ne);
        delta_x_calc = x2_minus_xc - x1_minus_xc;
        delta_y_calc = y2_minus_yc - y1_minus_yc;
        %Plot results
        halln_mag_ctr_plot_x1minusxc_vs_z(plt_file, 'Circ Pol Left Hand, Vert Probe Offset', z_peak_ctr, x1_minus_xc);
        halln_mag_ctr_plot_y1minusyc_vs_z(plt_file, 'Circ Pol Left Hand, Vert Probe Offset', z_peak_ctr, y1_minus_yc);
        halln_mag_ctr_plot_deltax_vs_z(plt_file, 'Circ Pol Left Hand, Vert Probe Offset', z_peak_ctr, delta_x_calc);
        halln_mag_ctr_plot_deltay_vs_z(plt_file, 'Circ Pol Left Hand, Vert Probe Offset', z_peak_ctr, delta_y_calc);
    end
end

%Find the magnetic center relative position, circular left handed polarization, probes offset horizontally
if strcmp(param.und_pol_mode, 'CIRC_POL_LEFT_HAND')
    if param.probe2_offset_dir == 'X' || param.probe2_offset_dir == 'x'
        %Calculate magnetic center parameters
        x1_minus_xc = (1 / ku) * (bz1_at_by1_peak_ne ./ by1_peak_ne);
        y1_minus_yc = -(1 / ku) * (bz1_at_bx1_peak_ne ./ bx1_peak_ne);
        x2_minus_xc = (1 / ku) * (bz2_at_by2_peak_ne ./ by2_peak_ne);
        y2_minus_yc = -(1 / ku) * (bz2_at_bx2_peak_ne ./ bx2_peak_ne);
        delta_x_calc = y1_minus_yc - y2_minus_yc;
        delta_y_calc = x2_minus_xc - x1_minus_xc;
        %Plot results
        halln_mag_ctr_plot_x1minusxc_vs_z(plt_file, 'Circ Pol Left Hand, Vert Probe Offset', z_peak_ctr, x1_minus_xc);
        halln_mag_ctr_plot_y1minusyc_vs_z(plt_file, 'Circ Pol Left Hand, Vert Probe Offset', z_peak_ctr, y1_minus_yc);
        halln_mag_ctr_plot_deltax_vs_z(plt_file, 'Circ Pol Left Hand, Vert Probe Offset', z_peak_ctr, delta_x_calc);
        halln_mag_ctr_plot_deltay_vs_z(plt_file, 'Circ Pol Left Hand, Vert Probe Offset', z_peak_ctr, delta_y_calc);
    end
end

%Write the results to the data file
hall2_mag_ctr_dat_mag_ctr(dat_file, mean(x1_minus_xc), mean(y1_minus_yc), mean(kx), mean(ky), mean(delta_x_calc), mean(delta_y_calc));

%Return the results in a structure
mag_ctr.ku = ku;
mag_ctr.z_peak_ctr = z_peak_ctr;
mag_ctr.x1_minus_xc = x1_minus_xc;
mag_ctr.y1_minus_yc = y1_minus_yc;
mag_ctr.kx = kx;
mag_ctr.ky = ky;
mag_ctr.delta_x_calc = delta_x_calc;
mag_ctr.delta_y_calc = delta_y_calc;

%Done
