function val = WP_beliefs(action,index,field,val_in)
% WP_beliefs - belief storage and access function
% On input:
%     action (int): action on store
%       1: GET - gets a field
%       2: SET - sets a field
%       3: SAVE - saves beliefs to specified file
%       4: MERGE - merges new belief with existing beliefs
%       5: MATCH - match info field
%       6: WP - get all WP indexes
%       7: DUMP - return beliefs
%       8: COUNT - return num_beliefs
%     index (int): index of belief (used for goals with known indexes)
%     field (string): name of belief field of interest
%        'uncertainty': variance (noise) of field
%        'info': has data structure providing description of belief
%     val_in (varies): depends on field to be set
% On output:
%     val (varies): depend on field demanded
% Call:
%     val = WP_beliefs(1,2,'uncertainty',0);  % gets pose uncertainty
% Author:
%     T. Henderson
%     UU
%     Spring 2016
%

% belief storage actions
NOOP = 0;
GET = 1;
SET = 2;
SAVE = 3;
MERGE = 4;
MATCH = 5;
CLEAR_UNCERTAIN_WP = 6;
FIND_WPS = 7;
DUMP = 8;
COUNT = 9;

% belief types
POSE = 1;
VECTOR = 2;
ACTION = 3;
WP = 4;
FACT = 5;
IMAGE = 6;

PLANE = 2;
MIN_PARAM_DIR = 0.9;
MIN_PARAM_DIST = 0.5;

persistent beliefs num_beliefs

if isempty(beliefs)

    % Set Basic Goal Beliefs
    beliefs(1).name = 'Robot Pose';
    beliefs(1).id = 1;
    beliefs(1).type = POSE;
    beliefs(1).info = eye(4,4);
    beliefs(1).uncertainty = Inf*eye(3,3);
    beliefs(1).children = 0;
    beliefs(1).parent = 0;

    beliefs(2).name = 'Gravity Vector';
    beliefs(2).id = 2;
    beliefs(2).type = VECTOR;
    beliefs(2).info = [0;0;-1];
    beliefs(2).uncertainty = [Inf,Inf,Inf];
    beliefs(2).children = 0;
    beliefs(2).parent = 0;

    beliefs(3).name = 'Find WP';
    beliefs(3).id = 3;
    beliefs(3).type = ACTION;
    beliefs(3).info = 0;    % 0: not active, 1: active
    beliefs(3).uncertainty = 0;
    beliefs(3).children = 0;
    beliefs(3).parent = 0;

    beliefs(4).name = 'Extend WP';
    beliefs(4).id = 4;
    beliefs(4).type = ACTION;
    beliefs(4).info = 0;    % 0: not active, 1: active
    beliefs(4).uncertainty = 0;
    beliefs(4).children = 0;
    beliefs(4).parent = 0;

    beliefs(5).name = 'Combine WPs';
    beliefs(5).id = 5;
    beliefs(5).type = ACTION;
    beliefs(5).info = 0;    % 0: not active, 1: active
    beliefs(5).uncertainty = 0;
    beliefs(5).children = 0;
    beliefs(5).parent = 0;

    beliefs(6).name = 'Camera Pose';
    beliefs(6).id = 6;
    beliefs(6).type = POSE;
    R = CS5320_gen_R([1;0;0],-(34.69+90)*pi/180)*CS5320_gen_R([0;0;1],pi);
    T = eye(4,4);
    T(1:3,1:3) = R;
    T(3,4) = 0.82;
    beliefs(6).info = T;    % pose of camera wrt robot
    beliefs(6).uncertainty = 0.0001*eye(3,3);
    beliefs(6).children = 0;
    beliefs(6).parent = 0;

    beliefs(7).name = 'Terminate';
    beliefs(7).id = 7;
    beliefs(7).type = ACTION;
    beliefs(7).info = 0;    % 0: not active, 1: active
    beliefs(7).uncertainty = 0;
    beliefs(7).children = 0;
    beliefs(7).parent = 0;

    beliefs(8).name = 'Ever Localized?';
    beliefs(8).id = 8;
    beliefs(8).type = FACT;
    beliefs(8).info = 0;    % 0: not active, 1: active
    beliefs(8).uncertainty = 0;
    beliefs(8).children = 0;
    beliefs(8).parent = 0;

    beliefs(9).name = 'Floor';
    beliefs(9).id = 9;
    beliefs(9).type = FACT;
    beliefs(9).info = 0;    % index of WP for floor
    beliefs(9).uncertainty = inf;
    beliefs(9).children = 0;
    beliefs(9).parent = 0;

    beliefs(10).name = 'WPs combined';
    beliefs(10).id = 10;
    beliefs(10).type = FACT;
    beliefs(10).info = 0;    % 0 if not combined, else 1
    beliefs(10).uncertainty = 0;
    beliefs(10).children = 0;
    beliefs(10).parent = 0;

    beliefs(11).name = 'World Frame Image';
    beliefs(11).id = 11;
    beliefs(11).type = IMAGE;
    beliefs(11).info = [];    % rgb registered image
    beliefs(11).uncertainty = 0;
    beliefs(11).children = 0;
    beliefs(11).parent = 0;

    beliefs(12).name = 'Plan Done?';
    beliefs(12).id = 12;
    beliefs(12).type = FACT;
    beliefs(12).info = 0;    % 0: not done, 1: done
    beliefs(12).uncertainty = 0;
    beliefs(12).children = 0;
    beliefs(12).parent = 0;

    num_beliefs = 12;
