%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%                       naam: WB_4173414_opgave3.m               %%
%%                       Datum: 09-09-11                          %%
%%               Gemaakt door: Henrik Johannes                    %%
%%                     studienummer:4173414                       %%
%%                          Wb3220                                %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Dataimport
parameters = getappdata(0,'parameters');
file = getappdata(0,'file');
%% Constantes
    straal = parameters.route.straal;
    xm = parameters.route.xm;
    ym = parameters.route.ym;

%% Berekeningen
%Inlezen data
fid = fopen(file.filename);
C = textscan(fid, '%s %s %s');
fclose(fid);


%Data verwerken
x= str2double(C{1});
y= str2double(C{2});
z= str2double(C{3});

if length(x)>2450
   x=x(1:2450); 
end
%Parameters
dx = 1; % Stapgrootte x-as
dy = 1; % Stapgrootte y-as
Xeind = 50; % Eindwaarde X
Yeind = 50; % EIndwaarde Y

% Weegfactoren
factor = 50; % Zoek gebied pieken
factor2 = 50;   % Interpolatie gebied
wf(1) = 0.7; % Weegfactor kruiselingse interpolatie
wf(2) = 0.15; % Weegfactor interpolatie x-as
wf(3) = 0.15; % Weegfactor interpolatie y-as

X = [0:dx:Xeind];   % x vector
Y = [0:dy:Yeind];   % y-vector 

% Grid maken
[XI, YI] = meshgrid(X, Y);

% Waarden op grid interpoleren
Z = griddata(x, y, z, XI, YI,'cubic');

% Waarden verwerken
data = Z(2:end,2:end);
Z = Z(2:end,2:end);
X = X(1:end-1);
Y = Y(1:end-1);

x_route = xm - straal*cos([0:0.001:2*pi]);           
y_route = ym - straal*sin([0:0.001:2*pi]);


%% Vervuiling weg werken
%while max(max(abs(data)))>1
for o=1:3
for i=1:Xeind
    
    for j=1:Yeind
        
        
        
                      
        %% Piekdetectie
        if isnan(data(i,j))
            data(i,j) = interp2(X,Y,data, X(i),Y(j),'spline');
        end
        
        if abs(data(i,j))>0.5
            if o == 3
                if ~(i==50 && j ==50)
                data(i,j) = data(i+1,j+1);
                else
                    data(i,j) = 0;
                end
            else
                data(i,j) = data(51-i,51-j);
            end
        end
        
        %% Nan detectie
        if isnan(data(i,j))
            data(i,j) = data(51-i,51-j); 
        end
        
        
    end
end
end
%end


z_route = interp2(data,x_route,y_route);


 s(1) = 0;
 for j=2:length(x_route)
     s(j) = s(j-1) + sqrt((x_route(j)-x_route(j-1))^2+(y_route(j)-y_route(j-1))^2);
 end

% %cirkel
%  k=0:pi/100:2*pi;
%     c=[xm+straal*cos(k+pi); ym+straal*sin(k+pi)];
% c_i=interp2(x,y,z_i,c(1,:),c(2,:));     

import.y_route = y_route;
import.x_route = x_route;
import.s = s;
import.data = data;
import.z_route = z_route;

setappdata(0,'import',import)



%gemiddlede hoogte van de route
% gemiddeldeHoogte=mean(c_i)

%maximale hellingshoek
% MaxHellingshoek=(360/(2*pi))*atan(max(diff(c_i))*200/(2*pi*straal))
% %% plot
% figure
% hold on
% surf(x,y,z);
% plot3(c(1,:),c(2,:),c_i,'c');
% title('Zonder Peaks');
% hold off
% 
% figure
% hold on
% surf(x,y,z_i);
% plot3(c(1,:),c(2,:),c_i,'c','Linewidth',6);
% title('Geinterpoleerd');
% hold off
% 
% figure
% hold on
% plot(c_i);
% axis([0, 201, 0.2, 0.5]);
% title('doorsnede van route');
% hold off







% for i=1:(Xeind-dx)/dx;
%     
%     for j=1:(Yeind-dy)/dy;
%         
%         
%         %% Nan detectie
%         if isnan(data(i,j))
%             data(i,j) = data(51-i,51-j); 
%         end
%                       
%         %% Piekdetectie
%         if isnan(data(i,j))
%             data(i,j) = interp2(X,Y,data, X(i),Y(j),'spline');
%         end
%         
%         if abs(data(i,j))>1
%             data(i,j) = data(51-i,51-j);
%         end
%         
%         
%         
%     end
% end




%%% Einde %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%							                                  %%
%%		Aanpassingen:				                          %%
%%		8.09.2011: versie 1.0	     		                  %%
%%		.09.2011: versie 1.1	     		                  %%
%%							                                  %%
%%							                                  %%
%%							                                  %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%