Full implementation with quite some C optimization