%%%=============================================%%%
%%% WASP SOCCER GAME (Single user)			%%%
%%%	Version 1.2 Oct 24, 2003			%%%
%%%								%%%
%%% 	Vrije University Amsterdam			%%%
%%%								%%%
%%% 	(C) Zhisheng Huang				%%%
%%%=============================================%%%

:-object waspsoccer : [bcilib].

%%var url = 'http://wasp.cs.vu.nl/wasp/wasp-soccer/vrml/soccer/soccer5.wrl'.


var url = './soccer6.wrl'.

main :-
	text_area(Browser),
	set_output(Browser),

	format('Load the game ..., Please wait...~n'),
	loadURL(url),
	Clock := new(game_clock),
      _Pulse := new(clock_pulse(Clock)),

      Clock <- get_time1(TimeLeft),
	format('the game will start in 5 seconds,~n'),
	format('the total playing time is ~w seconds,~n', [TimeLeft]),
	delay(5000),

	format('the game startup,~n'),
	_ball := new(ball(ball,  Clock)),
	_GoalKeeper1 := new(goalKeeper(goalKeeper1, Clock)),
	_GoalKeeper2 := new(goalKeeper(goalKeeper2, Clock)),
	_UserMe := new(soccerPlayerUser(me_red10, Clock)),
	_Blue9 := new(soccerPlayer(blue9, Clock)),
	_Blue8 := new(soccerPlayer(blue8, Clock)),
	_Blue7 := new(soccerPlayer(blue7, Clock)),
	_Blue11 := new(soccerPlayer(blue11, Clock)),
	_Red2 := new(soccerPlayer(red2, Clock)),
	_Red3 := new(soccerPlayer(red3, Clock)),
	_Red11 := new(soccerPlayer(red11, Clock)),
%	_Red6 := new(soccerPlayer(red6, Clock)),
	game_score <-setGameScore(red,blue,0,0),
	game_score <- showGameScore.



:-end_object waspsoccer.

:-object game_clock.
%%%	var time_left = 500.
	var time_left = 5000.

	get_time1(Time) :-
		Time := time_left.
	set_time(Time) :-
		time_left := Time.

:-end_object game_clock.


:-object clock_pulse.

	clock_pulse(Clock) :-
		repeat,
			sleep(1000),
			Clock <- get_time1(Time),
			Left is Time - 1,
			Clock <- set_time(Left),
		Left < 1,
		!.

:-end_object clock_pulse.


:-object ball : [bcilib].	%% RS: ball object.

var steps.
var xmax = 50.0.
var xmin = -50.0.
var zmax = 32.0.
var zmin = -32.0.
% var ballstep = 1.5.
var ballSleepTime=300.
var status := unlock.

ball(Name,Clock) :-
	format('~w thread active.~n',[Name]),
	activity(Name,Clock).

activity(Name,Clock) :-
	repeat,
		sleep(5000),
		Clock <-get_time1(TimeLeft),
		format(' ~w seconds left for the game.~n', [TimeLeft]),
		getPosition(Name,X,Y,Z),
		setValidBallPosition(Name,X,Y,Z),
	TimeLeft < 1,
%	format('The game is over!!!~n'),
	game_score <- showGameScore,
	!.


validMinMax(V0, Vmin, Vmax, VN) :-
		askValid_Min(V0, Vmin, V1),
		askValid_Max(V1, Vmax, VN).


	askValid_Min(X, Xmin, Xval) :-
		X < Xmin,
		!,
		Xval = Xmin.

	askValid_Min(X,_Xmin, Xval) :-
		Xval = X.


	askValid_Max(X, Xmax, Xval) :-
		X > Xmax,
		!,
		Xval = Xmax.

	askValid_Max(X,_Xmax, Xval) :-
		Xval = X.


	setValidBallPosition(_Ball,X,_Y,Z):-
		X =< xmax,
		X >= xmin,
		Z =< zmax,
		Z >= zmin,
		!.

	setValidBallPosition(Ball, X, Y, Z) :-
		validMinMax(X, xmin, xmax, Xnew),
		validMinMax(Z, zmin, zmax, Znew),
		setPosition(Ball, Xnew,Y,Znew).




getStatus(_,Status):-
	Status := status.

setStatus(_,Status):-
	status := Status.

isLocked(Ball):-
	getStatus(Ball,Status), 
	Status = lock.

isUnlocked(Ball):-
	getStatus(Ball,Status), 
	Status = unlock.

lock(Ball):-setStatus(Ball,lock).

