#include "df_positioning.h"
#include "worldmodel.h"
#include "log.h"
#include "agent.h"

/***********	DF_Arrangement	***********************/
DF_Arrangement::DF_Arrangement(UNum defender, UNum defendee){
	df_size =  Vector(ClientParam::defense_area_length, ClientParam::defense_area_width);
	valid = Arrange_DF_for(defender, defendee);
}


bool DF_Arrangement::getpressposition(UNum My,UNum Opp,Vector &presspos){  //ciwp-0715
	if (FieldInfo.IsBlocked(defendee))
		{
			presspos = situation.NextControlBallPos();
			return true;
		}

		Ray theircourse;
		Vector l_mark,r_mark;
		Vector sectionpost,theirtarget;
		Vector MyPos = MyPlayer(My).pos;
		Vector Opos  = TheirPlayer(Opp).pos;
		PitchInfo.mygoal = Vector(-ServerParam::semi_pitch_length,0.0f);
		Line splitline;
		Line me_Opp;
		me_Opp.LineFromTwoPoints(MyPos,Opos);
		Vector goalpnt = me_Opp.intersection(PitchInfo.SideLines[SL_Left]);
		if (fabs(goalpnt.y) > ServerParam::goal_width/2 * 1.05f)
		{
			l_mark = PitchInfo.l_mygoalpost;
			r_mark = PitchInfo.r_mygoalpost;
		}
		else
		{
			if (goalpnt.y < 0)
			{
				l_mark = PitchInfo.mygoal;
				r_mark = PitchInfo.r_mygoalpost;
			}
			else
			{
				l_mark = PitchInfo.l_mygoalpost;
				r_mark = PitchInfo.mygoal;
			}
		}
		splitline.LineFromTwoPoints((MyPos + Opos)/2,(l_mark + r_mark) /2);
			if (splitline.HalfPlaneTest(Opos) == splitline.HalfPlaneTest(l_mark))
			{
				sectionpost = l_mark;
				theirtarget = r_mark;
			}
			else 
			{
				sectionpost = r_mark;
				theirtarget = l_mark;
			}
		
		Vector midpost = (r_mark + l_mark)/2;
	
		theircourse.SetValues(Opos,(theirtarget - Opos).Angle());
		Line mycourse;
		sectionpost = (sectionpost + Opos) /2; 
		mycourse.LineFromTwoPoints(MyPos,sectionpost);
		if  (!(theircourse.intersection(mycourse,presspos) && 
			PitchInfo.WithInField(presspos)))
		{
			DoLog(LOG_BUG,"no press point 1");
			return false;
		}
		if (presspos.dist(MyPos) *1.1 < presspos.dist(Opos)// && AngleDif((presspos - MyPos).Angle(),(presspos - Opos).Angle()) > 60
			&& presspos.dist(Opos) > 2.5f)
		{
			Opos += Polar2Vector(TheirPlayer(Opp).max_speed * 2,(PitchInfo.mygoal - Opos).Angle());

			splitline.LineFrompline(MyPos,Opos);
			Vector savpresspos = presspos;
			if (!theircourse.intersection(splitline,presspos))
			{
				presspos = savpresspos;
			}
		}
		//consideration of bodyfacing
		if (My == Agent::MyNumber) 
		{
			Line bottomsect;
			Vector bottompoint;
			bottomsect.LineFromTwoPoints(Self.pos,midpost);
			if (!theircourse.intersection(bottomsect,bottompoint))
			{
				DoLog(LOG_BUG,"FT! why no section?");
				return false;
			}
			
			Ray SelfRay(Self.pos,Self.bodyfacing);
			Vector secpoint;
			float secdist;
			float pressdist = theircourse.DistanceFromOrigin(theirtarget);
			if (theircourse.intersection(SelfRay,secpoint))
			{
				secdist = secpoint.dist(presspos);
				if (secdist < 1.5f || Line(theircourse).InBetween(secpoint,Opos,bottompoint))//ciwp-0715
					presspos = secpoint;
			}
		}
		return true;
}

