%%%%%%%%%%%%%
%%% Model %%%
%%%%%%%%%%%%%

%% Facts %%

block(b1).
block(b2).
block(b3).
block(b4).
block(b5).
block(b6).
block(b7).
block(b8).

size(b1, large).
size(b2, large).
size(b3, large).
size(b4, large).
size(b5, small).
size(b6, small).
size(b7, small).
size(b8, small).

color(b1, yellow).
color(b2, red).
color(b3, blue).
color(b4, green).
color(b5, yellow).
color(b6, red).
color(b7, blue).
color(b8, green).

initial_state([on(b1, table), on(b2, table), on(b3, table), on(b4, table), on(b5, table),
    on(b6, table), on(b7, table), on(b8, table)]).
    
%%%%%%%%%%%%%
%%% Rules %%%
%%%%%%%%%%%%%
    
clear(Block, State) :- \+ member(on(_, Block), State).
empty_hand(State) :- \+ member(hand(_), State).
holding(State, Block) :- member(hand(Block), State).
is_on(Block1, Block2, State) :- member(on(Block1, Block2), State).

validate(pickup, Block, State) :- clear(Block, State), empty_hand(State).
validate(pickup, Block1, Block2, State) :- clear(Block1, State), is_on(Block1, Block2, State),
    (empty_hand(State)).
validate(put, Block1, Block2, State) :- clear(Block1, State), clear(Block2, State),
    (empty_hand(State) ; holding(State, Block1)).
validate(put, Block, table) :- clear(Block, State), 
    (empty_hand(State) ; holding(State, Block)).
validate(stack, Block1, Block2, State) :- clear(Block1, State), clear(Block2, State),
    (empty_hand(State) ; holding(State, Block1)).
validate(unstack, Block, State) :- clear(Block, State), empty_hand(State).
validate(unstack, Block1, Block2, State) :- clear(Block1, State), is_on(Block1, Block2, State),
    (empty_hand(State)).
%%%%%%%%%%%
%%ERRORS%%%
%%%%%%%%%%%

%%pickup%%
validate(pickup,Block,State):- \+clear(Block,State),write(Block),write(" Is not clear"),nl.
validate(pickup, Block, State) :- clear(Block, State), \+empty_hand(State),write("Hand not empty "),nl.
validate(pickup, Block1, Block2, State) :- \+clear(Block1, State),write(Block1),write(" Is not clear"),nl.
validate(pickup, Block1, Block2, State) :- clear(Block1, State), \+is_on(Block1, Block2, State),
    write(Block1),write(" Is not on "),write(Block2),nl.
validate(pickup, Block1, Block2, State) :- clear(Block1, State), is_on(Block1, Block2, State),
    \+(empty_hand(State)),write("Hand is not empty"),nl.



perform(pickup, Block, State, NewState) :- replace(on(Block,_), hand(Block), State, NewState).
perform(put, Block1, Target, State, NewState) :- perform(pickup, Block1, State, State1),
    replace(hand(Block1), on(Block1, Target), State1, NewState).
perform(stack, Block1, Target, State, NewState) :- perform(pickup, Block1, State, State1),
    replace(hand(Block1), on(Block1, Target), State1, NewState).
perform(unstack, Block, State, NewState) :- replace(on(Block,_), hand(Block), State, NewState).

%%%%%%%%%%%
%%% NLP %%%
%%%%%%%%%%%

%% Parsing %%

sentence(s(VP)) --> verb_phrase(VP).
sentence(s(VP)) --> verb_phrase(VP), dot.
noun_phrase(np(Det, Noun)) --> det(Det), noun(Noun).
noun_phrase(np(Det, Adjp, Noun)) --> det(Det), adj_phrase(Adjp), noun(Noun).
verb_phrase(vp(Verb, NP)) --> verb(Verb), noun_phrase(NP).
verb_phrase(vp(Verb, NP, PP)) --> verb(Verb), noun_phrase(NP), prep_phrase(PP).
prep_phrase(pp(Prep, NP)) --> prep(Prep), noun_phrase(NP).
adj_phrase(adjp(Adj)) --> adj(Adj).
adj_phrase(adjp(Adj, Adjp)) --> adj(Adj), adj_phrase(Adjp).
verb(v(put)) --> [put].
verb(v(pickup)) --> [pickup].
verb(v(stack)) --> [stack].
verb(v(unstack)) --> [unstack].
det(d(the)) --> [the].
adj(a(large)) --> [large].
adj(a(small)) --> [small].
adj(a(green)) --> [green].
adj(a(red)) --> [red].
adj(a(yellow)) --> [yellow].
adj(a(blue)) --> [blue].
noun(n(block)) --> [block].
noun(n(table)) --> [table].
prep(p(on)) --> [on].
prep(p(from)) --> [from].
dot --> ['.'].
dot -->[].

