%**************************************************************************
%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/9/13

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

%Local parameters
%B_THRESH = .02;  %200 Gauss default
%B_THRESH = .025;  %250 Gauss
B_THRESH = .0100;  %100 Gauss
NUM_MAX_CHECK_SIDE = 20;  %Default
%NUM_MAX_CHECK_SIDE = 50;
NUM_FIT_PTS_ONE_SIDE = 10;  %Default
%NUM_FIT_PTS_ONE_SIDE = 50;

%Print a warning if parameter values are changed
%warndlg('field_find_peaks: parameter values are changed');
  
%Check
if (length(z) ~= length(b))
    msgbox('find_peaks: z and b have different lengths', 'Error');
    return;
end
if ((length(z) < 10) || (length(b) < 10))
    msgbox('find_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 = NUM_MAX_CHECK_SIDE + 1 : 1 : length(b) - NUM_MAX_CHECK_SIDE
    if ((b_abs(ii) > B_THRESH) ...
        && (b_abs(ii) >= max(b_abs(ii - NUM_MAX_CHECK_SIDE : ii - 1))) && (b_abs(ii) > max(b_abs(ii + 1 : ii + NUM_MAX_CHECK_SIDE)))) 
            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_peak(ii) = 0.;
    
    %Fill arrays for fit
    index = 0;
    for jj = -NUM_FIT_PTS_ONE_SIDE : NUM_FIT_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_FIT_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_FIT_PTS_ONE_SIDE + 1)))
        z_peak(ii) = z_meas_peak(ii);
        b_peak(ii) = b_meas_peak(ii);
        %msgbox('idanal_find_field_peaks: problem with a peak', 'Error');
        %%Debug
        %figure;
        %plot(z_for_fit, b_for_fit, '-*');
    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

%Remove peaks due to noise
for ii = 1 : length(b_peak) - 1
    if (b_peak(ii) * b_peak(ii + 1) > 0) && (abs(b_peak(ii)) > abs(b_peak(ii + 1)))
        b_peak(ii + 1) = 0;
        z_peak(ii + 1) = 0;
    end
    if (b_peak(ii) * b_peak(ii + 1) > 0) && (abs(b_peak(ii)) <= abs(b_peak(ii + 1)))
        b_peak(ii) = 0;
        z_peak(ii) = 0;
    end
end
b_peak_temp = b_peak;
z_peak_temp = z_peak;
b_peak = b_peak_temp((b_peak_temp ~= 0) & (z_peak_temp ~= 0));
z_peak = z_peak_temp((b_peak_temp ~= 0) & (z_peak_temp ~= 0));

%Repeat remove peaks due to noise, in case of multiple peaks of the same sign
for ii = 1 : length(b_peak) - 1
    if (b_peak(ii) * b_peak(ii + 1) > 0) && (abs(b_peak(ii)) > abs(b_peak(ii + 1)))
        b_peak(ii + 1) = 0;
        z_peak(ii + 1) = 0;
    end
    if (b_peak(ii) * b_peak(ii + 1) > 0) && (abs(b_peak(ii)) <= abs(b_peak(ii + 1)))
        b_peak(ii) = 0;
        z_peak(ii) = 0;
    end
end
b_peak_temp = b_peak;
z_peak_temp = z_peak;
b_peak = b_peak_temp((b_peak_temp ~= 0) & (z_peak_temp ~= 0));
z_peak = z_peak_temp((b_peak_temp ~= 0) & (z_peak_temp ~= 0));

%Check that the peaks alternate in sign
check_ok = 1;
for ii = 1 : length(b_peak) - 1
    if b_peak(ii) * b_peak(ii + 1) > 0
        check_ok = 0;
    end
end
if check_ok == 0
    msgbox('find_peaks: peaks dont alternate in sign', 'Error');
end
   
%Done
