/* *  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, Sassan Haradji * *  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 <Basics.h>#include <InterceptCalculate.h>#include <cmath>#include <CrossPosPlan.h>#include <Line.h>#include <map>#include <algorithm>#include <Degree.h>#include <Types.h>#include <sstream>#include <cassert>using namespace std;using namespace Basics;using namespace Degree;CrossPosPlan::CrossPosPlan(const WorldModel *wm, Form &form,		const Library &library, Side side):		Plan("CrossPos", wm, form, library), side(side), updatedRoles(0)    {    config.add("CrossPos");    config["CrossPos"].setByFile("./Configs/CrossPos.conf");    LOG << "Loading cross positioning factors from conf..." << endl;}CrossPosPlan::~CrossPosPlan(){}void CrossPosPlan::decide(){	LOG << "CrossPosPlan::decide" << endl;	updatedRoles++;	if (wm->isBallKickable())	{		updateRoles();	}	else	{    try {        updateRoles();        if (myRole == Invalid) {            LOG << "namanaDERROR!" << endl;            assert (0);        }        Vector corrVec = 	preserveShapeUpdate(myRole);        throw (corrVec + wm->getBody().getPos()).asPoint();    }    catch (Point point)    {        LOG << "sending command..." << endl;        Command *command = library.positioning.getPositioningCommand(point);        command->setCreator(AT_OFFENSE);        throw command;    }	}	LOG << "CrossPosPlan Now Kill Program" << endl;	assert(0);}double CrossPosPlan::successRate(){    return 1.0;}bool CrossPosPlan::isFinished(){  if (!isInPlayers(wm->getBody().getUniNum(), "234AB"))    return true;    if(!wm->isBallKickable() &&       library.gwSelection == WOBS_OFFENSE &&       abs(wm->getBall().getPos().getY()) > 16)        return false;    return true;}void CrossPosPlan::updateRoles(){	RoleType tmpType;	if (updatedRoles > 10) updatedRoles = 0;	string hearString = wm->getHearedPlanInfo();	if (wm->getBody().getUniNum() == wm->getNearestTmmToBall().getUniNum())	{		form.sayForm.planSay = true;		form.sayForm.planHeader = "X";		form.sayForm.planInfo = "C";	}	if (wm->getBody().getUniNum() == wm->getNearestTmmToBall().getUniNum() ||			hearString.substr(0, 2) != "XC")	{		if (updatedRoles > 3)		{			for (unsigned i = 0; i < ROLE_NUM; ++i)				form.sayForm.planInfo += uniNumToChar(roles[RoleType(i)]->getUniNum());			for (unsigned i = 0 ; i < ROLE_NUM; ++i)				if (roles.find(RoleType(i)) != roles.end())					if (roles[RoleType(i)])					{						tmpType = RoleType(i);						LOG << tmpType << " : " << uniNumToChar(roles[RoleType(i)]->getUniNum())								<< endl;					}			return;		}		string playerLists[2][5];		playerLists[0][0] = "A234B";		playerLists[1][0] = "B432A";		playerLists[0][1] = "2A34B";		playerLists[1][1] = "4B32A";		playerLists[0][2] = "B234A";		playerLists[1][2] = "A432B";		playerLists[0][3] = "3B24A";		playerLists[1][3] = "3A42B";		playerLists[0][4] = "4B32A";		playerLists[1][4] = "2A34B";		roles.clear();		string busyPlayers = "156789";		const	Player* tmpPlayer = NULL;		tmpPlayer = wm->getNearestTmmToPointFromList(wm->getBall().getPos(),			"123456789AB", busyPlayers + "3");		if (tmpPlayer || !roles[Crosser]->isValid())			roles[Crosser] = tmpPlayer;		else if (!roles[Crosser])			roles[Crosser] = &wm->getFullPlayer(TID_TEAMMATE, firstPlayerInList(					playerLists[side][0], busyPlayers) - 1);		if (roles[Crosser]){			busyPlayers += uniNumToChar(roles[Crosser]->getUniNum());			form.sayForm.planInfo += uniNumToChar(roles[Crosser]->getUniNum());		}		float minDistance = 1000;		float tmpDistance = 0;		const Player* bestPlayer = NULL;		float oval_a = .6, oval_b = .45;		for (unsigned i = 0; i < FULL_PLAYERS_NUM; i++)			if (wm->getFullPlayer(TID_TEAMMATE, i).isValid() &&	  		!isInPlayers(wm->getFullPlayer(TID_TEAMMATE, i).getUniNum(),						busyPlayers.c_str()))				{					float tmmPlayerX = wm->getFullPlayer(TID_TEAMMATE, i).getPos().getX() -							roles[Crosser]->getPos().getX();					float tmmPlayerY = wm->getFullPlayer(TID_TEAMMATE, i).getPos().getY() -							roles[Crosser]->getPos().getY();					if (side == SI_LEFT)						tmmPlayerY = -tmmPlayerY;					tmpDistance = abs(2 * oval_a *							tmmPlayerX * tmmPlayerY *	sqrt(abs(							1 / (oval_a * tmmPlayerY) + 1 / (oval_b * tmmPlayerX))));					if (tmpDistance < minDistance)					{						minDistance = tmpDistance;						bestPlayer = &wm->getFullPlayer(TID_TEAMMATE, i);					}				}		if (bestPlayer)			roles[CrosserFriend] = bestPlayer;		else if (!roles[CrosserFriend])			roles[CrosserFriend] = &wm->getFullPlayer(TID_TEAMMATE, firstPlayerInList(					playerLists[side][1], busyPlayers) - 1);		if (roles[CrosserFriend])		{			busyPlayers += uniNumToChar(roles[CrosserFriend]->getUniNum());			form.sayForm.planInfo += uniNumToChar(roles[CrosserFriend]->getUniNum());		}				float ballImportance =				config["CrossPos"]["HomePos"]["BallImportance"].asFloat();		for (unsigned i = 2; i < ROLE_NUM; ++i)		{			if (!roles[RoleType(i)])			{				roles[RoleType(i)] =						wm->getNearestTmmToPointFromList((wm->getBall().getPos() /						ballImportance) +						Vector(Point(config["CrossPos"]["HomePos"]["RFX"][i].asFloat(),						((side == SI_LEFT) * 2 - 1) *						config["CrossPos"]["HomePos"]["RFY"][i].asFloat())) /						(1.0 + 1.0 / ballImportance), "123456789AB", busyPlayers);			}			if (!roles[RoleType(i)])				roles[RoleType(i)] = &wm->getFullPlayer(TID_TEAMMATE, firstPlayerInList(						playerLists[side][i], busyPlayers) - 1);			if (roles[RoleType(i)])			{				busyPlayers += uniNumToChar(roles[RoleType(i)]->getUniNum());				form.sayForm.planInfo = uniNumToChar(roles[RoleType(i)]->getUniNum());			}		}	}	else	{		/*if (hearString.substr(0, 2) != "XC")		{			hearString = "XC";			if (side == SI_LEFT &&					wm->getNearestTmmToPointFromList(wm->getBall().getPos(), "2A"))			{				hearString += uniNumToChar(wm->getNearestTmmToPointFromList(						wm->getBall().getPos(), "2A")->getUniNum());				hearString += uniNumToChar(-(wm->getNearestTmmToPointFromList(						wm->getBall().getPos(), "2A")->getUniNum() - 6) + 6);				hearString += "B34";			}			else if (side == SI_RIGHT &&					wm->getNearestTmmToPointFromList(wm->getBall().getPos(), "4B"))			{				hearString += uniNumToChar(wm->getNearestTmmToPointFromList(						wm->getBall().getPos(), "4B")->getUniNum());				hearString += uniNumToChar(-(wm->getNearestTmmToPointFromList(						wm->getBall().getPos(), "4B")->getUniNum() - 7) + 8);				hearString += "A32";			}			else if (side == SI_LEFT &&					!wm->getNearestTmmToPointFromList(wm->getBall().getPos(), "2A"))				hearString += "2AB34";			else if (side == SI_RIGHT &&					!wm->getNearestTmmToPointFromList(wm->getBall().getPos(), "4B"))				hearString += "4BA32";		}*/		if (hearString.substr(0, 2) == "XC")		{			if (wm->getFullPlayer(TID_TEAMMATE,					charToUniNum(hearString[2]) - 1).isValid())				roles[Crosser] = &wm->getFullPlayer(TID_TEAMMATE, 						charToUniNum(hearString[2]) - 1);			if (wm->getFullPlayer(TID_TEAMMATE,					charToUniNum(hearString[3]) - 1).isValid())				roles[CrosserFriend] = &wm->getFullPlayer(TID_TEAMMATE, 						charToUniNum(hearString[3]) - 1);			if (wm->getFullPlayer(TID_TEAMMATE,					charToUniNum(hearString[4]) - 1).isValid())				roles[Supporter] = &wm->getFullPlayer(TID_TEAMMATE, 						charToUniNum(hearString[4]) - 1);	 		if (wm->getFullPlayer(TID_TEAMMATE,					charToUniNum(hearString[5]) - 1).isValid())				roles[Insider] = &wm->getFullPlayer(TID_TEAMMATE, 						charToUniNum(hearString[5]) - 1);			if (wm->getFullPlayer(TID_TEAMMATE,					charToUniNum(hearString[6]) - 1).isValid())				roles[Outsider] = &wm->getFullPlayer(TID_TEAMMATE, 						charToUniNum(hearString[6]) - 1);		}	}	myRole = Invalid;	for (unsigned i = 0 ; i < ROLE_NUM; ++i)	{		if (roles.find(RoleType(i)) != roles.end())			if (roles[RoleType(i)])				if (roles[RoleType(i)]->getUniNum() == wm->getBody().getUniNum())					myRole = RoleType(i);		if (roles.find(RoleType(i)) != roles.end())			if (roles[RoleType(i)])			{				tmpType = RoleType(i);				LOG << tmpType << " : " << uniNumToChar(roles[RoleType(i)]->getUniNum())						<< endl;			}	}}const Vector CrossPosPlan::preserveShapeUpdate(RoleType role) {    ostringstream ss;    ss << "RF" << (int)role;    Vector totalCorrectionVector;    for (unsigned i = 0 ; i < ROLE_NUM + 1; ++i){        Vector requiredPos;				if (RoleType(i) == ConstantPoint) {					requiredPos.setAsCartesian(0,0);				} 				else {	        if (side == SI_LEFT) {  	          requiredPos.setAsCartesian(    	            config["CrossPos"]["RelativePos"][ss.str()+"X"][i].asFloat(),    		        	config["CrossPos"]["RelativePos"][ss.str()+"Y"][i].asFloat());	        } else {	            requiredPos.setAsCartesian(  	              config["CrossPos"]["RelativePos"][ss.str()+"X"][i].asFloat(),    			        -config["CrossPos"]["RelativePos"][ss.str()+"Y"][i].asFloat());	        }				}        Vector currentPos;        if (roles.find(RoleType(i)) != roles.end()) {            if (roles[RoleType(i)]) {							if(RoleType(i) == ConstantPoint) {											Point constPoint;				        if (side == SI_LEFT) {  	  			        constPoint. x =    	      			      config["CrossPos"]["RelativePos"][ss.str()+"X"][i].asFloat();										constPoint.y =												    		        				config["CrossPos"]["RelativePos"][ss.str()+"Y"][i].asFloat();				        } else {										constPoint.x =   	        			      config["CrossPos"]["RelativePos"][ss.str()+"X"][i].asFloat();										constPoint.y =    			        			-config["CrossPos"]["RelativePos"][ss.str()+"Y"][i].asFloat();				        }								currentPos.setByPoints(constPoint, wm->getBody().getPos());								}							else {                currentPos.setByPoints(roles[RoleType(i)]->getPos(),                                       wm->getBody().getPos());							}  	      	float extraLength = abs(requiredPos.getMagnitude() - currentPos.getMagnitude());                float extraAngle  = abs(requiredPos.getDirection() - currentPos.getDirection());                LOG << "extra: (R,theta) (" << extraLength << ", "<<                    extraAngle << ')' << endl;                Vector correctionVector = requiredPos - currentPos;                correctionVector.normalize();      	  	float correctionFactor =                    extraLength * config["CrossPos"]["RelativePos"][ss.str()+"kLength"][i].asFloat() +                    extraAngle  * config["CrossPos"]["RelativePos"][ss.str()+"kAngle"][i].asFloat();                correctionVector *= correctionFactor;                totalCorrectionVector += correctionVector;            } else {                LOG << "Warning.... Cannot find role num " <<  i << endl;            }        }        else {            LOG << "Warning... Cannot find in map:" << i << endl;        }            }    LOG << "..... Final Log for totalCorrectionVector..." << totalCorrectionVector        << endl;		DRAW << "DRAW LINE " << 50+wm->getBody().getUniNum() << ' ' << wm->getBody().getPos().getX() << ' ' << wm->getBody().getPos().getY()					<< " 0 0" << endl;		DRAW << "END OF DRXING" << endl;    return totalCorrectionVector;}