- if(!blocked && rsv_dist<brake_dist+margin)
- {
- blocked = true;
- next_ctrl->set_control("speed", 0);
- }
- else if((!approach && rsv_dist<brake_dist*1.3+approach_margin) || (blocked && rsv_dist>brake_dist+margin*2))
+ State old_state = state;
+
+ if(state==FOLLOW && !train.get_preceding_train())
+ state = NORMAL;
+
+ if(rsv_dist<brake_dist+margin)
+ state = BLOCKED;
+ else if(rsv_dist>brake_dist+margin*2 && rsv_dist<brake_dist*1.3+approach_margin)
+ state = APPROACH;
+ else if(rsv_dist>brake_dist*1.3+approach_margin*2)
+ state = NORMAL;
+
+ if(state==NORMAL && train.get_preceding_train())
+ state = FOLLOW;
+
+ if(state!=old_state || state==FOLLOW)