Code reorganization and tests added