unlock(Ball):-setStatus(Ball,unlock).



% kickedwithStaticStart(+Ball,+Field,+X,+Y,+Z,+Vx,+Vy,+Vz,+SleepTime).

kickedwithStaticStart(Ball,Field,X,Y,Z,Vx,Vy,Vz,ballSleepTime):-
		kickedSound(Ball),
		Time is Vy/4.9,
		BallSleepTimeInSecond is ballSleepTime/1000,
		steps := 1,
		repeat,
			sleep(ballSleepTime),
			CurrentTime is steps*BallSleepTimeInSecond,
			Xnew is X+ Vx*CurrentTime,
			Ynew is Y+ Vy*CurrentTime-4.9*CurrentTime*CurrentTime,
			Znew is Z+ Vz*CurrentTime,
			setSFVec3f(Ball,Field,Xnew,Ynew,Znew),
			++steps,
		steps > Time//BallSleepTimeInSecond,
		!.

%kickedwithStaticStart(_,_,_,_,_,_,_,_,_).

% kickedwithStaticStartRF(Ball,Field,X,Y,Z,R1,ForceXZ,ForceY)
		
kickedwithStaticStartRF(Ball,Field,X,Y,Z,R1,F1,Vy):-
		Vx is F1 * cos(R1),
		Vz is F1 * sin(R1),
		kickedwithStaticStart(Ball,Field,X,Y,Z,Vx,Vy,Vz,ballSleepTime),
		fallToGround(Ball,Field).


						
%kickedwithStaticStartRF(_,_,_,_,_,_,_,_).		
	

fallToGround(Ball,Field):- 
		getSFVec3f(Ball,Field,X,_Y,Z),
		setSFVec3f(Ball,Field,X,0.25,Z).

kickedSound(Ball) :-
		getSFTime(myTimeSensor, time, Time),
%		Time1 := Time +0.3,
%           Time2 := Time +1.1,
		setSFTime(Ball, kickedSoundStartTime,Time).
%		setSFTime(Ball, kickedSoundStopTime, Time2).
	
:- end_object ball.


:-object goalKeeper : [bcilib,common_process].

var steps.
var holdableDistance = 2.8.
var throwableDistance = 45.0.
var watchableDistance = 30.0.
var stopableDistance = 8.0.
var ballstep = 1.5.
var ballSleepTime = 300.
var kickBallForce = 10.0.
var kickBallForceY = 15.0.
var runstep = 1.5.
var runSleepTime=750.


goalKeeper(Name, Clock) :-
	setSFInt32(Name,whichChoice, 0),
	format('~w thread active.~n', [Name]),
	activity(Name,Clock).

activity(Name,Clock) :-
	repeat,
		sleep(2000),
		Clock <- get_time1(TimeLeft),
%		format('~w thread ~w seconds left.~n', [Name, TimeLeft]),
		setDefaultPosition(Name),
		look_at_ball(Name,ball),
		getPositionInformation(Name,ball,X,Y,Z,Xball,Yball,Zball,Dist),
		findHowtoReact(Name,ball,X,Y,Z,Xball,Yball,Zball,Dist,Action),
		nonvar(Action),
		format('~w action: ~w ~n',[Name,Action]),
		doAction(Action,Name,ball,X,Y,Z,Xball,Yball,Zball,Dist),
	TimeLeft < 1,
	quitGame(Name),
	!.




stadiumExit(0.0, 0.0, -50.0).

quitGame(Player):-
	format('~w is gone.~n',[Player]),
	getSFVec3f(Player, position, X,Y,Z),
	stadiumExit(X1, Y1,Z1),
	distance2D(X,Z,X1,Z1,Dist),
	run_to_position(Player,X,Y,Z,X1,Y1,Z1,Dist),
	setSFInt32(Player,whichChoice, -1).



getPositionInformation(Player,Ball,X,Y,Z,X1,Y1,Z1,Dist) :-
	getPosition(Ball,X1,Y1,Z1),
	getSFVec3f(Player,position,X,Y,Z),
	distance2D(X,Z,X1,Z1,Dist).

%getPositionInformation(_Player,_Ball,_X,_Y,_Z,_X1,_Y1,_Z1,_Dist). 

%findHowtoReact(+Player,+Ball,+X,+Y,+Z,+Xball,+Yball,+Zball,+Dist,-Action).


