perch_center.m

function [pos, method, m1_pos] = perch_center(points, labels, idxs, template)
%PERCH_CENTER  Per-trial perch centre (MARKER 1), reconstructed when occluded.
%
%   [pos, method, m1_pos] = PERCH_CENTER(points, labels, idxs, template)
%
% The perch MOVES between trials, but its markers form a RIGID body whose
% internal geometry is fixed within a session. So marker 1 is located for THIS
% trial: used directly if tracked, otherwise reconstructed from the other perch
% markers visible this trial via the session rigid template (see
% resolve_session_perches).
%
% INPUT
%   points   - 3 x nMarkers x nFrames (ezc3d), mm.
%   labels   - cell array of POINT labels.
%   idxs     - this perch's marker indices (from find_perch_indices).
%   template - session rigid template for this perch (from
%              resolve_session_perches): struct with
%                .labels (cellstr), .pos (Nx3, mm, one consistent frame),
%                .m1lab  (label of marker 1). [] = none.
%
% OUTPUT
%   pos     - 1x3 perch centre = marker 1 for this trial (mm), or [].
%   method  - how pos was obtained.
%   m1_pos  - this trial's DIRECTLY-OBSERVED marker 1 (mm), or [] if occluded.

    pos = []; method = 'none'; m1_pos = [];
    if nargin < 4, template = []; end
    if isempty(idxs), return; end

    [obs_lab, obs_pos, m1lab] = perch_marker_means(points, labels, idxs);

    % --- (1) marker 1 observed this trial -> use it directly ---
    if ~isempty(m1lab)
        hit = strcmp(obs_lab, m1lab);
        if any(hit)
            m1_pos = obs_pos(find(hit,1), :);
            pos = m1_pos; method = 'marker1 (this trial)';
            return;
        end
    end

    % --- (2) marker 1 occluded -> reconstruct from the rigid template ---
    if is_valid_template(template) && any(strcmp(template.labels, template.m1lab))
        tlab = template.labels;
        tpos = template.pos;
        tm1  = tpos(strcmp(tlab, template.m1lab), :);

        % markers common to this trial and the template (excludes marker 1,
        % which is not in obs here), matched by label
        [common, ia, ib] = intersect(obs_lab, tlab, 'stable');   % ia->obs, ib->tpos
        common = common(~strcmp(common, template.m1lab));         % safety
        keepA = ia(~strcmp(obs_lab(ia), template.m1lab));
        keepB = ib(~strcmp(tlab(ib),  template.m1lab));
        Aobs = obs_pos(keepA, :);      % observed positions this trial
        Btpl = tpos(keepB, :);         % same markers in the template

        n = size(Aobs,1);
        if n >= 3 && rank(Btpl - mean(Btpl,1)) >= 2
            % full rigid fit template -> observed, then map template marker 1
            [R,t] = kabsch(Btpl, Aobs);
            pos = (R*tm1' + t)';
            method = 'marker1 (rigid-fit from template)';
        elseif n >= 1
            % too few / collinear for rotation -> translation only (offset)
            pos = mean(Aobs,1) + (tm1 - mean(Btpl,1));
            method = 'marker1 (template offset)';
        end
        if ~isempty(pos), return; end
    end

    % --- (3) last resort: centroid of this trial's visible perch markers ---
    if ~isempty(obs_pos)
        pos = mean(obs_pos, 1);
        method = 'centroid (no marker 1, no template)';
    end
end

% ------------------------------------------------------------------------
function tf = is_valid_template(t)
    tf = ~isempty(t) && isstruct(t) && all(isfield(t,{'labels','pos','m1lab'})) ...
         && ~isempty(t.labels) && ~isempty(t.m1lab);
end

function [R,t] = kabsch(A, B)
% Least-squares rigid transform (R,t) with B ~= R*A + t. A,B are N x 3.
    cA = mean(A,1); cB = mean(B,1);
    H  = (A - cA)' * (B - cB);
    [U,~,V] = svd(H);
    d = sign(det(V*U'));
    R = V * diag([1 1 d]) * U';
    t = cB' - R*cA';
end