:- object capabilities : [bcilib, positioning].

var x.
var z.
var r.
var reach = 0.6.

capabilities(Queue, ActionQueue, BeliefBase) :-
  repeat,
    get_queue(Queue, Event),
    proces_event(Event, ActionQueue, BeliefBase),
  fail,
  !.

proces_event(walk, ActionQueue, BeliefBase) :-
  !,
  move(ActionQueue, BeliefBase).

proces_event(grab(Object), ActionQueue, BeliefBase) :-
  !,
  grab(Object, ActionQueue, BeliefBase).

move(ActionQueue, BeliefBase) :-
  BeliefBase <- lookup_attr(belief, position(X,_,Z)),
  BeliefBase <- lookup_attr(belief, rotation(R)),
  BeliefBase <- lookup_attr(belief, destination(Xdest, Zdest)),
  get_direction(X,Z, R, Xdest,Zdest, R1),
  set_queue(ActionQueue, rot(R1)),
  x := X, 
  z := Z, 
  r := R1,
  
  repeat,
    Xdif is Xdest - x,
    Zdif is Zdest - z,
    Distance is sqrt(Xdif*Xdif + Zdif*Zdif),
    destination(Distance, StepLength, Destination),
    Xstep is (sin(r) * StepLength) / 2,			% devide by 2 because walk does 2 steps(left&right)
    Zstep is (cos(r) * StepLength) / 2,
    set_queue(ActionQueue, walk(Xstep,0.0,Zstep)),    
    x := x + Xstep * 2,
    z := z + Zstep * 2,
    format('x is ~w, z is ~w~n',[x,z]),
  Destination,
  BeliefBase <- delete_attr(belief, destination(Xdest,Zdest)),
  set_queue(ActionQueue, stand), 
  !.

grab(Object, ActionQueue, BeliefBase) :-
  BeliefBase <- lookup_attr(belief, position(X,Y,Z)),
  BeliefBase <- lookup_attr(belief, rotation(R)),
  BeliefBase <- lookup_attr(Object, position(X1,Y1,Z1)),
%  BeliefBase <- lookup_attr(Object, rotation(R1)),

  distance(X,Y,Z, X1,Y1,Z1, Dist, Object),
  get_direction(X,Z, R, X1,Z1, R2),
  
  Dist2dest is Dist - reach,
  Xdest is X + sin(R2) * Dist2dest,
  Zdest is Z + cos(R2) * Dist2dest,
  
  BeliefBase <- add_attr(belief, destination(Xdest, Zdest)),
  move(ActionQueue, BeliefBase).
%  set_queue(ActionQueue, grab).



:- end_object capabilities.

:- object positioning : [bcilib].

var maxStepLength = 0.6.
var maxHeight = 1.2.
var minHeight = 0.6.
var reach = 0.6.

get_direction(X,Z, R, Xdest,Zdest, R1) :-
  Xdif is Xdest - X,
  Zdif is Zdest - Z,
  rotation(Xdif, Zdif, R, R1).                      % bepaal de hoek tot dest.
  
get_direction(_,_, R, false, false, R):-            % geen destination -> loop in de huidige richting
  !.
  
rotation(0.0,0.0, R,R) :-
  !.

rotation(0.0,Zdif, _,R) :-
  Zdif \== 0.0,
  !,
  R is acos(sign(Zdif)).
rotation(Xdif,0.0,  _,R):-
  Xdif \== 0.0,
  !,
  R is asin(1) * sign(Xdif).

rotation(Xdif, Zdif, _,R):-
  Xdif \== 0.0,
  Zdif > 0.0,
  !,
  R is atan(Xdif / Zdif).
rotation(Xdif,Zdif, _,R):-
  Xdif \== 0.0,
  Zdif < 0.0,
  !,
  R is acos(-1) + atan(Xdif / Zdif).

%avoid_obstacle(X,Z, Xdest,Zdest, R, Xdest2, Zdest2, R1) :-
%  obstacle(X,Z, Xdest,Zdest, R, Xdest1, Zdest1)
%  set_destination(Xdest,Zdest),
%  set_destination(Xdest1,Zdest1),
%  direction(X,Z, R, Xdest1,Zdest1).

avoid_obstacle(_,_, Xdest,Zdest, R, Xdest,Zdest, R).
% als er geen obstakel is.

%%% next step ligt voor de destination %%%

destination(Distance, maxStepLength, fail) :-
  Distance > maxStepLength,
  !.
  
destination(Distance, Distance, true) :-
  Distance =< maxStepLength,
  !.

%distance(_,_,_, _,Y1,_, _, Object) :-
%	Y1 > maxHeight,
%	BeliefBase <- set_attr(belief, high(Object)),
%	fail.

%distance(_,_,_, _,Y1,_, _,Object) :-
%  Y1 < minHeight,
%  BeliefBase <- set_attr(belief, low(Object)),
%  fail.

distance(X,_,Z, X1,_,Z1, Dist, _) :-
%  Y1 =< maxHeight,
%  Y1 >= minHeight,
  Dist is sqrt( ((X1 - X)*(X1 - X)) + ((Z1 - Z)*(Z1 - Z)) ).


% X lays between X1 and X2
between1(X,X1,X2):-
  X > X1,
  X <  X2,
  !.
between1(X,X1,X2):-
  X > X2,
  X < X1,
  !.

:- end_object positioning.