#include "interception.h"
#include "strategy.h"
#include "worldmodel.h"
#include "skill.h"
#include "agent.h"
#include "log.h"
/***********************   Interception   *****************************************/
Interception::Interception(){
	setsmartit(false);
}

void Interception::SetITPlayer(Player* p){
	dash_rate = p->dash_power_rate * p->effort_max;
	if(!p->Is_goalie){
		kickarea = ServerParam::ball_size + p->player_size + p->kickable_margin;
		reliable_kickarea = (float)(int(kickarea*10)/10.0f);
	}else{
		kickarea = ServerParam::catch_area_l;
		reliable_kickarea = ServerParam::catch_area_l;
	}
	maxdash_effect = ServerParam::max_power * dash_rate * p->effort_max;
	rundash_power = ServerParam::max_power;
	max_speed = p->max_speed;
}

void Interception::SetSelfIT(){
	dash_rate = Skill::action.Dashrate();
	kickarea = ServerParam::ball_size + Self.player_size + Self.kickable_margin;
	reliable_kickarea = (float)(int(kickarea*10)/10.0f);
	maxdash_effect = Skill::action.Max_DashEffect();
	max_speed = Self.max_speed; 
	
	Command command;
	Agent::motion.run(Self.max_speed, command);
	rundash_power = command.power;
}

void Interception::prepare_intercept_info(const Vector& ballpos, const Vector& ballvel, const Vector& startpoint, const Vector& selfvel, float bdfacing){
	int i;
	ball_pos[0] = ballpos;	ball_vel[0] = ballvel; 
	ball_speed[0] = ball_vel[0].mod();
	ballangle = ballvel.Angle();
	for(i = 1; i <= IT_simulate_cycles; i++){
		ball_pos[i] = ball_pos[i-1] + ball_vel[i-1];
		ball_vel[i] = ball_vel[i-1] * ServerParam::ball_decay;
		ball_speed[i] = ball_speed[i-1] * ServerParam::ball_decay;
	}

	ballcourse.SetValues(ballpos, ballangle);
	Vector boderpoint;
	if(PitchInfo.shrinked_field.IsWithin(ballpos)){
		PitchInfo.shrinked_field.RayIntersection(ballcourse, boderpoint);
	}else{
		PitchInfo.field.RayIntersection(ballcourse, boderpoint);
	}	
	dist_2_bounder = ballpos.dist(boderpoint);

	max_ballmove = Min(dist_2_bounder, (float)MoveFormula(ball_speed[0], log(0.1/ball_speed[0])/log(ServerParam::ball_decay), ServerParam::ball_decay)+0.1f);
	
	player_pos[0] = startpoint;	player_vel[0] = selfvel; 
	for(i = 1; i < 3;i++){
		player_pos[i] = player_pos[i-1] + player_vel[i-1];
		player_vel[i] = player_vel[i-1] * ServerParam::player_decay;
	}
	bodyfacing = bdfacing;
}
//輸入為turn intercept 的截球狀況
//Return value: 是否需要smart intercept,(yes/smart
bool Interception::Is_smart_intercept(Vector* it_infs, unsigned char it_state){
	if(!Is_smartit_needed) return false;

	float IT_reserved_opp_cycles = 2.0f;
	if(Self.IsForward() && Self.IsMid()){
		IT_reserved_opp_cycles = 1.0f;
	}
		
	float IT_smart_ball_minvel = 0.9f;

	int idx = get_closest_it_index(it_state);

	//某些情況下不能或不需要smart intercept
	if(	it_infs[idx].y >= IT_simulate_cycles
		|| it_infs[idx].y > their_min_cycles - IT_reserved_opp_cycles	//對手逼近
		|| pow(ball_speed[1], it_infs[idx].y) < IT_smart_ball_minvel	//球速太慢
		|| it_infs[idx].x >= dist_2_bounder - IT_reserved_opp_cycles	//快出界了
		|| fabs(ballangle) > 60){										//球向后滾 
		return false;
	}

	float max_dist;
	//對手的限制
	max_dist = Min(max_ballmove, (float)MoveFormula(ball_speed[0], their_min_cycles - IT_reserved_opp_cycles, ServerParam::ball_decay));
	//邊界的限制
	max_dist = Min(max_dist, dist_2_bounder - IT_reserved_opp_cycles);
	//當不是直塞時, 不如直接朝前跑接球,不必往后讓
	if(fabs(ballangle) > 30){
		Line l;
		l.LineFromRay(ballcourse); 
		float x = l.get_x(player_pos[1].y);
		if(!ballcourse.InRightDir(Vector(x, player_pos[1].y))) return false;		
		max_dist = Min(max_dist, ballcourse.DistanceFromOrigin(Vector(x, player_pos[1].y)));
	} 
	
	max_dist += 0.001f;
	//max_dist = Min(max_dist, 0.5f*(ball_speed[0] + IT_smart_ball_minvel)*(ball_speed[0] - IT_smart_ball_minvel)/ServerParam::ball_decay);
	if(it_state == 6){
		if(it_infs[2].x > max_dist){
			//只有一個peakpoint點+后段IT時, 而且后段不好
			return false;
		}else{//只取后段
			it_state = 4;
			idx = 2;
		}
	}

	//it_infs[idx].x - max_dist
	Index<Vector, IT_simulate_cycles> values;
	int i = int(it_infs[idx].y+0.9f) + 1;
	int min_cycle = i;
	if(i > IT_simulate_cycles) return false;

	float max_turnagl = (float)fabs(Skill::action.Max_TurnAng()); 
	float dist = ball_pos[i].dist(ball_pos[0]);// (float)MoveFormula(ball_speed[0], i, ServerParam::ball_decay);
	Vector tmp;
	while(dist < max_dist && i < IT_simulate_cycles){
		while((it_state & 2) && i-1 >= it_infs[1].y && i-1 <= (int)it_infs[2].y){
			//有峰值點
			i ++;
			if(i > IT_simulate_cycles) break;
			dist += ball_speed[i-1];
		}
		if(i > IT_simulate_cycles) break;
		if(dist > max_dist) break;

		//確保能一次轉到, 否則不如等到下次再考慮更優的
		Vector gap = ball_pos[i]-player_pos[1];
		if(fabs(NormalizeAngle(gap.Angle() - bodyfacing)) < max_turnagl 
			|| gap.mod2() < kickarea ){
			tmp.x = (float)i;
			tmp.y = FieldInfo.GetOffensiveSensitivity(ball_pos[i]);
			if(fabs(NormalizeAngle((ball_pos[i] - player_pos[1]).Angle() - bodyfacing)) < 7){
				//鼓勵不轉身截球
				tmp.y += 0.03f;
			}
			//懲罰最近點的危險截球
			if(i == min_cycle)	tmp.y -= 0.02f;
			values.Add(tmp);
		}

		i ++;
		dist += ball_speed[i-1];
	}

	if(!values.IsEmpty()){
		int bestvalue = values.Num_datas() - 1 ;
		for(i = bestvalue - 1; i >= 0; i--){
			if(values.Data(i).y > values.Data(bestvalue).y){
				bestvalue = i;
			}
		}
		best_IT_cycle = (int)values.Data(bestvalue).x;
		return true;
	}else{
		return false;
	}
}