bool DF_Arrangement::getblockposition(UNum My, UNum Opp, Vector &blockpos){ //ciwp
	if (!Player::IsTheirPlayer(Opp) || !Player::IsMyPlayer(My)) return false;
	float cycles = 0.0f;
	Vector opppos;
	if (situation.BallFree){
		opppos = TheirPlayer(Opp).IT_inf.IT_point;
		cycles = TheirPlayer(Opp).IT_inf.IT_cycles;
	}
	else{
		opppos = TheirPlayer(Opp).pos;
		cycles = 0.0f;
	}
	float my_block_speed = ServerParam::player_speed_max;
	//generally their attacking speeds are slower because of dribbling
	float their_break_speed = ServerParam::player_speed_max * 0.8f;
 	/*if (!getblockpoint(opppos, MyPlayer(My).pos, cycles, my_block_speed, their_break_speed, blockpos))
		return false; by Criver*/

	/*if (!PitchInfo.WithInField(blockpos)) return false; by Criver*/
	
	getblockpoint(opppos, MyPlayer(My).pos, cycles, my_block_speed, their_break_speed, blockpos);
	
	/* Allow larger error of bodyfacing generally the error of 8-15
	   is allowed, here we allow a maximum error of 25            */
	   
	if (My == Agent::MyNumber){
		float targetbdfc = (blockpos - Self.pos).Angle();
		if (fabs(NormalizeAngle(targetbdfc-Self.bodyfacing)) < 25.0f){
			Ray SelfRay(Self.pos, Self.bodyfacing);
			float dist = Self.pos.dist(blockpos) + 2.0f;
			blockpos = SelfRay.GetPoint(dist);
			//trick patch by yjy
		}
	}
	
	/*if (My == Agent::MyNumber)
	{
		Ray blockRay(opppos,blockpos);
		Ray SelfRay(Self.pos,Self.bodyfacing);
		Vector secpoint;
		if (blockRay.intersection(SelfRay,secpoint))
		{
			if (blockRay.DistanceFromOrigin(secpoint) > blockRay.DistanceFromOrigin(blockpos)
				&& secpoint.dist(blockpos) < 0.1f * opppos.dist(PitchInfo.mygoal) && secpoint.dist(blockpos) < Self.pos.dist(blockpos))
			{
				DoLog(LOG_POSITIONING,"Adjust Block (%.1f %.1f) ->(%.1f,%.1f)",blockpos.x,blockpos.y,secpoint.x,secpoint.y);
				blockpos = secpoint;
			}
		}
	} by Criver*/
	return true;
}

bool DF_Arrangement::getblockpoint(Vector opp_IT_pos, Vector selfpos, float opp_IT_cycles, float myspeed, float oppspeed, Vector& blockpos){
	float predis = Max(myspeed * opp_IT_cycles,ClientParam::block_margin);//ciwp-0715

	float disleft = opp_IT_pos.dist(PitchInfo.l_mygoalpost);
	float ratio = disleft/(disleft + opp_IT_pos.dist(PitchInfo.r_mygoalpost));
	Vector opttarget = Vector(-ServerParam::pitch_length/2, ServerParam::goal_width * (ratio - 0.5f));
	Vector opponent_pos = (opttarget - opp_IT_pos);
	opponent_pos.Normalize();
	opponent_pos = opp_IT_pos + opponent_pos * ClientParam::block_margin;
	

	Vector vec_me_opt = selfpos - opponent_pos ;
	float dis_me_opt = vec_me_opt.mod();

	if(dis_me_opt <= predis || oppspeed < 1e-2){
		blockpos = opponent_pos;
		return true;
	}
	
	float ratio_speed = myspeed / oppspeed;
	
	AngleDeg goalstrechangle = AngleDif((PitchInfo.l_mygoalpost - opponent_pos).Angle(),(PitchInfo.r_mygoalpost - opponent_pos).Angle());//ciwp
	if(AngleDif(vec_me_opt.Angle(),(opttarget-opponent_pos).Angle())<goalstrechangle/4
		&& AngleDif(vec_me_opt.Angle(),(opttarget-opponent_pos).Angle()) < 10)
		return getsubpoint(opttarget,opponent_pos,selfpos,ratio_speed,predis,blockpos);

	return getsubpoint(opttarget,opponent_pos,selfpos,ratio_speed,predis,blockpos);
}

bool DF_Arrangement::getsubpoint(Vector opttarget, Vector opponent_pos, Vector selfpos, float ratio_speed,float predis, Vector& blockpos){
	Vector vec_me_opt = selfpos - opponent_pos ;
	float dis_me_opt = vec_me_opt.mod();

	float interangle = vec_me_opt.Angle()-(opttarget-opponent_pos).Angle();
	float tempf,tempc;
	float tempa = 1.0f - ratio_speed * ratio_speed;
	if(fabs(tempa)<1e-2){
		tempf = predis + dis_me_opt * Cos(interangle);

		if(tempf <= 1e-2){//ciwp-0715 --undo
			blockpos = (opponent_pos + opttarget)/2;
			return false;
		}

		tempc = 0.5f*(dis_me_opt * dis_me_opt - predis*predis) / tempf;
	}
	else{
		tempf = 2*predis*ratio_speed + 2*dis_me_opt * Cos(interangle);
		float tempz = tempf * tempf - 4 * tempa*(dis_me_opt*dis_me_opt-predis*predis);
		if(tempz < 0){
			blockpos = (opponent_pos+opttarget)/2;
			return false;
		}
		tempz = (float)sqrt(tempz);
		float tempx1 = 0.5f * (-tempz + tempf)/tempa;
		float tempx2 = 0.5f * (tempz + tempf)/tempa;
		if(tempx1 < 0 && tempx2 < 0){
			blockpos = (opponent_pos + opttarget)/2;
			return false;
		}
		else if(tempx1 > 0 && tempx2 > 0) tempc = Min(tempx1, tempx2);
		else tempc = Max(tempx1,tempx2);	
	}
	float odist = opponent_pos.dist(opttarget);

	if(tempc >= odist + 1e-2){
		 blockpos = (opponent_pos + opttarget)/2;
		 return false;
	}
	
	float ratio = tempc / odist;

	blockpos = (opponent_pos * (1-ratio) + opttarget * ratio);
	
	return true;
}

