## 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, dPs] = vecfit (Pt, P0, Pp, step)

  d = Pt-P0;
  D = norm(d)
  dn = d/D;

#  P1 = P0 + sum(Pp0)

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

  for i=1:length(Pp),
    p = Pp(i,:);
    pn = p/norm(p);
    # compare d and p
    k = norm(pn - dn)
    if k < best_k,
      best_k = k;
      best_i = i;
    end
  end

  best_i
  best_k

  P1 = P0;
  Ps1 = Pp;

  if best_i,
    ps_best = Pp(best_i,:);
    dps_best = step*ps_best;
    Ps1(best_i,:) = Pp(best_i,:) + dps_best;
    P1 = P0 + dps_best;
  end
  dPs = Ps1 - Pp;

endfunction