bool Interception::smart_intercept(int bestcycle, InterceptionInfo& inf){	
	inf.IT_point = ball_pos[bestcycle];
	inf.IT_cycles = (float)bestcycle;
	inf.IT_valid = true;
	inf.IT_is_closeball = true;

	Vector gap = ball_pos[bestcycle] - player_pos[1];
	float angle = gap.Angle();
	float dist = gap.mod();
	if(dist < kickarea){
		inf.IT_rapidness = 0;
		if(bestcycle < 2){
			inf.IT_angle = 0;
		}else{
			inf.IT_angle = IT_smartstay_angle;
		}
		return true;
	}
	float allow_angle_diff = Min(ASin(Min(kickarea, 1.0f)/dist), 35.0f);		
	if(fabs(NormalizeAngle(angle-bodyfacing) ) < Max(allow_angle_diff, 7.0f)){
		inf.IT_angle = 360.0f;
		inf.IT_rapidness = Min(dist/bestcycle + 0.2f, 1.0f);
		if (fabs(NormalizeAngle(player_vel[0].Angle() - bodyfacing))<90){
			inf.IT_dashpow = (inf.IT_rapidness - player_vel[0].mod()) / Skill::action.Dashrate();
		}else{
			inf.IT_dashpow = (inf.IT_rapidness +  player_vel[0].mod()) / Skill::action.Dashrate();
		}
	}else{
		//如果我沖一下, 是否還能夠得著球 //wait
		inf.IT_angle = angle;
		inf.IT_rapidness = 1.0f;
	}
	inf.IT_valid = true;
	inf.IT_is_closeball = true;
	return true;
}