%% Extracting Verbs and NPs %%

extract(s(vp(v(Verb), NounPhrase)), Verb, NounPhrase).
extract(s(vp(v(Verb), NounPhrase1, pp(p(Prep), NounPhrase2))), Verb, Prep,NounPhrase1, NounPhrase2).

%% Reference Resolution %%


%% 2-adjective
resolve_ref(NP, _, Block) :- NP = np(_, adjp(a(Size), adjp(a(Color))), n(block)), 
    size(Block, Size), color(Block, Color).
resolve_ref(NP, _, Block) :- NP = np(_, adjp(a(Color), adjp(a(Size))), n(block)), 
    size(Block, Size), color(Block, Color).

    
%% 1-adjective 
resolve_ref_color(NP,State,Block):-
    NP = np(_, adjp(a(Adjective)), n(block)), 
    member(on(Block,_),State),
    clear(Block,State),
    color(Block,Adjective).
resolve_ref_size(NP,State,Block):-
    NP = np(_, adjp(a(Adjective)), n(block)), 
    member(on(Block,_),State),
    clear(Block,State),
    size(Block,Adjective).

resolve_ref_on(NP1,NP2,State,Block1,Block2):-
    NP1 = np(_,adjp(a(Adjective1)),n(block)),
    NP2 = np(_,adjp(a(Adjective2)),n(block)),
    member(on(Block1,Block2),State),
    \+clear(Block2,State),
    clear(Block1,State),
    (color(Block1,Adjective1);size(Block1,Adjective1)),
    (color(Block2,Adjective2);size(Block2,Adjective2)),
    write(Block1),write(Block2),nl.

resolve_ref(NP, State, Block) :-
     findall(Block, resolve_ref_size(NP,State,Block), [Block]) ; 
     findall(Block, resolve_ref_size(NP,State,Block),[Block]).
    
%% 0-adjective  
resolve_ref(NP, State, Block) :- NP = np(d(the), n(block)), member(hand(Block), State).
resolve_ref(NP, _, table) :- NP = np(d(the), n(table)).

    

%If not refrence found display error
resolve_ref(NP,State,_):- write("Not able to resolve refrence: "), write(NP),nl.

%% Natural Language Generation %%

nlg(Block) :- size(Block, S), color(Block, C), write("the "), write(S), write(" "), 
    write(C), write(" block"). 
nlg(table) :- write("the table"). 
nlg(on(Block, Target)) :- nlg(Block), write(" is on "), nlg(Target), nl.
nlg(hand(Block)) :- nlg(Block), write(" is in the hand "), nl.

nlg([StateItem]) :- nlg(StateItem).
nlg([StateItem|Rest]) :- nlg(StateItem), nlg(Rest).

%%%%%%%%%%%%%%%%%
%%% Main Loop %%%
%%%%%%%%%%%%%%%%%

perform_act(Parse, State, NewState) :- extract(Parse, Verb, NP), 
    resolve_ref(NP, State, Block), 
    validate(Verb, Block, State), 
    perform(Verb, Block, State, NewState).

perform_act(Parse, State, NewState) :- extract(Parse, Verb,Prep,NP1,NP2), 
    Prep = on,
    resolve_ref(NP1, State, Block1), 
    resolve_ref(NP2, State, Block2), 
    validate(Verb, Block1, Block2, State), 
    (perform(Verb, Block1, State, NewState) ; perform(Verb, Block1, Block2, State, NewState)).

perform_act(Parse, State, NewState) :- extract(Parse, Verb,_,NP1,NP2), 
    resolve_ref_on(NP1,NP2,State,Block1,Block2),
    validate(Verb, Block1, Block2, State), 
    perform(Verb, Block1, State, NewState).

process_input(State,Input) :-  
    sentence(Parse, Input, []), 
    perform_act(Parse, State, NewState), 
    write("Currently..."), nl, 
    nlg(NewState), 
    talktome(NewState).

