% affect_fusion_demo.m -- two-timescale PAD estimation, one synthetic trial
% Transcribed from affect_sim.py. NOT EXECUTED (MATLAB unavailable).
rng(1); dt = 0.1; N = 1200; theta = 0.5; sw = 0.15; qmu = 0.02;
a = 1 - theta*dt; protos = [0.6 0.4 0.3; 0.5 -0.4 0.2; -0.5 0.5 -0.3; ...
                            -0.5 0.5 0.3; -0.5 -0.4 -0.3; 0 0 0];
% --- synthetic truth: mood jumps every 10-20 s, emotion reverts to mood
mu = zeros(N,3); k = 1;
while k <= N
    L = round((10 + 10*rand)/dt); mu(k:min(N,k+L-1),:) = repmat(protos(randi(6),:), min(L,N-k+1), 1); k = k + L;
end
x = zeros(N,3); xk = mu(1,:)';
for k = 1:N
    xk = a*xk + (1-a)*mu(k,:)' + sqrt(dt)*sw*randn(3,1); x(k,:) = xk';
end
% --- observation models (assumed): physio, face, voice
H = {[0 0.9 0], [1 0 0; 0 0.4 0], [0 0.6 0; 0 0 0.7]};
sig = {0.30, [0.25 0.35], [0.35 0.40]}; pdrop = 0.2; pout = 0.05; burst = 10;
Y = cell(1,3); av = cell(1,3);
for m = 1:3
    Y{m} = x*H{m}' + randn(N, numel(sig{m})).*sig{m};
    o = rand(N,1) < pout; Y{m}(o,:) = Y{m}(o,:) + 1.5*randn(nnz(o), numel(sig{m}));
    qe = 1/burst; qs = qe*pdrop/(1-pdrop); miss = rand < pdrop; av{m} = true(N,1);
    for k = 1:N
        if miss, miss = rand > qe; else, miss = rand < qs; end
        av{m}(k) = ~miss;
    end
end
% --- gated sequential Kalman filter on s = [x; mu]
F = [a*eye(3) (1-a)*eye(3); zeros(3) eye(3)];
Q = blkdiag(dt*sw^2*eye(3), qmu^2*eye(3));
s = zeros(6,1); P = 0.3*eye(6); est = zeros(N,6); alpha = 0.01;
for k = 1:N
    s = F*s; P = F*P*F' + Q;
    for m = 1:3
        if ~av{m}(k), continue; end
        Hm = [H{m} zeros(size(H{m},1),3)]; Rm = diag(sig{m}.^2);
        nu = Y{m}(k,:)' - Hm*s; S = Hm*P*Hm' + Rm;
        if nu'*(S\nu) > chi2inv(1-alpha, size(Hm,1)), continue; end   % gate
        K = P*Hm'/S; s = s + K*nu; IKH = eye(6) - K*Hm;
        P = IKH*P*IKH' + K*Rm*K';                                    % Joseph form
    end
    est(k,:) = s';
end
b = 51; rmse = sqrt(mean((est(b:end,1:3) - x(b:end,:)).^2));
fprintf('RMSE V=%.3f A=%.3f D=%.3f overall=%.3f\n', rmse, sqrt(mean(rmse.^2)));
t = (0:N-1)*dt; lab = {'Valence','Arousal','Dominance'};
figure('Color','w');
for i = 1:3
    subplot(3,1,i); plot(t, x(:,i), 'k', t, est(:,i), 'b'); ylabel(lab{i}); grid on; ylim([-1.2 1.1]);
end
xlabel('Time (s)'); legend('True (synthetic)','Gated fused KF');