//self intercept - deliberate consideration
void Interception::smart_self_intercept(const Vector& ballpos, const Vector& ballvel, 
		const Vector& startpoint, const Vector& selfvel, float bodyfacing, InterceptionInfo& inf){

	Vector it_infs[3], it_inf;
	unsigned char it_state;
//1) preparation step
	prepare_intercept_info(ballpos, ballvel, startpoint, selfvel, bodyfacing);
	playercourse.SetValues(startpoint, bodyfacing);

	float dash_miss =(max_speed - maxdash_effect)/maxdash_effect ;
	dash_miss = Min(dash_miss, 0.9f);
	set_intercept_param(kickarea - dash_miss, max_speed);	//reliable_kick_area

//2) turn - intercept 
	Vector rel_pos = player_pos[1] - ball_pos[1];
	rel_pos = rel_pos.Rotate(- ballangle);
	set_intercept_param(kickarea - dash_miss, max_speed);	//reliable_kick_area
	it_state = GT_speed_utils::calc_interception_solution(rel_pos, ball_speed[1], it_infs);
	DoLog(LOG_IT, "TurnIT %u (%.1f,%.1f)(%.1f,%.1f)(%.1f,%.1f) ", it_state, 
		it_infs[0].x, it_infs[0].y, it_infs[1].x, it_infs[1].y, it_infs[2].x, it_infs[2].y);
/*	
	simulate_one_dash_step(player_pos[0], ball_pos[0], player_vel[0],ball_vel[0],bodyfacing,100);
	simulate_one_dash_step(player_pos[0], ball_pos[0], player_vel[0],ball_vel[0],bodyfacing,0);
	rel_pos = player_pos[0] - ball_pos[0];
	rel_pos = rel_pos.Rotate(- ballangle);
	it_state = GT_speed_utils::calc_interception_solution(rel_pos, ball_speed[2], it_infs);
*/	
//3) smart intercept consideration
	if(Is_smart_intercept(it_infs, it_state)){
		//smart intercept
		if(best_IT_cycle > 1){
			smart_intercept(best_IT_cycle, inf);
			DoLog(LOG_IT, "smart it %d", best_IT_cycle);
			if(inf.IT_angle < 200){//turn it
				float dist = player_pos[1].dist(ball_pos[best_IT_cycle]); 
				if(dist + dash_miss > (best_IT_cycle-1)*max_speed){
					//maybe dangerous 
					InterceptionInfo no_turn_inf;	
					float distball_noturn;
					if(interception_test(ballcourse, playercourse, ball_speed[0], bodyfacing, distball_noturn, no_turn_inf) == IT_test_ok){
						if(no_turn_inf.IT_cycles <= best_IT_cycle && no_turn_inf.IT_cycles > 5){
							inf = no_turn_inf;
							DoLog("no turn fix for smart it");
							return;
						}else{
							//no turn intercept is false because getcloseballinterception is more accurate
						}
					}
				}
			}
			/*
			*/
			return;	
		}
	}

//4) close ball consideration
	Vector it_infs_noturn[3];
	unsigned char it_state_noturn;
	rel_pos = player_pos[0] - ball_pos[0];
	rel_pos = rel_pos.Rotate(- ballangle); 
	set_intercept_param(kickarea, max_speed);	//reliable_kick_area
	it_state_noturn = GT_speed_utils::calc_interception_solution(rel_pos, ball_speed[0], it_infs_noturn);
	int idx = GT_speed_utils::get_closest_it_index(it_state_noturn);
	if(it_infs_noturn[idx].y <= IT_closeball_max_cycles){//may exist close interception
		safecloseit = bool(their_min_cycles > IT_closeball_max_cycles+0.5f);
		adversary_cycles = (int)their_min_cycles;
		if(getcloseballinterceptioninfo(player_pos[0], ball_pos[0], player_vel[0], ball_vel[0], bodyfacing, inf)){
			//patches
			if(inf.IT_cycles == IT_closeball_max_cycles && inf.IT_angle < 200.0f && noturn_cycles > 0){//turn it
				if(noturn_cycles < adversary_cycles){
					inf.IT_angle = 360.0f;
					inf.IT_point = ball_pos[noturn_cycles];
					inf.IT_cycles = (float)noturn_cycles;
					inf.IT_is_closeball = true;
					inf.IT_dashpow = ServerParam::max_power;
					DoLog("no turn close ball fix %d", noturn_cycles);
				}
			}else if(inf.IT_cycles < 2 && fabs(NormalizeAngle(ball.TheirGoalAngle() - bodyfacing)) > 50){
				float gap = (ball_pos[1] - player_pos[1]).mod() ;
				if(gap > ServerParam::ball_size + Self.player_size + 0.1f && gap < reliable_kickarea - 0.1f){
					inf.IT_angle = ball.TheirGoalAngle();
					DoLog(LOG_IT, "close ball, turn fix");
				}
			}
			DoLog(LOG_IT, "close ball it %.2f", getball_dist);
			return;
		}
	}

//5) no turn intercept consideration
	float distball_noturn;
	if (interception_test(ballcourse, playercourse, ball_speed[0], bodyfacing, distball_noturn, inf) == IT_test_ok){
		if(inf.IT_cycles >= IT_closeball_max_cycles && inf.IT_cycles >= it_infs_noturn[idx].y ){			
			int idx = get_closest_it_index(it_state);
			inf.IT_cycles = Min(inf.IT_cycles, float(int(it_infs[idx].y + 0.999f)) + 1.0f);
			DoLog(LOG_IT, "no turn it");
			return;
		}else{
			//no turn intercept is false because getcloseballinterception is more accurate
		}
	}

//6) has to turn intercept now.
	inf.IT_valid = true;
	inf.IT_rapidness = GT_speed_utils::Player_Intercept_Speed();
	inf.IT_is_closeball = false;
	if(it_state & 1){
		it_inf = it_infs[0];
	}else if(it_state & 2){
		it_inf = it_infs[1];
		if(it_inf.y <= 4.0f && (it_state & 4)){
			//dangerous peakpoint
			it_inf = it_infs[2];
		}
	}else{
		it_inf = it_infs[2];
	}
	inf.IT_cycles = float(int(it_inf.y + 0.999f));
	inf.IT_cycles += 1.0f;	

	bool turn_more = false;
	if(their_min_cycles > inf.IT_cycles + 1){
		if(MoveFormula(ball_speed[0], inf.IT_cycles+1, ServerParam::ball_decay) < max_ballmove){
			DoLog(LOG_IT, "turn more");
			turn_more = true;
			inf.IT_cycles ++;
		}
	}

	if(inf.IT_cycles < IT_simulate_cycles){
		inf.IT_point = ball_pos[(int)inf.IT_cycles];
	}else{
		it_inf.x = (float)MoveFormula(ball_speed[0], inf.IT_cycles, ServerParam::ball_decay);
		Vector rel_it_pos = Vector(it_inf.x, 0);
		inf.IT_point = rel_it_pos.Rotate(ballangle) + ball_pos[0];
	}
	inf.IT_angle = (inf.IT_point - startpoint).Angle();
	DoLog(LOG_IT, "normal turn it");
	if(turn_more)	inf.IT_cycles --; 
}

void Interception::SetInterceptionInfo(bool reset){
	int i, j, num_my, num_their, num_total;

	num_my = FieldInfo.Num_MyVisiblePlayers();
	num_their = FieldInfo.Num_TheirVisiblePlayers();
	num_total = num_my + num_their;
	
	Vector ballpos, ballvel;
	if(!reset){
		ballpos = ball.Pos(Self.updated_sight_time);
		ballvel = ball.Vel(Self.updated_sight_time);
	}else{
		ballpos = ball.pos;
		ballvel = ball.global_vel; 
	}
	Time sight_gap = situation.CurrentTime - Self.updated_sight_time; 
	if(sight_gap != 0){
		DoLog(LOG_IT, "sight gap%d", sight_gap);
	}

	Player *p;
	for(i = 0; i < num_my; i ++){
		MyPlayers_Interception[i] = FieldInfo.MyVisiblePlayers[i];
	}	
	for(i =0; i < num_their; i ++){
		TheirPlayers_Interception[i] = FieldInfo.TheirVisiblePlayers[i];