Initial code ported from java
Complete implementation without debugging
Fix compilation error