Алгоритм вычисления позиции

У меня 16-битный энкодер. По нему я знаю позицию мотора.
Проблема когда счетчик энкодера делает rollover.
Допустим энкодер считает вверх - первое чтение энкодера 65530, второе 10 – то есть произошел rollover и тогда позиция = 65535 + 10.
Энкодер считает вниз - первое чтение энкодера 10, второе 65530 - и тогда позиция = (65535-65530) + 10.
Но у меня почему то не получается построить правильный алгоритм. При смене направления UP-DOWN, DOWN-U что то ломается.

Вот пример симуляции в шарпе

UInt16 encoder_counter;
UInt16 prev_encoder_counter;
Int32 position;

 private void buttonEncUp_Click(object sender, EventArgs e)
        {
            encoder_counter += (UInt16)numericUpDownEncoderStep.Value;
            numericUpDowneEncoder.Value = encoder_counter;

            CalculatePosition();
        }

        private void buttonEncDown_Click(object sender, EventArgs e)
        {
            encoder_counter -= (UInt16)numericUpDownEncoderStep.Value;
            numericUpDowneEncoder.Value = encoder_counter;

            CalculatePosition();
        }

        void CalculatePosition()
        {
            if (comboBoxDirection.SelectedIndex == 0)  //UP
            {
                //overflow up - 0xFFFF -> 0
                if (encoder_counter < prev_encoder_counter)
                    position = 0xFFFF + encoder_counter;
                else
                    position = encoder_counter;
            }
            else if (comboBoxDirection.SelectedIndex == 1) //DOWN
            {
                //overflow down - 0 -> 0xFFFF 
                if (encoder_counter > prev_encoder_counter)
                    position = encoder_counter - 0xFFFF;
                else
                    position = encoder_counter;
            }

            prev_encoder_counter = encoder_counter;

            textBoxPosition.Text = position.ToString();
        }

Вроде мне удалось найти правильный алгоритм

        UInt16 encoder_counter;
        UInt16 prev_encoder_counter;
        Int32 position;
        Int32 rounds;
        UInt32 direction;

        private void buttonEncUp_Click(object sender, EventArgs e)
        {
            direction = 1;

            encoder_counter += (UInt16)numericUpDownEncoderStep.Value;
            numericUpDowneEncoder.Value = encoder_counter;

            GetEncoder();
       }

        private void buttonEncDown_Click(object sender, EventArgs e)
        {
            direction = 2;

            encoder_counter -= (UInt16)numericUpDownEncoderStep.Value;
            numericUpDowneEncoder.Value = encoder_counter;

            GetEncoder();
        }

        void GetEncoder()
        {
            if (Math.Abs(encoder_counter- prev_encoder_counter)>40000) //rollover
            {
                if (encoder_counter < prev_encoder_counter)
                    rounds++;
                else
                    rounds--;    
            }
            position = (Int32)((rounds * 65535) + encoder_counter);

            prev_encoder_counter = encoder_counter;

            textBoxPosition.Text = position.ToString();
        }