
%% Run multiple forced response curves
warning('off', 'MATLAB:interp1:UsePCHIP');

p0 = [2*pi; 0.001; 0.04];
[~, x0] = ode45(@(t,x) bistable(t,x,p0), [0 500*pi], [0; 1]); % Transients
[t0 x0] = ode45(@(t,x) bistable(t,x,p0), [0 2*pi], x0(end,:)'); % Approximate periodic orbit
prob = coco_prob();
prob = coco_set(prob, 'ode', 'autonomous', false);
prob = coco_set(prob, 'po', 'bifus', 'on');
prob = coco_set(prob, 'coll', 'NTST', 60);
funcs = { @bistable, @bistable_dx, @bistable_dp };
coll_args = { funcs{:}, t0, x0, {'T' 'A' 'd'}, p0 }; %#ok<CCAT>
prob = ode_isol2po(prob, '', coll_args{:});
[data uidx] = coco_get_func_data(prob, 'po.orb.coll', 'data', 'uidx');
maps = data.coll_seg.maps;
prob = coco_add_glue(prob, 'glue', uidx(maps.T_idx), uidx(maps.p_idx(1)));
prob = po_mult_add(prob, 'po.orb');  % Store Floquet multipliers with bifurcation data
%prob = coco_set(prob, 'cont', 'PtMX', [0 101]);
prob = coco_add_event(prob, 'UZ', 'A', 0.001:0.001:0.4);
cont_args = { 1, {'po.period' 'A'}, [1/0.22 1/0.12]};
bd1  = coco(prob, 'force', [], cont_args{:});

x1  = coco_bd_col(bd1, '||po.orb.x||');
T1  = coco_bd_col(bd1, 'T');
A1 = coco_bd_col(bd1, 'A');

x = [] ; T = [] ; A = [] ;
labs  = coco_bd_labs(bd1, 'UZ');
for lab=labs
    prob = coco_prob();
    prob = coco_set(prob, 'ode', 'autonomous', false);
    prob = coco_set(prob, 'po', 'bifus', 'on');
    prob = ode_po2po(prob, '', 'force', lab);
    [data uidx] = coco_get_func_data(prob, 'po.orb.coll', 'data', 'uidx');
    maps = data.coll_seg.maps;
    prob = coco_add_glue(prob, 'glue', uidx(maps.T_idx), uidx(maps.p_idx(1)));
    prob = coco_set(prob, 'cont', 'NAdapt', 10);%, 'PtMX', [0 101]);
    cont_args = { 1, {'po.period' 'T'},  [1/0.22 1/0.12] };
    coco(prob, sprintf('lab=%d', lab), [], cont_args{:});
    
    bd = coco_bd_read(sprintf('lab=%d', lab));
    x  = [x, coco_bd_col(bd, '||po.orb.x||')];
    T  = [T, coco_bd_col(bd, 'T')];
    A = [A, coco_bd_col(bd, 'A')];
end

%% Track LP curve
lab = 72 ;
bd = coco_bd_read(sprintf('lab=%d', lab));
labs = coco_bd_labs(bd, 'SN');

prob = coco_prob();
prob = coco_set(prob, 'coll', 'NTST', 60);
prob = ode_po2SN(prob, '', sprintf('lab=%d', lab), labs(1));
[data uidx] = coco_get_func_data(prob, 'po.orb.coll', 'data', 'uidx');
maps = data.coll_seg.maps;
prob = coco_add_glue(prob, 'glue', uidx(maps.T_idx), uidx(maps.p_idx(1)));
prob = coco_set(prob, 'cont', 'NAdapt', 10, 'PtMX', [0 300]);
prob = coco_add_event(prob, 'UZ', 'A', 0.005:0.005:0.4);
cont_args = { 1, {'po.period' 'T' 'A'}, {[1/0.22 1/0.12],[],[0 0.4]}};
bd_sn  = coco(prob, 'saddle-node', [], cont_args{:});
x_sn = coco_bd_col(bd_sn, '||po.orb.x||');
T_sn = coco_bd_col(bd_sn, 'T');
A_sn = coco_bd_col(bd_sn, 'A');

%% Track S-shape curves
freq_lines = linspace(0.12,0.22,50) ;

x_Sc = [] ;
T_Sc = x_Sc ;
A_Sc = x_Sc ;
n_Sc = x_Sc ;

