#
# Copyright 2022 Philipp Schönberger - mail@phschoen.de
#
# 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.
#
# See <http://www.gnu.org/licenses/>.
function [p1,m1] = find_next_switch_pos(start_pos,X,Y,Z,switch_offset)
  D = [];
  for i = [1:length(X)]
    dist = euklid_dist(start_pos,[X(i),Y(i),Z(i)]);
    D = [D, dist];
  end
  D2=D;
  start_idx = local_minimas(abs(D));
  D(start_idx)=0;

  for i = [start_idx+1:length(X)-1]
    dist = euklid_dist([X(i+1),Y(i+1),Z(i+1)],[X(i),Y(i),Z(i)]);
    D(i+1)= D(i) + dist;
  end

  for i = [start_idx-1:-1:2]
    dist = euklid_dist([X(i-1),Y(i-1),Z(i-1)],[X(i),Y(i),Z(i)]);
    D(i-1)= D(i) + dist;
  end

  #plot (D);
  minimas = local_minimas(abs(D - switch_offset));
  p1 = minimas(1);
  m1 = minimas(2);

end