#include "Worldmodel.h"

void WorldModel::makeCommand( TCommand *com, CommandTypeT comType, double comP0, double comP1 )
{
	com->CommandType = comType;
	com->ComParam0	 = comP0;
	com->ComParam1   = comP1;
}

/*! This method determines the angle that should be sent to the soccerserver
when
the player wants to turn angDesiredAngle. This value depends on the current
velocity and the inertia moment of the player
\param angDesiredAngle angle that player wants to turn
\param dSpeed current speed of the player
\return angle that can be sent with turn command */
AngDeg WorldModel::getAngleForTurn( AngDeg angDesiredAngle, double dSpeed )
{
    AngDeg a = angDesiredAngle * (1.0 + CINERTIAMOMENT * dSpeed );
    if( a > CMAXMOMENT )
        return CMAXMOMENT ;
    else if ( a < CMINMOMENT )
        return CMINMOMENT ;
    else
        return a;
}

/*! This method determines the actual angle that is used when 'angTurn' is
sent to the SoccerServer. This value depends on the current velocity and
the inertia moment of the player
\param angAngleForSend angle send with turn command
\param dSpeed current speed of the player
\return actual angle that player is turned */
AngDeg WorldModel::getActualTurnAngle( AngDeg angTurn,double dSpeed )
{
    return angTurn / (1.0 + CINERTIAMOMENT * dSpeed );
}


/*! This method determines the optimal dash power to mantain an optimal speed
When the current speed is too high and the distance is very small, a
negative dash is performed. Otherwise the difference with the maximal speed
is determined and the dash power rate is set to compensate for this
difference.
\param posRelTo relative point to which we want to dash
\param angBody body angle of the agent
\param vel current velocity of the agent
\param dEffort current effort of the player
\param iCycles desired number of cycles to reach this point
\return dash power that should be sent with dash command */
double WorldModel::getPowerForDash( VecPosition posRelTo, AngDeg angBody,
                                    VecPosition vel, double dEffort, int iCycles )
{
    // the distance desired is the x-direction to the relative position we
    // we want to move to. If point lies far away, we dash maximal. Furthermore
    // we subtract the x contribution of the velocity because it is not necessary
    // to dash maximal.
    double dDist = posRelTo.rotate(-angBody).getX(); // get distance in direction
    if( iCycles <= 0 ) iCycles = 1;
    double dAcc  = getFirstSpeedFromDist(dDist,iCycles,CMAXROBOTDECAY);
    // get speed to travel now
    if( dAcc > CMAXROBOTV )             // if too far away
        dAcc = CMAXROBOTV;                // set maximum speed
    dAcc -= vel.rotate(-angBody).getX();             // subtract current velocity

    // acceleration = dash_power * dash_power_rate * effort ->
    // dash_power = acceleration / (dash_power_rate * effort )
    double dDashPower = dAcc/(CDASHPOWERRATE * dEffort );
    if( dDashPower > CMAXPOWER )
        return CMAXPOWER;
    else if( dDashPower < CMINPOWER )
        return CMINPOWER;
    else
        return dDashPower;
}


/*! This method returns the speed that has to be given to an object when
it should have travelled a distance 'dDist' after 'dCycles' number of
cycles. This can be calculated using a geometric series where 'dDecay'
is the used decay factor (default this value equals ball_decay).
\param dDist distance the ball has to travel
\param dCycles nr of cycles after which ball should have travelled 'dDist'
\param dDecay decay of the geometric series.
\return initial speed for the ball to travel 'dDist' in 'dCycles' cycles */
double WorldModel::getFirstSpeedFromDist( double dDist, double dCycles, double
                                         dDecay )
{
    return Geometry::getFirstGeomSeries( dDist, dDecay, dCycles);
}

AngDeg WorldModel::getObjectDirction( ObjectT o )
{
	if(o == O_Ball)
	{
		return 0;
	}
	
	int num  = o - 1;
	if(o > O_Own_5)
	{
		num -= 5;
		return opsRobots[num].getDirection();
	}
	return ownRobots[num].getDirection();
}

Stamina WorldModel::getObjectStamina( ObjectT o )
{
	Stamina sta;
	if( o == O_Ball)
	{
		return sta;
	}
	int num  = o - 1;
	if(o > O_Own_5)
	{
		num -= 5;
		return opsRobots[num].getStamina();
	}
	return ownRobots[num].getStamina();

}

