clear all; close all;
% Line parameters
m = 4;
b = 5;
points=100;
n_outliers = 80;

% Script Parameters
% How often to repeat the line generation and parameter
% estimation (for accuracy and performance measurements)
iterations = 1;
% plots intermediate steps. set to False for performance
% measurements
show_steps = true;
% only shows every nth line, to avoid plotting too much.
% has no effect if show_steps = false.
show_every_nth = 5;
% plots the original points and the estimation of the line
show_results = true;
generate_gif = false;
filename = 'animated.gif';

% Algorithm parameters
% amount of outliers to eliminate per iteration
dmax = 1;

err_sqr = zeros(1,iterations);
if generate_gif
    show_steps = true;
end
if show_steps
    show_results = true;
end

tic %performance measurement
for j = 1:iterations
%% Generation of perturbed line
    % generate line
    x = linspace(0, 100, points);
    y_orig = m*x + b;
    minval = y_orig(1);
    maxval = y_orig(end);
    range = maxval-minval;
    %minval = 0 %uncomment this for horizontal lines
    %range = 10 %uncomment this for horizontal lines
    
    y_err = y_orig;
    error_indices = randperm(points);
    error_indices = error_indices(1:n_outliers);
    
    % Outliers will be somewhere between the minimum and
    % maximum value of the observed line
    error_values = rand(1,n_outliers) * range + minval;
    y_err(error_indices) = error_values;
    
    if show_results
        h=figure
        scatter(x,y_err)
        hold on
        plot(x,y_orig)
        axis([-10 110 minval-.1*range maxval+.1*range])
        %axis([-10 110 -1 11]) %uncomment this for horizontal lines
        pause
        if generate_gif
              drawnow 
              % Capture the plot as an image 
              frame = getframe(h); 
              im = frame2im(frame); 
              [imind,cm] = rgb2ind(im,256); 
              % Write to the GIF File 
              imwrite(imind,cm,filename,'gif', 'Loopcount',inf); 
        end
    end
    x2 = x;
    
%% Algorithm
    for i = 1:n_outliers/dmax % default dmax=1
        % fit regression line through the points
        % replace by pseude inverse in higher dimensions.
        [p,S] = polyfit(x2, y_err,1);
        ms = p(1);
        bs = p(2);
        y_est = ms*x2+bs; % estimated regression line y values
        if show_steps
            if mod(i, show_every_nth) == 0
                plot(x2,y_est);
                pause;
                if generate_gif
                  drawnow 
                  % Capture the plot as an image 
                  frame = getframe(h); 
                  im = frame2im(frame); 
                  [imind,cm] = rgb2ind(im,256); 
                  % Write to the GIF File 
                  imwrite(imind,cm,filename,'gif','WriteMode','append'); 
                end
            end
        end
        % delete dmax elements with the highest distance to the
        % estimated line.
        for k = 1:dmax
            [diff, ind] = max(abs(y_err-y_est));
            x2(ind) = [];
            y_err(ind) = [];
            y_est(ind) = [];
        end;
    %pause
    end;
    % The curve needs to be calculated
    % because the y_est was evaluated
    % for x2 (a subset of x)	
    % (Only for plotting and error calculation)
    y_est2 = ms*x+bs;

    if show_results
        % Plot regression curve
        plot(x,y_est2);
        
        if ~ show_steps
            legend('Observed Points', 'Original Curve', ...
	           'Estimated Curve', 'Location', 'southeast');
        end
        if generate_gif
          drawnow 
          % Capture the plot as an image 
          frame = getframe(h); 
          im = frame2im(frame); 
          [imind,cm] = rgb2ind(im,256); 
          % Write to the GIF File 
          imwrite(imind,cm,filename,'gif','WriteMode','append');  
        end
    end
    
    %plot(x,ys);
    err_sqr(j) = sum((y_orig-y_est2).^2);
end
toc
plot(err_sqr)
n_corr = sum(err_sqr<100);
percentage_corr = n_corr/iterations * 100

% Iterationsergebnisse: (je 10 000 Iterationen, parallel toolbox 2thr)
%100 Punkte, 90 Ausreisser,dmax=1 --> 0.2781 korrekt. (358s)
%100 Punkte, 90 Ausreisser,dmax=2 --> 0.2188 korrekt. (200s)
%100 Punkte, 90 Ausreisser,dmax=5 --> 0.0443 korrekt (98.6s)
%100 Punkte, 80 Ausreisser,dmax=1 --> 0.8340 korrekt (328s)
%100 Punkte, 80 Ausreisser,dmax=2 --> 0.7906 korrekt (178s)
