function action = EAGER_KB_agent(percept)
% EAGER_KB_agent - KB board analysis agent
% On input:
%     percept (1x5 Boolean vector): percept values
%      (1): Stench
%      (2): Breeze
%      (3): Glitters
%      (4): Bumped
%      (5): Screamed
% On output:
%     action (int): action selected by agent
%       FORWARD = 1;
%       ROTATE_RIGHT = 2;
%       ROTATE_LEFT = 3;
%       GRAB = 4;
%       SHOOT = 5;
%       CLIMB = 6;
% Call:
%     a = EAGER_KB_agent([0,0,0,0,0]);
% Author:
%     T. Henderson
%     UU
%     Fall 2017/Spring 2022
%

persistent safe board pit_probs wumpus_probs breezes stenches wumpus_dead
persistent visited x y dir gold action_taken escape kill go
persistent KBWW KBWorking breeze_indexes lin2WW atom_KB_indexes
persistent gold_indexes pit_indexes stench_indexes wumpus_indexes

FORWARD = 1;
ROTATE_RIGHT = 2;
ROTATE_LEFT = 3;
GRAB = 4;
SHOOT = 5;
CLIMB = 6;

GOLD_PROB = 1/15;
PIT_PROB = 0.2;
WUMPUS_PROB = 1/15;

if isempty(safe)
    first = 1;
    load('KBWW');
    atom_KB_indexes = zeros(80,2);  % atom index, KB index
    % Add breeze probs
    lin2WW = [13,9,5,1,14,10,6,2,15,11,7,3,16,12,8,4];
    KBWorking = KBWW;
    safe = -ones(4,4);
    safe(4,1) = 1;
    board = -ones(4,4);
    board(4,1) = 0;
    visited = zeros(4,4);
    pit_probs = zeros(4,4);
    wumpus_probs = zeros(4,4);
    breezes = -ones(4,4);
    breezes(4,1) = percept(2);
    stenches = -ones(4,4);
    stenches(4,1) = percept(1);
    x = 1;
    y = 1;
    dir = 0;
    action_taken = 0;
    escape = [];
    kill = [];
    go = [];
    wumpus_dead = 0;
end


% Update pose
[x,y,dir] = CS4300_act(x,y,dir,action_taken,percept);
[x,y,dir]';
ra = 4 - y + 1;
ca = x;
lin_index = (ca-1)*4 + ra;
WW_index = lin2WW(lin_index);

