Revised C implementation code - still not complete