% %%K-means+ISODATA Result

function [lambda, I, error] = kda(X, Kmax, map)

ons = ones(size(X, 1), 1);
K = 1;
N = size(X, 1);
c1 = 1; c2 = 10;
thc = 10^-2;
% Init
lambda = X\ons;
d = abs(X*lambda - 1);
s = sqrt(sum(lambda.^2));
d = d/s;
dm = zeros(N, K);
for i = 1:K
    for j = 1:N
        dm(j,i) = dpl(map(:,j), lambda(:,i));
    end
end
dd = c1*d + c2*dm;
[C,I] = min(dd, [], 2);
for i = 1:N
    Cerr(i) = d(i, I(i));
end
error(1) = sum(Cerr);

% Start Iteration
flag = 1;
iter = 1;
while(flag)
    iter = iter + 1;
    IP = I;
    LP = lambda;
    % Calculate fitting error
    d = abs(X*lambda(:,1:K) - 1);
    s = sqrt(sum(lambda(:,1:K).^2));
    dm = zeros(N, K);
    for i = 1:K
        d(:,i) = d(:,i)/s(i);
        for j = 1:N
            dm(j,i) = dpl(map(:,j), lambda(:,i));
        end
    end
    dd = c1*d + c2*dm;
    [C,I] = min(dd, [], 2);
    for i = 1:N
        Cerr(i) = d(i, I(i));
    end
    error(iter) = sum(Cerr);
        % Decide if need to split a group into two
    for i = 1:N
        if (C(i)< thc)
            C(i) = 0;
        end
    end
    for i = 1:K
        Cm(i) = mean(C(I == i));
    end
    for i = 1:N    
        if(C(i)>Cm(I(i))&& Cerr(i)>0)
            I(i) = K+1;
        end
    end
    K = max(I);
    if (K>Kmax)
        K = Kmax;
    end
    % Check empty set
    for i = 1:K
        if (sum(I==i) < 3)
            for j = i:K
                I(I==(j+1)) = j;
            end
            K = K - 1;
        end
    end

    % Calculate lambda
    for i = 1:K
        lambda(:,i) = X(I==i,:)\ones(size(X(I==i,:),1),1);
    end
    % Decide if to end the loop   
    if((iter >= 100) || ((mean(IP == I)==1)&& (mean(mean(lambda == LP)))==1))
        flag = 0;
    end
    % Decide if need to merge two groups
    for i = 1:K
        for j = i+1:K
            ca = (lambda(:,i)'*lambda(:,j))/(sqrt(sum(lambda(:,i).^2))*sqrt(sum(lambda(:,i).^2)));
            da = abs(acos(ca));
            dl = sqrt(sum((lambda(:,i) - lambda(:,j)).^2));
            if((da < pi/30) && (dl < 0.1))
                for k = j:K-1
                    lambda(:,k) = lambda(:,k+1);
                    I(I == k) = k - 1;
                end
                lambda(:,K) = 0;
                K = K - 1;
                j = j - 1;
            end
        end
    end
end
% Finalize clustering
for i = 1:K
    lambda(:,i) = X(I==i,:)\ones(size(X(I==i,:),1),1);
end
d = abs(X*lambda - 1);
s = sqrt(sum(lambda.^2));
for i = 1:K
    d(:,i) = d(:,i)/s(i);
end
[C,I] = min(d, [], 2);
error(iter+1) = sum(C);