Der Code soll mit dem expliziten Casting damit eben auch auf 8bit Arduinos lauffähig sein.

Teste einfach meinen Testcode, in den du deine Algorithmen einsetzt, du bekommst dann ja über Serial die Debug-Informationen zur Kontrolle.