Almost full port of what I had in c#

Proper sense algorythm missing