function K = curv(x,y)
% function K = curv(x,y)
% Calculates the curvature along an x,y profile by
% finding the radii of circles that fit triplets of points.
% The circle centers are found by intersecting two perpendicular bisectors
% of line segments that connect two pairs of points in the triplet.
% An arc that is concave up has a positive curvature.
% An arc that is concave down has a negative curvature.
% Input parameters
% x = array of x-coordinates of points
% y = array of y-coordinates of points
% Output paramenter
% K = curvature

l = length(x);
x_1 = x(1:l-2);
x_2 = x(2:l-1);
x_3 = x(3:l);
y_1 = y(1:l-2);
y_2 = y(2:l-1);
y_3 = y(3:l);

% Find the perpendicular bisectors
% Point A is the midpoint of segment a between pts 1 and 2
% Point B is the midpoint of segment b between pts 2 and 3
xA = (x_1 + x_2)/2;
yA = (y_1 + y_2)/2;
xB = (x_2 + x_3)/2;
yB = (y_2 + y_3)/2;
dxA = x_2 - x_1;
dyA = y_2 - y_1;
dxB = x_3 - x_2;
dyB = y_3 - y_2;
lA = sqrt(xA.*xA + yA.*yA);
lB = sqrt(xB.*xB + yB.*yB);

% Set up normal forms of lines n ¥ v = d
% The normal to the perp. bisector of segment a is parallel to segment a
nxA = dxA./lA;
nyA = dyA./lA;
dA  = nxA.*xA + nyA.*yA;
nxB = dxB./lB;
nyB = dyB./lB;
dB  = nxB.*xB + nyB.*yB;

% Find the intersection(s) of the perpendicular bisectors
% This is the center of the circle trough point 1, 2, and 3
x_c = zeros(size(x_2));
y_c = zeros(size(y_2));
csign = zeros(size(x_2));

for i = 1:length(x_2);
    int = [nxA(i) nyA(i); nxB(i) nyB(i)]\[dA(i); dB(i)];
    x_c(i) = int(1,1);
    y_c(i) = int(2,1);
    % Find the sign of the curvature using the sign of the z-component 
    % of the cross product of vector "a" and vector "b"
    % Positive = conave up, negative = concave down
    xprod = cross([dxA(i);dyA(i);0],[dxB(i);dyB(i);0]);
    csign(i) = sign(xprod(3));
end

% Find the distance from the intersection to point 2
% This is the radius of the circle
r = sqrt( (x_c-x_2).*(x_c-x_2) + (y_c-y_2).*(y_c-y_2) );

% Find the curvature (the reciprocal of the circle radius)
K = (1./r).*csign;

% plot the topographic and curvature profiles
%subplot(3,1,1)
%plot(x,y)
%xlabel('Distance from base (m)')
%ylabel('Relative Elevation (m)')
%axis([x(1) x(l) min(y) max(y)]);
%title('Topography')
%subplot(3,1,2)
%plot(x,y)
%xlabel('Distance from base (m)')
%ylabel('Relative Elevation (m)')
%axis('equal');
%title('Topography')
%subplot(3,1,3)
%plot(x_2,K)
%xlabel('Distance from base (m)')
%ylabel('Curvature (m^{-1})')
%axis([x(1) x(l) min(K) max(K)]);
%title('Curvature')