Improved mode selection algorithm
Fixed compilation issue