function C = plate_fem_contact(params, P, freqs, zeta, phi_hit, t)
% PLATE_FEM_CONTACT  The hammer and the plate as one system during the collision.
%
%   C = plate_fem_contact(params, P, freqs, zeta, phi_hit, t) integrates the
%   coupled equations of plate_fem_rhs from the hammer touching the plate
%   at its speed (x_h = 0, v_h = speed, the plate at rest) until the two
%   separate, delta = x_h - phi' q falling through zero.  All the modes take
%   part, the three rigid-body ones included, so that the very light plate
%   can move away from the hammer while they are in contact: this is what
%   makes the force lower and the contact longer than the rigid-target
%   pulse of plate_rr_contact, and the hammer's momentum transfer smaller.
%
%   The integrator is classical fourth-order Runge-Kutta with a fixed step,
%   the Hertz duration estimate divided by 2000; at 0.05 us the highest
%   mode kept (below 0.47 fs) advances by less than a hundredth of a radian
%   per step.  Separation is located by bisection within the step in which
%   delta changes sign.  The stored steps and their derivatives give a cubic
%   Hermite interpolant, from which the states at the audio samples inside
%   the contact (C.qs, C.vs, C.fs at C.ts), and the histories on a 600-point
%   grid (C.tg: the force C.fg, the hammer C.hammer_x and C.hammer_v, the
%   plate under it C.plate_w and C.plate_v, and the modal states C.qg, C.vg
%   for the frames) are read.  The impulse is the trapezoidal integral of
%   the force; the effective restitution is the relative separation speed
%   over the approach speed.

hm = params.hammer;
H = P.hertz;
omega = 2 * pi * freqs(:);
zeta = zeta(:);
phi = phi_hit(:)';
nm = numel(omega);
twozw = 2 * zeta .* omega;
k = H.contact_k;
hc = H.hunt_crossley;
mass = hm.mass;
est = H.duration;
nsub = 2000;
h = est / nsub;
nmax = 8 * nsub;

y = zeros(2 * nm + 2, 1);
y(end - 1) = 1e-12;
y(end) = hm.speed;
Ys = zeros(2 * nm + 2, nmax + 1);
Fs = zeros(2 * nm + 2, nmax + 1);
Ys(:, 1) = y;
Fs(:, 1) = plate_fem_rhs(y, phi, nm, omega, twozw, k, hc, mass);
found = false;
tc = 0;
yc = y;
nsteps = 0;
for step = 1:nmax
    [ynew, fnew] = rk4_step(y, h, phi, nm, omega, twozw, k, hc, mass);
    sep = ynew(end - 1) - phi * ynew(1:nm);
    if step * h >= 1e-10 && sep < 0
        % bisection on the step length from the last stored state
        lo = 0;
        hi = h;
        for b = 1:60
            mid = 0.5 * (lo + hi);
            ym = rk4_step(y, mid, phi, nm, omega, twozw, k, hc, mass);
            if ym(end - 1) - phi * ym(1:nm) < 0
                hi = mid;
            else
                lo = mid;
            end
            if hi - lo < 1e-18
                break
            end
        end
        tc = (step - 1) * h + hi;
        yc = rk4_step(y, hi, phi, nm, omega, twozw, k, hc, mass);
        found = true;
        nsteps = step - 1;
        break
    end
    y = ynew;
    Ys(:, step + 1) = y;
    Fs(:, step + 1) = fnew;
    nsteps = step;
end
if ~found
    error('plate_fem_contact: the hammer did not separate within %.3g us (8 Hertz durations)', 8 * est * 1e6);
end
Ys = Ys(:, 1:nsteps + 1);
Fs = Fs(:, 1:nsteps + 1);
fc = plate_fem_rhs(yc, phi, nm, omega, twozw, k, hc, mass);

