Add majorize function with tests