findHowtoReact(Player,_,_,_,_,X1,_,Z1,_Dist,record_score_then_throw):-
	goalArea(Player,_,GoalXMin,GoalXMax,GoalZMin,GoalZMax),
	GoalXMin =< X1,
	GoalXMax >= X1,
	GoalZMin =< Z1,
	GoalZMax >= Z1,
	!.
%	format('ball position: x=~w, z=~w, dist to ~w: ~w:record throw~n',[X1,Z1,Player,Dist]).

findHowtoReact(_Player,_,_,_,_,_X1,_,_Z1,Dist,run_then_kick):-
	Dist > holdableDistance,
	Dist =< stopableDistance,
	!.


findHowtoReact(_Player,_,_,_,_,_,_,_,Dist,hold_then_throw):-
	Dist =< holdableDistance,
	!.
% format('Dist to ~w is ~w:action hold then throw!~n',[Player,Dist]).
	

findHowtoReact(_,_,_,_,_,_,_,_,Dist,do_nothing):-
	Dist > watchableDistance,
	!.

findHowtoReact(_,_,_,_,_,_,_,_,Dist,do_nothing):-
	var(Dist),
	!.


findHowtoReact(_,_,_,_,_,_,_,_,Dist,ready_for_goal):-
	Dist > stopableDistance,
	Dist < watchableDistance,
	!.


findHowtoReact(_,_,_,_,_,_,_,_,_,_):-
	format('~n~w : findHowtoReact : CASE UNDEFINED (SHOULD NEVER HAPPEN)~n~n', [this]).



%GoalArea(Player,Team,GoalXMin,GoalXMax,GoalZMin,GoalZMax).

goalArea(goalKeeper1, blue, 47.0, 50.0, -3.5, 3.5):-
	!.

goalArea(goalKeeper2, red, -50.0, -47.0, -3.5, 3.5).

defaultPosition(goalKeeper1, 47.0, 1.8, 0.0):-
	!.
defaultPosition(goalKeeper2, -47.0, 1.8, 0.0).

setDefaultPosition(GoalKeeper):-
	defaultPosition(GoalKeeper,X,Y,Z),
	setSFVec3f(GoalKeeper,position,X,Y,Z).

%ballThrowPosition(Player,Position,X,Z).

ballThrowPosition(goalKeeper1, top, 5.0, -10.0).
ballThrowPosition(goalKeeper2, bottom,  -5.0, 10.0).

%doAction(GoalKeeper,Ball,X,Y,Z,Xball,Yball,Zball,Dist,Action).

doAction(do_nothing, _GoalKeeper,_,_,_,_,_,_,_,_):-
		!.


doAction(hold_then_throw,GoalKeeper,Ball,X,Y,Z,X1,Y1,Z1,_):-
		format('~w action: hold_then_throw.~n',[GoalKeeper]),
		Y2 is Y-0.3,
		Z2 is Z+0.2,
		setSFVec3f(Ball,translation,X,Y2,Z2),
		ballThrowPosition(GoalKeeper,_,X3,Z3),
		look_at_position(GoalKeeper,X3,Z3), 
		sleep(500),	
		getRotation(GoalKeeper,_,_,_,R),
		R1 is R -1.5708,
		getSFTime(myTimeSensor,time,Time),
		setSFTime(GoalKeeper,set_gesture5,Time),
		ball <- kickedwithStaticStartRF(Ball,translation,X1,Y1,Z1,R1,kickBallForce,kickBallForceY),
		!.

		
doAction(run_then_kick, GoalKeeper,Ball,_X,Y,_Z,X1,Y1,Z1,_):-
		format('~w action: run_then_kick.~n',[GoalKeeper]),
		setSFVec3f(GoalKeeper,set_position,X1,Y,Z1),
		ballThrowPosition(GoalKeeper,_,X3,Z3),
		look_at_position(GoalKeeper,X3,Z3), 
		getRotation(GoalKeeper,_,_,_,R),
		R1 is R -1.5708,
		getSFTime(myTimeSensor,time,Time),
		setSFTime(GoalKeeper,set_gesture3,Time),
		ball <- kickedwithStaticStartRF(Ball,translation,X1,Y1,Z1,R1,kickBallForce,kickBallForceY),
		!.




doAction(record_score_then_throw,GoalKeeper,Ball,X,Y,Z,X1,Y1,Z1,_):-
		!,
		format('one more score!!!~n'),
		opp_team(GoalKeeper,OppTeam),
		game_score <- getTeamScore(OppTeam,Score),
		nonvar(Score),
		NewScore is Score +1,
		game_score <- setTeamScore(OppTeam,NewScore),
		game_score <- showGameScore,
		setSFVec3f(GoalKeeper,position,X1,Y,Z1),
