close all
clear all

rho = 1025;
g = 9.81;

filename = 'MPC_BuoyA5.out';
FD_coefs = ReadData(filename);


add_fl = real(FD_coefs.F{3,3}); %added mass of floating body (heave)
dc_fl = imag(FD_coefs.F{3,3});  %damping coefficient of floating body (heave)


w = FD_coefs.w;

figure
subplot(2,1,1)
plot(w,add_fl);
xlabel('Freq (rad/s)');
ylabel('Added Mass');
subplot(2,1,2)
plot(w,dc_fl);
xlabel('Freq (rad/s)');
ylabel('Damping');

addpath('./FDI_Toolbox_v1.2');
A = add_fl;
Ainf = A(end);
B = dc_fl;
Dof = [1 1];

FDIopt.OrdMax = 20;
FDIopt.AinfFlag = 1;
FDIopt.Method = 3;
FDIopt.Iterations = 20;
FDIopt.PlotFlag = 1;
FDIopt.LogLin = 0;
FDIopt.wsFactor = .1;
FDIopt.wminFactor = .1;
FDIopt.wmaxFactor = 1;


[KradNum,KradDen,Ainf_hat]=FDIRadMod(w,A,Ainf,B,FDIopt,Dof);


Cinf = complex(B,w.*(A-Ainf));

C.Caa= KradNum(1);
C.Cab = KradNum(2);
C.Cba = KradDen(1);
C.Cbb = KradDen(2);
C.Cbc = KradDen(3);

save('StateSpaceCoeff.mat','C');

s = complex(0,w);
Chatinf = C.Caa*s./(C.Cba*s.^2+C.Cbb*s+C.Cbc);

figure
subplot(2,1,1)
plot(w,real(Cinf),w,real(Chatinf));
xlabel('freq (rad/s)');
legend('Real(Cinf)','Real(Chatinf)');
subplot(2,1,2)
plot(w,imag(Cinf),w,imag(Chatinf));
xlabel('freq (rad/s)');
legend('Imag(Cinf)','Imag(Chatinf)');


