function action = Research_tessa_agent(percept)
% CS4300_tessa_agent - A Wumpus World 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 = Research_tessa_agent([0,0,0,0,0]);
%     
% Author: 
%       Tessa Nishida
%       4/15/22

persistent board x y dir gold action_taken escape plan visit safe mBoard pBoard numMonsters

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

if isempty(board)
    board = -ones(4,4);
    safe = -ones(4,4);
    mBoard = -ones(4,4);
    pBoard = -ones(4,4);
    breezeBoard = -ones(4,4);
    x = 1;
    y = 1;
    dir = 0;
    action_taken = 0;
    escape = [];
    visit = [];
    plan = [];
    numMonsters = 0;
end

[x,y,dir] = CS4300_act(x,y,dir,action_taken);

if(board(4-y+1,x) == -1) % visit unmarked cells to update model
    board(4-y+1,x) = 0;
    safe(4-y+1,x) = 0;
    pBoard(4-y+1,x) = 0;
    mBoard(4-y+1,x) = 0;

    if percept(3)==1  % Glitter percept
        [sol,nodes] = CS4300_Wumpus_A_star(safe,[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 percept(1)==1                        % If there is a stench
        count = 0;
        if x+1 <= 4 && safe(4-y+1,x+1)~=0   % If we havent decided that the cell is safe
            if(percept(2)==0)               % If there is no breeze, get rid of adj. pits
                pBoard(4-y+1,x+1)=0;
            end
            if(mBoard(4-y+1,x+1)~=0 && numMonsters == 0)    % If we havent decided that the cell can't have a monster 
                mBoard(4-y+1,x+1)=1;                        % and only add monsters on the first stench
                safe(4-y+1,x+1)=3;
                count=count+1;
            elseif mBoard(4-y+1,x+1)~=1
                mBoard(4-y+1,x+1)=0;
            end
            if(mBoard(4-y+1,x+1)==0 && pBoard(4-y+1,x+1) == 0)
                safe(4-y+1,x+1)=0;
            end
        end
        if y+1 <= 4 && safe(4-y,x)~=0
            if(percept(2)==0)
                pBoard(4-y,x)=0;
            end
            if(mBoard(4-y,x)~=0 && numMonsters == 0)
                mBoard(4-y,x)=1;
                safe(4-y,x)=3;
                count=count+1;
            elseif mBoard(4-y,x)~=1
                mBoard(4-y,x)=0;
            end
            if(mBoard(4-y,x)==0 && pBoard(4-y,x)==0)
                safe(4-y,x)=0;
            end
        end
        if x-1 >= 1 && safe(4-y+1,x-1)~=0
            if(percept(2)==0)
                pBoard(4-y+1,x-1)=0;
            end
            if(mBoard(4-y+1,x-1)~=0 && numMonsters == 0)
                mBoard(4-y+1,x-1)=1;
                safe(4-y+1,x-1)=3;
                count=count+1;
            elseif mBoard(4-y+1,x-1)~=1
                mBoard(4-y+1,x-1)=0;
            end
            if(pBoard(4-y+1,x-1)==0 && mBoard(4-y+1,x-1)==0)
                safe(4-y+1,x-1)=0;
            end
        end
        if y-1 >= 1 && safe(4-y+2,x)~=0
            if(percept(2)==0)
                pBoard(4-y+2,x)=0;
            end
            if(mBoard(4-y+2,x)~=0 && numMonsters == 0)
                mBoard(4-y+2,x)=1;
                count=count+1;
            elseif mBoard(4-y+2,x)~=1
                mBoard(4-y+2,x)=0;
            end
            if(pBoard(4-y+2,x)==0 && mBoard(4-y+2,x)==0)
                safe(4-y+2,x)=0;
            end
        end
        numMonsters=numMonsters+count;
    end
    
    if percept(2)==1                        % If there is a breeze
        if x+1 <= 4 && safe(4-y+1,x+1)~=0   % If we havent decided that the cell is safe
            if(mBoard(4-y+1,x+1)==1 && percept(1)==0)  % If there is no stench, get rid of possible monsters
                mBoard(4-y+1,x+1)=0;
                safe(4-y+1,x+1)=-1;
                numMonsters = numMonsters - 1;
            end
            if(pBoard(4-y+1,x+1)>=1)       
                pBoard(4-y+1,x+1) = pBoard(4-y+1,x+1)+1;  % higher prob of pit
                safe(4-y+1,x+1)=1;
            elseif(pBoard(4-y+1,x+1)==-1)
                pBoard(4-y+1,x+1) = 1;
                safe(4-y+1,x+1)=1;
            elseif (pBoard(4-y+1,x+1)==0 && mBoard(4-y+1,x+1)==0)
                safe(4-y+1,x+1)=0;
            end
        end
        if y+1 <= 4 && safe(4-y,x)~=0
            if(mBoard(4-y,x)==1 && percept(1)==0)
                mBoard(4-y,x)=0;
                safe(4-y,x)=-1;
                numMonsters = numMonsters - 1;
            end
            if(pBoard(4-y,x)>=1)
                pBoard(4-y,x) = pBoard(4-y,x)+1;
                safe(4-y,x)=1;
            elseif(pBoard(4-y,x)==-1)
                pBoard(4-y,x)=1;
                safe(4-y,x)=1;
            elseif(pBoard(4-y,x)==0 && mBoard(4-y,x)==0)
                safe(4-y,x)=0;
            end
    
        end
        if x-1 >= 1 && safe(4-y+1,x-1)~=0
            if(mBoard(4-y+1,x-1)==1 && percept(1)==0)
                mBoard(4-y+1,x-1)=0;
                safe(4-y+1,x-1)=-1;
                numMonsters = numMonsters - 1;
            end
            if(pBoard(4-y+1,x-1)>=1)
                pBoard(4-y+1,x-1) = pBoard(4-y+1,x-1)+1;
                safe(4-y+1,x-1)=1;
            elseif(pBoard(4-y+1,x-1)==-1)
                pBoard(4-y+1,x-1) = 1;
                safe(4-y+1,x-1)=1;
            elseif(pBoard(4-y+1,x-1)==0 && mBoard(4-y+1,x-1)==0)
                safe(4-y+1,x-1)=0;
            end
        end
        if y-1 >= 1 && safe(4-y+2,x)~=0
            if(mBoard(4-y+2,x)==1 && percept(1)==0)
                mBoard(4-y+2,x)=0;
                safe(4-y+2,x)=-1;
                numMonsters = numMonsters - 1;
            end
            if(pBoard(4-y+2,x)>=1)
                pBoard(4-y+2,x) = pBoard(4-y+2,x)+1;
                safe(4-y+2,x)=1;
            elseif(pBoard(4-y+2,x)==-1)
                pBoard(4-y+2,x) = 1;
                safe(4-y+2,x)=1;
            elseif(pBoard(4-y+2,x)==0 && mBoard(4-y+2,x)==0)
                safe(4-y+2,x)=0;
            end
        end
    end
    
    if percept(1)==0 && percept(2)==0 % no breeze or stench
        if y-1 >= 1 && safe(4-y+2,x)~=0 
            safe(4-y+2,x)=0;
            if(mBoard(4-y+2,x)==1)
                numMonsters = numMonsters - 1;
            end
            mBoard(4-y+2,x)=0;
            pBoard(4-y+2,x)=0;
        end
        if y+1 <= 4 && safe(4-y,x)~=0 
           safe(4-y,x)=0;
           if(mBoard(4-y,x)==1)
               numMonsters = numMonsters - 1;
           end
           mBoard(4-y,x)=0;
           pBoard(4-y,x)=0;
        end
        if x-1 >= 1 && safe(4-y+1,x-1)~=0 
            safe(4-y+1,x-1)=0;
            if(mBoard(4-y+1,x-1)==1)
               numMonsters = numMonsters - 1;
            end
            mBoard(4-y+1,x-1)=0;
            pBoard(4-y+1,x-1)=0;
        end
        if x+1 <= 4 && safe(4-y+1,x+1)~=0 
           safe(4-y+1,x+1)=0;
           if(mBoard(4-y+1,x+1)==1)
               numMonsters = numMonsters - 1;
           end
           mBoard(4-y+1,x+1)=0;
           pBoard(4-y+1,x+1)=0;
        end
    end

    if numMonsters==1   % find last monster
        mX = -1;
        mY = -1;
        for i=1:4
            for j=1:4
                if(mBoard(i,j)==1)
                    mX = j;
                    mY = 4-i+1;
                    break
                end
            end
            if(mX ~= -1)
                break;
            end
        end
        visit = [mX; visit];    % visit the monster cell last 
        visit = [mY; visit];
        numMonsters = -1;
        safe(4-mY+1,mX)=3;
    end

    % add adjacent safe cells to visit
    if y-1 >= 1 && safe(4-y+2,x)==0 && board(4-y+2,x)==-1
        visit = [visit; y-1];
        visit = [visit; x];
    end
    if y+1 <= 4 && safe(4-y,x)==0 && board(4-y,x)==-1
        visit = [visit; y+1];
        visit = [visit; x];
    end
    if x-1 >= 1 && safe(4-y+1,x-1)==0 && board(4-y+1,x-1)==-1     
        visit = [visit; y];
        visit = [visit; x-1];
    end
    if x+1 <= 4 && safe(4-y+1,x+1)==0 && board(4-y+1,x+1)==-1
        visit = [visit; y];
        visit = [visit; x+1];
    end
end

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

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

if(percept(5)==1 && numMonsters==2) 
    numMonsters=-1;
    plan(end+1)=FORWARD;
end

if(percept(5)==0 && numMonsters==2 && action_taken == SHOOT) 
    numMonsters=-1;
    dstX = visit(end);
    visit(end) = [];
    dstY = visit(end);
    visit(end) = [];
    safe(4-dstY+1,dstX)=0;
    [sol,nodes] = CS4300_Wumpus_A_star(safe,[x,y,dir],[dstX,dstY,dir],...
        'CS4300_A_star_Man');
    sol = [x,y,dir,0;sol(2:end,:);1,1,1,0];
    plan = sol(2:end-1,end);
end



if ~isempty(visit) && isempty(plan)     % do not try to visit any cells that have already been explored
    dstX = visit(end);
    visit(end) = [];
    dstY = visit(end);
    visit(end) = [];
    while(board(4-dstY+1,dstX)==0 && ~isempty(visit))
        dstX = visit(end);
        visit(end) = [];
        dstY = visit(end);
        visit(end) = [];
    end
    if board(4-dstY+1,dstX)~=0
        if(mBoard(4-dstY+1,dstX)~=1) % no monster
            [sol,nodes] = CS4300_Wumpus_A_star(safe,[x,y,dir],[dstX,dstY,dir],...
                'CS4300_A_star_Man');
            sol = [x,y,dir,0;sol(2:end,:);1,1,1,0];
            plan = sol(2:end-1,end);
        else
            safe(4-dstY+1,dstX)=0;
            [sol,nodes] = CS4300_Wumpus_A_star(safe,[x,y,dir],[dstX,dstY,dir],...
                'CS4300_A_star_Man');
            sol = [x,y,dir,0;sol(2:end,:);1,1,1,0];
            plan = sol(2:end-1,end);
            plan(end) = SHOOT;
            plan(end+1) = FORWARD;
        end
    end
end

if ~isempty(plan)
    action = plan(1);
    plan = plan(2:end);
    action_taken = action;
    return
end

% if there is no where safe to go -- try shooting at remaining monsters
count = 0;
if(numMonsters == 2)
    for i=1:4
        for j=1:4
            if(mBoard(i,j)==1)
                visit = [j; visit];
                visit = [4-i+1; visit];
                count = count + 1;
            end
        end
        if(count == 2)
            break;
        end
    end
    dstX = visit(end);
    visit(end) = [];
    dstY = visit(end);
    visit(end) = [];
    safe(4-dstY+1,dstX)=0;
    [sol,nodes] = CS4300_Wumpus_A_star(safe,[x,y,dir],[dstX,dstY,dir],...
        'CS4300_A_star_Man');
    sol = [x,y,dir,0;sol(2:end,:);1,1,1,0];
    plan = sol(2:end-1,end);
    plan(end) = SHOOT;
    safe(4-dstY+1,dstX)=3;
    action = plan(1);
    plan = plan(2:end);
    action_taken = action;
    return
end

% find lowest probability & closest pit cell
minP = 1;
min_dist = Inf;
for i=1:4
    for j=1:4
        if(pBoard(i,j)>0)
            d=0;
            if i>1
                d=d+1;
            end
            if i<4
                d=d+1;
            end
            if j>1
                d=d+1;
            end
            if j<4
                d=d+1;
            end
            p = pBoard(i,j)/d;
            dist = norm([4-y+1,x]-[i,j]);
            if p < minP || (p == minP && dist<min_dist) 
                minP = p;
                dstX = j;
                dstY = 4-i+1;
                min_dist = dist;
            end
        end
    end
end

if minP < 1
    safe(4-dstY+1,dstX)=0;
    [sol,nodes] = CS4300_Wumpus_A_star(safe,[x,y,dir],[dstX,dstY,dir],...
        'CS4300_A_star_Man');
    sol = [x,y,dir,0;sol(2:end,:);1,1,1,0];
    plan = sol(2:end-1,end);
end


if ~isempty(plan)     
    action = plan(1);
    plan = plan(2:end);
    action_taken = action;
    return
end

%  If there is no other options, escape
[sol,nodes] = CS4300_Wumpus_A_star(safe,[x,y,dir],[1,1,0],...
    'CS4300_A_star_Man');
sol = [x,y,dir,0;sol(2:end,:);1,1,1,CLIMB];
escape = sol(2:end,end);
action = escape(1);
escape = escape(2:end);
action_taken = action;
