Add tests and fix color mapping bug