Code rewrite and bug fixes