function [d_est J] = EMexample3 (d, N, sigma2)

% CREATE x
p = length(d);
x = sqrt(sigma2)*randn(1,N);
for i = 1:p
    x = x + StepFunction (d(i), N);
end;

% try all d values for grid search
d_int = 0:0.1:N-1;

% d_est = [40 100 120];
d_est = ceil(rand(1,3)*N);
cnt = 0;
change = 1;
while (change>0 && cnt < 20 )
    % E-Step
    temp = zeros(p,N);
    for i = 1:p
        temp(i,:) = StepFunction (d_est(i), N);
    end;
    for i = 1:p
        y(i,:) = temp(i,:) + (1/p)*(x - sum(temp,1));
    end;
%     figure(1);clf;
%     subplot(3,1,1);hold on;
%     plot(x,'k-');
%     plot(sum(temp,1),'r-');
%     subplot(3,1,2);plot(temp');
%     subplot(3,1,3);plot(x - sum(temp,1));
%     pause(0.1);
    
    % M-step
    J = 0;
    for i = 1:p
        temp = zeros(1,length(d_int));
        for k = 1:length(d_int)
            temp(k) = sum(y(i,:).*StepFunction(d_int(k),N));
        end;
        [maxJ maxIndex] = max(temp);
        J = J + maxJ;
        d_new(i) = d_int(maxIndex);
    end;
    change = sum(abs(d_new-d_est))/p;
    d_est = d_new;
    cnt = cnt + 1;
end;
J

function f = StepFunction (d, N)
f = zeros(1,N);
% for k = d:min(d+9,N)
%     f(k) = 1;
% end;

var = 25;
for k=1:N
    f(k) = exp(-(k-d).^2/(2*var));
end;