Add unittest
Fix implementation