Rewrite and add tests