%**************************************************************************
%This function finds the peak field values.    It returns the
%fitted field values along with their fitted z positions.
%
%Input:
%z, position the field was measured at (m)
%b, measured field strength (T)
%
%Output:
%z_peak, fitted z position of each peak value (m)
%b_peak, fitted value of the field at the peak (T)
%
%Zack Wolf  7/16/03

function [z_peak, b_peak] = field_find_field_peaks(z, b)

%Local parameters
%B_THRESH = .0150;  %150 Gauss
B_THRESH = .0200;  %200 Gauss
NUM_PTS_ONE_SIDE = 4;

%Initialize
debug_msg = 1;  %Turn on debug messages
  
%Check
if length(z) ~= length(b)
    msgbox('field_find_field_peaks: z and b have different lengths', 'Error');
    return;
end
if length(z) < 10 || length(b) < 10
    msgbox('field_find_field_peaks: problem with array sizes', 'Error');
    return;
end

%Initial if no peaks
z_peak = 0;
b_peak = 0;
index_meas_peak(1) = 0;

%Loop through the measured field values looking for a peak
num_peaks = 0;
b_abs = abs(b);
for ii=4:1:length(b)-3
    if (b_abs(ii) > B_THRESH) ...
        && (b_abs(ii) >= b_abs(ii-3)) && (b_abs(ii) >= b_abs(ii-2)) && (b_abs(ii) >= b_abs(ii-1)) ...
        && (b_abs(ii) > b_abs(ii+1)) && (b_abs(ii) > b_abs(ii+2)) && (b_abs(ii) > b_abs(ii+3))
            num_peaks = num_peaks + 1;
            index_meas_peak(num_peaks) = ii;
            z_meas_peak(num_peaks) = z(ii);
            b_meas_peak(num_peaks) = b(ii);
    end     
end

%Check
if index_meas_peak(1) == 0
    return;
end

%Turn off a warning
warning('off', 'MATLAB:polyfit:RepeatedPointsOrRescale');
    
%Loop through the peaks
for ii=1:1:length(index_meas_peak)
    %Initialize
    z_peak(ii) = -1000.;
    b_beak(ii) = 0.;
    
    %Fill arrays for fit
    index = 0;
    for jj = -NUM_PTS_ONE_SIDE:1:NUM_PTS_ONE_SIDE
        index = index + 1;
        z_for_fit(index) = z(index_meas_peak(ii) + jj);
        b_for_fit(index) = b(index_meas_peak(ii) + jj);
    end
    
    %Fit the peak with a second order polynomial
    %p2 = polyfit(z_for_fit, b_for_fit, 2);
    %Find z_peak and b_peak
    %p2_deriv = polyder(p2);
    %z_peak(ii) = roots(p2_deriv);
    %b_peak(ii) = polyval(p2, z_peak(ii));
    
    %Fit the peak with a fourth order polynomial
    p4 = polyfit(z_for_fit, b_for_fit, 4);
    
    %Find z_peak and b_peak
    p4_deriv = polyder(p4);
    r4 = roots(p4_deriv);
    for jj = 1 : length(r4)
        if (isreal(r4(jj))) && (r4(jj) >= z_for_fit(1)) && (r4(jj) <= z_for_fit(2 * NUM_PTS_ONE_SIDE + 1))
            z_peak(ii) = r4(jj);
            b_peak(ii) = polyval(p4, z_peak(ii));
            break;
        end
    end
    
    %Check
    if ((z_peak(ii) <= z_for_fit(1)) || (z_peak(ii) >= z_for_fit(2 * NUM_PTS_ONE_SIDE + 1))) && (debug_msg == 1)
        msgbox('field_find_field_peaks: problem with a peak', 'Error');
        %Debug
        figure;
        plot(z_for_fit, b_for_fit, '-*');
        debug_msg = 0;
    end
    
    %Debug
    %peak_to_plot = 5;
    %if ii == peak_to_plot
    %    z_curve = z_for_fit(1) : (z_for_fit(5) - z_for_fit(1)) / 25 : z_for_fit(5);
    %    b_curve = polyval(p2, z_curve);
    %    plot(z_curve, b_curve, '-', z_for_fit, b_for_fit, 'kx');
    %end
end
   
%Done
