#include "Worldmodel.h"



/*! This method determines the global position of the object o after iCycles
When the object is the ball, only the decay of the ball is taken into
account. When the object is a player it
is assumed that the player dashes with 'iDashPower' every cycle.
\param o objectT of which global position will be predicted
\param iCycles pos is predicted after this number of cycles
\param iDashPower dash power that is used every cycle in dash (default 100)
\param vel will be filled with predicted global velocity after iCycles
\return predicted global position after iCycles. */
VecPosition WorldModel::predictPosAfterNrCycles( ObjectT o,double dCycles, int iDashPower, 
												 VecPosition *posIn, VecPosition *velIn,
												 bool bUpdate )
{
    VecPosition vel = *velIn ;
    VecPosition pos = *posIn ;

    if( o == O_Ball )
    {
        // get the speed and the distance it travels in iCycle's.
        // use this distance and direction it travels in, to calculate new pos
        // geom series is serie s=a+ar+..+ar^n...decay=r,iCycles=n,dSpeed=a
        double dDist = Geometry::getSumGeomSeries( vel.getMagnitude(),
												   CBALLDECAY, dCycles);
        pos         += VecPosition( dDist, vel.getDirection(), POLAR );
        vel         *= pow( CBALLDECAY, dCycles );
    }
    else 
    {
        double      dDirection = getObjectDirction(o); // used when no info about global body
        Stamina     stamina =getObjectStamina(o);      // used when object is agent
		
        predictStateAfterDash( iDashPower, &pos, &vel, &stamina, dDirection );
    }

    if( posIn != NULL && bUpdate )
        *posIn = pos;
    if( velIn != NULL && bUpdate )
        *velIn = vel;

    return pos;
}


/*! This method updates all the stamina variables using the calculations from
the soccer manual. It is not really important since stamina is read from
sense_body every cycle. That information is more up to date.
\param power of last dash command
\param stamina pointer to all stamina values, will change to new value
\return stamina class will be updated to new stamina values */
void WorldModel::predictStaminaAfterDash( double dPower, Stamina *stamina )
{
    double sta = stamina->getStamina();
    double eff = stamina->getEffort();
    double rec = stamina->getRecovery();

	// double negative value when dashed backwards
	sta -= ( dPower > 0.0 ) ? dPower : -2 * dPower ;
	if( sta < 0 ) sta = 0;

	// stamina below recovery threshold, lower recovery
	if( sta <= CRECOVERDECTHR*CSTAMINAMAX && rec > CRECOVERMIN)
		rec -= CRECOVERDEC;
	
	// stamina below effort decrease threshold, lower effort
	if( sta <= CRECOVERDECTHR*CSTAMINAMAX && eff > CEFFORTMIN )
		eff -= CEFFORTDEC;
	
	// stamina higher than effort incr threshold, raise effort and check maximum
	if( sta >= CEFFORTINCTHR * CSTAMINAMAX && eff < 1.0)
	{
		eff += CEFFORTINC;
		if ( eff > 1.0 )
			eff = 1.0;
	}
	
	// increase stamina with (new) recovery value and check for maximum
	sta += rec*CSTAMINAINCMAX;
	if ( sta > CSTAMINAMAX )
		sta = CSTAMINAMAX;
	
    stamina->setStamina ( sta );
    stamina->setEffort  ( eff );
    stamina->setRecovery( rec );
}



/*! This method determines the state of a player after a dash command is
performed. The current state of the player is specified by the passed
arguments. After this method returns, all arguments are updated.
\param pos initial position, will be changed to the predicted position
\param vel intital velocity, will be changed to the predicted velocity
\param dActualPower actual power that is send with dash command
\param sta pointer to stamina, when not NULL, effort is used and updated
\param dDirection direction of dash */
void WorldModel::predictStateAfterDash( double dActualPower, VecPosition *pos,
                                        VecPosition *vel, Stamina *sta, double dDirection )
{
    // get acceleration associated with actualpower
    double dEffort = sta->getEffort();
    double dAcc    = dActualPower * CDASHPOWERRATE * dEffort;

    // add it to the velocity; negative acceleration in backward direction
    if( dAcc > 0 )
        *vel += VecPosition::getVecPositionFromPolar( dAcc, dDirection );
    else
        *vel += VecPosition::getVecPositionFromPolar( fabs(dAcc),
        VecPosition::normalizeAngle(dDirection+180));

    // check if velocity doesn't exceed maximum speed
    if( vel->getMagnitude() > CMAXROBOTV )
        vel->setMagnitude( CMAXROBOTV );

    // add velocity to current global position and decrease velocity
    *pos += *vel;
    *vel *= CMAXROBOTDECAY;
    predictStaminaAfterDash( dActualPower, sta );
}




