%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% filtering by 10x10 cells
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
min_x  = min(small_dataset(:,2)); 
min_y  = min(small_dataset(:,1));

max_x  = max(small_dataset(:,2)); 
max_y  = max(small_dataset(:,1));

length_x = max_x - min_x;
length_y = max_y - min_y;


% Distance in meters for x and y axis
distance_meter_x = distanceGPS(min_y,min_x,min_y,max_x); % 290.727318201 m
distance_meter_y = distanceGPS(min_y,min_x,max_y,min_x); % 1107.98493863 m

% 10x10 row=110,column=29
% 25x25 row=44,column=12
% 20x20 row=55,column=14
% 30x30 row=37,column=10
% 40x40 row=28,column=7
% 50x50 row=20,column=5

row_num=110;
column_num=29;

distance_x = length_x/column_num;
distance_y = length_y/row_num;

counter = 0;
max_cell_size = 0;

number_of_cells_not_conforming_to_std = 0;
cells_within_std = ones(row_num,column_num);
cells_std = zeros(row_num,column_num);
cells_with_data = 0;
cells_no_data = 0;
for row=1:1:row_num
    for column=1:1:column_num
        if row == row_num && column == column_num
            temp = small_dataset(small_dataset(:,1)>=(min_y + ((row-1)*distance_y)) & small_dataset(:,1)<=max_y & ...
                small_dataset(:,2)>=(min_x + ((column-1)*distance_x)) & small_dataset(:,2)<=max_x,1:4);
         
        elseif row == row_num
            temp = small_dataset(small_dataset(:,1)>=(min_y + ((row-1)*distance_y)) & small_dataset(:,1)<=max_y & ...
                small_dataset(:,2)>=(min_x + ((column-1)*distance_x)) & small_dataset(:,2)<(min_x + column*distance_x),1:4);
       
        elseif column == column_num
            temp = small_dataset(small_dataset(:,1)>=(min_y + ((row-1)*distance_y)) & small_dataset(:,1)<(min_y + row*distance_y) & ...
                small_dataset(:,2)>=(min_x + ((column-1)*distance_x)) & small_dataset(:,2)<=max_x,1:4);
       
        else
            temp = small_dataset(small_dataset(:,1)>=(min_y + ((row-1)*distance_y)) & small_dataset(:,1)<(min_y + row*distance_y) & ...
                small_dataset(:,2)>=(min_x + ((column-1)*distance_x)) & small_dataset(:,2)<(min_x + column*distance_x),1:4);
        end
        
        % isempty returns 1 if true and 0 for false
        if isempty(temp) == 0
            cells_with_data = cells_with_data + 1;
            weighted_cells{(row-1)*column_num + column} = temp;
            cells_std(row,column) = std(temp(:,3));
            counter = counter + length(temp); 
            if length(temp) > max_cell_size
                max_cell_size = length(temp);
            end
  
        else
            weighted_cells{(row-1)*column_num + column} = [];
            cells_no_data = cells_no_data + 1;
        end
    end
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Weight the data cells
%%%%%%%%%%%%%%%%%%%%%%%%%%%%

weighted_rss = zeros(row_num,column_num);
weighted_std = zeros(row_num,column_num);
weighted_energy = zeros(row_num,column_num);
for row=1:1:row_num
    for column=1:1:column_num
        temp = weighted_cells{(row-1)*column_num + column};
        weighted_values = [];
        if isempty(temp) == 0           
            temp_max = max(temp(:,4))+1;
            for i=1:1:length(temp(:,4))
                % (max - x)^2
                % where x = error margin of data point
                accuracy = temp(i:i,4);
                rss = (temp(i:i,3));
                weight = (temp_max-accuracy+1)^2;
                weight_value = weight*rss;
                weighted_values = [weighted_values; accuracy weight weight_value];
            end
            weighted_rss(row,column) = sum(weighted_values(:,3)) / sum(weighted_values(:,2));
            weighted_std(row,column) = std(weighted_values(:,3));                                      
            strength = weighted_rss(row,column);
            if strength >= -82
               weighted_energy(row,column) = 1;
            elseif strength >= -92
               weighted_energy(row,column) = 2;
            else
               weighted_energy(row,column) = 3;
            end
        end
        weighted_map{(row-1)*column_num + column} = weighted_values;
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%

figure(91);
imagesc(weighted_rss)
colorbar();

figure(92);
imagesc(weighted_energy)
colorbar();