:- object rules : [bcilib, knowledge_base].

var reach = 0.35.
var xRightHand = -0.35.
var xLeftHand = 0.35.
var yHand = 0.6.
var zHand = 0.35.
var maxStepLength = 1.0.
var shoulderY = 0.8.
var shoulderX = 0.3.
var legLength = 0.25.

rule(Rule) :-
  call(Rule).

get_grab_side(Xsize,_Ysize,Zsize, 0.0,0.0,Zgrab) :-
  Xsize =< Zsize,
  !,
  Zgrab is Zsize / 2.

get_grab_side(Xsize,_Ysize,Zsize, Xgrab,0.0,0.0) :-
  Xsize > Zsize,
  !,
  Xgrab is Xsize / 2.

grab_object_position(XT,YT,ZT, X,Y,Z) :-
  X is xLeftHand - XT,
  Y is yHand - ZT,
  Z is zHand - YT.

release_object(X,_Y,Z, R, Hand, XO,_,ZO, XT,ZT, RO) :-
  %%plaats van object na release
  Xtouch is X + cos(R) * (xRightHand * Hand) + sin(R) * reach,
  Ztouch is Z - sin(R) * (xRightHand * Hand) + cos(R) * reach,
  % correctie voor de touch positie
  touch_position(Xtouch,Ztouch, XT,ZT, R, XO,ZO),
  change_rotation(R,RO,-1).
  
get_node(Object, Node, ObjectRef) :-
  getMFNode(Object, Node, ObjectRef).
  
arm_rotation(YO, YT, Rarm, RarmUp, Up) :-
  lookup_attr(belief, [position,_,YA,_]),
  Ygrab is YO + YT,
  Yshoulder is YA + shoulderY,
  Ydist is Ygrab - Yshoulder,
  % 1.1 is rotatie waar hand horizontaal is
  Rarm is (1.1 + atan(Ydist/reach)) * -1,
  RarmUp is Rarm - Up.

move_back(R, -1, _Distance, 0.0,0.0, 0.0,0.0, 0.0,0.0, R) :-
  !.
move_back(R, 1, Distance, Xstep,Zstep, Rfront,Rback, NewX,NewZ, NewR) :-
  !,
  change_rotation(R, NewR, -1),
  get_stepLength(Distance, StepLength, _),
  next_step(R, StepLength, Xstep,Zstep, Rfront,Rback),
  NewX is Xstep * 2,
  NewZ is Zstep * 2.

next_step(R, StepLength, Xstep,Zstep, Rfront, Rback) :-
  % devide by 2 because walk does 2 steps(left&right)
  Xstep is (sin(R) * StepLength) / 2,
  Zstep is (cos(R) * StepLength) / 2,
  % rotatie schouder en heup afhankelijk van StepLength
  Rfront is atan(StepLength),
  Rback is Rfront * -1.

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

distance(X,Z, Xdest,Zdest, Distance) :-
  Xdif is Xdest - X,
  Zdif is Zdest - Z,
  Distance is sqrt(Xdif*Xdif + Zdif*Zdif).

% bepaal destination binnen touch bereik (Rmin, Rmax)
touch_destination(Xtouch,Ztouch, Rmin,Rmax, Hand, Xdest,Zdest,Rdest) :-
  get_direction(Xtouch,Ztouch,R),
  change_rotation(R,R1,-1),
  between(R1, Rmin, Rmax, Rdest),
  XarmCorrection is shoulderX * Hand * cos(Rdest),
  ZarmCorrection is shoulderX * Hand * sin(Rdest),
  Xdest is Xtouch + sin(Rdest) * reach - XarmCorrection,
  Zdest is Ztouch + cos(Rdest) * reach + ZarmCorrection.
  
touch_position(X,Z, XT,ZT, R, Xtouch,Ztouch) :-
  Xtouch is X + cos(R) * XT + sin(R) * ZT,
  Ztouch is Z + cos(R) * ZT + sin(R) * XT * -1.

%%% Stand is 1, Sit is -1 %%%
sit_position(_X,Y,Z, RO, Xsit,Ysit,Zsit, Stand) :-
  Xsit is sin(RO) * Z * Stand,
  Zsit is cos(RO) * Z * Stand,
  Ysit is (Y - legLength) * Stand * -1.

touch_rotation(Rdest, Hand, R) :-
  change_rotation(Rdest, R2, -1),
  R is R2 + 0.7 * Hand.

get_destination(X,Z, R, Distance, Xdest, Zdest) :-
  Xdest is X + sin(R) * Distance,
  Zdest is Z + cos(R) * Distance.

get_direction(Xdest,Zdest, R1) :-
  lookup_attr(belief, [position,X,_,Z]),
  lookup_attr(belief, [rotation,R]),
  Xdif is Xdest - X,
  Zdif is Zdest - Z,
  rotation(Xdif, Zdif, R, R1).