% states at the audio samples inside the contact and on the plotting grid
ts = t(t < tc);
Yq = hermite(ts, h, tc, Ys, Fs, yc, fc);
tg = linspace(0, tc, 600);
Yg = hermite(tg, h, tc, Ys, Fs, yc, fc);
fs_ = zeros(1, numel(ts));
for j = 1:numel(ts)
    fs_(j) = plate_fem_force(Yq(:, j), phi, nm, k, hc);
end
fg = zeros(1, 600);
for j = 1:600
    fg(j) = plate_fem_force(Yg(:, j), phi, nm, k, hc);
end

C.model = 'coupled Hunt-Crossley/Hertz contact with the moving plate';
C.tc = tc;
C.q0 = yc(1:nm)';
C.v0 = yc(nm + 1:2 * nm)';
C.ts = ts;
C.qs = Yq(1:nm, :);
C.vs = Yq(nm + 1:2 * nm, :);
C.fs = fs_;
C.tg = tg;
C.qg = Yg(1:nm, :);
C.vg = Yg(nm + 1:2 * nm, :);
C.fg = fg;
C.hammer_x = Yg(end - 1, :);
C.hammer_v = Yg(end, :);
C.plate_w = phi * C.qg;
C.plate_v = phi * C.vg;
C.impulse = trapz(tg, fg);
C.peak_force = max(fg);
C.hammer_exit_v = yc(end);
C.plate_exit_v = phi * yc(nm + 1:2 * nm);
C.relative_separation_v = C.hammer_exit_v - C.plate_exit_v;
C.effective_restitution = -C.relative_separation_v / max(hm.speed, 1e-9);
C.steps = nsteps;
C.notes = {sprintf('coupled contact: %.4g us long, %.4g N peak, %.4g mN s; the hammer leaves at %.3g m/s into the plate, the plate under it at %.3g m/s (%d Runge-Kutta steps of %.3g ns)', ...
    tc * 1e6, C.peak_force, C.impulse * 1e3, C.hammer_exit_v, C.plate_exit_v, nsteps, h * 1e9)};
end

function [ynew, fnew] = rk4_step(y, h, phi, nm, omega, twozw, k, hc, mass)
k1 = plate_fem_rhs(y, phi, nm, omega, twozw, k, hc, mass);
k2 = plate_fem_rhs(y + 0.5 * h * k1, phi, nm, omega, twozw, k, hc, mass);
k3 = plate_fem_rhs(y + 0.5 * h * k2, phi, nm, omega, twozw, k, hc, mass);
k4 = plate_fem_rhs(y + h * k3, phi, nm, omega, twozw, k, hc, mass);
ynew = y + h / 6 * (k1 + 2 * k2 + 2 * k3 + k4);
fnew = plate_fem_rhs(ynew, phi, nm, omega, twozw, k, hc, mass);
end

function Y = hermite(times, h, tc, Ys, Fs, yc, fc)
% cubic Hermite interpolation of the stored trajectory at the given times in [0, tc]
n = size(Ys, 2) - 1;                 % whole steps stored
Y = zeros(size(Ys, 1), numel(times));
for j = 1:numel(times)
    tj = min(max(times(j), 0), tc);
    i = floor(tj / h);
    if i < n
        t0 = i * h;
        hh = h;
        y0 = Ys(:, i + 1); f0 = Fs(:, i + 1);
        y1 = Ys(:, i + 2); f1 = Fs(:, i + 2);
    else
        t0 = n * h;
        hh = tc - t0;
        y0 = Ys(:, n + 1); f0 = Fs(:, n + 1);
        y1 = yc; f1 = fc;
    end
    if hh <= 0
        Y(:, j) = y0;
        continue
    end
    s = (tj - t0) / hh;
    h00 = 2 * s^3 - 3 * s^2 + 1;
    h10 = s^3 - 2 * s^2 + s;
    h01 = -2 * s^3 + 3 * s^2;
    h11 = s^3 - s^2;
    Y(:, j) = h00 * y0 + h10 * hh * f0 + h01 * y1 + h11 * hh * f1;
end
end