Predictive Control Algorithm for Efficient Maneuver of Autonomous Vehicles in Mixed Traffic