%% The robot opens and closes its mouth

%% Talk to others, make sure we have been thought of by society

%% lift([[[0,1],[1,2]],[[0,0],[1,1]]],[[0,2.2],[1,4]],S2,FBP).

%% lift(Static

%% test_not_over_edge (for block A on block B)

test_not_over_edge([[[XA1, _YA1], [XA2, _YA2]], [[XB1, _YB1], [XB2, _YB2]]]) :-
	cog(XA1, XA2, AverageA), %% cog(XB1, XB2, AverageB),
	XB1 =< AverageA, AverageA =< XB2.

%% cog(ShapeList)
%% Returns the centre of gravity
%% ShapeList, e.g. [[X1, Y1], [X2, Y2]] where X1, Y1 is the bottom-left hand corner of a rectangle, and X2, Y2 is it's top-right hand corner.

cog(X1,X2,Average) :-
	Sum is X1 + X2, Average = Sum / 2.

%% find_supported_block(SupportingBlock,Blocks,SupportedBlock)

find_supported_block([[XB1, YB1], [XB2, YB2]],[[[XA1, YA1], [XA2, YA2]]|_Blocks],[[XA1, YA1], [XA2, YA2]]) :-
	on1([[[XA1, YA1], [XA2, YA2]], [[XB1, YB1], [XB2, YB2]]]), !.
find_supported_block(SupportingBlock,[_|Blocks],SupportedBlock) :-
	find_supported_block(SupportingBlock,Blocks,SupportedBlock).

	
%% lift (for block A on block B), tests whether block A is balanced on block B, then lifts it until insruced to stop or there is an obstacle

lift([[[XA1, YA1], [XA2, YA2]], [[XB1, YB1], [XB2, YB2]]],TopBlockPos,Seconds2,FinalBlockPositions) :-
	on1([[[XA1, YA1], [XA2, YA2]], [[XB1, YB1], [XB2, YB2]]]),
	test_not_over_edge([[[XA1, YA1], [XA2, YA2]], [[XB1, YB1], [XB2, YB1]]]),
	move([[[XA1, YA1], [XA2, YA2]], [[XB1, YB1], [XB2, YB2]]],TopBlockPos,0.1,0.2,0,Seconds2,FinalBlockPositions).

%% on(BlockA, BlockB)
%% Test whether Block A is on Block B

on1([[[_XA1, Y], [_XA2, _YA2]], [[_XB1, _YB1], [_XB2, Y]]]).

on2([[[_XA1, Y], [_XA2, _YA2]], [[_XB1, _YB1], [_XB2, Y]]]).

%% move(Blocks,TopBlockPos,Speed,FrameRate,InitialPositions,FinalBlockPositions)

move(Positions,_TopBlockPos,_,_,1.0,1.0,Positions) :- !.
move([[[IXA1,IYA1],[IXA2,IYA2]],[[IXB1,IYB1],[IXB2,IYB2]]],TopBlockPos,Speed,FrameRate,Seconds1,Seconds2,FinalBlockPositions) :-
	move(IYA1,Speed,FrameRate,FYA1), move(IYA2,Speed,FrameRate,FYA2),
	move(IYB1,Speed,FrameRate,FYB1), move(IYB2,Speed,FrameRate,FYB2),
	Seconds3 is Seconds1+Speed,
	move1([[[IXA1,FYA1],[IXA2,FYA2]],[[IXB1,FYB1],[IXB2,FYB2]]],TopBlockPos,Speed,FrameRate,Seconds3,Seconds2,FinalBlockPositions).

move(IValue,Speed,_FrameRate,FValue) :-
	FValue is IValue+Speed.

move1(Positions,[[XC1, YC1], [XC2, YC2]],_Speed,_FrameRate,Seconds,Seconds,Positions) :-
	Positions = [[[XA1, YA1], [XA2, YC1]], [[_XB1, _YB1], [_XB2, _YB2]]].
	%%on2([[[XC1, YC1], [XC2, YC2]], [[XA1, YA1], [XA2, YA2]]]).
move1([[[XA1,YA1],[XA2,YA2]],[[XB1,YB1],[XB2,YB2]]],[[XC1, YC1], [XC2, YC2]],Speed,FrameRate,Seconds3,Seconds2,FinalBlockPositions) :-
	YC1=\=YA2,
	%%not(on1([[[XC1, YC1], [XC2, YC2]], [[XA1, YA1], [XA2, YA2]]])),
	move([[[XA1,YA1],[XA2,YA2]],[[XB1,YB1],[XB2,YB2]]],[[XC1, YC1], [XC2, YC2]],Speed,FrameRate,Seconds3,Seconds2,FinalBlockPositions).