% [M] = smatNi(S,X,OP,<US>,<R>,<M>)
%
% Synopsis: Tracks the features at locations X of frame 1 of sequence S
%    with user selected optimisation algorithm OP, and Update Strategy US.
% Outputs:
%   *M - Model storing state of current track.
% Inputs:
%    S  - Sequence of Images (3D cube of pixels: x*y*frames)
%    X  - Initial position of features (2,3,4,6 values for translation,
%         euclidean, similarity or affine coordinates) + 2 for patch sizes
%    OP - Optimisation Parameters (5-7 value vector, type "help warthog" for
%         detailed help)
%    US - [NotVerbose]/Verbose, <Pre-search Size>, <Pre-search density>,
%         [Thick]/Thin, [Normal]/Zero Interp. 
%    R  - Index of Parent-Child relationships between patches.
%
% Note: <> means optional, [] means default, * means both in/output.

% Author:  Nicholas Dowson
% Revised: 6 Jul 2006 - 16.34
% (c) CVSSP, University of Surrey
%---------------------------------------------------------------------

function [gM] = smatN(iSeq,ilX,iOP,iUpd,ilLnk,iM)
  
%% Initialisation
  if numel(findstr(path,'zPers/dev/zuseful'))==0
    addpath ~/zPers/dev/zuseful;
  end

  %% Constants 
  
  % Learning rate
  global gkLearnRate; gkLearnRate = 0.03;
  
  % Check params
  if nargin<4
    disp('smatNi Error: 4-6 inputs required (iSeq,ilX,iOP,gUpd,ilLnk,gM)');
    return;
  end

  % Check gUpd
  if numel(iUpd)<5
    disp(['smatNi Error: gUpd needs 5 inputs [v,gsz,gres,struct,Ttype]']);
    return;
  end
  global gUpd; gUpd = iUpd; clear iUpd;
    
  % Make shape correct
  iOP = reshape(iOP,6,[]); %Auto-error - must be divisible by 6
    
  % Pull out all X and OP values
  tlX = ilX;
  tOP = iOP;
  nMdl = 0;
  while numel(tlX) > 0,
    nMdl = nMdl + 1;
    
    % Catch errors
    if numel(tlX) < tOP(1)+2,
      disp(['smatN error: incorrect no. ilX values for warp / template '...
            'size']);
      return;
    end
    
    % Pull out X, OP and sz values
    tM.OP{nMdl}(:,1) = tOP(1:6); %Optimisation params
    tM.X{nMdl}(:,1) = tlX( 1:tOP(1) );
    tM.sz{nMdl}(:,1) = tlX( tOP(1)+[1 2] );
    
    % Remove x thats just been processed
    tlX(1:tOP(1)+2) = [];
    if size(tOP,2) > 1, tOP(:,1) = []; end %Default - keep last op
  end

  %Get correct grayscale range of sequence
  global gBin; gBin = 256 / 2^ceil(log(max(iSeq(:)))/log(2));
  
  %-------------------------------------------------------------------
  % Build the model
  if ~exist('ilLnk','var'), 
    ilLnk = [-1; -1]; 
  else
    ilLnk = [-1; -1; ilLnk(:)];
  end

  ilLnk = reshape(ilLnk,2,[]); %Auto-error numel(ilLnk) must be even

  %Check for children w more than 1 parent
  if sum( diff(sort(ilLnk(2,:)))==0 ) > 0
    disp('smatNi Error: Children with 2 or more parents found!');
    return;
  end
  
  % Add orphans to uberparent
  for eKid = 1:numel(tM.X),
    if sum(ilLnk(2,:)==eKid) == 0
      ilLnk = [ilLnk, [0; eKid] ];
    end
  end
  ilLnk(:,1) = [];
  ilLnk = ilLnk+1;
  
  clear global gM;
  global gM; 
  if exist('iM','var'),
    gM = iM; clear iM;
  else
    for cMdl = 1:numel(tM.X)+1,
      if cMdl==1 %Make the Uberparent
        gM{1}.lpMKid = ilLnk( 2, ilLnk(1,:)==cMdl );
        gM{1}.lXt0IHistory = zeros(2,1);
        gM{1}.lTmplt = 0;
      else
        gM{cMdl} = newMdl( iSeq(:,:,1), tM.X{cMdl-1}, ...
                           [ilLnk( 2, ilLnk(1,:)==cMdl )], ...
                           tM.OP{cMdl-1}, tM.sz{cMdl-1} );
      end
    end
    newShpMdl( 1 );
  end
  
  %-------------------------------------------------------------------
  %% Main algorithm
  trkStatus( iSeq(:,:,1) ); % Display 1st frame
    
  for cFrm=[2:size(iSeq,3)],
    
    fImg = iSeq(:,:,cFrm); % Get current image
    %Track all children of uberparent
    for eKid = gM{1}.lpMKid,
      TrackApp( fImg, eKid, [0 0]' );
      Update( fImg, eKid );
    end
    
    trkStatus( fImg ); % Display current frame
  end
  
  clear global gkLearnRate gBin;

%-------------------------------------------------------------------------
% Given an image, position, list of children, template size and opt params
% create a single new model (one in the big list)
function [oMdl] = newMdl( iImg, iXt0I, iliMKid, iOP, iSz )

  oMdl.OP = iOP;
  
  [fXziI fXziXt0] = XtoXzi( iXt0I ); %Get zero interp pos
  oMdl.lTmplt = wartr(iImg,fXziI,iSz); %Templates: nx*ny*nTmplt
  oMdl.lXtcXt0 = fXziXt0; %Xsz * nTmplt
  oMdl.px = [1:2]; if(oMdl.OP(1) == 6) oMdl.px = [5:6]; end

  %Get foreground and background exemplars
  [t,t1,tidx] = msrGrid( oMdl.lTmplt, iImg, fXziI, iOP );
  oMdl.lYfg = t(tidx); %List of foreground Y values
  t = t(:); t(tidx)=[];
  oMdl.lYbg = t; %List of background Y values
    
  %Current position and History
  oMdl.tcXt0I = fXziI;
  oMdl.lXt0IHistory = fXziI;
  
  %The models Children and Parents
  oMdl.lpMKid = iliMKid;%(1:end-1);
  %oMdl.lpMParent = iliMKid(end);
  
  %Create the current and waste of the model
  oMdl = newCmpt( oMdl, 1 ); %Components: nCmpt*1
  oMdl = newCmpt( oMdl, 1 ); %Waste Component: nCmpt*1
  oMdl.lCmptWt = [1 0]'; %List of component weights
  
%-------------------------------------------------------------------------
% Given the index of a model member, create the initial structure from
% the positions of the models children
function newShpMdl(p)
  global gM;
  nKid = size(gM{p}.lpMKid(:),1);
  
  if nKid>0
    gM{p}.lShp = zeros(nKid*2,1);
    gM{p}.lpShpCmpt{1} = 1;
    gM{p}.lpShpCmptRep = 1;
    gM{p}.lShpCmptWt = 1;
    for cKid = 1:nKid
      tpKid = gM{p}.lpMKid(cKid);
      if isfield( gM{p}, 'px' )
        gM{p}.lShp( cKid*2+[-1:0] ) = ...
          gM{tpKid}.lXt0IHistory(gM{tpKid}.px)...
          - gM{p}.lXt0IHistory(gM{p}.px);
      end
      newShpMdl( tpKid );
    end
  end
  
%-------------------------------------------------------------------------
%Finds the zero interpolation position and the position relative to this.
function [oXziI,oXziX] = XtoXzi( iXI )
%global gkx gky;
  px = [1:2]; if size(iXI,1)==6 px=[5:6]; end
  oXziI = zeros(size(iXI));
  oXziI(px,:) = round( iXI(px,:) );
  oXziX = oXziI - iXI;
  
%-------------------------------------------------------------------------
%Creates a new component in the global model list at index P using ipTmplts
function M = newCmpt( M, ipTmplt )
  if isfield(M,'pCmptRep')
    nCmpt = size(M.pCmptRep,1);
  else
    nCmpt = 0;
  end
  
  M.lpCmptTmplt{nCmpt+1} = ipTmplt; %Data indexed by I
  M.pCmptRep(nCmpt+1,1) = ipTmplt(1); %Representative
  M.lCmptWt(nCmpt+1,1) = 0;
    
%-------------------------------------------------------------------------
%Offset iX1 with shape (x,y) by iX2 with arb shape
function [oWarp] = XWarpX( iX1, iX2, iType )
  
  if ~exist('iType','var'), iType='norm'; end

  %Pull out translation components of iX1
  if numel(iX1) ~= 6, iX1 = iX1(1:2);
  else iX1 = iX1(5:6); end

  R = getR( iX2(:) );
  if strcmp(iType,'inv'), R = inv(R); end;
    
  oWarp = R * [iX1(:); 1];
  oWarp = oWarp(1:2);
    
%-------------------------------------------------------------------------
%Returns the 3x3 rotation matrix for a given set of parameters
function oR = getR(iX)

  oR=eye(3); 
  oR(1:2,3) = iX(1:2);
  switch numel(iX),
   case 3,
    c = cos( iX(3) ); s = sin(iX(3));
    oR(1:2,1:2) = [c s; -s c];
   case 4,
    scale = iX(4)+1; c = cos( iX(3) ); s = sin(iX(3));
    oR(1:2,1:2) = scale*[c s; -s c];
   case 6,
    iX([1 4]) = iX([1 4])+1;
    oR(1:2,1:3) = reshape(iX,2,3);
  end
  
%-------------------------------------------------------------------------
%Returns the normal probability for a scalar iX, given a list of scalars ilY
function [oP] = ProbNorm( iX, ilY )
  stdY = std(ilY(:));
  if stdY ~= 0
    meanY = mean(ilY(:));
    oP = 1/sqrt(2*pi)/stdY*exp( -0.5 * ((meanY-iX)/stdY)^2 );
  else
    oP = 1;
  end
  
%-------------------------------------------------------------------------
%TrackApps using top down approach
function TrackApp( iImg, p, iXOffset )

  global gM gUpd; %gkx gky gkWType
  
  %Initialisation
  nKid = numel(gM{p}.lpMKid);
    
  %Number of components, templates and children in current model
  nCmpt = numel(gM{p}.lpCmptTmplt);
  nTmplt = size(gM{p}.lTmplt,3);
  
  % 1. Get likelihoods of all model components-----------------------
  %Find pos after shift by parent
  fXt0I = gM{p}.tcXt0I;
  fXt0I( gM{p}.px ) = fXt0I( gM{p}.px ) + iXOffset;
  %fXt0I( gM{p}.px ) = XWarpX( fXt0I(gM{p}.px), iXOffset, 'norm' ); 
  
  %Llhd's based on warp * Llhd of each component based on recent use
  %lCmptLlhdWarp = DWarp( fXtoI, gM{p}.lXtcXt0(:,gM{p}.pCmptRep) );%!!
  lCmptLlhd = gM{p}.lCmptWt; %* lCmptLlhdWarp

  %Get top 5 components + waste  component based on Llhd
  [t lpCmptTrk] = sort(-lCmptLlhd); 
  lpCmptTrk = [lpCmptTrk(1:min(size(lpCmptTrk,1),5)); nCmpt];
  %Make sure waste component figures in the calculations
  %flCmptWt = gM{p}.lCmptWt;  flCmptWt(end+1) = flCmpt(end);
  if length(lCmptLlhd)<=5 lpCmptTrk(end+1) = nCmpt; end
  
  % 2. TrackApp using chosen components --------------------------------
  for cCmpt = [1:size(lpCmptTrk,1)]
    %Initialise
    fpCmpt = lpCmptTrk(cCmpt);
    tTmplt = gM{p}.lTmplt(:,:,gM{p}.pCmptRep(fpCmpt));

    %Get chosen templates X:I, and search in a grid around it.
    fXtcI = fXt0I + gM{p}.lXtcXt0(:,gM{p}.pCmptRep(fpCmpt));
    [tY,fXBest,tIdxBest] = msrGrid( tTmplt,iImg, fXtcI, gM{p}.OP );
    
    %Run optimisation algorithm from best X
    [fY(cCmpt),fXtcI(:,cCmpt)]=warthog( tTmplt, iImg, fXBest, gM{p}.OP );

    if nTmplt < 4 break; end
    if ProbNorm( fY(cCmpt),gM{p}.lYfg ) / ...
          ProbNorm( fY(cCmpt),gM{p}.lYbg ) > 1
      break;
    end
  end
  
  % 3. Log an update -------------------------------------------------
  %Check if new component is required
  update.newCmpt = 0;
  if fpCmpt==nCmpt
    update.newCmpt = 1;
  end

  update.Xt0I = fXtcI(:,cCmpt) - gM{p}.lXtcXt0(:,gM{p}.pCmptRep(fpCmpt));
  update.Yfg = fY(cCmpt);
  update.cCmpt = cCmpt; % For display
  tY(tIdxBest) = []; update.Ybg = tY;
  update.pCmpt = fpCmpt;

  gM{p}.update = update;
  
  % 4. Get offset and make children track-----------------------------
  fXOffset = update.Xt0I - gM{p}.lXt0IHistory(:,end);

  %fSI = zeros(gM{p}.OP(1),nKid); fSP = fSI;
  fSI =  zeros(2,nKid); 
  fSP = fSI;
  for cKid = [1:nKid]
    tpKid = gM{p}.lpMKid(cKid);

    txKidI = gM{tpKid}.lXt0IHistory(gM{tpKid}.px,end); %Get kids x
    txKidP = txKidI - gM{p}.lXt0IHistory(gM{p}.px,end); %rltv 2 self
    txKidOff = XWarpX( txKidP, fXOffset, 'norm' ) - txKidP;  %Offset it
    TrackApp( iImg, tpKid, txKidOff );
    %TrackApp( iImg, tpKid, fXOffset );
    
    fSI(:,cKid) = gM{tpKid}.update.Xt0I(gM{tpKid}.px); %Get structure
    fSP(:,cKid) = XWarpX( fSI(:,cKid), update.Xt0I, 'inv' ); %rltv2 to parent
    %fSP(:,cKid) = XWarpX(fSI(:,cKid),fXOffset, 'inv'); %rltv2 to parent(me) last time
    
    clear tpKid txKid*;
  end
  
  %Check if fits our existing model
  ShpScore=0;
  if nKid > 0 
    %Pull full structure out
    fS = reshape( fSP, [], 1 );

    [ShpScore, fS2] = TrackShp(p,fS,1); 
  end
  
  for cKid = [1:nKid]
    tpKid = gM{p}.lpMKid(cKid);
    
    if ShpScore > 1
      CorSz = fS(cKid*2+[-1:0])-fS2(cKid*2+[-1:0]);
      if (CorSz' * CorSz) < 2
        Update( iImg, tpKid );
      end
      gM{tpKid}.tcXt0I(gM{tpKid}.px) = fS2(cKid*2+[-1:0]) + ...
        update.Xt0I(gM{p}.px);
    else
      Update( iImg, tpKid );
    end    
  end
  gM{p}.tShpScore = ShpScore;
  
%-------------------------------------------------------------------------
function Update( iImg, p )
  global gkLearnRate gM gUpd;  
  
  %update position and template based on whether normal templates are
  %being used, or zero interpolation templates are being used.
  if gUpd(5)==1% ZIP?
    [fXziI fXzit0] = XtoXzi( gM{p}.update.Xt0I );
  else
    fXziI = gM{p}.update.Xt0I; 
    fXzit0 = zeros( gM{p}.OP(1),1);
  end
  
  nCmpt = size(gM{p}.pCmptRep,1);
  nTmplt = size(gM{p}.lTmplt,3);
  if gM{p}.update.newCmpt == 1
    gM{p}.lpCmptTmplt{end} = [nTmplt-2:nTmplt]; %Upgrade waste Cmpt
    gM{p} = newCmpt( gM{p}, nTmplt+1); %Mk new waste C
    gM{p}.lCmptWt(nCmpt+1,1) = 0; %Update weights
  end
  
  %Add template, fg ,bg
  fShp = size(gM{p}.lTmplt); fShp = fShp(1:2);  
  gM{p}.lTmplt(:,:,end+1) = wartr( iImg, fXziI, fShp );

  gM{p}.lXtcXt0(:,end+1) = fXzit0;
  gM{p}.lXt0IHistory(:,end+1) = gM{p}.update.Xt0I;
  gM{p}.tcXt0I = gM{p}.update.Xt0I;
  gM{p}.lYfg = [gM{p}.lYfg(:); gM{p}.update.Yfg];
  gM{p}.lYbg = [gM{p}.lYbg(:); gM{p}.update.Ybg(:)];
    
  %Update weights
  fpUpdCmpt = gM{p}.update.pCmpt;
  gM{p}.lCmptWt(fpUpdCmpt) = gM{p}.lCmptWt(fpUpdCmpt) + gkLearnRate; 
  gM{p}.lCmptWt = gM{p}.lCmptWt / sum(gM{p}.lCmptWt); %Normalise

  %Some component always references the new template
  gM{p}.lpCmptTmplt{gM{p}.update.pCmpt}(end+1) = nTmplt+1;
  %Update Waste component
  gM{p}.lpCmptTmplt{end} = nTmplt+1; 
  gM{p}.pCmptRep(end) = nTmplt+1;
    
%-------------------------------------------------------------------------
%Get mahalanobis distance matrix
function oDM = getDM_MH( iShp )
  
  nShp = size(iShp,2);
  
  fShpMn = mean(iShp,2);
  [vec,val] = eig(cov( iShp' )); val=diag(val); 
  val(find(val<=sum(val)*.01))=inf;
  oDM = abs( vec'*(iShp-repmat(fShpMn,1,nShp)))' * val.^-1;
  
%-------------------------------------------------------------------------
%Finds a matching structure component in the model and matches it,
%returning the match score and an updated shape
function [oScore, oShp] = TrackShp( p, iShp, iCorFactor )

  global gkLearnRate gM gUpd;
  
  %If thin layers are being used just return a score of 0 and the same shape
  if gUpd(4)==0
    oScore = 0;
    oShp = iShp;
    return;
  end
  
% $$$   %--------------------------------------------------------------
% $$$   %Initialisation
% $$$   tShp = gM{p}.lShp;
% $$$   nShp = size(tShp,2);
% $$$   nVecCmpts = size(tShp,1);
% $$$   oShp = iShp;
% $$$   oScore = 0;
% $$$   
% $$$   %Check if we have enough exemplars
% $$$   if nShp < 5%nVecCmpts
% $$$     %If not add the current exemplar and quit
% $$$     gM{p}.lShp(:,end+1) = iShp;
% $$$     return;
% $$$   end
% $$$   
% $$$   %Fit a PCA model to last 20 exemplars
% $$$   nExUse = min(nShp,8);
% $$$   tShp2 = tShp(:,end-nExUse+1:end);
% $$$   tShpMn = mean(tShp2,2);
% $$$   [vec,val] = eig( cov(tShp2') );
% $$$     
% $$$   %Remove all components with where cumulative power is less than 1%
% $$$   val=diag(val); val(find(val<0))=0;
% $$$   pcaRm = find( cumsum(val/sum(val))<0.01 );
% $$$   val(pcaRm) = [];
% $$$   vec(:,pcaRm) = [];
% $$$   
% $$$   %Get MH dist
% $$$   tDist = (vec'*(iShp-tShpMn))./sqrt(val);
% $$$   tMahaD= sqrt(tDist'*tDist);
% $$$   oScore = tMahaD;
% $$$   
% $$$   %Correct the output if necessary
% $$$   tDist2 = tDist;
% $$$   if tMahaD > 2
% $$$     tDist2 = tDist*2/tMahaD;
% $$$     %pfailDims = find( abs(tDist)>iCorFactor );
% $$$     %tDist2( pfailDims ) = 0;%sign(tDist2( pfailDims ))*iCorFactor;
% $$$   end
% $$$ 
% $$$   oShp = vec * (tDist2.*val) + tShpMn;
% $$$   
% $$$   gM{p}.lShp(:,end+1) = iShp;
% $$$   return;
  %===================================================================
  %Don't use the code below
  
  %Initialisation
  nCmpt = length(gM{p}.lpShpCmpt);
  nShp = size(gM{p}.lShp,2);
  oShp = iShp;
    
  %Get weighting order
  [t plOrdCmpt] = sort(-gM{p}.lShpCmptWt);
  if nShp > 4
    tBroke = 0;
    %Check which component to add it to in order of weight
    for cCmpt = 1:length(plOrdCmpt)
      tCmpt = plOrdCmpt(cCmpt);
      tlShp{cCmpt} = gM{p}.lShp(:,gM{p}.lpShpCmpt{tCmpt});
      tShpMn = mean(tlShp{cCmpt},2);
      %Eigenvectors and vals - w.t low values infed for stability
      [vec,val] = eig(cov( tlShp{cCmpt}' )); 
      val=diag(val); val(find(val<0))=0;
      %Remove compoents with cumulative power below 1%
      %val(find(val<=sum(val)*.01))=inf;
      pcaRm = find( cumsum(val/sum(val))<0.01 );
      val(pcaRm) = [];
      vec(:,pcaRm) = [];
      
      %Mahalanobis distance
      fDistMH(cCmpt) = abs((vec' * (iShp-tShpMn))' * val.^-1);
      if fDistMH(cCmpt) <= 1.0
        oScore = fDistMH(cCmpt);
        tBroke=1; break; 
      end
    end
  else
    tBroke=1; oScore = 0;
    tCmpt = 1; cCmpt = 1;
    tlShp{1} = gM{p}.lShp;
  end
  
  %If theshold never met then
  if tBroke==0
    %Get a whitened value from best matching result
    [oScore cCmpt] = min(fDistMH);
    
    tShpMn = mean(tlShp{cCmpt},2);
    [vec,val] = eig(cov( tlShp{cCmpt}' )); 
    val=diag(val);
    %val(find(val<=sum(val)*.01))=0; %zero low vals for stability
    pcaRm = find( cumsum(val/sum(val))<0.01 );
    val(pcaRm) = [];
    vec(:,pcaRm) = [];
    
    tShpWht = vec*(min(abs(vec'*(iShp-tShpMn)), val*1.0 ) .* ...
                   sign(   vec'*(iShp-tShpMn)) );
    oShp = tShpWht + tShpMn;
    
    %Create a new component
    gM{p}.lpShpCmpt{nCmpt+1} = [nShp-2:nShp];
    gM{p}.lShpCmptWt(nCmpt+1) = 0;
    gM{p}.lpShpCmptRep(nCmpt+1) = 1;
    tCmpt = nCmpt+1;
    cCmpt=cCmpt+1;
    
    %Get new tlShp
    tlShp{cCmpt} = gM{p}.lShp(:,gM{p}.lpShpCmpt{nCmpt+1});
  end
  
  gM{p}.lpShpCmpt{tCmpt}(end+1) = nShp+1; %Always update some component
  
  %Get representative
  fDM =getDM_MH([tlShp{cCmpt} iShp]);%Get distance matrix for current compt
  [t iRep] = min((fDM)); %Choose median
  gM{p}.lpShpCmptRep(tCmpt) = gM{p}.lpShpCmpt{tCmpt}(iRep); %Assign it

  %Update weights
  gM{p}.lShpCmptWt(tCmpt) = gM{p}.lShpCmptWt(tCmpt) + 0.01;%gkLearnRate;
  gM{p}.lShpCmptWt = gM{p}.lShpCmptWt / sum(gM{p}.lShpCmptWt);
  
  gM{p}.lShp(:,nShp+1) = iShp; %Add new shape to list
    
% $$$ %---nTmplt+1;------------------------------------------------------------------
% $$$ %Update threshold and representative of a component
% $$$ function gMdl = updThr( gMdl, iiCmpt )
% $$$     global gOP;
% $$$   szCmpt = size(ioMdl.lpCmptTmplt{iiCmpt},1);
% $$$   dtCmpt = reshape( ioMdl.lTmplt(:,:,ioMdl.lpCmptTmplt{iiCmpt}), [], szCmpt );
% $$$   fCntr = floor(size(dtCmpt,1)/2);
% $$$   corr = zeros( szCmpt );
% $$$   %Get correlation matrix
% $$$   for cEx = 1:1%szCmpt
% $$$     t = warts( dtCmpt(:,cEx), dtCmpt(:,cEx:szCmpt), ...
% $$$                   [repmat(fCntr,1,szCmpt-cEx+1); [1:szCmpt-cEx+1]-1], ...
% $$$                   gOP ); %Parameters
% $$$     corr(cEx:szCmpt,cEx) = t(:); 
% $$$     corr(cEx,cEx:szCmpt) = t(:)';
% $$$   end
% $$$   %Choose representative
% $$$   %[t, ioMdl.rep{C}] = max(sum(corr));
% $$$   ioMdl.CiRep(iiCmpt) = 1;
% $$$   %Get threshold
% $$$   ioMdl.thr{iiCmpt} = corr(ioMdl.CiRep(iiCmpt),ioMdl.CiRep(iiCmpt)) + ...
% $$$       2.0*std(corr(:,ioMdl.CiRep(iiCmpt)));
% $$$   %ioMdl.thr{iiCmpt} = -0.2*measMI( D(:,:,ioMdl.idx{iiCmpt}(1)), D(:,:,ioMdl.idx{iiCmpt}(1)), 16 );

%---------------------------------------------------------------------
% Constructs a grid of points around iX based on iXv and measures the values
% similarity values for them
function [oYall,oXylo,oIdxLo] = msrGrid(iT,iI,iX,OP,iXv)
  
  global gUpd;
  
  [gx gy] = ndgrid( -gUpd(2):gUpd(3):gUpd(2) ); %Generate grid
    
  if numel(gx)==0 %Check grid is not []
    oXylo = iX;
    oYall = warts(iT,iT,size(iT)/2,OP);
    oIdxLo = 1;
    return;
  end
  
  fX = repmat( iX(:), [1 numel(gx)] );
  if OP(1) == 6, %Affine warps
    fX(5,:) = iX(5)+gx(:)';
    fX(6,:) = iX(6)+gy(:)';
  else %Other warps
    fX(1,:) = iX(1)+gx(:)';
    fX(2,:) = iX(2)+gy(:)';
  end

  [oYall] = warts( iT, iI, fX, OP ); %Measure values on grid
  
  [t oIdxLo] = min(oYall);
  oXylo = fX(:,oIdxLo(1));
  
%---------------------------------------------------------------------
% Shows the status of a tracked region in an image
function trkStatus( iI )

  global gBin gM gUpd;
  
  %Only display if verbose
  if gUpd(1) == 0, return; end
    
  % Draw the main picture
  oFig = figure(2);
  clf;  
  image( iI*gBin );
  colormap(gray(256));
  hold on;
  axis off;

  % Resize the picture to a reasonable size, with same aspect ratio
  fSz = fliplr(size(iI)); 
  fSz(1) = fSz(1)*2;
  t = min(ceil( [960 360]./fSz )) * fSz;
  fSz(1) = fSz(1)/2;
  %t = min(ceil( [480 360]./fSz )) * fSz;
  t1 = get(gcf,'Position');
  set(gcf,'Position',[t1(1:2) t]);
  set(gca,'Position',[0 0 1 1]);
  set(gca,'XLim',[0 fSz(1)*2]);
  
  % What is the frame number
  fFrm = size( gM{2}.lXt0IHistory, 2 );
  
  % Parameters
  fOP = gM{2}.OP;
  
  % What measurement is being used?
  switch fOP(2),
   case 1, tMsr = 'SSD';
   case 2, tMsr = 'NC';
   case 3, tMsr = 'MI-std';
   case 4, tMsr = 'MI-pve';
   case 5, tMsr = 'MI-ipz';
   case 6, tMsr = 'MI-npw';
  end
  th = text( 2,10, ['Msr: ' tMsr]);
  set( th,'HorizontalAlignment','Left','VerticalAlignment','Top', ...
          'BackgroundColor',[1 1 1]);   
  
  % Thick or Thin
  switch gUpd(4)
   case 0, tThinThick = 'Thin';
   case 1, tThinThick = 'Thick';    
  end
  % ZIP / Norm
  switch gUpd(5)
   case 0, tNormZip = 'Norm';
   case 1, tNormZip = 'Zip';    
  end
  
  %% Write frame number
  th = text( 2,2, ['m = ' num2str(fFrm)] );
  set( th,'HorizontalAlignment','Left','VerticalAlignment','Top', ...
          'BackgroundColor',[1 1 1]); 
  
  %% Draw positions
  for l=2:numel(gM) %%!!
    % Translation data
    tx = 2; ty = 1;
    if gM{l}.OP(1) == 6, tx=6; ty=5; end;
    
    t = ( size(gM{l}.lTmplt)-1 )/2; t=t(1:2); %Template size
    fRect1 = [ -t(1) t(1) t(1) -t(1); -t(2) -t(2) t(2) t(2); 1 1 1 1 ];

    % Current position
    fX = gM{l}.tcXt0I;
    W = getR(fX);
       
    fRect = W(1:2,:)*fRect1;
    patch( fRect(2,:),fRect(1,:),[0 1 0],'FaceAlpha',0.1,'EdgeColor',[0 1 0]);
    plot(fX(tx),fX(ty),'g+');
    
    % Previous position
    %fXp = gM{l}.lXt0IHistory(:,end);
    %plot(fXp(tx),fXp(ty),'rx');
    
    % Children
    for eKid = gM{1}.lpMKid,
      fXkid= gM{eKid}.tcXt0I;
      txk = 2; tyk = 1; if gM{eKid}.OP(1) == 6, txk=6; tyk=5; end;
      line( [fX(tx) fXkid(txk)], [fX(ty) fXkid(tyk)],...
            'Color',[0 1 0]);
    end
  end
  
  %% Draw the models
  rectangle('Position',[fSz(1) 0 fSz],'FaceColor',[0 0 0]);
  th = 0;
  for l=2:numel(gM)
    % Templates 
    t = gM{l}.lTmplt(:,:,gM{l}.pCmptRep(1:end-1));
    fTmpSz = size(t); if numel(fTmpSz)==2, fTmpSz(3)=1; end;
    t = reshape( t, [fTmpSz(1) fTmpSz(2)*fTmpSz(3)] )*gBin;
    image(fSz(1)+.5,th+.5,t);
    
    % Get Parents
    for l2=1:numel(gM),
      if sum(gM{l2}.lpMKid==l)>0, fParent = l2; end
    end
    
    %Label the features
    tColor = [0 .4 0];
    if fParent==1, tColor = [0 0 .5]; end;
    t = text(fSz(1)+.5,th+.5,[num2str(l)],'Color',[0 1 0]);
    set( t,'HorizontalAlignment','Left','VerticalAlignment','Top', ...
           'BackgroundColor',tColor); 
    t = text(fSz(1)+fX(tx),fX(ty),[num2str(l)],'Color',[0 1 0]);
    set( t,'HorizontalAlignment','Left','VerticalAlignment','Top', ...
           'BackgroundColor',tColor); 

    % Trace of scores    
    ty = gM{l}.lYfg(:);
    ty = ty / max([1; abs(ty)]);
    tmy = fTmpSz(2);
    
    tyScl = max(1,size(ty,2)/size(iI,2));
    tx = [1:numel(ty)]/tyScl;
    
    if ty(end)<0, tmy=-tmy; end
    %plot( tx+fSz(1), 1+tmy*ty+th, 'k-', 'LineWidth', 3 );
    plot( tx+fSz(1), 1+tmy*ty+th, 'y-' );

    % Weights
    ty = (gM{l}.lCmptWt(1:end-1)); %* (fTmpSz(2)-2);
    tx = ((1:numel(ty))-.5) * fTmpSz(1);
    %plot( tx+fSz(1), 1+tmy*ty+th+fTmpSz(2), 'k.-', 'LineWidth', 3 );
    plot( tx+fSz(1), 1+tmy*ty+th+fTmpSz(2), 'm.-' );

    % Which component was last used
    if isfield(gM{l},'update')
      t = gM{l}.update.cCmpt;
      plot( (t-.5)*fTmpSz(1)+fSz(1),th+fTmpSz(2)/2,'r+');
    end

    % Structure
    fX = gM{l}.tcXt0I;
    tx = 2; ty = 1; %Get Translation data
    if gM{l}.OP(1) == 6, tx=6; ty=5; end;
    
    % Draw region
    t = ( size(gM{l}.lTmplt)-1 )/2; t=t(1:2); %Template size
    fRect1 = [ -t(1) t(1) t(1) -t(1); -t(2) -t(2) t(2) t(2); 1 1 1 1 ];

    % Current position
    fX = gM{l}.tcXt0I;
    W = getR(fX);
    fRect = W(1:2,:)*fRect1;
    patch( fRect(2,:)+fSz(1),fRect(1,:),[0 1 0],'FaceAlpha',0.1,'EdgeColor',[0 1 0]);

    %Shape components
    if isfield(gM{l},'lpShpCmpt'),
      for l2=1:numel(gM{l}.lpShpCmpt),
        t = gM{l}.lShp( :, gM{l}.lpShpCmpt{l2} );
        tmu = mean(t,2);
        line( fX(tx)+[0 tmu(tx)]+fSz(1), fX(ty)+[0 tmu(ty)],...
              'Color',[0.3 gM{l}.lShpCmptWt(l2) .2]);
      end
    end
    
    for eKid = gM{l}.lpMKid,
      fXkid= gM{eKid}.tcXt0I;
      txk = 2; tyk = 1; if gM{eKid}.OP(1) == 6, txk=6; tyk=5; end;
      line( [fX(tx) fXkid(txk)]+fSz(1), [fX(ty) fXkid(tyk)],...
            'Color',[0 1 0]);      
    end
    
    %Iterate
    th = th + fTmpSz(2);
  end
        
  %% Printing stuff
  if gUpd(1)==2
    if fFrm<10, fPad = '000';
    elseif fFrm<100, fPad = '00';
    elseif fFrm<1000, fPad = '0';
    else fPad = '';
    end
    stdFontSz(gcf,20);
    set(gcf, 'PaperPositionMode', 'auto');
    set(gcf,'PaperUnits','points');
    t = [480 360]*2;
    set(gcf,'PaperPosition',[0 0 t]);
    set(gcf,'PaperSize',[t]);
    print('-dpng','-r72',['vid/smat' tMsr tThickThin tNormZip ... 
                        fPad num2str(fFrm) '.png']);
  end  
  
  hold off;
  pause(0.01);