Predictive Control of Heat Pump and Thermal Energy Storage for Flexible Heating