% if shot arrow and killed Wumpus, make update
if percept(5)==1
    [rw,cw] = find(wumpus_probs==1);
    if ~isempty(rw)
        safe(rw,cw) = 1;
    end
    wumpus_probs = zeros(4,4);
    stenches = zeros(4,4);
    wumpus_dead = 1;
    percept(1) = 0;
    % Update KB with no Wumpus on board at each cell
    for r = 1:4
        for c = 1:4
            lin_indexW = (c-1)*4 + r;
            WW_indexW = lin2WW(lin_index);
            wumpus_index = find(atom_KB_indexes(:,1)==WW_index+64);
            if isempty(wumpus_index) % add to KB
                zero_indexes = find(atom_KB_indexes(:,1)==0);
                wumpus_index = zero_indexes(1);
                atom_KB_indexes(wumpus_index,1) = WW_index+64;
                atom_KB_indexes(wumpus_index,2) = length(KBWorking) + 1;
                KBWorking(end+1).clauses = -(WW_index+64);
                KBWorking(end).prob = 1;
            else
                KB_index = atom_KB_indexes(wumpus_index,2);
                KBWorking(KB_index).clauses = -(WW_index+64);
                KBWorking(KB_index).prob = 1;
            end
        end
        % zero probability of Wumpus anywhere (i.e., negates rule with
        %      W21 v W22 v ... v W44
        for k = 447:460
            KBWorking(k).prob = 0;
        end
    end
end

% Update info
stenches(ra,ca) = percept(1);
breezes(ra,ca) = percept(2);

% Update KB if new info
if (visited(ra,ca)==0)
    % Update no pit in x,y
    pit_index = find(atom_KB_indexes(:,1)==WW_index+32);
    if isempty(pit_index) % add to KB
        zero_indexes = find(atom_KB_indexes(:,1)==0);
        pit_index = zero_indexes(1);
        atom_KB_indexes(pit_index,1) = WW_index+32;
        atom_KB_indexes(pit_index,2) = length(KBWorking) + 1;
        KBWorking(end+1).clauses = -(WW_index+32);
    end
    KB_index = atom_KB_indexes(pit_index,2);
    KBWorking(KB_index).prob = 1;

    % Update no Wumpus in x,y
    if wumpus_dead==0
        wumpus_index = find(atom_KB_indexes(:,1)==WW_index+64);
        if isempty(wumpus_index) % add to KB
            zero_indexes = find(atom_KB_indexes(:,1)==0);
            wumpus_index = zero_indexes(1);
            atom_KB_indexes(wumpus_index,1) = WW_index+64;
            atom_KB_indexes(wumpus_index,2) = length(KBWorking) + 1;
            KBWorking(end+1).clauses = -(WW_index+64);
        end
        KB_index = atom_KB_indexes(wumpus_index,2);
        KBWorking(KB_index).prob = 1;
    end
    
    % Update breeze
    breeze_index = find(atom_KB_indexes(:,1)==WW_index);
    if isempty(breeze_index) % add to KB
        zero_indexes = find(atom_KB_indexes(:,1)==0);
        breeze_index = zero_indexes(1);
        atom_KB_indexes(breeze_index,1) = WW_index;
        atom_KB_indexes(breeze_index,2) = length(KBWorking) + 1;
        if percept(2)==0
            KBWorking(end+1).clauses = -(WW_index);
        else
            KBWorking(end+1).clauses = WW_index;
        end
    end
    KB_index = atom_KB_indexes(breeze_index,2);
    KBWorking(KB_index).prob = 1;

    % Update stench
    if wumpus_dead==0
        stench_index = find(atom_KB_indexes(:,1)==WW_index+48);
        if isempty(stench_index) % add to KB
            zero_indexes = find(atom_KB_indexes(:,1)==0);
            stench_index = zero_indexes(1);
            atom_KB_indexes(stench_index,1) = WW_index+48;
            atom_KB_indexes(stench_index,2) = length(KBWorking) + 1;
            if percept(1)==0
                KBWorking(end+1).clauses = -(WW_index+48);
            else
                KBWorking(end+1).clauses = WW_index+48;
            end
        end
        KB_index = atom_KB_indexes(stench_index,2);
        KBWorking(KB_index).prob = 1;
    end
    
    % Recalculate KB probs
    BR_KB2F_ind(KBWorking,'KBWorkingi');
    num_vars = max(BR_vars(KBWorking,[]));
    x0 = 0.5*ones(num_vars,1); % should initialize 0/1 probs
    [atom_probs,err,ap_t,err_tt] = BR_KB_Jacobian(KBWorking,'KBWorkingi',...
        1,0.001,100,x0);
    v = [atom_probs(33:36);atom_probs(37:40);atom_probs(41:44);...
        atom_probs(45:48)];
    v = v(lin2WW);
    pit_probs = reshape(v,4,4);
    v = [atom_probs(65:68);atom_probs(69:72);atom_probs(73:76);...
        atom_probs(77:80)];
    v = v(lin2WW);
    wumpus_probs = reshape(v,4,4);
end

% set current location safe
safe(4-y+1,x) = 1;
visited(4-y+1,x) = 1;
board(4-y+1,x) = 0;

for r = 1:4
    yb = 4 - r + 1;
    for c = 1:4
        xb = c;
        if breezes(r,c)==0
            neis = BR_Wumpus_neighbors(xb,yb);
            for n = 1:length(neis(:,1))
                pit_probs(4-neis(n,2)+1,neis(n,1)) = 0;
            end
        end
        if stenches(r,c)==0
            neis = BR_Wumpus_neighbors(xb,yb);
            for n = 1:length(neis(:,1))
                wumpus_probs(4-neis(n,2)+1,neis(n,1)) = 0;
            end
        end
    end
end

% if stench detected, zero out non-neighbors of stench
if max(stenches(:))>0
    [rows,cols] = find(stenches==1);
    count = zeros(4,4);
    for s = 1:length(rows)
        neis = BR_Wumpus_neighbors(cols(s),4-rows(s)+1);
        for n = 1:length(neis(:,1))
            count(4-neis(n,2)+1,neis(n,1)) = ...
                count(4-neis(n,2)+1,neis(n,1)) + 1;
        end
    end
    [rows,cols] = find(count<max(count(:))&safe~=1);
    for k = 1:length(rows)
        wumpus_probs(rows(k),cols(k)) = 0;
    end
    wumpus_probs = wumpus_probs/sum(wumpus_probs(:));
end

for r = 1:4
    for c = 1:4
        if pit_probs(r,c)==0&wumpus_probs(r,c)==0
            safe(r,c) = 1;
            board(r,c) = 0;
        end
    end
end

% zero out pit and Wumpus probs of cells known to be safe
for r = 1:4
    for c = 1:4
        if visited(r,c)==1|safe(r,c)==1
            pit_probs(r,c) = 0;
            wumpus_probs(r,c) = 0;
        end
        if breezes(r,c)==0
            neis = BR_Wumpus_neighbors(c,4-r+1);
            num_neis = length(neis(:,1));
            for n = 1:num_neis
                pit_probs(4-neis(n,2)+1,neis(n,1)) = 0;
            end
        end
        if stenches(r,c)==0
            neis = BR_Wumpus_neighbors(c,4-r+1);
            num_neis = length(neis(:,1));
            for n = 1:num_neis
                wumpus_probs(4-neis(n,2)+1,neis(n,1)) = 0;
            end
        end
    end
end

% update safe places using probs
for r = 1:4
    for c = 1:4
        if pit_probs(r,c)==0&wumpus_probs(r,c)==0
            safe(r,c) = 1;
            board(r,c) = 0;
        elseif pit_probs(r,c)==1|wumpus_probs(r,c)==1
            safe(r,c) = 0;
            board(r,c) = 1;
        end
    end
end

%Make sure safe cells have a visited neighbor
for r = 1:4
    for c = 1:4
        if safe(r,c)==1
            rmin = max(1,r-1);
            rmax = min(4,r+1);
            cmin = max(1,c-1);
            cmax = min(4,c+1);
            found = 0;
            for rr = rmin:rmax
                for cc = cmin:cmax
                    if norm([r,c]-[rr,cc])<1.1&visited(rr,cc)==1
                        found = 1;
                    end
                end
            end
            if found==0
                safe(r,c) = 0;
            end
        end
    end
end

% if in cell with gold and no escape plan, plan escape
if percept(3)==1&isempty(escape)  % Glitter percept
    safe(ra,ca) = 1;
    board(ra,ca) = 0;
    [sol,nodes] = CS4300_Wumpus_A_star(board,[x,y,dir],[1,1,0],...
        'CS4300_A_star_Man');
    sol = [x,y,dir,GRAB;sol(2:end,:);1,1,1,CLIMB];
    escape = sol(:,end);
    action = escape(1);
    escape = escape(2:end);
    action_taken = action;
    return
end

% If escape plan, follow it
if ~isempty(escape)   % pre-planned escape sequence of actions
    action = escape(1);
    escape = escape(2:end);
    action_taken = action;
    return
end

% If kill wumpus plan, follow it
if ~isempty(kill)   % pre-planned kill Wumpus sequence
    action = kill(1);
    kill = kill(2:end);
    action_taken = action;
    return
end

% if go to plan, then follow it
if ~isempty(go)   % pre-planned movements to unvisited safe cell
    action = go(1);
    go = go(2:end);
    action_taken = action;
    return
end

% If Wumpus found, plan kill sequence
[rw,cw] = find(wumpus_probs==1);
if ~isempty(rw)
    xw = cw;
    yw = 4 - rw + 1;
    % find visited cell to shoot from
    [rowsv,colsv] = find(visited==1);
    indexesr = find(rowsv==rw);
    indexesc = find(colsv==cw);
    if ~isempty(indexesr)
        rv = rowsv(indexesr(1));
        cv = colsv(indexesr(1));
        xv = cv;
        yv = 4 - rv + 1;
    else
        rv = rowsv(indexesc(1));
        cv = colsv(indexesc(1));
        xv = cv;
        yv = 4 - rv + 1;
    end
    dirg = round(180*posori(atan2(yw-yv,xw-xv))/pi/90);    
    [sol,nodes] = CS4300_Wumpus_A_star(board,[x,y,dir],[xv,yv,dirg],...
        'CS4300_A_star_Man');
    sol = [sol(2:end,:);xv,yv,dirg,SHOOT];
    kill = sol(:,end);
    action = kill(1);
    kill = kill(2:end);
    action_taken = action;
    return
end

% If there is a safe unvisited cell, make go to plan
rn = 0;
cn = 0;
min_dist = Inf;
for r = 1:4
    for c = 1:4
        if safe(r,c)==1&visited(r,c)==0
            dist = norm([ra,ca]-[r,c]);
            if dist<min_dist
                rn = r;
                cn = c;
                min_dist = dist;
            end
        end
    end
end
if rn>0  % found safe unvisited cell
    xn = cn;
    yn = 4 - rn + 1;
    dirn = dir;
    [sol,nodes] = CS4300_Wumpus_A_star(board,[x,y,dir],[xn,yn,dirn],...
        'CS4300_A_star_Man');
    sol = sol(2:end,:);
    go = sol(:,end);
    action = go(1);
    go = go(2:end);
    action_taken = action;
    return
end

% if no safe unvisited cell, make go to plan for safest frontier cell
frontier = zeros(4,4);
f_probs = ones(4,4);
for r = 1:4
    for c = 1:4
        if safe(r,c)==1
            rmin = max(1,r-1);
            rmax = min(4,r+1);
            cmin = max(1,c-1);
            cmax = min(4,c+1);
            for rr = rmin:rmax
                for cc = cmin:cmax
                    dist = norm([r,c]-[rr,cc]);
                    if dist<1.1&visited(rr,cc)==0
                        frontier(rr,cc) = 1;
                        f_probs(rr,cc) = min(f_probs(rr,cc),...
                            max(pit_probs(rr,cc),...
                            wumpus_probs(rr,cc)));
                    end
                end
            end
        end
    end
end
% need to find lowest prob pit and Wumpus
[rows,cols] = find(f_probs==min(min(f_probs)));
rn = rows(1);
cn = cols(1);
dirn = dir;
xn = cn;
yn = 4 - rn + 1;
if x==xn&y==yn
    [rows,cols] = find(visited==0);
    if isempty(rows)
        [vrows,vcols] = find(visited==1);
        for k = 1:length(vrows)
            if vrows(k)~=4-y+1|vcols(k)~=x
                xn = vcols(k);
                y = 4 - vrows(k) + 1;
            end
        end
    else
        xn = cols(1);
        yn = 4 - rows(1) + 1;
    end
end
board(rn,cn) = 0;
[sol,nodes] = CS4300_Wumpus_A_star(board,[x,y,dir],[xn,yn,dirn],...
    'CS4300_A_star_Man');
sol = sol(2:end,:);
go = sol(:,end);
action = go(1);
go = go(2:end);
action_taken = action;
return