/*! This method returns the command for object 'obj' to turn towards a point
'posTo' on the field when it has 'iCycles' to reach that point. If
the point is within 'dDistBack' behind the object it will try to dash
backwards. In the case that 'bMoveBack' is true, it will always try to
move backwards. When posIn, velIn and angBodyIn are equal to NULL, the
current agent information is used. */
TCommand WorldModel::predictCommandTurnTowards( ObjectT o,VecPosition posTo, int iCycles, 
											    double dDistBack, bool bMoveBack,
												VecPosition *posIn, VecPosition *velIn,
												AngDeg *angBodyIn )
{
    TCommand      soc, socFirst;
    VecPosition   pos, vel;
    AngDeg        angBody, ang, angTo;
    Stamina       sta;
    bool          bFirst = true;

    // fill in all values
    angBody = *angBodyIn;
    pos     = *posIn;
    vel     = *velIn;
   
	socFirst.CommandType = Com_Stay;
    // predict where we will finally stand when our current vel is propogated
    // and then check the orthogonal distance w.r.t. our body direction
    VecPosition posPred=predictPosAfterNrCycles( o, min(iCycles,4),
											     0, &pos, &vel, false );
    Line        line   =Line::makeLineFromPositionAndAngle( posPred, angBody );
    double      dDist  =line.getDistanceWithPoint( posTo );

    // get the angle to this point
    angTo = (posTo - posPred).getDirection();
    angTo = VecPosition::normalizeAngle( angTo - angBody );

    // determine whether we want to turn based on orthogonal distance 
    double dRatioTurn;
    if( pos.getDistanceTo(posTo) > 30.0 ) 
        dRatioTurn = 4.0;  
    if( pos.getDistanceTo(posTo) > 20.0 ) 
        dRatioTurn = 3.0;  
    else if( pos.getDistanceTo(posTo) > 10 ) 
        dRatioTurn = 2.0;
    else
        dRatioTurn = 0.90 ;

    AngDeg angTmp = angTo + (bMoveBack) ? 180 : 0;
    angTmp = VecPosition::normalizeAngle( angTmp );

    // turn when: 
    //  1. point lies outside our body range (forward and backwards)
    //  2. point lies outside distBack and behind us (forward move) 
    //  3. point lies outside distBack and in front of us backwards move)
    int    turn = 0;
    while( ( dDist > dRatioTurn * CKICKABLEDIST ||
         ( posPred.getDistanceTo( posTo ) > dDistBack && 
         ( ( fabs( angTo ) > 90 && bMoveBack == false  ) ||
         ( fabs( angTo ) < 90 && bMoveBack == true ) ) ) )
           && turn < 5 && fabs( angTmp ) > dPlayerWhenToTurnAngle )
    {

        ang = (posTo - posPred).getDirection() + (( bMoveBack == true )?180:0);
        ang = VecPosition::normalizeAngle( ang - angBody );
        makeCommand(&soc,Com_Turn,getAngleForTurn(ang,vel.getMagnitude()),0);
        
        if( bFirst == true )
            socFirst = soc;
        bFirst = false;
        predictStateAfterTurn(soc.ComParam0, &pos, &vel, &angBody,&sta);
        line = Line::makeLineFromPositionAndAngle( posPred, angBody );
        dDist = line.getDistanceWithPoint( posTo );
        angTo = (posTo - posPred).getDirection();
        angTo = VecPosition::normalizeAngle( angTo - angBody );
        turn++;
    }

    // if very close and have to turn a lot, it may be better to move with our
    // back to that point
    if( turn > 1 && iCycles < 4 && posPred.getDistanceTo( posTo ) < dDistBack &&
        bMoveBack == false)
    {
        angBody =  *angBodyIn;
        pos     =  *posIn;
        vel     =  *velIn;
        ang = (posTo - posPred).getDirection() + 180;
        ang = VecPosition::normalizeAngle( ang - angBody );
        makeCommand(&soc,Com_Turn,getAngleForTurn(ang,vel.getMagnitude()),0);
        predictStateAfterTurn( soc.ComParam0,&pos,&vel,&angBody,&sta);
        line = Line::makeLineFromPositionAndAngle( posPred, angBody );
        dDist = line.getDistanceWithPoint( posTo );
        if( dDist < 0.9 * CKICKABLEDIST  )
        {
            //Log.log( 463, "turn around and intercept with back" );
            return soc;
        }
    }

    return socFirst;
}


