:- object sensors.

sensors(ControllerQueue) :-
  new(listener(ControllerQueue),_),
  new(viewer,_).
  
:- end_object sensors.

:- object listener : [jsilib, bcilib, knowledge_base].

listener(ControllerQueue) :-
  repeat,
    sleep(500),
    get_queue(script, queue, Term),
    proces_event(Term, ControllerQueue),
  Term = quit,
  !.

proces_event(resolution(Width, Height), ControllerQueue) :-
  !,
  set_queue(resolution(Width, Height)).

proces_event(viewpoint(View), _ControllerQueue) :-
  !,
  set_view(View).

proces_event(module(Module), _ControllerQueue) :-
  !,
  set_module(Module).

proces_event(tell(Desire), ControllerQueue) :-
  !,
  set_queue(ControllerQueue, tell(Desire)).

proces_event(show(Field), _ControllerQueue) :-
  !,
  show_attr(Field, Value),
  teller <- output(field(Value, Field)).

proces_event(properties(Object), _ControllerQueue) :-
  !,
  show_attr(Object, Value),
  teller <- output(field(Value, Object)).
  
proces_event(Desire, ControllerQueue) :-
  set_queue(ControllerQueue, command(Desire)),
  !.

set_view(Viewpoint) :-
  getSFVec3f(Viewpoint, position, X,Y,Z),
  getSFRotation(Viewpoint, orientation, XR,YR,ZR,R),
  setViewpointPosition(viewpoint, X,Y,Z),
  setViewpointOrientation(viewpoint, XR,YR,ZR,R).

set_module(Module) :-
  delete_attr(module, [Module,Value]),
  switch(Value, NewValue),
  add_attr(module, [Module,NewValue]).

switch(true, false) :- !.
switch(false, true) :- !.
  
:- end_object listener.

:- object viewer : [bcilib, perception, knowledge_base].

var pi = asin(1) * 2.
var halfPi = asin(1).

viewer :-
  lookup_attr(belief, [name,Name]),
  init(Name),
  create_field(user),
  repeat,
    lookup_attr(belief, [position,X,Y,Z]),
    lookup_attr(belief, [rotation,R1]),
    lookup_attr(belief, [head,R2]),
    lookup_attr(belief, [inside,Room]),
    R is R1 + R2,
    
    getMFNode(Room, objects, Objects1),
    get_objects_in_fov(X,Y,Z,R, Objects1, Objects),
    get_doors(Room, X,Y,Z,R, Doors),

    view_user(proxSensor),
    view_objects(Objects, Room, Names),
    view_doors(Doors),
    
    modify_attr(object, _, Names),
    sleep(100),
  fail,
  !.

init(Name) :-
  localisation(Name),
  
  getMFNode(house, rooms, Rooms),
  getMFNode(house, doors, Doors),
  get_objects(Rooms, Objects),
  
  append(Doors, Objects, Items),
  view_attributes(Items).
  
get_objects([],[]) :- 
  !.
get_objects([Room|Rest], Objects) :-
  getMFNode(Room, objects, Objects1),
  append(Objects1, Objects2, Objects),
  get_objects(Rest, Objects2).

get_doors(Room, X,Y,Z,R, Doors) :-
  getMFNode(house, doors, Doors1),
  get_doors_in_room(Doors1, Room, Doors2),
  get_objects_in_fov(X,Y,Z,R, Doors2, Doors).

get_doors_in_room([], _Room, []) :-
  !.  
get_doors_in_room([Door|Rest], Room, [Door|Doors]) :-
  getMFNode(Door, connect, [Room1, Room2]),
  getSFString(Room1, name, Name1),
  getSFString(Room2, name, Name2),
  match_room(Room, Name1, Name2),
  get_doors_in_room(Rest, Room, Doors),
  !.

get_doors_in_room([_Door|Rest], Room, Doors) :-
  get_doors_in_room(Rest, Room, Doors).

match_room(Room, Room, _Room2) :-
  !.
match_room(Room, _Room1, Room) :-
  !.
  
get_objects_in_fov(_X,_Y,_Z,_R, [], []) :-
  !.
  
get_objects_in_fov(X,Y,Z, R, [Object|Rest], [Object|NewRest]) :-
  object_in_fov(X,Y,Z,R,Object),
  !,
  get_objects_in_fov(X,Y,Z, R, Rest, NewRest).
  
get_objects_in_fov(X,Y,Z, R, [_Object|Rest], NewRest) :-
  !,
  get_objects_in_fov(X,Y,Z, R, Rest, NewRest).

object_in_fov(X,_Y,Z, R, Object) :-
  getSFVec3f(Object, translation, XO,_YO,ZO),
  in_fov(XO,ZO, X,Z, R).
  
in_fov(XO,_ZO, X,_Z,R) :-
  R == asin(1),
  XO >= X,
  !.
  
in_fov(XO,_ZO, X,_Z,R) :-
  R == asin(-1),
  XO =< X,
  !.
  
