clear vars
reading_text_dk;
reading_text_2dk;
reading_text_3dk;

X_uni = location_3d_bad.';%(x,y,z) column vectors
x_camera = pixelLocation.'; % (column, row) column vectors

[ W_est,T_est,R_est ] = camera_param_est( X_uni,x_camera );
x_camera_est = project3d_2d( X_uni,W_est,R_est, T_est);
distance = (sum((x_camera_est-x_camera).^2,1)).^1/2;
average_distance = mean(distance);

modelSize = 15;
maxCount = 0;
maxIndexes = 0;
maxModelSet = zeros(1,modelSize);
maxIteration = 10;
errorMargin = average_distance/1e2;

prob = 1 - (1-(1-0.18)^modelSize)^maxIteration
% prob = %desired probability that t
% noIteration = ln(1-0.99)/ln(1-(1-0.18)^10)
countOfInlier = zeros( 1, maxIteration);
meanDistance = zeros( 1, maxIteration);
inlier_Index_matrix = zeros( maxIteration, size(x_camera,2));

j = 1;

while ( j <= maxIteration)

    rand_vector = [];
    while( size(rand_vector,2) <modelSize )
        rand_no = randi(72);
        dk = find(rand_vector ==rand_no);
        if isempty(dk)
            rand_vector = [rand_vector rand_no];
        end
    end
    rand_index = rand_vector;
%     rand_index = randi(72,1,modelSize);
    model_set = X_uni(:,rand_index); % selecting our samples from the input
    model_set_output = x_camera(:,rand_index);

    %learning part
    [ W_est,T_est,R_est ] = camera_param_est( model_set,model_set_output ); % we estimate the parameters according to our model set
    %testing part
    x_camera_est = project3d_2d( X_uni,W_est,R_est, T_est); % estimate all yi according to parameters obtained using model samples

    distance = (sum((x_camera_est-x_camera).^2,1)).^1/2; %euclidean distance between (c_est,r_est) (c,r)

    inlier_index = find( distance <= errorMargin );  %finding the data instance of inliers
    totalCount = length(inlier_index);                  %counting the total number of inliers

%     countOfInlier(1,j) = totalCount;
%     meanDistance(1,j) = mean(distance);
%
%     if ( totalCount > 0)
%         inlier_Index_matrix(j,1:totalCount) =inlier_index ;
%     end
    if( totalCount > maxCount)                          %if number of inliers is bigger than current best
        maxCount = totalCount;                          %store the inlier indexes and set the maxCount
        maxIndexes = inlier_index;
        maxModelSet = rand_index;
    end
    j = j+1;
%     display(countOfInlier)
end

selectIndex = union(maxIndexes,maxModelSet); % selected Inliers + elements of model set which have the most total count of inliers
X_inliers = X_uni(:,selectIndex);
x_inliers =x_camera(:,selectIndex);
[ W_est,T_est,R_est ] = camera_param_est( X_inliers,x_inliers ); % we estimate the parameters according to our model set

x_camera_est = project3d_2d( X_uni,W_est,R_est, T_est); %estimating total error using the parameters obtained from selected inliers and model set
distance = (sum((x_camera_est-x_camera).^2,1)).^1/2;
average_distance_final = mean(distance);
display(['max total count = ' num2str(maxCount) ]);
display(['number of union inliners and model set = ' num2str(size(X_inliers,2)) ]);

display(['average distance' num2str(average_distance_final)] );



% we want to ideally select modelset + inlinear amount points from the
% distorted 3dcoordinates by comparing them to actual 3d coordinates
X_uni_ideal = location_3d.';
distance_betweenIdeal =(sum((X_uni_ideal-X_uni).^2,1)).^1/2;
[B,I] = sort(distance_betweenIdeal);
idealInliners = I(1:size(X_inliers,2));

[ W_est,T_est,R_est ] = camera_param_est( X_uni(:,idealInliners),x_camera(:,idealInliners)); % we estimate the parameters according to our model set
x_camera_est = project3d_2d( X_uni,W_est,R_est, T_est); %estimating total error using the parameters obtained from selected inliers and model set
distance_id = (sum((x_camera_est-x_camera).^2,1)).^1/2;
average_distance_final_id = mean(distance_id);