%% Simulatie & MATLAB
% Made by Nick Tsutsunava
% 4085396
% Faculty of Mechanical Engineering
% Delft University of Technology

%% Parameters and Constants
sn1 = 4;
sn2 = 0;
sn3 = 8;
sn4 = 5;
sn5 = 3;
sn6 = 9;
sn7 = 6;

% Ship parameters
m  = 25000+sn5*10000+sn6*7000+sn7*600;
Cv = 200+sn5*20+sn6*3+sn7*0.5;
T  = 10000+sn5*6000+sn6*200+sn7*50;

% Import route file
n = floor(1+(sn6/2));
str = sprintf('Route%d.txt',n);
route = importdata(str);
lon = route(:,1);
lat = route(:,2);
wr = route(:,3);% Windrichting

% Negatieve waarden positief maken
for i=1:size(lon)
    if lon(i) <=0
        lon(i)=lon(i)+360;
    end

end
lat = lat+90;

% Import Wind Data file
data = importdata('winddata_clean.txt');

%% Calculations Course Angle

% Global Vector
u = [0;1];
v = [lon lat];
n=1;

for i = 1:length(v)-1
    w=[v(n+1,1)-v(n,1),v(n+1,2)-v(n,2)];
    if w(1,2)<0
        vector(i)=1;
        alpha(n,:) = acos((dot(u,w))/(norm(u)*norm(w)))*(180/pi);
        alpha = 360-alpha;
    else
        alpha(n,:) = acos((dot(u,w))/(norm(u)*norm(w)))*(180/pi);
    end
    n = n+1;
end
alpha(length(alpha)+1)=alpha(end);

%% Calculation Windspeed
% for i = 1:(length(lon))
%     if round(lat(i)) == 0
%         lat(i) = 1;
%     end
%     f(i,:) = data(round(lon(i)),round(lat(i)));
%     i = i+1;
% end
%
% % Windspeed
% ws = 0.836.*f.^(3/2)
for i = 1:length(lon)
    V(i,1) = interp2(data,round(lat(i)),round(lon(i)));
    %Beaufort dude
    V(i,1) = mean(0.836.*V(i,1).^(3/2))  ;
end
for i = 1:length(wr)
    delta(i,:)=abs(wr(i)-alpha(i));
    if delta(i) >=180
        delta(i) = delta(i)-180;
    end
end