Clean up and small bug fix + extended test scenario