/*! This method determines the state of a player after a turn command is
performed. The global position is updated with the velocity and the
velocity is updated. Then the actual turn angle is calculated taken the
inertia into account. This actual turn angle is used to update both
the global body and global neck direction.
\param dSendAngle actual angle given in command
\param pos initial position, will be changed to the predicted position
\param vel intital velocity, will be changed to the predicted velocity
\param angBody global body direction
\param angNeck global neck direction
\param sta Stamina of player can be NULL */
void WorldModel::predictStateAfterTurn( AngDeg dSendAngle, VecPosition *pos, 
									    VecPosition *vel, AngDeg *angBody, 
										Stamina *sta )
{
    // calculate effective turning angle and neck accordingly
    double dEffectiveAngle;
    dEffectiveAngle = getActualTurnAngle( dSendAngle, vel->getMagnitude() );
    *angBody = VecPosition::normalizeAngle( *angBody + dEffectiveAngle );
    // update as if dashed with no power
    predictStateAfterDash( 0.0, pos, vel, sta, *angBody );
    return;
}


/*! This method returns the command to move to a position, first it checks
whether a turn is necessary. When this is the case, it performs the turn.
Otherwise a dash command is generated to move in 'iCycles' cycles to
the point 'posTo'. */
TCommand WorldModel::predictCommandToMoveToPos( ObjectT o, VecPosition posTo, int iCycles, 
											    VecPosition *posIn, VecPosition *velIn, 
												AngDeg *angBodyIn, double dDistBack /* = 2.5 */, 
												bool bMoveBack /* = false */)
{
    VecPosition   pos, vel;
    AngDeg        angBody;
    TCommand	  soc;
    double        dPower;

    // fill in all values
    angBody =  *angBodyIn;
    pos     =  *posIn;
    vel     =  *velIn;

    soc = predictCommandTurnTowards(o, posTo, iCycles, dDistBack, bMoveBack,
									posIn, velIn,   angBodyIn);
    if( soc.CommandType != Com_Stay )
        return soc;

    dPower = getPowerForDash( posTo-pos, angBody, vel,getObjectStamina(o).getEffort(),
							  iCycles );
	makeCommand(&soc,Com_Dash,dPower,0);
    return soc;
}

void WorldModel::predictStateAfterCommand( TCommand com, VecPosition *pos, VecPosition *vel,
										   AngDeg *dDirection, Stamina *sta /* = NULL  */)
{
	//Com_Stay, Com_Turn, Com_Kick, Com_Dash, Com_Catch
	switch(com.CommandType) {
	case Com_Stay:
	case Com_Kick:
		predictStateAfterDash(0,pos,vel,sta,*dDirection);
		break;
	case Com_Turn:
		predictStateAfterTurn(com.ComParam0,pos,vel,dDirection,sta);
		break;
	case Com_Dash:
		predictStateAfterDash(com.ComParam0,pos,vel,sta,*dDirection);
		break;
	case Com_Catch:
		*pos = (*pos + ball.getPosition()) / 2.0;
		*vel = VecPosition(0,0);
	}
}