in_fov(XO,ZO, X,Z,R) :-
  R < asin(1),
  R > asin(-1),
  !,
  RC is tan(R * -1),
  on_line(XO, X,Z, RC, ZL),
  ZO >= ZL.

in_fov(XO,ZO, X,Z,R) :-
  RC is tan(R * -1),
  on_line(XO, X,Z, RC, ZL),
  ZO =< ZL.

%%% bepaal lineaire functie met X,Z en RC %%%
%%% vul hier XO in -> ZL                  %%%

on_line(XO, X,Z, RC, ZL) :-
  B is Z - RC * X,
  ZL is RC * XO + B.

:- end_object viewer.

:- object perception : [bcilib, knowledge_base].

localisation(Name) :-
  getRotation(hanim_HumanoidRoot, _X,_Y,_Z, R1),
  getRotation(hanim_skullbase, _X,_Y,_Z, R2),
  getSFVec3f(Name, translation, X,Y,Z),
  rules <- valid_rotation(R1, ValidR1),
  rules <- valid_rotation(R2, ValidR2),
  add_attr(belief, [position,X,Y,Z]),
  add_attr(belief, [rotation,ValidR1]),
  add_attr(belief, [head,ValidR2]).

view_attributes([]) :-
  !.
view_attributes([Object|Rest]) :-
  getSFString(Object, name, Name),
  getSFVec3f(Object, scale, Xscale,Yscale,Zscale),
  getSFVec3f(Object, bboxSize, Xsize,Ysize,Zsize),
  getSFVec3f(Object, center, Xcenter,Ycenter,Zcenter),
  getMFNode(Object, connect, List),

  NewXsize is Xsize * Xscale,
  NewYsize is Ysize * Yscale,
  NewZsize is Zsize * Zscale,
  NewXcenter is Xcenter * Xscale,
  NewYcenter is Ycenter * Yscale,
  NewZcenter is Zcenter * Zscale,

  create_field(Name),
  add_attr(Name, [scale,Xscale,Yscale,Zscale]),
  add_attr(Name, [center,NewXcenter,NewYcenter,NewZcenter]),
  add_attr(Name, [size,NewXsize,NewYsize,NewZsize]),
  add_connect_attr(Name, List),
  view_attributes(Rest).

add_connect_attr(_Name, []) :-
  !.
add_connect_attr(Name, [Room1,Room2]) :-
  getSFString(Room1, name, Name1),
  getSFString(Room2, name, Name2),
  add_attr(fact, [door,Name,Name1,Name2]).

view_user(User) :-
  getSFVec3f(User, position_changed, X,Y,Z),
  getSFRotation(User, orientation_changed, _X,_Y,_Z, R),
  modify_attr(user, [position,_,_,_], [position,X,Y,Z]),
  modify_attr(user, [rotation,_], [rotation,R]).

view_objects([], _Room, []) :-
  !.
view_objects([Object|Rest], Room, [Name|Names]) :-
  getSFString(Object, name, Name),
  getSFVec3f(Object, translation, X,Y,Z),
  getSFVec3f(Object, center, XC,_YC,ZC),
  getRotation(Object, _X,_Y,_Z, R),
    
  modify(X,Z, XC,ZC, R,0.0, NewX,NewZ),
  
  modify_attr(Name, [position,_,_,_], [position,NewX,Y,NewZ]),
  modify_attr(Name, [rotation,_], [rotation,R]),
  modify_attr(belief, [inside,Name,_], [inside,Name,Room]),
  view_objects(Rest, Room, Names).

view_doors([]) :- 
  !.  
view_doors([Door|Rest]) :-
  getSFString(Door, name, Name),
  getRotation(Door, _X,_Y,_Z, R1),
  getSFVec3f(Door, translation, X,Y,Z),

  atom_list_concat([Name,'_object'],DoorObject),
  getRotation(DoorObject, _X,_Y,_Z, R2),
  getSFVec3f(DoorObject, center, XC,_YC,ZC),

  R is R1 + R2,
  modify_door(X,Z, XC,ZC, R1,R2, NewX,NewZ),

  modify_attr(Name, [position,_,_,_], [position,NewX,Y,NewZ]),
  modify_attr(Name, [rotation,_], [rotation,R]),
  view_doors(Rest).

modify_door(X,Z,_,_, _R,0.0, X,Z) :-
  !.
modify_door(X,Z, Xcenter,Zcenter, R1,R2, NewX,NewZ) :-
  modify(X,Z, Xcenter,Zcenter, R1,R2, NewX,NewZ).
  
modify(X,Z, Xcenter,Zcenter, R1,R2, NewX,NewZ) :-
  XC is X + cos(R1) * Xcenter + sin(R1) * Zcenter,
  ZC is Z + cos(R1) * Zcenter + sin(R1) * Xcenter * -1,
  R is R1 + R2,
  NewX is XC - cos(R) * Xcenter + sin(R) * Zcenter,
  NewZ is ZC + sin(R) * Xcenter + cos(R) * Zcenter.

:- end_object perception.