/* *  Copyright 2002-2005, Mersad Team, Allameh Helli High School (NODET). * *  This program is free software, you can redistribute it and/or modify *  it under the terms of the GNU General Public License as published by *  the Free Software Foundation. * *  This program is distributed in the hope that it will be useful, *  but WITHOUT ANY WARRANTY; without even the implied warranty of *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *  GNU Library General Public License for more details. * *  This file is created by: Ahmad Boorghany *  and is modified by: Mohammad Salehe, Sassan Haradji * *  Released on Monday 1 August 2005, 10 Mordad 1384 by Mersad RoboCup Team. *  For more information please read README file.*/#include <cmath>#include <cassert>#include <Pass.h>#include <Shoot.h>#include <Degree.h>#include <Logger.h>#include <Offense.h>#include <Defense.h>#include <Dribble.h>#include <Command.h>#include <Intercept.h>#include <SRPDribble.h>#include <AdvancedAgent.h>#include <Config.h>#include <RootPlan.h>using namespace std;// Library's fuctionsLibrary::Library(const WorldModel *worldModel): pass(worldModel), positioning(worldModel){	pass.setCurKickCalculate(&worldModel->getCurKickCalculate());}// Form's fuctionsForm::Form(){	reset();}void Form::reset(){	sayForm.reset();	headForm.reset();	setBreakExperiment = 0;}// AdvancedAgent's fuctionsAdvancedAgent::AdvancedAgent(int argc, char **argv): SyncedAgent(argc, argv),		groupWorks(worldModel), sayDecision(worldModel),		headDecision(this), pointToDecision(worldModel),		library(worldModel), radar(worldModel){	statelessPlan = new StatelessPlan(worldModel, form, library);	if (config["Agent"]["PlanningSystem"]["UseHPS"].asBool())		rootPlan = new RootPlan(worldModel, form, library);	else		rootPlan = NULL;}AdvancedAgent::~AdvancedAgent(){	delete statelessPlan;	delete rootPlan;}void AdvancedAgent::senseBody(const SExpression &exp){	SyncedAgent::senseBody(exp);	groupWorks.updateAfterSenseBody();	if (form.setCycle != worldModel->getCurCycle())	{		form.setCycle = worldModel->getCurCycle();		form.reset();	}/*	if (lastDecide == DT_BLOCK ||		lastDecide == DT_INTERCEPT ||		lastDecide == DT_DRIBBLE_INTERCEPT ||		lastDecide == DT_GOALIE_INTERCEPT)		worldModel->setBall().setUpdateByHearPermitted(false);	else*/	worldModel->setBall().setUpdateByHearPermitted(true);	worldModel->updateCurInterCalculate();	worldModel->updateBallStatus();	if (rootPlan)		rootPlan->senseBody(exp);	statelessPlan->senseBody(exp);}void AdvancedAgent::see(const SExpression &exp){	SyncedAgent::see(exp);	worldModel->updateCurInterCalculate();	worldModel->updateBallStatus();	worldModel->updateMarkingPlayers();	statelessPlan->see(exp);}void AdvancedAgent::hear(const SExpression &exp){	SyncedAgent::hear(exp);		SExpAtomic *at;	at = dynamic_cast<SExpAtomic *>(exp[2]);	assert(at);	if (at->toString() == "referee")	{		groupWorks.resetByChangePlayMode();		if (worldModel->checkPlayModeForPenalty() == 1)		{			LOG << "AdvancedAgent::hear : Reversing Field" << endl;			if (!worldModel->getBody().isGoalie())			{				LOG << "Reverse. You are player" << endl;				worldModel->setFieldData().setPenaltySideRate(-1);			}			else				LOG << "NO! Don't Reverse. You are goalie" << endl;  		}		else if (worldModel->checkPlayModeForPenalty() == 2)		{			LOG << "AdvancedAgent::hear : -1 Reversing Field" << endl;			if (worldModel->getBody().isGoalie())			{				LOG << "Reverse. You are goalie" << endl;  				worldModel->setFieldData().setPenaltySideRate(-1);			}			else				LOG << "NO! Don't Reverse. You are player" << endl;   		}	}	if ((worldModel->getHearedHeader() == 'A' ||		 worldModel->getHearedHeader() == 'B') &&		worldModel->getHearedSender() == GOALIE_UNIFORM_NUM &&		worldModel->getHearedInterceptNum() != GOALIE_UNIFORM_NUM &&	    worldModel->getPlayMode() == PM_GOAL_KICK)	{		if (worldModel->getPlayModeRemainCycle() < 199)		{			LOG << "GOAL_KICK --> GOAL_KICK_TAKEN" << endl;			worldModel->setVirtualPlayMode(VPM_GOAL_KICK_TAKEN);		}		else			LOG << "GOAL_KICK --> NO CHANGE TO PLAY MODE" << endl;	}	if (worldModel->getHearedHeader() == 'A' ||		worldModel->getHearedHeader() == 'B')		groupWorks.setInterceptNumByPassSay(				worldModel->getHearedInterceptNum());/*	else if (worldModel->getHearedHeader() == 'Y' ||			worldModel->getHearedHeader() == 'Z')	{		groupWorks.changeInterceptNumByHear(				worldModel->getHearedSender(),				worldModel->getHearedInterceptNum());		groupWorks.setHearedRequestPassNum(				worldModel->getHearedRequestPassNum());		groupWorks.setHearedRequestPassSender(				worldModel->getHearedSender());		groupWorks.setHearedRequestPassTime(worldModel->getCurTime());	}*//*	if (worldModel->getMustAttention())	{		form.sayForm.attentionNum =				worldModel->getHearedInterceptNum();	}*/	worldModel->updateCurInterCalculate();	worldModel->updateBallStatus();	statelessPlan->hear(exp);}bool AdvancedAgent::bodyDecide(int quickLevel){	LOG << "AdvancedAgent::bodyDecide" << endl;	if (isBodyCycleCommandLost() && lastBodyCycleCommand->getCreator() == AT_PASS)		groupWorks.resetByChangePlayMode();	if (form.setCycle != worldModel->getCurCycle())	{		form.setCycle = worldModel->getCurCycle();		form.reset();	}	library.lastBodyCycleCommand = lastBodyCycleCommand;	library.positioning.setHomePoints();	groupWorks.updateInterceptNum(library);	library.gwSelection = groupWorks.getWithoutBallSelection(library);	library.quickLevel = quickLevel;	if (quickLevel == 0 && worldModel->getBall().isValid() &&			worldModel->isBallKickable())	{		worldModel->updateCurKickCalculate();		library.pass.calculate();	}	form.headForm.tnMode = TNM_AUTO_PLAY_ON;	try	{		if (rootPlan && !rootPlan->isFinished() &&				rootPlan->successRate() > 				config["Agent"]["PlanningSystem"]["MinSuccessRate"].asFloat())			rootPlan->decide();		else		{			statelessPlan->decide();		}	}	catch (Command *cmd)	{		bodyCycleCommand = cmd;	}	if (bodyCycleCommand == NULL)		bodyCycleCommand = new EmptyCommand(AT_NONE);	if (form.setBreakExperiment)		library.breakExperiment = form.setBreakExperiment;		if (bodyCycleCommand->getCreator() == AT_POSITIONING ||		bodyCycleCommand->getCreator() == AT_BASIC_AGENT ||		bodyCycleCommand->getCreator() == AT_BASIC_COACH ||		bodyCycleCommand->getCreator() == AT_ADVANCED_COACH ||		bodyCycleCommand->getCreator() == AT_KICK_CALCULATE ||		bodyCycleCommand->getCreator() == AT_CROSS_CALCULATE ||		bodyCycleCommand->getCreator() == AT_SAY_DECISION ||		bodyCycleCommand->getCreator() == AT_HEAD_DECISION ||		bodyCycleCommand->getCreator() == AT_POINTTO_DECISION)		assert(0);	if (worldModel->getBall().getAbsVec().getMagnitude() < 7.5)		return true;	return false;}void AdvancedAgent::sayDecide(){	radar.sender(form, library);	radar.receiver(form, library);/*	try	{		if (rootPlan && !rootPlan->isFinished() && rootPlan->successRate() > 			config["Agent"]["PlanningSystem"]["MinSuccessRate"].asFloat())			rootPlan->sayDecide();		else			statelessPlan->sayDecide();	}	catch (pair<Command *, Command *> cmds)	{		sayCycleCommand = cmds.first;		attentionCycleCommand = cmds.second;	}*/	groupWorks.updateInterceptNum(library);	groupWorks.signInterceptNum(form.sayForm);	sayDecision.decide(bodyCycleCommand, form.sayForm);	sayCycleCommand = sayDecision.getSayCommand();	attentionCycleCommand = sayDecision.getAttentionCommand();}void AdvancedAgent::pointToDecide(){/*	try	{		if (rootPlan && !rootPlan->isFinished() && rootPlan->successRate() > 			config["Agent"]["PlanningSystem"]["MinSuccessRate"].asFloat())			rootPlan->pointToDecide();		else			statelessPlan->pointToDecide();	}	catch (Command *cmd)	{		pointToCycleCommand = cmd;	}*/	pointToDecision.decide(headCycleCommand);	pointToCycleCommand = pointToDecision.getPointToCommand();}void AdvancedAgent::headDecide(){/*	try	{		if (rootPlan && !rootPlan->isFinished() && rootPlan->successRate() > 			config["Agent"]["PlanningSystem"]["MinSuccessRate"].asFloat())			rootPlan->headDecide();		else			statelessPlan->headDecide();	}	catch (pair<Command *, Command *> cmds)	{		LOG << "Catched by Handler" << endl;		headCycleCommand = cmds.first;		changeViewCycleCommand = cmds.second;		LOG << "End Catch" << endl;	}*/	LOG << "AdvancedAgent::headDecide" << endl;	headDecision.decide(bodyCycleCommand, form.headForm);	headCycleCommand = headDecision.getTurnNeckCommand();	changeViewCycleCommand = headDecision.getChangeViewCommand();}// Radar functionsRadarSystem::RadarSystem(const WorldModel *worldModel): worldModel(worldModel){	sayingsNum = 0;	for (int i = 0; i < 3; i++)		sayings[i] = NULL;	semiRandom = 0;	lastUpdateTime = 0;	lastTargetPlayer = NULL;}void RadarSystem::sender(Form &form, const Library &library){	LOG << "RadarSystem::sender" << endl;	if (lastUpdateTime != (int)worldModel->getCurTime())	{		for (int i = 0; i < 3; i++)		{			saids[i] = sayings[i];			sayings[i] = NULL;		}		saidsNum = sayingsNum;		sayingsNum = 0;		lastUpdateTime = worldModel->getCurTime();	}	if (worldModel->getBall().getSeeTime() - worldModel->getCurTime() >= 2 ||		library.gwSelection == WOBS_INTERCEPT ||		worldModel->isBallKickable())		return;	const Player &targetPlayer(worldModel->getNearestTmmToBall());	if (&targetPlayer != lastTargetPlayer)	{		saidsNum = 0;		for (int i = 0; i < 3; i++)			saids[i] = NULL;	}	lastTargetPlayer = &targetPlayer;LOG << "targetPlayer: " << targetPlayer.getUniNum() << endl;	const Player *candidatePlayers[3];	const Player *starredPlayer = NULL;	for (int i = 0; i < 3; i++)		candidatePlayers[i] = NULL;	switch (worldModel->getBody().getUniNum())	{	case 10:		if (targetPlayer.getUniNum() == 2)		{			candidatePlayers[0] = &worldModel->getFullPlayer(TID_TEAMMATE, 10 - 1);			candidatePlayers[1] = &worldModel->getFullPlayer(TID_TEAMMATE, 3 - 1);		}		else if (targetPlayer.getUniNum() == 3)		{			candidatePlayers[0] = &worldModel->getFullPlayer(TID_TEAMMATE, 2 - 1);			candidatePlayers[1] = &worldModel->getFullPlayer(TID_TEAMMATE, 4 - 1);		}		break;	case 11:		if (targetPlayer.getUniNum() == 4)		{			candidatePlayers[0] = &worldModel->getFullPlayer(TID_TEAMMATE, 11 - 1);			candidatePlayers[1] = &worldModel->getFullPlayer(TID_TEAMMATE, 3 - 1);		}		break;	case 6:		if (targetPlayer.getUniNum() == 10)		{			candidatePlayers[0] = &worldModel->getFullPlayer(TID_TEAMMATE, 6 - 1);			candidatePlayers[1] = &worldModel->getFullPlayer(TID_TEAMMATE, 2 - 1);		}		break;	case 9:		if (targetPlayer.getUniNum() == 11)		{			candidatePlayers[0] = &worldModel->getFullPlayer(TID_TEAMMATE, 9 - 1);			candidatePlayers[1] = &worldModel->getFullPlayer(TID_TEAMMATE, 4 - 1);		}		break;	}	if (candidatePlayers[0] == NULL)		return;	form.headForm.tnMode = TNM_LOOK_NORMALLY_TO_BALL;	LOG << "\tTNM_LOOK_NORMALLY_TO_BALL" << endl;	if (worldModel->getBall().getDistance(targetPlayer) > 5)		return;	int counter = 0;	while (!(candidatePlayers[semiRandom] != NULL &&		!isSaid(*candidatePlayers[semiRandom]) &&		candidatePlayers[semiRandom]->isValid()))	{		semiRandom++;		if (semiRandom >= 3)			semiRandom = 0;		counter++;		if (counter == 10) // I have fallen in a loop			return;	}	starredPlayer = candidatePlayers[semiRandom];LOG << "\tstarredPlayer: " << starredPlayer->getUniNum() << endl;	sayings[0] = starredPlayer;	sayingsNum = 1;	anglesNum = 0;	int i;	for (i = 0; i < FULL_PLAYERS_NUM; i++)	{		if (worldModel->getFullPlayer(TID_OPPONENT, i).isValid() &&			!isSaid(worldModel->getFullPlayer(TID_OPPONENT, i)) &&			worldModel->getFullPlayer(TID_OPPONENT, i).getDistance(worldModel->getBall()) <				starredPlayer->getDistance(worldModel->getBall()) + 3 &&			worldModel->getFullPlayer(TID_OPPONENT, i).getDistance(worldModel->getBall()) < 28)		{			angPlayers[anglesNum] = &worldModel->getFullPlayer(TID_OPPONENT, i);			Vector passVec;			Vector enemyVec;			enemyVec.setByPoints(targetPlayer.getPos(), worldModel->getFullPlayer(TID_OPPONENT, i).getPos());			passVec.setByPoints(targetPlayer.getPos(), starredPlayer->getPos());			anglesList[anglesNum] = fabs(Degree::getDeltaAngle(					enemyVec.getDirection(), passVec.getDirection()));LOG << "\tangPlayer[" << anglesNum << "]: " << angPlayers[anglesNum]->getUniNum() << endl;LOG << "\tanglesList[" << anglesNum << "]: " << anglesList[anglesNum] << endl;			anglesNum++;		}	}	if (anglesNum != 0)	{		int miner = 0;		for (i = 1; i < anglesNum; i++)			if (anglesList[i] < anglesList[miner])				miner = i;		sayings[1] = angPlayers[miner];		sayingsNum = 2;LOG << "\tsayings[1]: " << sayings[1]->getUniNum() << endl;		if (anglesNum != 1)		{			anglesList[miner] = (float)0xFFFF; // putting this out			miner = 0;			for (i = 1; i < anglesNum; i++)				if (anglesList[i] < anglesList[miner])					miner = i;			sayings[2] = angPlayers[miner];			sayingsNum = 3;LOG << "\tsayings[2]: " << sayings[2]->getUniNum() << endl;		}	}	Ball ball(worldModel->getBall());	ball.simulateByDynamics(worldModel->getBody());	Player simPlayers[3];	for (int i = 0; i < sayingsNum; i++)	{		simPlayers[i] = *sayings[i];		simPlayers[i].simulateByDynamics(worldModel->getBody());	}	form.sayForm.radarSay = true;	form.sayForm.radarPlayersNum = sayingsNum;	for (int i = 0; i < sayingsNum; i++)	{		Vector vec;		vec.setByPoints(ball.getPos(), simPlayers[i].getPos());		form.sayForm.radarPlayers[i] = vec;		int id = simPlayers[i].getTeamId() * 11 +					simPlayers[i].getUniNum() - 1;		form.sayForm.ids[i] = id;	}}bool RadarSystem::isSaid(const Player &player){	for (int i = 0; i < saidsNum; i++)		if (&player == saids[i])			return true;	return false;}void RadarSystem::receiver(Form &form, const Library &library){	if (worldModel->isBallKickable() ||		(library.gwSelection == WOBS_INTERCEPT &&		 worldModel->getBall().getAbsVec().getMagnitude() < 5))	{	}	else		return;	switch (worldModel->getBody().getUniNum())	{		case 2: case 3:			form.sayForm.attentionNum = 10;			break;		case 4:			form.sayForm.attentionNum = 11;			break;		case 10:			form.sayForm.attentionNum = 6;			break;		case 11:			form.sayForm.attentionNum = 9;			break;	}	LOG << "RadarSystem::receiver setted" << endl;}