process_input(_,Input):- Input = [Head|_],Head = exit,write("End talktome.").
process_input(State,Input):-Input = [Head|_],Head = getstate,write(State),talktome(State).
process_input(State,Input):- \+sentence(_,Input,[]),write("Could not pare input"),nl,nlg(State),talktome(State).
process_input(State,_) :- write("Im not sure how to do that."), nl, talktome(State).


talktome :- initial_state(State), write("Currently..."), nl, nlg(State), talktome(State).

talktome(State) :- write("What would you like to do next?, or type exit to stop"), nl,read_word_list(Input), process_input(State,Input).
        
%%%%%%%%%%%%%%%%
%%% Planning %%%
%%%%%%%%%%%%%%%%

%% Simple, horribly inefficient, brute-force plan
plan(CurrentState, CurrentState, []).
plan(CurrentState, GoalState, Steps) :- block(Block1), block(Block2), Block1 \= Block2, 
    validate(stack, Block1, Block2, CurrentState), 
    perform(stack, Block1, Block2, CurrentState, NextState),
    plan(NextState, GoalState, Steps1), Steps = [putOn(Block1,Block2)|Steps1].

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%Heuristic: Start at the bottom row of blocks and stack blocks one at a time layer  by layer cutting off stacks
%% when there is no block on top of that block.
%%This method will insure that all blocks are stacked but no blocks can be used twice
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%Generate the lowest level of blocks
getSteps(FinalState):-
    initial_state(State),
    findall(Block,member(on(Block,table),FinalState),Ground),
    process(Ground,[],State,[],FinalState).

%%Recursivly call this process looks at the next block in the current level and looks at the final state to see if there is a block stacked on this block in the final state
%%It then checks the validity, preforms the action on the state, finally if the block is added to the  next level
    %%write("Process1: "),write(Ground),nl,
    Ground = [Head|Tail],
    \+Tail=[],
    findall(Block,member(on(Block,Head),FinalState),[Block]),
    validate(stack,Block,Head,State),
    perform(stack,Block,Head,State,NextState),
    append(NewGround,[Block],NG),
    %%write(Block),nl,write(NextState),nl,write("Next Ground: "),write(NG),nl,
    append([puton(Block,Head)],Steps,NextSteps),
    process(Tail,NG,NextState,NextSteps,FinalState).
%%If this is the last block in the current level do the same actions as before and make the next level into the current level and run the process again
process(Ground,NextGround,State,Steps,FinalState):-
    %%write("Process2"),write(Ground),nl,
    Ground = [Head],
    findall(Block,member(on(Block,Head),FinalState),[Block]),
    validate(stack,Block,Head,State),
    perform(stack,Block,Head,State,NextState),
    append(NextGround,[Block],NG),
    %%write(Block),nl,write(NextState),nl,write("Next Ground: "),write(NG),nl,
    append([puton(Block,Head)],Steps,NextSteps),
    process(NG,[],NextState,NextSteps,FinalState).
%%If the block has nothing stacked on top of it and it is not the only block in the list then remove it from the current level and run process again
process(Ground,NextGround,State,Steps,FinalState):-
    %%write("CHOP This Stack"),nl,
    Ground = [Head|Tail],
    \+Tail=[],
    %%write("Suscessful Chop"),nl,
    process(Tail,NextGround,State,Steps,FinalState).
%%If there is noting on top of the block and it is the last block in the current level then the stacking is complete verify that the current state matches the final
%% if so print the steps listing from last to first and if not print that an error has occured
process(Ground,NextGround,State,Steps,FinalState):-
    %%write("CHOP This Stack"),nl,
    Ground = [Head|Tail],
    Tail=[],
    (State = FinalState, write(Steps);write("Error")).
  


% Utilities

replace(_, _, [], []).
replace(O, R, [O|T], [R|T2]) :- replace(O, R, T, T2).
replace(O, R, [H|T], [H|T2]) :- dif(H,O), replace(O, R, T, T2).

find_in(Pattern, [H|_], Result) :- Pattern = H, Result = H.
find_in(Pattern, [_|R], Result) :- find_in(Pattern, R, Result).
find_in(_, [], _) :- fail.

read_word_list(Ws) :-
    read_line_to_codes(user_input, Cs),
    atom_codes(A, Cs),
    tokenize_atom(A, Ws).
