
phiLim = (1/2)*pi/2;    % Cutoff angle
% Damping ratio (c = zet*C_bar)

if forceCase == 0
    load('data\Phys2Mod_forcing_12.mat')
elseif forceCase == 1
    load('data\Phys2Mod_forcing_23.mat')
elseif forceCase == 2
    load('data\Phys2Mod_forcing_30.mat')
elseif forceCase == 3
    load('data\Phys2Mod_forcing_218.mat')
elseif forceCase == 4
    load('data\Phys2Mod_forcing_285.mat')
elseif forceCase == 5
    load('data\Phys2Mod_forcing_244.mat')
end
P1 = fmod(1);
P2 = fmod(2);

%% Load parameters
load('data\CCCB_BT_FS_00-46_SC_95_MIMO.mat')

c1 = zet*C_bar(1,1);
c2 = zet*C_bar(2,2);

om1 = sqrt(K_bar(1,1));
om2 = sqrt(K_bar(2,2));

bet1 = Knl(1,4);
bet2 = Knl(2,4);
bet3 = Knl(1,6);
bet4 = Knl(1,7);
bet5 = Knl(2,7);

%% Compute NNMs

U1Max = 0.08;%1e-2;
R = 2;
U1A = (linspace(0,U1Max^(1/R),N).^R).';   U1A(1) = [];

p = +1;
R0 = -p*3*bet2*U1A.^3;
R1 = 4*(om1^2 - om2^2) + 3*(bet1-bet3)*U1A.^2;
R2 = p*9*(bet2-bet4)*U1A;
R3 = 3*(bet3-bet5)*ones(size(U1A));
R4 = p*3*bet4./U1A;

% In-phase
Rp = [+R4, R3, +R2, R1, +R0];
Rp = mat2cell(Rp,ones(length(U1A),1),5);
Rp = cellfun(@roots,Rp,'UniformOutput',false);
U2p = horzcat(Rp{:});
U2p(imag(U2p) ~= 0) = 0;
U2p(U2p < 0) = 0;
U2p = sort(U2p);
U2p(sum(U2p,2) == 0,:) = [];
U1p = repmat(U1A.',size(U2p,1));

p = +1;
Omp = sqrt( om1^2 + (3*bet1*U1p.^2 + p*9*bet2*U1p.*U2p + 3*bet3*U2p.^2 + p*3*bet4./U1p.*U2p.^3 )/4 );

% Out-of-phase
Rm = [-R4, R3, -R2, R1, -R0];
Rm = mat2cell(Rm,ones(length(U1A),1),5);
Rm = cellfun(@roots,Rm,'UniformOutput',false);
U2m = horzcat(Rm{:});
U2m(imag(U2m) ~= 0) = 0;
U2m(U2m < 0) = 0;
U2m = sort(U2m);
U2m(sum(U2m,2) == 0,:) = [];
U1m = repmat(U1A.',size(U2m,1));

p = -1;
Omm = sqrt( om1^2 + (3*bet1*U1m.^2 + p*9*bet2*U1m.*U2m + 3*bet3*U2m.^2 + p*3*bet4./U1m.*U2m.^3 )/4 );

%% Energy transfer analysis

% In-phase
Fp = Omp.*(c1*U1p.^2 + c2*U2p.^2)./abs(P1*U1p + P2*U2p);

SP1p = ones(size(U1p));
SP1p( (P1*U1p + P2*U2p) < 0 ) = -1;

if ~isempty( find( (P1*U1p + P2*U2p).*SP1p < 0 ,1) );   stop;   end

SPdP = @(phid) 4*(Fp*P1.*SP1p - c1*Omp.*U1p)./U2p./( 3*bet2*U1p.^2 + 2*bet3*U1p.*U2p.*cos(phid) + 3*bet4*U2p.^2 );
phidP = zeros(size(U1p));
phidP = get_phid(phidP, SPdP);
phidP = abs(phidP);

% Out-of-phase
Fm = Omm.*(c1*U1m.^2 + c2*U2m.^2)./abs(P1*U1m - P2*U2m);

SP1m = ones(size(U1m));
SP1m( (P1*U1m - P2*U2m) < 0 ) = -1;

if ~isempty( find( (P1*U1m - P2*U2m).*SP1m < 0 ,1) );   stop;   end

SPdM = @(phid) 4*(Fm*P1.*SP1m - c1*Omm.*U1m)./U2m./( 3*bet2*U1m.^2 + 2*bet3*U1m.*U2m.*cos(phid) + 3*bet4*U2m.^2 );
phidM = pi*ones(size(U1m));
phidM = get_phid(phidM, SPdM);
phidM = abs(phidM);

C1 = 1.536368492550149 ;
C2 = 0.552201510632662 ;
Pnnm1 = ([C1 ; C2]*Omm).*[U1m;U2m] ;
Pnnm2 = ([C1 ; C2]*Omp).*[U1p;U2p] ;

%% Parameters for display
OmMax = 118;
COLS = jet(1e3);
v = linspace(0,phiLim,size(COLS,1));
COLp = interp1(v,COLS,phidP);
COLm = interp1(v,COLS,phidM);

