## Copyright (C) 2018 Jens Ahrensfeld
## 
## 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} =} motor_eval (@var{input1}, @var{input2})
##
## @seealso{}
## @end deftypefn

## Author: Jens Ahrensfeld <ahrensfeld@w2ess001vm>
## Created: 2018-08-01

function cnt_expected = motor_eval (cnt_tooth, cnt, round_mode)

cnt_expected = 0

cnt_err = mod(abs(cnt),cnt_tooth);
if (cnt_err >= 0.5*cnt_tooth)
  cnt_err = cnt_err - cnt_tooth;
end

if strcmpi(round_mode, 'nearest')
	cnt_expected = cnt - sign(cnt)*cnt_err;

elseif strcmpi(round_mode, 'positive')
	cnt_expected = cnt - cnt_err 
	if (cnt < 0)
		cnt_expected = cnt - cnt_err + cnt_tooth
	end

elseif strcmpi(round_mode, 'negative')
	cnt_expected = cnt - cnt_err

end

err = cnt - cnt_expected

endfunction