% Harmonic Predictive Control test jig
%
% Initialize
lim = 10;
qq = 0;
figure(2); clf; grid on; box on; hold on
xlim([0 lim])
ylim([0 1])
xlabel('Time (years)')
ylabel('Fraction of carrying capacity')
set(gcf, 'Position', [-50 100 100000 500]);
y = 0.5; text(.2, .5, 'y = 0.5  (goal)')
e = .1;  text(.2, .1, 'e = 0.1 (extinction)')
m = .13; text(.35 , .3, 'm = 0.13 (max acceleration)')
for phase = 1:2
    rbg = ['r' 'b' 'g'];
    s = .3; v = -.05; a = -.1;
    y = .5; 
    e = .1;                       % y = goal, e = extinction
    m = .13;
    dt = 0.01;
    pause
    if phase == 1
        title('An unsustainably fished population')
    else
        title('A sustainably fished population')
        text(2.4, .9, 'Collapse ....', 'HorizontalAlignment', 'Right', 'Color', 'b')
        text(2.5, .9, 'p = v/(e - s)', 'Color', 'b')
        text(2.5, .8, 'h = m - a', 'Color', 'b')
        text(2.5, .7, 'j = ph', 'Color', 'b')
        pause
    end
    for t = 0:dt:lim
        j = hcstep(s, v, a, y, e, m, dt) * (phase - 1);
        % Simulate environment for this demo
        a = a + j*dt;
        v = v + a*dt;
        s = s + v*dt;
        if s < 0
            continue
        end
        plot(t, s, '.', 'Color', rbg(phase + (v > 0)))
        if v > 0 && qq == 0
            pause
            text(4, .9, 'p = v/(y - s)  ....  Recovery', 'Color', [0 .7 0])
            text(4, .8, 'h = -(3a + 2vp)', 'Color', [0 .7 0])
            text(4, .7, 'j = ph', 'Color', [0 .7 0])
            pause
            qq = 1;
        end
        pause(.001)    
    end
end
