## Copyright (C) 2023 Jens
##
## This program is free software: you can redistribute it and/or modify
## it under the terms of the GNU General Public License as published by
## the Free Software Foundation, either version 3 of the License, or
## (at your option) any later version.
##
## This program is distributed in the hope that it will be useful,
## but WITHOUT ANY WARRANTY; without even the implied warranty of
## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
## GNU General Public License for more details.
##
## You should have received a copy of the GNU General Public License
## along with this program.  If not, see <https://www.gnu.org/licenses/>.

## -*- texinfo -*-
## @deftypefn {} {@var{retval} =} vecfit (@var{input1}, @var{input2})
##
## @seealso{}
## @end deftypefn

## Author: Jens <jens@orion>
## Created: 2023-06-10

function [P1, Pa1, D] = vecfit (Pp, mask, Pt, P0, Pa0, step)

  [N_subst, N_elem] = size(Pp);

  # Create default mask
  if isempty(mask),
    mask = ones(1,N_elem);
  end

  # A-Priory state with mask
  P1 = P0 + sum(Pp.*Pa0.*mask);

  # Calc distance to target
  d = (Pt-P1);
  D = norm(d);
  dn = d/D;

  # Find best prototype
  best_i = 0;
  best_k = 10000;
  best_dir = 1;

  for i=1:N_subst,
    p = Pp(i,:);
    pn = p/norm(p);

    # compare d and p
    for vecdir=[1,-1],
      k = norm(vecdir*pn - dn);
      if k < best_k,
        best_k = k;
        best_i = i;
        best_dir = vecdir;
      end
    end
  end

  # Calc next Pa
  Pa1 = Pa0;

  # Calc P1
  P1 = P0 + sum(Pp.*Pa0);

  if best_i,
    # Get value from best prototype
    delta = best_dir*step;
    dps = delta*Pp(best_i,:);

    # Update point
    P1 += dps;

    # Update Point accu
    Pa1(best_i) += delta;
  end

endfunction