Reworked all algorithms according to new state diagramm.