Discrete Control Algorithm for a Heat Storage System