:- object perception : [bcilib].

var x.
var y.
var z.
var r.

perception(Name, BeliefBase) :-
  localisation(Name, BeliefBase).
  
localisation(Name, BeliefBase) :-
  getRotation(Name, _,_,_,R1),
  getSFVec3f(Name, translation, X1,Y1,Z1),
  BeliefBase <- add_attr(belief, position(X1,Y1,Z1)),
  BeliefBase <- add_attr(belief, rotation(R1)),
  x := X1,
  y := Y1,
  z := Z1,
  r := R1,
  repeat,
    getRotation(Name, _,_,_,R),
    getSFVec3f(Name, translation, X,Y,Z),
    position_changed([X,Y,Z], BeliefBase),
    rotation_changed(R, BeliefBase),
    sleep(100),
  fail.

position_changed([x,y,z], _).
position_changed([X,Y,Z], BeliefBase) :-
  changed([X,Y,Z], [x,y,z]),
  !,
  BeliefBase <- delete_attr(belief, position(_,_,_)),
  BeliefBase <- add_attr(belief, position(X,Y,Z)),
  x := X,
  y := Y,
  z := Z.
  
rotation_changed(r, _).
rotation_changed(R, BeliefBase) :-
  R \== r,
  !,
  BeliefBase <- delete_attr(belief, rotation(_)),
  BeliefBase <- add_attr(belief, rotation(R)),
  r := R.

changed([H|_], [H1|_]) :-
  H \== H1.
changed([_|T], [_|T1]) :-
  changed(T,T1).

:- end_object perception.