%		goalkeeper walks to the ball
		getSFTime(myTimeSensor,time,Time),
		setSFTime(GoalKeeper,set_gesture2,Time),
		sleep(1000),
		setSFVec3f(GoalKeeper,position,X,Y,Z),
%		goalKeeper back to the standard position
		doAction(hold_then_throw,GoalKeeper,Ball,X,Y,Z,X1,Y1,Z1,_).
		
doAction(ready_for_goal,GoalKeeper,_,_,_,_,_,_,_,_):-
	getSFTime(myTimeSensor,time,Time),
	setSFTime(GoalKeeper,set_gesture4,Time),
	!.

doAction(Action,_,_,_,_,_,_,_,_,_) :-
	format('~n~w : UNKNOWN ACTION = ~w~n', [this, Action]).




	

kick_ball_to_position(Ball,X,Y,Z,X1,Y1,Z1,Dist):-
	move_to_position(Ball,translation, ballstep, ballSleepTime, X,Y,Z,X1,Y1,Z1,Dist).

run_to_position(Player,X,Y,Z,X1,Y1,Z1,Dist):-
	look_at_position(Player,X1,Z1),
	move_to_position(Player,set_position,runstep,runSleepTime,X,Y,Z,X1,Y1,Z1,Dist).




opp_team(goalKeeper1,red):-
	!.
opp_team(goalKeeper2,blue).

:- end_object goalKeeper.


:-object soccerPlayer : [common_process].

var allplayers = [goalKeeper2, red2, red3, goalKeeper1, blue9, blue8, blue7].
var runstep = 1.0.
var runSleepTime=500.
var ballstep = 1.5.
var ballSleepTime=250.
var kickableDistance = 1.5.
var runableDistance =40.0.
var kickableGoalDistance = 30.0.
%var watchDistance =40.0.
var passableDistanceMax = 40.0.
var passableDistanceMin = 3.0.
var kickBallForce = 10.0.
var kickBallForceY =6.0.
var steps.
var balldistance.
var xold.
var zold.

soccerPlayer(Name, Clock) :-
	setSFInt32(Name,whichChoice, 0),
	format('~w thread active.~n', [Name]),
	setDescription('WASP SOCCER (C)Zhisheng Huang 2000-2004, Vrije University Amsterdam'),
	activity(Name,Clock).

activity(Name,Clock) :-
	repeat,
		sleep(2000),
		Clock <- get_time1(TimeLeft),
%		format(' player ~w  thread ~w seconds left~n', [Name,TimeLeft]),
		look_at_ball(Name,ball),
		getPositionInformation(Name,ball,X,Y,Z,Xball,Yball,Zball,Dist,Xgoal,Zgoal,Dist1),
		findHowtoReact(Name,ball,X,Y,Z,Xball,Yball,Zball,Dist,Xgoal,Zgoal,Dist1,Action),
		nonvar(Action),
		format('player ~w action: ~w~n',[Name,Action]),
		doAction(Action,Name,ball,X,Y,Z,Xball,Yball,Zball,Dist,Xgoal,Zgoal,Dist1),
	TimeLeft < 1,
	quitGame(Name),
	!.









%findHowtoReact(+Player,+Ball,+X,+Y,+Z,+Xball,+Yball,+Zball,+Distball,+Xgoal,+Zgoal,+Distgoal,-Action).

findHowtoReact(_,Ball,_,_,_,_,_,_,Dist,_,_,Dist1,shooting):-
	ball <- isUnlocked(Ball),
	Dist =< kickableDistance,
	Dist1 =< kickableGoalDistance,
	!.
	
findHowtoReact(_,Ball,_,_,_,_,_,_,Dist,_,_,Dist1,passing):-
	ball <- isUnlocked(Ball),
	Dist =< kickableDistance,
	Dist1 > kickableGoalDistance,
	!.

findHowtoReact(Player,_,_,_,_,X1,Y1,Z1,Dist,_,_,_,run_to_ball):-
	Dist > kickableDistance,
	getFieldAreaInformation(Player,_,_,FieldMin,FieldMax),
	FieldMin =< X1,
	FieldMax >= X1,
	nearest(Player, Dist, position(X1,Y1,Z1)),
	!.


findHowtoReact(Player,_,_,_,_,X1,_,_,Dist,_,_,_,move_around):-
	Dist > kickableDistance,
	getFieldAreaInformation(Player,_,_,FieldMin,_),
	X1 < FieldMin,
	!.

