## Copyright (C) 2016 Nicklas SB Karlsson
## 
## 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 <http://www.gnu.org/licenses/>.

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

## Author: Nicklas SB Karlsson <karlnick@Utveckling2>
## Created: 2016-04-30

function stepper()
  t = linspace(0, 1, 1001);
  x = 650*sin(2*pi*t);
  y = cos(2*pi*t);

  tFlanks = [];
  for n = 1:length(x)-1
    steps = setPosition(t(n), x(n));
    tFlanks = [tFlanks steps];
  endfor

  len = length(tFlanks);
  tSquared(2*(1:len-1)) = tFlanks(1:len-1);
  tSquared(2*(1:len)-1) = tFlanks(1:len);

  pulse = 1;
  for n = 1:2*len-1
    if mod(pulse, 4) < 2
      pulses(n) = 0;
    else
      pulses(n) = 1;
    endif
    pulse = pulse + 1;
  endfor

  for n = 1:length(t)
    if mod(pulse, 2) < 1
      tPulses(n) = 0;
    else
      tPulses(n) = 1;
    endif
    pulse = pulse + 1;
  endfor

  figure
  plot(tSquared, pulses, t, 0.25 + 0.5*tPulses)
  for n = 1:length(x)-1
    %text(t(n), 0.9, sprintf("t=%f\nx=%f", t(n), x(n)))
  endfor
endfunction

# Generate one toggle at each integer position change. The algorithm
# should be of general use then intepolating periodically received
# positions. For steppers a square wave pulses without jitter around
# the periodicity could be generated.
function tFlanks = setPosition(t, x)
  persistent xOld = 0;
  xDelta = abs(x - xOld);
  pulses = abs(round(x) - round(xOld));

  if(pulses < 1)
    tFlanks = [];
  else
    period = 1e-3/xDelta;			% in (ms)
    if(x > xOld)
      x0 = period*(mod(1/2 - xOld, 1));		% First flank
    else
      x0 = period*(mod(xOld - 1/2, 1));		% First flank
    endif
    for n = 1:pulses				% For all pulses within this period
      tFlanks(n) = t + x0 + (n - 1)*period;
    endfor
  endif

  xOld = x;
endfunction