get_direction(X,Z, R, Xdest,Zdest, R1) :-
  Xdif is Xdest - X,
  Zdif is Zdest - Z,
  rotation(Xdif, Zdif, R, R1).


object_rotation(R, Rmin,Rmax, Side, NewRmin,NewRmax) :-
  Rmin1 is Rmin + R + acos(Side),
  Rmax1 is Rmax + R + acos(Side),
  valid_rotation(Rmin1,NewRmin),
  valid_rotation(Rmax1,NewRmax).

increment([position,XA,YA,ZA], [position,X,Y,Z], [position,NewX,NewY,NewZ]) :-
  NewX is XA + X,
  NewY is YA + Y,
  NewZ is ZA + Z.

position(Field, X,Y,Z) :-
  lookup_attr(Field, [position,X,Y,Z]).
rotation(Field, R) :-
  lookup_attr(Field, [rotation,R]).

mirror_rotation(R,R,1) :-!. 
mirror_rotation(R,NewR,-1) :-
  R < 0, R >= acos(-1) * -1,
  !,
  NewR is acos(-1) * -1 + R.
mirror_rotation(R,NewR,-1) :-
  R >= 0, R =< acos(-1),
  !,
  NewR is acos(-1) - R.
change_rotation(R,R,1):-!.
change_rotation(R,NewR,-1) :-
  R =< 0, R >= acos(-1) * -1,
  !,
  NewR is acos(-1) + R.
change_rotation(R,NewR,-1) :-
  R > 0, R =< acos(-1),
  !,
  NewR is acos(-1) * -1 + R.

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):-
  Zdif > 0.0,
  Xdif > 0.0,
  !,
  R is atan(Xdif / Zdif).
rotation(Xdif, Zdif, _,R):-
  Zdif < 0.0,
  Xdif > 0.0,
  !,
  R is acos(-1) + atan(Xdif / Zdif).
rotation(Xdif,Zdif, _,R):-
  Zdif > 0.0,
  Xdif < 0.0,
  !,
  R is 0 + atan(Xdif / Zdif).
rotation(Xdif, Zdif, _,R):-
  Zdif < 0.0,
  Xdif < 0.0,
  !,
  R is acos(-1) * -1 + atan(Xdif / Zdif).

valid_rotation(R, NewR) :-
  R < -3.14,
  !,
  NewR is 3.14 + (R + 3.14).
valid_rotation(R, NewR) :-
  R > 3.14,
  !,
  NewR is -3.14 + (R - 3.14).
valid_rotation(R, R) :-
  R =< 3.14,
  R >= -3.14,
  !.

between(R, Rmin, Rmax, R) :-
  Rmin =< Rmax,
  R >= Rmin,
  R =< Rmax,
  !.
between(R, Rmin, Rmax, Rmin) :-
  Rmin =< Rmax,
  R    < Rmin,
  !.
between(R, Rmin, Rmax, Rmax) :-
  Rmin =< Rmax,
  R > Rmax,
  !.
between(R,Rmin,Rmax,R) :-
  Rmin > Rmax,
  R >= Rmin,
  R =< Rmax,
  !.
between(R,Rmin,Rmax,Rmax) :-
  Rmin > Rmax,
  R > Rmax,
  R =< 0,
  !.
between(R,Rmin,Rmax,Rmin) :-
  Rmin > Rmax,
  R > Rmax,
  R > 0,
  !.

switch_status(false,true):-!.
switch_status(true,false):-!.

conc([],List,List). 
conc([H|T],List,[H|Rest]) :-
  conc(T, List, Rest).

get_object_corners([],[]).
get_object_corners([Object|Objects], Corners) :-
  lookup_attr(Object, [size,Width,_Height,Depth]),
  lookup_attr(Object, [position,X,_,Z]),
  lookup_attr(Object, [rotation,R]),
  get_corners(X,Z, R, Width,Depth, Corners1),
  append(Corners1,CornersRest,Corners),
  get_object_corners(Objects, CornersRest).
  
get_corners(X,Z, R, Width,Depth, Corners) :-
  Corners = [
    XW1,ZN1,XE1,ZN1,
    XW1,ZS1,XE1,ZS1,
    XW1,ZN1,XW1,ZS1,
    XE1,ZN1,XE1,ZS1
  ],
  Width1 is Width/2,
  Depth1 is Depth/2,
  XW1 is X + Width1 * cos(R) + Depth1 * sin(R),
  XE1 is X - Width1 * cos(R) - Depth1 * sin(R),
  ZS1 is Z + Depth1 * cos(R) + Width1 * sin(R),
  ZN1 is Z - Depth1 * cos(R) - Width1 * sin(R).
  