findHowtoReact(Player,_,_,_,_,X1,_,_,Dist,_,_,_,move_around):-
	Dist > kickableDistance,
	getFieldAreaInformation(Player,_,_,_,FieldMax),
	X1 > FieldMax,
	!.


findHowtoReact(_,_,_,_,_,_,_,_,_,_,_,_,_).

%getFiledAreaInformation(+Player,-Team,-Role,-FieldMin,-FieldMax).

getFieldAreaInformation(Player,Team,Role,FieldMin,FieldMax):-
	playerRole(Player,Role),
	team(Team,Player),
	fieldArea(Team,Role,FieldMin,FieldMax).



doAction(shooting,Player,Ball,_,_,_,X1,Y1,Z1,_,Xgoal,Zgoal,Dist1):-
	ball <- isUnlocked(Ball),
	ball <- lock(Ball),
	getSFTime(myTimeSensor,time,Time),
	setSFTime(Player,set_gesture3,Time),
	kick_ball_to_position(Ball,X1,Y1,Z1,Xgoal,0.0,Zgoal,Dist1),
	ball <- unlock(Ball),
	!.


doAction(passing,Player,Ball,X,Y,Z,X1,Y1,Z1,_,_,_,_):-
	ball <- isUnlocked(Ball),
	ball <- lock(Ball),
	findCoTeammate(Player,X,Y,Z,Teammate,X2,_,Z2,_),
	!,
	doPassing(Player,Ball,X,Y,Z,X1,Y1,Z1,Teammate,X2,_,Z2),
	ball <- unlock(Ball).
	

doAction(run_to_ball,Player,Ball,X,Y,Z,_X1,_Y1,_Z1,_Dist,_,_,_):-
		!,
		run_and_trace(Player,Ball,X,Y,Z,kickableDistance,runableDistance,runstep,runSleepTime).
	
	
doAction(move_around,Player,_,X,Y,Z,_,_,_,_,_,_,_):-
	!,
	getFieldAreaInformation(Player,_,_,FieldMin,FieldMax),
	FieldMid is (FieldMin+FieldMax)/2,
	distance2D(X,Z,FieldMid,Z,Dist),
	run_to_position(Player,X,Y,Z,FieldMid,Y,Z,Dist).

	
		
doAction(Action,_,_,_,_,_,_,_,_,_,_,_,_) :-
	format('~n~w : unknown action = ~w~n', [this, Action]).	


doPassing(Player,Ball,X,_,_,X1,Y1,Z1,Teammate,X2,_,Z2):-
	nonvar(Teammate),
	format('player ~w is passing the ball to the teammate ~w.~n', [Player,Teammate]),
	kickBalltoDirection(Player,Ball,X,_,_,X1,Y1,Z1,X2,_,Z2),
	!.


doPassing(Player,Ball,X,_,_,X1,Y1,Z1,Teammate,_,_,_):-
	var(Teammate),
	team(Team,Player),
	goalPosition(Team,X2,Z2),
	format('player ~w is kicking the ball.~n', [Player]),
	kickBalltoDirection(Player,Ball,X,_,_,X1,Y1,Z1,X2,_,Z2),
	!.



doPassing(_Player,_Ball, _X,_Y,_Z,_X1,_Y1,_Z1,_Teammate,_X2,_Y2,_Z2).

kickBalltoDirection(Player,Ball,X,_,_,X1,Y1,Z1,X2,_,Z2):-
	look_at_position(Player,X2,Z2),
	getRotation(Player,_,_,_,R),
	getCorrectKickRotation(X,X1,R,R1),
	KickBallForceY is kickBallForceY + random*10.0,
	getSFTime(myTimeSensor,time,Time),
	setSFTime(Player,set_gesture3,Time),
	ball <- kickedwithStaticStartRF(Ball,translation,X1,Y1,Z1,R1,kickBallForce,KickBallForceY).

%kickBalltoDirection(_Player, _Ball,_X,_Y,_Z,_X1,_Y1,_Z1,_X2,_Y2,_Z2).

getCorrectKickRotation(X,X1,R,R1):-
		X =< X1,
		R1 is R - 1.5708.

getCorrectKickRotation(X,X1,R,R1):-
		X > X1,
		R1 is 1.5708 - R.