ObjectT WorldModel::getFastestInSetTo( ObjectSetT objectSet, VecPosition *pos, 
									   VecPosition vel, double dDecay, 
									   int *iCycles /* = NULL  */)
{


	setInfT setInf[10];
	getInfbySet(objectSet,setInf);
	struct logInfT{
		VecPosition pos;
		VecPosition vel;
		AngDeg		dir;
		Stamina		sta;
	}logInf[10];
	
	// initial
	for(int i = 0 ; i < 10 ; i++)
	{
		if(setInf[i].availabe)
		{
			logInf[i].pos = getObjectPosition(getObjectbyInf(setInf[i]));
			logInf[i].vel = getObjectVelocity(getObjectbyInf(setInf[i]));
			logInf[i].dir = getObjectDirction(getObjectbyInf(setInf[i]));
			logInf[i].sta = getObjectStamina(getObjectbyInf(setInf[i]));
		}		
	}

	// simulation
	int tempCycles = 0;
	while(tempCycles < *iCycles)
	{
		tempCycles ++;
		if(ball.doMove(pos,&vel,&VecPosition(0,0)) != BS_Normal)
		{
			return O_Unknow;
		}
		for(int i = 0 ; i < 10 ; i++)
		{
			if(!setInf[i].availabe)
			{
				continue;
			}

			if(pos->getDistanceTo(logInf[i].pos) <= CKICKABLEDIST)
			{
				*iCycles = tempCycles;
				return getObjectbyInf(setInf[i]);
			}
			logInf[i].pos = getObjectPosition(getObjectbyInf(setInf[i]));
			logInf[i].vel = getObjectVelocity(getObjectbyInf(setInf[i]));
			logInf[i].dir = getObjectDirction(getObjectbyInf(setInf[i]));
			logInf[i].sta = getObjectStamina(getObjectbyInf(setInf[i]));
			for(int j = 0 ; j < tempCycles ; j ++)
			{
				TCommand tempCommand = predictCommandToMoveToPos(getObjectbyInf(setInf[i]),*pos,tempCycles,&logInf[i].pos,
															     &logInf[i].vel,&logInf[i].dir);
				predictStateAfterCommand(tempCommand,&logInf[i].pos,&logInf[i].vel,&logInf[i].dir,&logInf[i].sta);				
			}
		}		
	}
	return O_Unknow;
}

void WorldModel::getInfbySet( ObjectSetT objectSet, setInfT *setInf )
{
	
//	OBJECT_SET_TEAMMATES,			 /*!< teammates                       */
//  OBJECT_SET_OPPONENTS,			 /*!< opponents                       */
//  OBJECT_SET_PLAYERS,				 /*!< players                         */
//  OBJECT_SET_TEAMMATES_NO_GOALIE,	 /*!< teammates without the goalie    */

	// initial
	for(int i = 0 ; i < 10 ; i ++)
	{
		setInf[i].availabe = false;
	}

	switch(objectSet) {
	case  OBJECT_SET_TEAMMATES:
		for( i = 0 ; i < 5 ; i ++)
		{
			setInf[i].team = 0;
			setInf[i].num  = i;
			setInf[i].availabe = true;
		}
		break;
	case  OBJECT_SET_OPPONENTS:
		for( i = 0 ; i < 5 ; i ++)
		{
			setInf[i].team = 1;
			setInf[i].num  = i;
			setInf[i].availabe = true;
		}
		break;
	case  OBJECT_SET_PLAYERS:
		for( i = 0 ; i < 5 ; i ++)
		{
			setInf[i].team = 0;
			setInf[i].num  = i;
			setInf[i].availabe = true;
		}
		for( i = 5 ; i < 10 ; i ++)
		{
			setInf[i].team = 1;
			setInf[i].num  = i;
			setInf[i].availabe = true;
		}
		break;
	case  OBJECT_SET_TEAMMATES_NO_GOALIE:
		for( i = 0 ; i < 4 ; i ++)
		{
			setInf[i].team = 0;
			setInf[i].num  = i + 1;
			setInf[i].availabe = true;
		}
	}
}


ObjectT WorldModel::getObjectbyInf( setInfT setInf )
{
	if(setInf.team == 0)
	{
		return (ObjectT)(setInf.num + 1);
	}
	else
	{
		return (ObjectT)(setInf.num + 6);
	}
}

VecPosition WorldModel::getObjectPosition( ObjectT o )
{
	if( o == O_Ball)
	{
		return ball.getPosition();
	}
	else if( o < O_Ops_1)
	{
		return ownRobots[o-1].getPosition();
	}
	else
	{
		return opsRobots[o-6].getPosition();
	}
}

VecPosition WorldModel::getObjectVelocity( ObjectT o )
{
	if( o == O_Ball)
	{
		return ball.getVelocity();
	}
	else if( o < O_Ops_1)
	{
		return ownRobots[o-1].getVelocity();
	}
	else
	{
		return opsRobots[o-6].getVelocity();
	}

}