%Contact point algorithm

%Usage:
%q = contactpoint3(d_app,z_app,tipf,maxind);

%d_app: vector of approaching deflection values in nm

%z_app: vector of approaching scanner height values in nm

%tipf: struct containing information about the tip.
    %type: 'Cone' or 'Bead'
    %geom: cone angle in degrees, or bead radius in nm.
    %k: spring constant of cantilever in N/m

    %Example:

        %tip.geom=35;
        %tip.type='Cone';
        %tip.k=0.06907;
    
%maxind: maximum range of indentation to use when fitting, in nm. Note that
%this is indentation, not deflection or z position.

%Internal parameters
%MODE: eiher 1 or 2. Mode 1 uses only a hertz model fit, and is faster.
%Mode 2 uses a hertz model fit on the right and a linear fit on the left.
%Mode 2 is more robust, but slower and requires adjustment of the weighting
%parameter.

%Weight: parameter that adjusts the weighting of the linear fit error 
%before it is added to he hertz model fit in mode 2. Adjust this depending 
%on the noise level of the cantilever/fluid ensemble.

%The function returns the INDEX of the contact point, not the z or d value.


function guess = contactpoint3(d_app,z_app,tipf,maxind)

MODE = 1;
Weight=1;

d_app = smooth(d_app,10);
step = 50;
guess = round(length(d_app)/2);

%set up the step sizes for golden-section search
stepsize = [40 20 10 5 3 1];

%initialize limits
lowerlimit = 1;
upperlimit = length(d_app);
quarterlength = round(upperlimit / 4);

%check if glass
line1 = polyfit(z_app(end-4:end), d_app(end-4:end),1);
if line1(1) > 0.1
    decreases = find(diff(d_app) < 0);
    lastdecrease = decreases(end);
    guess = lastdecrease + 1;
    return;
end

%now do a search for the best fit
for j = 1:length(stepsize)
    for k = lowerlimit:stepsize(j):upperlimit
        
        if(MODE == 2)
        if(k>3)      
        [m c] = polyfit(z_app(1:k),d_app(1:k),1);
            lin_err(k) = abs(c.normr)*1e9;
            %disp(lin_err(k))
        else
            lin_err(k) = 1e50;
        end
        end
        if(k < 50)
            hz_err(k) = 1e50;
            err.n = length(d_app)-k;
        elseif(length(d_app)-k > 3)
            [E ctp err] = afmcntp(z_app,d_app,tipf,[k maxind], 0);
            mn = mean(d_app(1:k));
            st = std(d_app(1:k));
             %pf=polyfit(z_app(1:k),d_app(1:k),1);             
             %d_app_corr=d_app-polyval(pf,z_app);
             %slpchk = polyfit(z_app(3:quarterlength),d_app_corr(3:quarterlength),1);
             %st = std(d_app(1:quarterlength)) * 2e6;
            %************************************************************
            if(E<1)
            %************************************************************
                %kill any values that come back negative
                hz_err(k) = 1e50;
            %elseif (abs(slpchk(1) - pf(1)) > st)
                %kill points far from teh baseline
                %hz_err(k) = 1e50;
            elseif d_app(k) > (3*st + mn)
                hz_err(k) = 1e50;
            elseif k < quarterlength
                hz_err(k) = 1e50;
            else
                hz_err(k) = err.rel*50;
            end
        else
            hz_err(k) = 1e50;
        end
        if(MODE == 2)
            %scale the linear error and add them together
            lin_err(k) = lin_err(k)*Weight*err.n/k;
            total_err(k) =  hz_err(k)+lin_err(k);
        else
            total_err(k) = hz_err(k);
        end
    end

    
    indices = find(total_err ~= 0);
    if(j==1 || j == 2)
        [leasterr bestindex] = min(abs(total_err(indices(2:end-1))));
        bestk = indices(bestindex + 1);
    else
        [leasterr bestindex] = min(abs(total_err(indices)));
        bestk = indices(bestindex);
    end
    
    lowerlimit = bestk - round(2*stepsize(j));
    upperlimit = bestk + round(2*stepsize(j));
    if(lowerlimit < 1)
        lowerlimit = 1;
    end
    if (upperlimit > length(d_app))
        upperlimit = length(d_app);
    end
    clear total_err lin_err hz_err;
end
guess = bestk;

end