end

val = 0;

if action==NOOP
    return
end

if action==GET
    if index<1&index>num_beliefs
        return
    end
    switch field
        case 'name'
            val = beliefs(index).name;
        case 'uncertainty'
            val = beliefs(index).uncertainty;
        case 'info'
            val = beliefs(index).info;
        case 'type'
            val = beliefs(index).type;
        case 'subtype'
            val = beliefs(index).subtype;
        case 'convhull'
            val = beliefs(index).convhull;
        case 'convhull_pts'
            val = beliefs(index).convhull_pts;
    end
    return
end

if action==SET
    switch field
        case 'uncertainty'
            beliefs(index).uncertainty = val_in;
        case 'info'
            beliefs(index).info = val_in;
    end
    return
end

if action==SAVE
    save(field,'beliefs');
end

if action==MERGE
    num_vals = length(val_in);
    for w = 1:num_vals
        if val_in(w).type==WP&val_in(w).subtype==PLANE
            params_w = val_in(w).info;
            found = 0;
            for b = 1:num_beliefs
                if beliefs(b).type==WP&beliefs(b).subtype==PLANE
                    params_b = beliefs(b).info;
                    dir_dot = dot(params_w(1:3),params_b(1:3));
                    dist = abs(params_w(4)-params_b(4));
                    if dist<MIN_PARAM_DIST&dir_dot>MIN_PARAM_DIR
                        found = 1;
                        ch_w = val_in(w).convhull_pts;
                        ch_b = beliefs(b).convhull_pts;
                        pts = [ch_w;ch_b];
                        pts_sp = WP_standard_plane([ch_w;ch_b],params_b);
                        K = convhull(pts_sp(:,1:2));
                        beliefs(b).convhull = K;
                        beliefs(b).convhull_pts = pts(K,:);
                    end
                end
            end
            if found==0
                num_beliefs = num_beliefs + 1;
                beliefs(num_beliefs).name = 'WP: plane';
                beliefs(num_beliefs).id = num_beliefs;
                beliefs(num_beliefs).type = WP;
                beliefs(num_beliefs).subtype = PLANE;
                beliefs(num_beliefs).info = val_in(w).info;
%                u = max(max(beliefs(1).uncertainty),val_in(w).uncertainty);
                u = val_in(w).uncertainty;
                beliefs(num_beliefs).uncertainty = u;
                beliefs(num_beliefs).convhull = val_in(w).convhull;
                beliefs(num_beliefs).convhull_pts = val_in(w).convhull_pts;
                beliefs(num_beliefs).children = 0;
                beliefs(num_beliefs).parent = 0;
            end
        end
    end
    return
end

if action==MATCH
    val = [];
    return
end

if action==FIND_WPS
    indexes = [];
    for ind = 1:num_beliefs
        if beliefs(ind).type==WP
            indexes = [indexes,ind];
        end
    end
    val = indexes;
end

if action==CLEAR_UNCERTAIN_WP
    clear_list = zeros(num_beliefs,1);
    for b = 1:num_beliefs
        if beliefs(b).type==WP&beliefs(b).uncertainty>val_in
            clear_list(b) = 1;
        end
    end
    if sum(clear_list)>0
        l2 = num_beliefs + 1;
        for l1 = 1:num_beliefs
            if l1>=l2
                num_beliefs = sum(clear_list==0);
                return
            end
            if clear_list(l1)==1
                l2 = l2 - 1;
                done = 0;
                while done==0
                    if l2==l1
                        done = 1;
                    elseif clear_list(l2)==0
                        beliefs(l1).name = beliefs(l2).name;
                        beliefs(l1).id = l1;
                        beliefs(l1).type = WP;
                        beliefs(l1).subtype = beliefs(l2).subtype;
                        beliefs(l1).info = beliefs(l2).info;
                        beliefs(l1).uncertainty = beliefs(l2).uncertainty;
                        beliefs(l1).convhull = beliefs(l2).convhull;
                        beliefs(l1).convhull_pts = beliefs(l2).convhull_pts;
                        beliefs(l1).children = 0;
                        beliefs(l1).parent = 0;
                        clear_list(l1) = 0;
                        clear_list(l2) = 1;
                        done = 1;
                    else
                        l2 = l2 - 1;
                    end
                end
            end
        end
    end
end

if action==DUMP
    clear val
    val = beliefs;
    return
end

if action==COUNT
    val = num_beliefs;
    return
end

tch = 0;