bool DF_Arrangement::getmarkposition(UNum My, UNum Opp, Vector ballpos, Vector& markpos){ //ciwp
//here global numbering scheme is assumed
	if (!Player::IsTheirPlayer(Opp) || !Player::IsMyPlayer(My)) return false;
	Vector dir1, dir2;
	Vector OppPos = TheirPlayer(Opp).pos;
	Vector target = ballpos.y>0 ? PitchInfo.r_mygoalpost : PitchInfo.l_mygoalpost;
	float dispersion =TheirPlayer(Opp).ActiveRadius(situation.CurrentTime) + TheirPlayer(Opp).max_speed;
	if (AngleDif((target - OppPos).Angle(),(target - Self.pos).Angle()) > 90)
		dispersion += 1.0f;
	pose_limitation(dispersion,0.0f,5.0f);

	float tmp_ = 2*(FieldInfo.GetDefensiveSensitivity(OppPos) -0.5f);
	float sensfactor;
	if(tmp_ < 0.05f)
		sensfactor = 0.2f;
	else if(tmp_ < 0.7f)
		sensfactor = 0.2f + 0.3f*(tmp_ - 0.05f)/0.65f;
	else if (tmp_< 0.9f)
		sensfactor = 0.5f + 1.0f*(tmp_-0.7f);
	else
		sensfactor = 0.7f + 1.0f*(tmp_-0.9f);

	pose_limitation(sensfactor,0,1);
	Vector predopos1 = OppPos + Polar2Vector(dispersion* (1 - sensfactor),(target - OppPos).Angle());	
	Vector predopos2 = OppPos.y > Self.pos.y ? Polar2Vector((float)sqrt(dispersion),90) : Polar2Vector((float)sqrt(dispersion),-90);
	predopos2 += OppPos;
	Line balltrace1,balltrace2;
	balltrace1.LineFromTwoPoints(situation.NextControlBallPos(),predopos1);
	balltrace2.LineFromTwoPoints(situation.NextControlBallPos(),predopos2);
	float odist1 = balltrace1.dist(Self.pos) - 1.0f;
	float odist2 = balltrace2.dist(Self.pos) - 1.0f;
	if (odist1 <=0.1f) odist1 = 0.1f;
	if (odist2 <=0.1f) odist2 = 0.1f;
	if (!balltrace2.InBetween(Self.pos,OppPos,situation.NextControlBallPos()))
		odist2 = 0;
	predopos1 -= OppPos;
	predopos2 -= OppPos;
	OppPos += (predopos1 * odist1 + predopos2 * odist2) / (odist1 + odist2);
	
	//OppPos = predopos1;
	dir1 = ballpos - OppPos;
	dir1.Normalize();
	dir2 = PitchInfo.mygoal - OppPos;
	dir2.Normalize();
	dir1 *= sensfactor;
	dir2 *= 1 - sensfactor;
	//markdist = Min(MyPlayer(My).pos.dist(TheirPlayer(Opp).pos), ClientParam::mark_dist);
	float	markdist = ClientParam::mark_dist;
	if(MyPlayer(My).IsRoleKnown()){
	//if a player's aggressiveness is high, he may act more aggressively when marking the opponent.
	//for instance, he will stand just in front of the opponent
		if (MyPlayer(My).aggressiveness >= 0.7f){
			markpos = dir1;
		}
		else{
			markpos = dir1 * 0.5f + dir2 * 0.5f;
		}
	}
	else{
		markpos = dir1;
	}

	markpos.Normalize();
	markpos *= markdist;
	if (My == Agent::MyNumber)
	{
		//consider body facing
		Ray markRay(OppPos,markpos);
		Ray SelfRay(Self.pos,Self.bodyfacing);
		Vector secpoint;
		float secdist;
		float balldist = ballpos.dist(OppPos);
		if (markRay.intersection(SelfRay,secpoint))
		{
			secpoint -=OppPos;
			secdist = secpoint.dist(markpos);
			if (secdist < 0.5f || secdist < 0.2f * Self.pos.dist(OppPos))
				markpos = secpoint;
		}
	}
	markpos += OppPos;
	return true;
}

