function x = curvature (P, range, EA)

n=length(P);

Ds = range/50;
R = Ds:Ds:range;
D = length(R);

% Curvature estimation over length ranges R
for d = 1:D,
  for l = 1:n,
    len = 0;
    k = 1;
    % Find three points giving minimum length range R(d)
    while len < R(d),
      if (l-k > 0) && (l+k <= n),
        a = P(1:2,l) - P(1:2,l-k);
        b = P(1:2,l+k) - P(1:2,l);
        len = norm(a) + norm(b);
        k = k + 1;
      else
        break;
      end
    end
    if k > 1,
      % Turning angle from three points
      T(d,l) = acos((a'*b) / (norm(a) * norm (b)));
      c = cross ([a;0],[b;0]);
      if c(3) >= 0,
        T(d,l) = -T(d,l);
      end
      T(d,l) =  2 * T(d,l) / len;
      % Curvature from circle radius thorugh three points
      k = k - 1;
      [c,r] = circle (P(1,l), P(2,l), P(1,l-k), P(2,l-k), P(1,l+k), P(2,l+k));
      K(d,l) =  1/r;
      x = cross ([a;0],[b;0]);
      aa = c - P(:,l);
      y = cross ([a;0],[aa;0]);
      if x .* y >= 0,
        x(3) = -x(3);
      end
      if x(3) < 0,
        K(d,l) = -K(d,l);
      end
    else
      T(d,l) = 0;
      K(d,l) = 0;
    end
  end
end

% Standard deviation and mean of curvature estimates
for l = 1:n
  X = K(:,l);
  sel = X(:) < 1000;
  stdK(l) = std(X(sel));
  meanK(l) = mean(X(sel));
  X = T(:,l);
  sel = X(:) < 1000;
  stdT(l) = std(X(sel));
  meanT(l) = mean(X(sel));
  minT(l) = min(X(sel));
  maxT(l) = max(X(sel));
  zeroT(l) = min(abs(X(sel)));
end

% Plot
I=[1:n];
X = P(1,I);
Y = P(2,I);

% Bad segments by difference between mean/closest-to-zero turning angle and turning angle
%delta = T(1,I) - meanT(I);
delta = abs(T(1,I)) - zeroT(I);

S = 2 / max([max(abs(meanT(I))), max(abs(minT(I))), max(abs(maxT(I)))]);
SS = 1 / max(max(abs(delta)));

M = length(X);
plot ([X(1),X(M)],[0 0],'k:');
hold on;
%plot ([X(1),X(M)],[3 3],'k:');
plot ([X(1),X(M)],[0 0] + EA * SS,'r:');
plot ([X(1),X(M)],[-3 -3],'k:');


% Mean and standard deviation of circle curvature
%S = 2 / max(abs(meanK(I)));
%plot (X, meanK(I)*S + 3, 'b')
%plot (X, stdK(I)*S + 3, 'g')

% Bad segments by turning angle variation maxima
%for k = I
%  ext(k) = (k > 1) && (k <= n) && (stdT(k) > stdT(k-1)) && (stdT(k) > stdT(k+1));
%end
%plot (X(ext), Y(ext), 'ro', 'MarkerEdgeColor', 'g', 'MarkerFaceColor', 'g', 'MarkerSize', 10);

% Bad segments by difference between mean/closest-to-zero turning angle and turning angle
plot (X, T(1,I) * S - 3, 'r')
plot (X, delta * SS, 'm');
bad = abs(delta) > EA;
plot (X(bad), Y(bad), 'ro', 'MarkerEdgeColor', 'b', 'MarkerFaceColor', 'b', 'MarkerSize', 8);

% Mean and standard eviation of trurning angle curvature
plot (X, meanT(I)*S - 3, 'b')
%plot (X, stdT(I)*S - 3, 'g')
plot (X, minT(I)*S - 3, 'g--')
plot (X, maxT(I)*S - 3, 'g--')

plot (X,Y,'ko-','LineWidth',2, 'MarkerSize', 4);