Реализация алгоритма наполнения емкости с тремя стадиями

[Григори]

Всем добрый день. Недавно подкинули интересную задачку, но которую не могу нормально реализовать.
Дано:
Аналоговый датчик уровня воды (0-6м, но я делал динамическую подгонку).
Необходимо реализовать три стадии:
1) Если опустилось ниже определенного порога 1 высоты, включается Насос 1. Если поднялось до порога 1.1 Выключить насос 1(полное заполнение)
2) Если опустилось ниже порога 2, отключить насос 1 и перейти на насос 2. Если поднялось до порога 2.1 выключить насос 2 и перейти на насос 1
3) Если опустилось ниже порога 3, работают насосы 1 и 2. Если поднялось до порога 3.1 выключить насос 1, насос 2 оставить в работе
(То есть каждая стадия имеет пороги включения и выключения)

Сам реализовывал в ФБ STшном на IFах с >= и <=, выходила полная шляпа(Стадии накладывались друг на друга)
Прикладываю проект со своими пробами, но там все уже перемешано и напеределывано 100 раз.