function action = WP_plan_find_floor
%

FIND_WPS = 7;

action = [0,0,0];
persistent state

if isempty(state)
    state = 0;
end

switch state
    case 0
        action = [1,0,0];
        state = 1;
        return
    case 1
        indexes = WP_beliefs(FIND_WPS,0,[],0);
        floor_index = WP_find_floor_WP(indexes);
        found = ~isempty(floor_index);
        if found==1
            action = [0,0,0];
            WP_beliefs(2,9,'info',floor_index);
            WP_beliefs(2,9,'uncertainty',...
                WP_beliefs(1,floor_index,'uncertainty',0));
            WP_beliefs(2,12,'info',1);
        else
            action = [0,1,-1];
            state = 2;
        end
    case 2
        action = [1,0,0];
        state = 3;
    case 3
        indexes = WP_beliefs(FIND_WPS,0,[],0);
        floor_index = WP_find_floor_WP(indexes);
        found = ~isempty(floor_index);
        if found==1
            action = [0,0,0];
            WP_beliefs(2,9,'info',floor_index);
            WP_beliefs(2,9,'uncertainty',...
                WP_beliefs(1,floor_index,'uncertainty',0));
            WP_beliefs(2,12,'info',1);
        else
            action = [0,1,-1];
            state = 4;
        end
    case 4
        action = [1,0,0];
        state = 5;
    case 5
        indexes = WP_beliefs(FIND_WPS,0,[],0);
        floor_index = WP_find_floor_WP(indexes);
        found = ~isempty(floor_index);
        if found==1
            action = [0,0,0];
            WP_beliefs(2,9,'info',floor_index);
            WP_beliefs(2,9,'uncertainty',...
                WP_beliefs(1,floor_index,'uncertainty',0));
            WP_beliefs(2,12,'info',1);
        else
            action = [0,0,0];
            state = 6;
        end
    case 6
        WP_beliefs(2,12,'info',1);
end