getPositionInformation(Player,Ball,X,Y,Z,X1,Y1,Z1,Dist,Xgoal,Zgoal,Dist1) :-
	getPosition(Ball,X1,Y1,Z1),
	getSFVec3f(Player,position,X,Y,Z),
	distance2D(X,Z,X1,Z1,Dist),
	distanceGoal(Player,X,Y,Z,Xgoal,Zgoal,Dist1).


run_to_position(Player,X,Y,Z,X1,Y1,Z1,Dist):-
	look_at_position(Player,X1,Z1),
	move_to_position(Player,set_position,runstep,runSleepTime,X,Y,Z,X1,Y1,Z1,Dist).

%run_and_trace(+Player,+Ball,+X,+Y,+Z,+kickableDistance,+RunableDist,+runstep,+runSleepTime).
run_and_trace(Player,Ball,X,Y,Z,kickableDistance,Dist,runstep,runSleepTime):-
		xold := X,
		zold := Z,
		repeat,
			sleep(runSleepTime),
			getPosition(Ball,X1,_,Z1),
			look_at_position(Player,X1,Z1),
			getRotation(Player,_,_,_,R),
			R1 is R + 1.5708,
			Xnew is xold-runstep * cos(R1),
			Znew is zold+runstep * sin(R1),
			setSFVec3f(Player,set_position, Xnew, Y, Znew),
			xold := Xnew,
			zold := Znew,
			distance2D(Xnew,Znew,X1,Z1,Distball),
			balldistance := Distball,
  			end_of_run_and_trace(balldistance, kickableDistance, Dist),
                !.

run_and_trace(_Player,_Ball,_X,_Y,_Z,_KickableDist,_Dist,_Runstep,_RunSleepTime).       


end_of_run_and_trace(BallDist, KickDist, _Dist) :-
        BallDist =< KickDist,
        !.

end_of_run_and_trace(BallDist, _KickDist, Dist) :-
        BallDist > Dist.



nearest(Player, D, Pos):-
		partner(Player,P1,P2),
		nonvar(P1),
		nonvar(P2),
		getPlayerDistance(P1,Pos,D1),
		getPlayerDistance(P2,Pos,D2),
		D =< D1,
		D =< D2.
		

getPlayerDistance(Player,position(X,_Y,Z),D):-
		getSFVec3f(Player,position,X1,_Y1,Z1),
		distance2D(X,Z,X1,Z1,D).		





team(red, goalKeeper2):-!.
team(red, red2):-!.
team(red, red3):-!.
team(blue, goalKeeper1):-!.
team(blue, blue9):-!.
team(blue, blue8):-!.
team(blue, blue7):-!.
%team(red, red10):-!.
team(blue,blue11):-!.
team(red, red11):-!.
%team(red, red6):-!.

partner(red2, red3, red11):-!.
partner(red3, red2, red11):-!.
partner(red11, red3, red2):-!.
partner(blue9, blue8, blue11):-!.
partner(blue8, blue9, blue11):-!.
partner(blue11, blue8, blue9):-!.
partner(blue7, blue8, blue11):-!.

partner(_,_,_).


playerRole(red2, midfielder):-!.
playerRole(red3, forward):-!.
playerRole(blue7,midfielder):-!.
playerRole(blue8,forward):-!.
playerRole(blue9,defender):-!.
%playerRole(red10,midfielder):-!.
playerRole(red11,defender):-!.
playerRole(blue11,forward):-!.
%playerRole(red6, forward):-!.

goalPosition(red, 49.0, -3.0):-!.
goalPosition(blue, -49.0, -3.0):-!.

fieldArea(red, midfielder,-25.0,20.0):-!.
fieldArea(blue,midfielder,-20.0,25.0):-!.
fieldArea(red,defender,-50.0,-25.0):-!.
fieldArea(blue,defender, 25.0,50.0):-!.
fieldArea(blue,forward,-50.0,20.0):-!.
fieldArea(red,forward,20.0,50.0):-!.


stadiumExit(0.0, 0.0, -50.0):-!.

quitGame(Player):-
	format('~w is gone.~n',[Player]),
	getSFVec3f(Player, position, X,Y,Z),
	stadiumExit(X1, Y1,Z1),
	distance2D(X,Z,X1,Z1,Dist),
	run_to_position(Player,X,Y,Z,X1,Y1,Z1,Dist),
	setSFInt32(Player,whichChoice, -1).

teammate(Player1,Player2,Team) :-
	team(Team,Player1),
	team(Team,Player2),
	Player1 \== Player2.

%teammateRole(Player+,Team+,Role+,Mate-).

