clear;

% start with a constant kd, use the given default
kd=15:24;
kp=1:10;
% loop through kp
for i=kp-0
    for j=kd-14
        pd_tanker(kp(i),kd(j),0);
        load(['holy_ship_' num2str(kp(i)) '_' num2str(kd(j))]);
        perf(i,j) = calc_perf_data(time, psi, desired_heading);
    end
end

% generate Y from performance data
Y=perf;

% this happens to be 10x10 for this example. We'd have to consider
% a different case if we want to expand this
order=length(Y);
% X is the order, conveniently.

X = (1:order)';

% Now loop through the order, which is also the same as kp values
mse=zeros(1,order);
for i = 1:length(Y) % loop through kd values
    Phi = ones(length(X),1);
    for j = 1:order % order and kd
        Phi = [X.^j Phi];
        Theta = Phi\Y(:,i);
        Yls = Phi*Theta;
        e = Y(:,i)-Yls;
        mse(i,j) = (e'*e)/2;
    end
end
% Someone smarter than me could probably do this in the upper loop
% But the data is messing with my brain, so i'm taking it externally
% and then getting the min values and then the kd/kp by order.

for i=1:length(mse)
   % it's square. We'll get the min row. That's KD
   % We'll get the min column. That's KP.
   % i is the order.
   [min_value, index] = min(abs(mse(:,i)));
   best_kp(i)=kp(index);
   [min_value, index] = min(abs(mse(i,:)));
   best_kd(i)=kd(index);
end

% plot MSE/order
figure
plot3(mse(1,:), mse(:,1),1:10)
hold on
for i=kd-14
    for j=kp-0
        plot3(mse(i,:), mse(:,j),1:10)
    end
end

zlabel('Order');
ylabel('Kd');
xlabel('Kp');


% plot min
plot(1:order,best_kp,1:order,best_kd)
xlabel('Order')
ylabel('Value');
legend('Best kp', 'Best kd');
title('Order vs best kp; best kd');