for ifreq = 1:length(freq_lines),
    p0 = [1./freq_lines(ifreq); 0.001; 0.04];
    [~, x0] = ode45(@(t,x) bistable(t,x,p0), [0 500/freq_lines(ifreq)], [0; 0]); % Transients
    [t0 x0] = ode45(@(t,x) bistable(t,x,p0), [0 1./freq_lines(ifreq)], x0(end,:)'); % Approximate periodic orbit
    prob = coco_prob();
    prob = coco_set(prob, 'ode', 'autonomous', false);
    %prob = coco_set(prob, 'po', 'bifus', 'on');
    prob = coco_set(prob, 'coll', 'NTST', 60);
    prob = coco_set(prob, 'cont', 'NAdapt', 10, 'PtMX', [0 501]);
    funcs = { @bistable, @bistable_dx, @bistable_dp };
    coll_args = { funcs{:}, t0, x0, {'T' 'A' 'd'}, p0 }; %#ok<CCAT>
    prob = ode_isol2po(prob, '', coll_args{:});
    [data uidx] = coco_get_func_data(prob, 'po.orb.coll', 'data', 'uidx');
    maps = data.coll_seg.maps;
    prob = coco_add_glue(prob, 'glue', uidx(maps.T_idx), uidx(maps.p_idx(1)));
    %prob = po_mult_add(prob, 'po.orb');  % Store Floquet multipliers with bifurcation data
    %prob = coco_add_event(prob, 'UZ', 'A', 0.001:0.001:0.05);
    cont_args = { 1, {'po.period' 'A'}, {[1./freq_lines(end)-0.1 1./freq_lines(1)+0.1], [0 1]}};
    bdf = coco(prob, sprintf('Scurve=%d', ifreq), [], cont_args{:});
    
    bdS = coco_bd_read(sprintf('Scurve=%d', ifreq));
    x_Sc = [x_Sc, coco_bd_col(bdS, '||po.orb.x||')] ;
    T_Sc = [T_Sc, coco_bd_col(bdS, 'T')] ;
    A_Sc = [A_Sc, coco_bd_col(bdS, 'A')] ;
    n_Sc = [n_Sc, length(coco_bd_col(bdS, '||po.orb.x||'))] ;
    
end
figure ; grid on ; plot3(1./T_Sc,A_Sc,x_Sc,'*k') ;

%% 3D surface without GP regression

% Remove duplicate points from foldCurve
foldCurve = [x_sn;1./T_sn;A_sn] ;
foldCurve = foldCurve(:, diff([foldCurve(1, :),0])~=0);

% Remove points oustide focring amplitude
nFold = size(foldCurve, 2);

% Prepend the fold curve points to the data points
xx3d = [foldCurve(1,:), x_Sc];%, x] ;
yy3d = [foldCurve(2,:), 1./T_Sc];%, 1./T] ;
zz3d = [foldCurve(3,:), A_Sc];%, A] ;

% Isolate the end points of each run to avoid artifacts
iStart = [1, 1 + cumsum(n_Sc(1:end-1))] + nFold;
isStart = false(1, length(xx3d));
isStart(iStart) = true;
iEnd = cumsum(n_Sc) + nFold ;
isEnd = false(1, length(xx3d));
isEnd(iEnd) = true;

figure ; plot3(yy3d,zz3d,xx3d,'.k') ; hold on ; plot3(yy3d(iStart),zz3d(iStart),xx3d(iStart),'or') ; plot3(yy3d(iEnd),zz3d(iEnd),xx3d(iEnd),'ob') ; 
hold on ; plot3(foldCurve(2,:),foldCurve(3,:),foldCurve(1,:),'-m') ;

% Find all the points in the existing data that lies on the boundary
idx = find((yy3d(1,:) == foldCurve(2, end)) & (xx3d(1,:) > foldCurve(1, end)) & (xx3d(1,:) < foldCurve(1, 1)));

% Sort them in terms of distance from the end point of the fold curve
[s, ii] = sort(abs(xx3d(1,idx)-foldCurve(1, end)));
idx = idx(ii) ;

% Do the triangulation
edges = [1:nFold, idx; 2:nFold, idx, 1]' ;
edges = edges(end:-1:1,:) ;
dt = DelaunayTri(xx3d', yy3d', edges(1:end,:));

% Find the interior points
in = inOutStatus(dt);

% Extract the appropriate triangulations
interiorTri = dt.Triangulation(in, :);
exteriorTri = dt.Triangulation(~in, :);
exteriorTri = exteriorTri(~all(isStart(exteriorTri), 2), :);
exteriorTri = exteriorTri(~all(isEnd(exteriorTri), 2), :);

%% Display

figure;
hold on; grid on ;
trisurf(interiorTri, yy3d, zz3d, xx3d, 'LineStyle', 'none', 'FaceColor', [0.5, 0.5, 0.5], 'FaceAlpha', 0.4);
trisurf(exteriorTri, yy3d, zz3d, xx3d, 'LineStyle', 'none', 'FaceColor', [0.75, 0.75, 0.75], 'FaceAlpha', 0.4);
h = plot3(foldCurve(2, :), foldCurve(3, :), foldCurve(1, :), 'k-', 'linewidth', 2);
xlabel('Frequency') ; ylabel('Forcing amplitude') ; zlabel('Response amplitude') ;
xlim([0.13 0.21])
set(gca,'linewidth',2)
set(gca,'XTickLabel',[]) ; set(gca,'YTickLabel',[]) ; set(gca,'ZTickLabel',[]) ;
set(gcf,'Renderer','zbuffer') ;

return

% add NLFR curves
labs = coco_bd_labs(bd1, 'UZ');
for lab=labs,
    bd = coco_bd_read(sprintf('lab=%d', lab));
    x  = coco_bd_col(bd, '||po.orb.x||');
    T  = coco_bd_col(bd, 'T');
    A = coco_bd_col(bd, 'A');
    idxSN  = find(strcmp(coco_bd_col(bd, 'TYPE'),'FP'));
    if A < 0.06,
    plot3(1./T, A, x, 'k', 'linewidth', 2) ;
    plot3(1./T(idxSN), A(idxSN), x(idxSN), 'ok', 'linewidth', 2, 'markersize', 8) ;
    end
end

% add S curves
for ifreq = 1:length(freq_lines),
    bd = coco_bd_read(sprintf('Scurve=%d', ifreq));
    x  = coco_bd_col(bd, '||po.orb.x||');
    T  = coco_bd_col(bd, 'T');
    A = coco_bd_col(bd, 'A');
    plot3(1./T, A, x, '--k', 'linewidth', 2) ;
end

Abckbc = max(A) ;
x  = coco_bd_col(bds, '||po.orb.x||');
T  = coco_bd_col(bds, 'T');
A  = coco_bd_col(bds, 'A');
plot3(2*pi./T, A, x, 'color', [222 125 0]/255, 'linewidth', 2, 'markersize', 8)
plot3(2*pi./T, 0.06*ones(size(x)), x, '--', 'color', [204 204 204]/255, 'linewidth', 2);