teammateRole(Player,Team,Role,Mate) :-
	team(Team,Mate),
	Player \== Mate,
	playerRole(Mate,Role).





distanceGoal(Player,X,_,Z,X1,Z1,Dist) :-
	team(Team,Player),
	goalPosition(Team,X1,Z2),
	Z1 := Z2 + random* 7.0,
% set goal shooting point as a random point around the gate
	distance2D(X,Z,X1,Z1,Dist).




kick_ball_to_position(Ball,X,Y,Z,X1,Y1,Z1,Dist):-
	move_to_position(Ball,translation, ballstep, ballSleepTime, X,Y,Z,X1,Y1,Z1,Dist).
 

%findCoTeammate(Player+,X+,Y+,Z+,Teammate-,X1-,Y1-,Z1-,Dist-).

findCoTeammate(Player,X,Y,Z,Teammate,X1,Y1,Z1,Dist):-
	playerRole(Player,Role),
	roleOrder(Role,TeammateRole,attack),
%	format('Teammate role is ~w~n',[TeammateRole]),
	findRoleTeammate(Player,X,Y,Z,TeammateRole,Teammate,X1,Y1,Z1,Dist),
	nonvar(Dist),
	Dist =< passableDistanceMax,
	Dist 	> passableDistanceMin,
	!.

findCoTeammate(_,_,_,_,_,_,_,_,_).	


%roleOrder(Role1, Role2, strategy).

roleOrder(midfielder,forward,attack):-!.
roleOrder(defender,midfielder,attack):-!.
roleOrder(forward,forward,attack):-!.
roleOrder(midfielder,midfielder,attack):-!.
roleOrder(defender,defender,attack):-!.

%findRoleTeammate(Player+,X+,Y+,Z+,Role+,Mate-,X1-,Y1-,Z1-,Dist-).

findRoleTeammate(Player,X,_,Z,Role,Mate,X1,Y1,Z1,Dist):-
		team(Team,Player),
		teammateRole(Player,Team,Role,Mate),
%		format('Mate is ~w~n',[Mate]),
		nonvar(Mate),
		getSFVec3f(Mate,position,X1,Y1,Z1),
		distance2D(X,Z,X1,Z1,Dist),
		!.



findRoleTeammate(_,_,_,_,_,_,_,_,_,_).
				

:- end_object soccerPlayer.


:-object soccerPlayerUser : [common_process].

var kickableDistance = 3.0.
var kickBallForce = 10.0.
var kickBallForceY =6.0.

soccerPlayerUser(Name, Clock) :-
	format('The user ~w thread active.~n', [Name]),
%	enableMyAvatar(Name),
	activity(Name,Clock).

activity(Name,Clock) :-
	repeat,
		sleep(1000),
		Clock <- get_time1(TimeLeft),
%		format(' The player ~w  thread ~w seconds left~n', [Name, TimeLeft]),
		near_ball_then_kick(Name,ball),
%		controlMyAvatar(Name),
	TimeLeft < 1,
	!.



near_ball_then_kick(Agent, Ball):-
	getViewpointPositionEx(Agent,X,_,Z),
	getPosition(Ball,X1,Y1,Z1),
	X < X1,
	distance2D(X,Z,X1,Z1,Dist),
	Dist < kickableDistance,
	getViewpointOrientationEx(Agent,_,_,_,R),
	R1 is R -1.5708,
	ball <- isUnlocked(Ball),
	ball <- lock(Ball),
	ball <- kickedwithStaticStartRF(Ball,translation,X1,Y1,Z1,R1,kickBallForce,kickBallForceY),
	ball <- unlock(Ball).

near_ball_then_kick(Agent, Ball):-
	getViewpointPositionEx(Agent,X,_,Z),
	getPosition(Ball,X1,Y1,Z1),
	X >= X1,
	distance2D(X,Z,X1,Z1,Dist),
	Dist < kickableDistance,
	getViewpointOrientationEx(Agent,_,_,_,R),
	R1 is -R -1.5708,
	ball <- isUnlocked(Ball),
	ball <- lock(Ball),
	ball <- kickedwithStaticStartRF(Ball,translation,X1,Y1,Z1,R1,kickBallForce,kickBallForceY),
	ball <- unlock(Ball).


near_ball_then_kick(_, _).



:- end_object soccerPlayerUser.

:-object game_score : [bcilib].

var score1.
var score2.
var team1.
var team2.

setGameScore(Team1, Team2, Score1, Score2):-
		team1:= Team1,
		team2:= Team2,
		score1:= Score1,
		score2:= Score2.