get_crosspoints(_,_, _,_, [], []).
get_crosspoints(X,Z, Xdest,Zdest, [X1,Z1,X2,Z2|CornersRest], Crosspoints) :-
  intersection(X,Z, Xdest,Zdest, X1,Z1, X2,Z2, Crosspoint),
  append(Crosspoint, CrosspointsRest, Crosspoints),
  get_crosspoints(X,Z, Xdest,Zdest, CornersRest, CrosspointsRest).

nearest_corner(_,_,[], []).
nearest_corner(Xdest,Zdest,[_, Xcross,Zcross, X1,Z1,X2,Z2], [NewXdest,NewZdest]) :-
  distance2D(Xdest, Zdest, X1,Z1, Dist1),
  distance2D(Xdest, Zdest, X2,Z2, Dist2),
  nearest(X1,Z1,Dist1, X2,Z2,Dist2, XN,ZN),
  get_direction(Xcross,Zcross,0.0,XN,ZN,R),
  NewXdest is XN + zHand * sin(R),
  NewZdest is ZN + zHand * cos(R).

nearest(X1,Z1,Dist1, _,_,Dist2, X1,Z1) :-
  Dist1 =< Dist2,
  !.
nearest(_,_,Dist1, X2,Z2,Dist2, X2,Z2) :-
  Dist2 < Dist1,
  !.  

nearest_crosspoint(X,Z, Crosspoints, Crosspoint) :-
  nearest_crosspoint(X,Z, Crosspoints, [], Crosspoint).
  
nearest_crosspoint(_,_, [], Crosspoint, Crosspoint) :-
  !.

% leeg element
nearest_crosspoint(X,Z, [[]|Rest], Nearest, NearestCrosspoint) :-
  nearest_crosspoint(X,Z, Rest, Nearest, NearestCrosspoint).

% eerste element is altijd dichtsbijzijnde      
nearest_crosspoint(X,Z, [[Xcross,Zcross|Corners]|Rest], [], NearestCrosspoint) :-
  distance2D(X,Z, Xcross,Zcross, Dist),
  nearest_crosspoint(X,Z, Rest, [Dist,Xcross,Zcross|Corners], NearestCrosspoint).
  
nearest_crosspoint(X,Z, [[Xcross,Zcross|Corners]|Rest], [Dist1|_], NearestCrosspoint) :-
  distance2D(X,Z, Xcross,Zcross, Dist2),
  Dist2 =< Dist1,
  !,
  nearest_crosspoint(X,Z, Rest, [Dist2,Xcross,Zcross|Corners], NearestCrosspoint).

nearest_crosspoint(X,Z, [[Xcross,Zcross|_]|Rest], [Dist1|Crosspoint], NearestCrosspoint) :-
  distance2D(X,Z, Xcross,Zcross, Dist2),
  Dist1 < Dist2,
  !,
  nearest_crosspoint(X,Z, Rest, [Dist1|Crosspoint], NearestCrosspoint).  

intersection(Ax1,Az1, Ax2,Az2, Bx1,Bz1, Bx2,Bz2, Crosspoint) :-
  Ax21Diff is Ax2 - Ax1,
  Az21Diff is Az2 - Az1,

  Bx21Diff is Bx2 - Bx1,
  Bz21Diff is Bz2 - Bz1,

  ABx1Diff is Ax1 - Bx1,
  ABz1Diff is Az1 - Bz1,

  CD is Bz21Diff * Ax21Diff - Bx21Diff * Az21Diff,

  CD =\= 0,!,   %% otherwise parallel

  Na is Bx21Diff * ABz1Diff - Bz21Diff * ABx1Diff,
  Nb is Ax21Diff * ABz1Diff - Az21Diff * ABx1Diff,

  Ua is Na / CD,
  Ub is Nb / CD,
  
  Xi is Ax1 + Ua * Ax21Diff,
  Zi is Az1 + Ua * Az21Diff,
  on_line(Ua,Ub, [Xi,Zi, Bx1,Bz1, Bx2,Bz2], Crosspoint).

%% CD =\= 0 goal failed in the previous clause;
%% i.e. line segments are parallel, return CD=0
intersection(_Ax1,_Az1, _Ax2,_Az2, _Bx1,_Bz1, _Bx2,_Bz2, []).

on_line(Ua,Ub, Crosspoint,[Crosspoint]) :-
  between1(Ua, 0,1),
  between1(Ub, 0,1),
  !.
on_line(_,_, _Crosspoint, []).  

between1(X, X1,X2) :-
  X1 < X2,
  X  > X1,
  X  < X2,
  !.
between1(X, X1,X2) :-
  X1 > X2,
  X < X1,
  X > X2,
  !.
between1(X, X1,X2) :-
  X1 == X2,
  X == X1,
  !.

var pi = acos(1) * 2.

rotation_difference(R1, R2, Rdif) :-
  R1 > 1.57,
  R2 < -1.57,
  !,
  Rdif is (pi - R1) + (pi + R2).
  
rotation_difference(R1, R2, Rdif) :-
  Rdif is R1 - R2.
    
:- end_object rules.