Code fixed and tests added