function Model = wecModel2Body(paramFile)
% wecModel2Body
%   Model = wecModel2Body(paramFile) creates a model in frequency domain for
%   the heave motion for WECs type 3 or type 4.
%   It returns the Model in a structure with the fields mag, phase and
%   frequency

%   Project: IMPACT
%   Title of Code: wecModel2Body
%   Relevant additional documentation:
%   Developer: Carlos Gonzalez
%   External Dependencies: NA
%
%   Disclaimer
%   CAUTION: It is strongly recommended that changes to this code
%   should be avoided without first consulting with authors.

% Run parameters file
if ~exist(paramFile,'file')
    errordlg(sprintf('File %s cannot be found',paramFile),'Error')
    Model=[];
    return;
end

[iSuccess, stErrorMsg] = readFile(paramFile);
if ~iSuccess
    errordlg(stErrorMsg,'Error');
    Model=[];
    return;
end

% Read BEM parameters
if ~exist(stH5File,'file')
    errordlg(sprintf('File %s cannot be found',stH5File),'Error')
    Model=[];
    return;
end
h5Data = getParamH5(stH5File,2);
[n,m,p] = size(h5Data(1).addedMassC);

arModelFreq = h5Data(1).BEMFreq;
numFreq = numel(arModelFreq);

% Body 1
A1BEMnorm = h5Data(1).addedMassC;
B1BEMnorm = h5Data(1).radDampC;
C1BEMnorm = h5Data(1).hydroStiffC;

A1BEM = A1BEMnorm * rDENSITY;
B1BEM = B1BEMnorm * rDENSITY;
C1BEM = C1BEMnorm * rDENSITY*rGRAVITY;

A1_1DOF = reshape(A1BEM(:,3,3),1,n);
B1_1DOF = reshape(B1BEM(:,3,3),1,n);
C1 = C1BEM(3,3);

A12_1DOF = reshape(A1BEM(:,9,3),1,n);
B12_1DOF = reshape(B1BEM(:,9,3),1,n);


%Body2
A2BEMnorm = h5Data(2).addedMassC;
B2BEMnorm = h5Data(2).radDampC;
C2BEMnorm = h5Data(2).hydroStiffC;

A2BEM = A2BEMnorm * rDENSITY;
B2BEM = B2BEMnorm * rDENSITY;
C2BEM = C2BEMnorm * rDENSITY*rGRAVITY;

A2_1DOF = reshape(A2BEM(:,9,3),1,n);
B2_1DOF = reshape(B2BEM(:,9,3),1,n);
C2 = C2BEM(3,3);

A21_1DOF = reshape(A2BEM(:,3,3),1,n);
B21_1DOF = reshape(B2BEM(:,3,3),1,n);


Z1 = (-arModelFreq.^2.*(rFloaterMass+A1_1DOF)+ C1) + 1i*(arModelFreq.*(B1_1DOF + rAddedDampingBody1));
Z2 = (-arModelFreq.^2.*(rSparMass+A2_1DOF)+ C2) + 1i*(arModelFreq.*(B2_1DOF + rAddedDampingBody2));

if iBodyToBodyInteraction
    Z12 = -((-arModelFreq.^2.*(A12_1DOF)) + 1i*(arModelFreq.*(B12_1DOF)));
    Z21 = -((-arModelFreq.^2.*(A21_1DOF)) + 1i*(arModelFreq.*(B21_1DOF)));
else
    Z12 = 0;
    Z21 = 0;
end

% Model from pto force to relative displacement
Z = (Z1.*Z2-Z12.*Z21)./(Z1+Z2-(Z12+Z21))+ rTetherSpring;

magZ = abs(Z);
phaseZ = angle(Z);
[PTOBodeMag,PTOBodePhase,freqPTO] = bode(tfPto,arModelFreq);
PTOMag =reshape(PTOBodeMag,1,numFreq);
PTOPhase = reshape(PTOBodePhase,1,numFreq);

modelPto.arMagnitude = 1./magZ.*PTOMag;
modelPto.arPhase = wrapAngle(-phaseZ*180/pi+PTOPhase);
modelPto.arFreq = arModelFreq;


% Model Floater alone
magZ1 = abs(Z1);
phaseZ1 = angle(Z1);

modelFloater.arMagnitude = 1./magZ1;
modelFloater.arPhase = wrapAngle(-phaseZ1*180/pi);
modelFloater.arFreq = arModelFreq;


% Model Spar alone
magZ2 = abs(Z2);
phaseZ2 = angle(Z2);

modelSpar.arMagnitude = 1./magZ2;
modelSpar.arPhase = wrapAngle(-phaseZ2*180/pi);
modelSpar.arFreq = arModelFreq;


% Model from excitation force on the floater to relative displacement

ZFeFloater = (Z2-Z12)./(Z1+Z2-Z12-Z21);

magZFeFl =abs(ZFeFloater);
phaseZFeFl = angle(ZFeFloater);


modelFeFloater.arMagnitude = magZFeFl;
modelFeFloater.arPhase = wrapAngle(phaseZFeFl*180/pi);
modelFeFloater.arFreq = arModelFreq;


% Model from excitation force on the spar to relative displacement

ZFeSpar = -(Z1-Z21)./(Z1+Z2-Z12-Z21);

magZFeSp =abs(ZFeSpar);
phaseZFeSp = angle(ZFeSpar);

modelFeSpar.arMagnitude = magZFeSp;
modelFeSpar.arPhase = wrapAngle(phaseZFeSp*180/pi);
modelFeSpar.arFreq = arModelFreq;


% Assign output
Model.zModelPto = modelPto;
Model.zModelFloater = modelFloater;
Model.zModelSpar = modelSpar;
Model.zModelFeFloater = modelFeFloater;
Model.zModelFeSpar = modelFeSpar;