getGameScore(Team1, Team2, Score1, Score2):-
		Team1 := team1,
		Team2 := team2,
		Score1 := score1,
		Score2 := score2.




getTeamScore(Team, Score):-
		Team = team1,
		Score := score1.

getTeamScore(Team, Score):-
		Team = team2,
		Score := score2.

setTeamScore(Team, Score):-
		Team = team1,
		score1:=Score.

setTeamScore(Team, Score):-
		Team = team2,
		score2:=Score.		



showGameScore :-
                getGameScore(Team1, Team2, Score1, Score2),
                format('game score: ~w:~w---~w:~w~n',[Team1,Team2,Score1,Score2]),
                number_chars(Score1,Score1C),
                number_chars(Score2,Score2C),
                atom_chars(Score1A,Score1C),
                atom_chars(Score2A,Score2C),
		    atom_list_concat([Score1A, ':', Score2A], TheScore),
                setMFString(scorePlate1,score,[TheScore]),
                setMFString(scorePlate2,score,[TheScore]).

:-end_object game_score.


:-object common_process : [bcilib].
var steps.

distance2d(X,Z,X1,Z1,Dist):-
	Xdif is X1-X,
	Zdif is Z1-Z,
	Dist is sqrt(Xdif*Xdif + Zdif*Zdif).

distance3d(X,Y,Z,X1,Y1,Z1,Dist):-
	Xdif is X1-X,
	Ydif is Y1-Y,
	Zdif is Z1-Z,
	Dist is sqrt(Xdif*Xdif + Ydif*Ydif+Zdif*Zdif).

distance2dObject(Object1,Field1,Object2,Field2,Dist):-
	getSFVec3f(Object1, Field1, X1, _, Z1),
	getSFVec3f(Object2, Field2, X2, _, Z2),
	distance2D(X1,Z1,X2,Z2,Dist).

distance3dObject(Object1,Field1,Object2,Field2,Dist):-
	getSFVec3f(Object1, Field1, X1, Y1, Z1),
	getSFVec3f(Object2, Field2, X2, Y2, Z2),
	distance3d(X1,Y1,Z1,X2,Y2,Z2,Dist).


distanceBall(Player,Ball, Dist) :-
	distance2dObject(Player,position,Ball,position,Dist).


distancePlayer(Player1,Player2, Dist) :-
	distance2dObject(Player1,position,Player2,position,Dist).


look_at_ball(Player,Ball) :-
	getSFVec3f(Player,position, X,_,Z),
	getPosition(Ball, X1,_,Z1),
	X =\= X1,
	!,	%% RS: added
	Xdif is X-X1,
	Zdif is Z1-Z,
	R is atan(Zdif/Xdif) - sign(X-X1)*1.5708,
	setRotation(Player,0.0, 1.0, 0.0, R).

look_at_ball(_,_).	%% RS: catch ball?

look_at_position(Player,X1,Z1) :-
	getSFVec3f(Player,position, X,_,Z),
	X =\= X1,
	!,	%% RS: added
	Xdif is X-X1,
	Zdif is Z1-Z,
	R is atan(Zdif/Xdif) - sign(X-X1)*1.5708,
	setRotation(Player,0.0, 1.0, 0.0, R).


look_at_position(_,_,_).	



%move_to_position(+Object,+Field,+Steplength,+Sleeptime,+X,+Y,+Z,+X1,+Y1,+Z1,+Dist).

move_to_position(_,_,_,_,X,_,Z,X,_,Z,_):-!.

move_to_position(Object,Field, StepUnit, SleepTime, X,Y,Z,X1,_,Z1,Dist) :-
	Xstep is sign(X-X1)*StepUnit*abs(X-X1)/Dist,
	Zstep is sign(Z-Z1)*StepUnit*abs(Z-Z1)/Dist,
	steps := Dist // StepUnit,
	repeat,
		sleep(SleepTime),
		Steps is Dist//StepUnit-steps+1,
		Xnew is X - Xstep*Steps,
		Znew is Z - Zstep*Steps,
		setSFVec3f(Object,Field,Xnew,Y,Znew),
		--steps,
	steps < 1,
	!.




move_to_position(_,_,_,_,_,_,_,_,_,_,_).

getViewpointPositionEx(_,X,Y,Z) :- getSFVec3f(proxSensor,position,X,Y,Z).
getViewpointOrientationEx(_,X,Y,Z,R):- getSFRotation(proxSensor,orientation,X,Y,Z,R).



:-end_object common_process.