% % Geometrical Segmentation using DA approach

%%
function [lambda, I, error] = isodata(X, Kmax, map)
% % Initialization
% Ti = 35;
% Tmin = 0.1;
% alpha = 0.9;
% c1 = 1; c2 = 10;
% T = Ti;
% K = 1;
% [m,n] = size(X);
% N = m;
% ons = ones(m, 1);
% lambda = zeros(3,Kmax);
% lambda(:,1) = X\ons;
% lambda(:,2) = lambda(:,1);
% p = zeros(m,Kmax);
% p(:,1:2) = 1/2;
% ed = zeros(m,Kmax);
% %%
% % Main Loop
% flag = 1;
% iter = 0;
% while(flag)
%     iter = iter + 1;
%     % Compute the ed
%     for j = 1:K
%         for i = 1:m
%             ed(i,j) = exp(-1/T*dist(X(i,:), map(:,i), lambda(:,j)));
%         end
%     end
%     Zx = sum(ed,2);
%     for i = 1:m
%         for j = 1:K
%             p(i,j) = ed(i,j)./Zx(i);
%         end
%     end
%     for j = 1:K
%         W = zeros(m,m);
%         for i = 1:m
%             W(i,i) = sqrt(p(i,j));
%         end
%         lambda(:,j) = inv(X'*W*X)*X'*W*ons;
%     end
%     % Lo = D-TH
%     D(iter) = Ddst(X, map, lambda, p, m, K);
%     H(iter) = Hetp(X, p, m, K);
%     Lo(iter) = D(iter) - T*H(iter);
%     % end Loop
%     
%     % Decide if need to split a group into two
%         
%     % Decide if need to merge two groups
%     
%     % Check empty set
% 
%     % Cooling Step
%     T = alpha * T;
%     if (T < Tmin)
%         T = 0;
%         flag = 0;
%     end
% end
% 
% %% T = 0; Hard clustering
% [C, I] = max(p, [], 2);
% K = max(I);
% 
% % Start Iteration
% flag = 1;
% iter = 0;
% while(flag)
%     iter = iter + 1;
%     IP = I;
%     LP = lambda;
%     % Estimate lambda
%     for i = 1:K 
%         lambda(:,i) = X(I==i,:)\ones(size(X(I==i,:),1),1);
%     end
%     % Calculate distance
%     d = abs(X*lambda - 1);
%     s = sqrt(sum(lambda.^2));
%     for i = 1:K
%         d(:,i) = d(:,i)/s(i);
%     end
%     % Calculate model difference
%     for i = 1:K
%         for j = 1:m
%             dm(j,i) = dpl(map(:,j), lambda(:,i));
%         end
%     end
%     % Final distance
%     dd = c1*d + c2*dm;
%     % Assign cluster
%     [C,I] = min(dd, [], 2);
%     error(iter) = sum(C);
%     % End iteration
%     if (iter >= 1000) || (mean(IP == I) == 1 && mean(mean(lambda==LP))==1)
%         flag = 0;
%     end
% end

ons = ones(size(X, 1), 1);
K = 1;
N = size(X, 1);
c1 = 1; c2 = 1;
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);