[Test] Unit tests in util package

DataStructure and Math util Unit tests.
[Update] Slight modification to utility

Minor bugs fixed while writing unit tests
Main