%%K-Means Result
function [lambda, I, error] = mykmeans(data, K, map)

X = data;
N = size(data, 1);
c1 = 1; c2 = 0;
% Choose Init clusters
rp = randperm(N);
p = 1: K*4;
plist = reshape(rp(p), 4, []);
ons = ones(size(plist, 1), 1);
lambda = zeros(3,K);
for i = 1:K
    lambda(:,i) = X(plist(:,i),:)\ons;
end
% Init distance
d = abs(X*lambda - 1);
s = sqrt(sum(lambda.^2));
for i = 1:K
    d(:,i) = d(:,i)/s(i);
end
% First Assign
[C,I] = min(d, [], 2);
error(1) = sum(C);
% Start Iteration
flag = 1;
iter = 1;
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:N
            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
% 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);