void DF_Arrangement::setpriority(){ 
	switch(df_type){
	case DF_Block:
		  priority = blockpriority = getblockpriority();
		  break;
	case DF_Formation:
		  priority = formationpriority = getformationpriority();
		  break;
	case DF_Mark:
		  priority = markpriority = getmarkpriority();
		  break;
	case DF_Press:
		if (FieldInfo.IsBlocked(defendee))
			presspriority = getblockpriority();
		else
			presspriority = getpresspriority();
			blockpriority = getblockpriority();
		priority = blockpriority;
		break;
	default:
		break;
	}
}

float DF_Arrangement::QueryPriority(bool block_applied){ 
	if (df_type == DF_Press && block_applied)
	{
		return presspriority;
	}
	else
		return priority;
}
float DF_Arrangement::getformationpriority(){
	double input,output;
	pos_dist = df_point.dist(MyPlayer(defender).pos);
	deviation_dist = df_point.dist(fm_pos);
	input = FieldInfo.GetDefensiveSensitivity(fm_pos);
	Agent::positioning.FM_Priority_Net.SimulateNet(&input,&output);
	return (float)output;
}
float DF_Arrangement::getblockpriority(){
/*
	double input[3], output;
	pos_dist = TheirPlayer(defendee).pos.dist(MyPlayer(defender).pos);
	deviation_dist = Min(TheirPlayer(defendee).pos.dist(fm_pos), df_point.dist(fm_pos));
	// enhance the block priority by Criver
	deviation_dist /= 1.3f;
	pos_dist /= 1.3f;
	input[0] = threat;
	input[1] = deviation_dist / ClientParam::deviation_max;
	input[2] = pos_dist / ClientParam::deviation_max;
	Agent::motion.Block_Net.SimulateNet(&input[0], &output);
	output = (output - 0.1) * 1.25;
	if	(MyPlayer(defender).IsForward())
			output =0.5 + (output + TheirPlayer(defendee).GetOffsensitivity())/4;
	return (float)output;
}//*/
	double input[3], output;
	pos_dist = TheirPlayer(defendee).pos.dist(MyPlayer(defender).pos);
	deviation_dist = Min(TheirPlayer(defendee).pos.dist(fm_pos), df_point.dist(fm_pos));
	/* enhance the block priority by Criver*/
	deviation_dist /= 1.2f;
	pos_dist /= 1.2f;

	/*remedy for back block, give it punishment*/
	Vector pos_diff = TheirPlayer(defendee).pos - MyPlayer(defender).pos;
	if (pos_diff.x < 0){
		float factor = 100.0f;
		if (fabs(TheirPlayer(defendee).pos.y) < 5.5f){
			factor = 1.3f + 0.3f * float(fabs(TheirPlayer(defendee).pos.y)) / 5.5f;
		}else{
			factor = 1.6f + 8.4f * float(fabs(TheirPlayer(defendee).pos.y)) / 20.f;
		}
		pos_dist += float(fabs(pos_diff.x / factor));
	}
	/* END OF REMEDY */

	input[0] = threat;
	input[1] = deviation_dist / ClientParam::deviation_max;
	input[2] = pos_dist / ClientParam::deviation_max;
	Agent::positioning.Block_Net.SimulateNet(&input[0], &output);
	output = (output - 0.1) * 1.25;
	if	(MyPlayer(defender).IsForward())
			output =0.5 + (output + TheirPlayer(defendee).GetOffsensitivity())/4;
	return (float)output;
}

float DF_Arrangement::getpresspriority(){
	double input[3], output;
	input[0] = threat;
	pos_dist = TheirPlayer(defendee).pos.dist(MyPlayer(defender).pos);
	deviation_dist = Min(TheirPlayer(defendee).pos.dist(fm_pos),df_point.dist(fm_pos));
	input[1] = deviation_dist / ClientParam::deviation_max;
	input[2] = pos_dist / ClientParam::deviation_max;
	Agent::positioning.Press_Net.SimulateNet(&input[0], &output);
	output = (output - 0.1) * 1.25;
	if	(MyPlayer(defender).IsForward())
			output =0.5 + (output + TheirPlayer(defendee).GetOffsensitivity() )/4;
	return (float)output;


}

float DF_Arrangement::getmarkpriority(){
	double input[3], output;
	input[0] = threat;
	pos_dist = df_point.dist(MyPlayer(defender).pos);
	deviation_dist = df_point.dist(fm_pos);
	input[1] = deviation_dist / ClientParam::deviation_max;