First working C version of pii.A