/* *  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: Darioush Jalali, Mohammad Salehe * *  Released on Monday 1 August 2005, 10 Mordad 1384 by Mersad RoboCup Team. *  For more information please read README file.*/#include <AdvancedAgent.h>#include <Logger.h>#include <cmath>#include <BasicDash.h>#include <GoalPlan.h>#include <ShootPlan.h>#include <DribblePlan.h>#include <SRPDribblePlan.h>#include <PassPlan.h>#include <InterceptPlan.h>#include <GoalStraightPosPlan.h>#include <OffensePosPlan.h>#include <CrossPlan.h>#include <Basics.h>#include <Degree.h>#include <cassert>using namespace std;using namespace Basics;using namespace Degree;GoalPlan::GoalPlan(const WorldModel *wm, Form &form, const Library &library):				Plan("Goal", wm, form, library){		config.add("GoalPlan");		config["GoalPlan"].setByFile("./Configs/GoalPlan.conf");		LOG << "Loading goal  from conf..." << endl;	/*		30 = very good		27 = good		23 = normal		20 = not good (oh my god)	*/	passClasses.reserve(4);	passClasses.push_back(.30);		passClasses.push_back(.27);	passClasses.push_back(.24);		passClasses.push_back(.23);}GoalPlan::~GoalPlan(){}void GoalPlan::decide(){		LOG << "GoalPlan::decide" << endl;		if (wm->isBallKickable())		{/*				startPlan(new ShootPlan(wm, form, library), .3);				Plan::decide();					vector <string> priorities;				unsigned uniNum = 0;				bool AInPassArea = false;				bool BInPassArea = false;				uniNum = findBestShooter("234AB");				if (uniNum)				{					string pushP;					pushP += uniNumToChar(uniNum);					priorities.push_back(pushP);				}				switch (wm->getBody().getUniNum()) {					case 2:						if (wm->getBody().getPos().getX() < 36)						{							priorities.push_back("A");							priorities.push_back("3");							priorities.push_back("4");						}						else						{							priorities.push_back("3");							priorities.push_back("4");							priorities.push_back("A");						}//						priorities.push_back("23456789AB");						break;					case 4:						if (wm->getBody().getPos().getX() < 36)						{								priorities.push_back("B");								priorities.push_back("3");								priorities.push_back("2");						}						else						{								priorities.push_back("3");								priorities.push_back("2");								priorities.push_back("B");						}//						priorities.push_back("23456789AB");												break;					case 3:						uniNum = findBestShooter("24");						LOG << "Player 3 -> BestShooter(24)=" << uniNum << endl;						if (uniNum) {							string firstP;							firstP += uniNumToChar(uniNum);							string secondP;							if (uniNum == 2) 								secondP = "4";							else								secondP = "2";							priorities.push_back(firstP);							priorities.push_back(secondP);						} else 							priorities.push_back("24");						if (wm->getFullPlayer(TID_TEAMMATE, 9).isValid())							if (wm->getFullPlayer(TID_TEAMMATE, 9).getPos().getX() > 36 &&								abs(wm->getFullPlayer(TID_TEAMMATE, 9).getPos().getY()) < 20)								AInPassArea = true;						if (wm->getFullPlayer(TID_TEAMMATE, 10).isValid())							if (wm->getFullPlayer(TID_TEAMMATE, 10).getPos().getX() > 36 &&								abs(wm->getFullPlayer(TID_TEAMMATE, 10).getPos().getY()) < 20)								BInPassArea = true;						if (AInPassArea && !BInPassArea)							priorities.push_back("A");						else if (!AInPassArea && BInPassArea)							priorities.push_back("B");						else if (AInPassArea && BInPassArea)						{							uniNum = findBestShooter("AB");							LOG << "Player 3 -> BestShooter(AB)=" << uniNum << endl;							if (uniNum) {								string firstP;								firstP += uniNumToChar(uniNum);								string secondP;								if (uniNum == 9) 									secondP = "B";								else									secondP = "A";								priorities.push_back(firstP);								priorities.push_back(secondP);							} else 								priorities.push_back("AB");						}//						priorities.push_back("23456789AB");						break;					case 10:						priorities.push_back("2B");							priorities.push_back("3");	//						priorities.push_back("23456789AB");							break;					case 11:						priorities.push_back("4A");							priorities.push_back("3");	//						priorities.push_back("23456789AB");							break; *				}				*					note: push_back("23456789AB") is COed because of IQlogical problems....				*				startPassPlanWithPriorities(passClasses, priorities, true);				startPlan(bestPlan(3, true, new DribblePlan(wm, form, library),											 new ShootPlan(wm, form, library),											 new PassPlan(wm, form, library)), .22);				startPlan(bestPlan(4, true,									 new PassPlan(wm, form, library),									 new ShootPlan(wm, form, library),									 new DribblePlan(wm, form, library),									 new SRPDribblePlan(wm, form, library)), 0.15);				LOG << "Goal Plan WARNING!!! : "						<< "All WithBall Plans Finished Without Return Any Command." << endl;				LOG << "Goal Plan : "						<< "Now Run Dribble Plan Without Check SuccessRate or isFinished"						<< "(Urgent State)." << endl;				DribblePlan dribblePlan(wm, form, library);				dribblePlan.decide();*/			// Welcome to A&M security tofs			// Do not change this unless you are passed			//         OFFICIAL PROCEDURE			Plan *passPlan = new PassPlan(wm, form, library);			float passVal = passPlan->successRate();			Plan *shootPlan = new ShootPlan(wm, form, library);			float shootVal = shootPlan->successRate();			Plan *dribblePlan = new DribblePlan(wm, form, library);			float dribbleVal = dribblePlan->successRate();			Plan *srpDribblePlan = new SRPDribblePlan(wm, form, library);			float srpDribbleVal = srpDribblePlan->successRate();			LOG << "A&M security values: " << endl;			LOG << "\tshootVal = " << shootVal << endl;			LOG << "\tpassVal = " << passVal << endl;			LOG << "\tsrpDribbleVal = " << srpDribbleVal << endl;			LOG << "\tdribbleVal = " << dribbleVal << endl;			if (shootVal >= dribbleVal &&				shootVal >= srpDribbleVal &&				shootVal >= passVal)			{ // Shoot				delete passPlan;				delete dribblePlan;				delete srpDribblePlan;				startPlan(shootPlan);			}			else if (passVal >= dribbleVal && passVal >= srpDribbleVal)			{ // Pass				delete shootPlan;				delete dribblePlan;				delete srpDribblePlan;				startPlan(passPlan);			}			else if (srpDribbleVal >= dribbleVal)			{ // SRPDribble				delete passPlan;				delete shootPlan;				delete dribblePlan;				startPlan(srpDribblePlan);			}			else			{ // Dribble				delete passPlan;				delete shootPlan;				delete srpDribblePlan;				dribblePlan->decide();			}		}		else		{			if (!wm->isBallKickable() &&					library.gwSelection == WOBS_OFFENSE)			{				Plan::resetPlans();				startPlan(new GoalStraightPosPlan(wm, form, library));			}					Plan::resetPlans();				if (library.gwSelection == WOBS_INTERCEPT)					startPlan(new InterceptPlan(wm, form, library));				startPlan(new OffensePosPlan(wm, form, library));		}		LOG << "GoalPlan Now Kill Program" << endl;		assert(0);}double GoalPlan::successRate(){		return 1.0;}bool GoalPlan::isFinished(){	if (wm->getBody().getPos().getX() >= 			config["Agent"]["PlanningSystem"]["GoalPlanX"].asFloat() - 5)		return false;//		if ((wm->getBallStatus() == BS_KICKABLE_OPP) ||//				(wm->getBallStatus() == BS_FREE_BALL_OPP))//				return true;	return true;}voidGoalPlan::startPassPlanWithPriorities(const vector <float>& passClasses, const vector <string>& passPriorities,										bool tryDribble) {	for (unsigned i =0 ; i < passClasses.size(); ++i) {		LOG << "passClass : " << passClasses[i] << endl;		for (unsigned j = 0; j < passPriorities.size(); ++j) {			LOG << "passPriorities : " << passPriorities[j] << endl;			vector < Plan * > possiblePasses;			vector < float  > helpPass;			possiblePasses.reserve(passPriorities[j].size()+tryDribble);			if (tryDribble)				helpPass.reserve(passPriorities[j].size()+tryDribble);			for (unsigned p = 0; p < passPriorities[j].size(); ++p) {				possiblePasses.push_back( new PassPlan(wm, form, library, BPM_ALL,												charToUniNum(passPriorities[j][p])));				helpPass.push_back(0.03);			}			if (tryDribble) 			{				LOG << "try Dribble" << endl;				possiblePasses.push_back(new DribblePlan(wm, form, library));				helpPass.push_back(0.0);			}			LOG << "...." << endl;			startPlan(bestPlan(possiblePasses, helpPass, true),  passClasses[i]);		}	}}unsignedGoalPlan::findBestShooter(string inPlayers) {	unsigned retVal = 0;	float maxValue = -0xFFFF;	for (unsigned i =0 ; i < inPlayers.size(); ++i) {		unsigned uniNum = charToUniNum(inPlayers[i]);		const Player * player = NULL;		for (int fp = 0 ; fp < FULL_PLAYERS_NUM; ++fp)			if (wm->getFullPlayer(TID_TEAMMATE, fp).isValid())				if (wm->getFullPlayer(TID_TEAMMATE, fp).getUniNum() == uniNum)					player = &(wm->getFullPlayer(TID_TEAMMATE,fp));		if (!player) continue;		pair<float,float> weights = getWeights(player);		float value = config["GoalPlan"]["SelectPasser"]["GoalieDist"].asFloat() * weights.first					+ config["GoalPlan"]["SelectPasser"]["Alpha"].asFloat() * weights.second;		if (!retVal || (value>maxValue)) {			maxValue = value;			retVal = uniNum;		}	}	return retVal;}pair<float,float>GoalPlan::getWeights(const Player * player) {	pair <float, float> retval;	float goalieDist = -1;	Vector v1, v2, v;	Point calcPoint = player->getPos().asPoint();	LOG << " cP: " << calcPoint;	v1.setByPoints(calcPoint, Point(52.5, +6.3));	v2.setByPoints(calcPoint, Point(52.5, -6.3));	v.setByPoints(calcPoint, calcPoint);	bool goalieFound = false;	for (int i =0 ; i <  HALF_PLAYERS_NUM; ++i)			if (wm->getHalfPlayer(TID_TEAMMATE, i).isGoalie()) {					goalieFound = true;					v.setByPoints (calcPoint, wm->getHalfPlayer(TID_TEAMMATE, i).getPos().asPoint());			}	for (int i =0 ; i <  FULL_PLAYERS_NUM; ++i)			if (wm->getFullPlayer(TID_TEAMMATE, i).isGoalie()) {					goalieFound = true;					v.setByPoints (calcPoint, wm->getFullPlayer(TID_TEAMMATE, i).getPos().asPoint());			}	LOG << " goalieFound " << goalieFound;	LOG << " v,v1,v2:" << v << " * " << v1 << "* " << v2;	float alpha = 0.0;	float d1 = normalizeAngle(v1.getDirection()),			d2 = normalizeAngle(v2.getDirection());	if (goalieFound) {			float d = normalizeAngle(v.getDirection());			LOG << "d1: " << d1 << "d2: " << d2 << "d: " << d << endl;							if ((d1 >= d) && (d >= d2)) {					// positioning is OK...					float pa1 = getProperAngle (d1,d);					float pa2 = getProperAngle (d,d2);					if (pa1 > pa2) 							alpha = pa1;					else							alpha = pa2;			}			else {					alpha = getProperAngle(d1,d2);			}			goalieDist = v.getMagnitude();	}	else {			LOG << "Warning.. goalie not found. (not isValid checked)" << endl;			alpha = 0.0;			goalieDist = 0.0;	}	LOG << "alpha: " << alpha << " gd: " << goalieDist <<endl;	retval.first = goalieDist;	retval.second = alpha;	return retval;}floatGoalPlan::getProperAngle(float angle1, float angle2) const{		return abs(normalizeAngle(angle1-angle2));}