#include <math.h>
#include <map>
#include <sstream>
#include <fstream>

#include "GameData.h"
#include "AI_StateInspector_Impl.h"
#include "LogicPlayer.h"
#include "Ball.h"
#include "Marionette.h"

using namespace std;

AI_StateInspector_Impl::AI_StateInspector_Impl(GameData* _game, int _pnum): AI_StateInspector(si_standard),
	game(_game), pnum(_pnum), opnum(1-_pnum), prev_player_score(0), prev_opponent_score(0), prev_distance(0)
{ 
	player = game->players[pnum];
	opponent = game->players[opnum];

	string p(player->physics_player->name());

	// MAJOR ATTACK POINTS
	attack_sections[p + "Head"] = 2;
	attack_sections[p + "Chest"] = 1;
	attack_sections[p + "Pelvis"] = 0;
	
	// BLOCKING
	attack_sections[p + "Left Humerus"] = 3;
	attack_sections[p + "Right Humerus"] = 3;
	attack_sections[p + "Left Radius"] = 3;
	attack_sections[p + "Right Radius"] = 3;
	attack_sections[p + "Left Hand"] = 3;
	attack_sections[p + "Right Hand"] = 3;
	attack_sections[p + "Left Fingers"] = 3;
	attack_sections[p + "Right Fingers"] = 3;
	attack_sections[p + "Left Thumb"] = 3;
	attack_sections[p + "Right Thumb"] = 3;
	attack_sections[p + "Left Femur"] = 3;
	attack_sections[p + "Right Femur"] = 3;
	attack_sections[p + "Left Foot"] = 3;
	attack_sections[p + "Right Foot"] = 3;
	attack_sections[p + "Left Tibia"] = 3;
	attack_sections[p + "Right Tibia"] = 3;

	// NONE
	attack_sections["NULL"] = -1;
}

void AI_StateInspector_Impl::load(PLAYER_ID pid)
{
	ostringstream filename;
	filename << "conf\\p" << pid << ".states";
	ifstream states(filename.str().c_str());

	if (!states)
	{
		states.close();
		states.clear();
		states.open("conf\\default.states");
	}

	string buf;
	while (getline(states, buf))
	{
		istringstream line(buf);
		state_var_info info;
		int id;
		line >> id >> info.name >> info.minimum_value >> info.maximum_value >> info.resolution;
		info.id = id;
		map_to_info[info.id] = info;
		map_to_id[info.name] = info.id;
	}

	states.close();

	map_to_value[get_state_id("DISTANCE")] = &AI_StateInspector_Impl::get_distance;
	map_to_value[get_state_id("CURRENT_MOVE")] = &AI_StateInspector_Impl::get_current_move;
	map_to_value[get_state_id("OPP_CURRENT_MOVE")] = &AI_StateInspector_Impl::get_opp_current_move;
	map_to_value[get_state_id("OBJ_DELTA")] = &AI_StateInspector_Impl::get_obj_delta;
	map_to_value[get_state_id("USER_MOVE_CHOICE")] = &AI_StateInspector_Impl::get_user_move_choice;
	map_to_value[get_state_id("USER_REINFORCEMENT")] = &AI_StateInspector_Impl::get_user_reinforcement;
	map_to_value[get_state_id("ANGLE")] = &AI_StateInspector_Impl::get_angle;
	map_to_value[get_state_id("OPP_APPROACH_SPEED")] = &AI_StateInspector_Impl::get_opp_approach_speed;
	map_to_value[get_state_id("OPP_ATTACKING")] = &AI_StateInspector_Impl::get_opp_attacking;
	map_to_value[get_state_id("OPP_ATTACK_SECTION")] = &AI_StateInspector_Impl::get_opp_attack_section;
	map_to_value[get_state_id("OPP_EXTENSION")] = &AI_StateInspector_Impl::get_opp_extension;
	map_to_value[get_state_id("SPEED")] = &AI_StateInspector_Impl::get_speed;
	map_to_value[get_state_id("END")] = &AI_StateInspector_Impl::get_end;

	state_var_init();
}

void AI_StateInspector_Impl::get_state(AI_State& state, bool constrain)
{
	AI_SearchHeuristic* sh = player->search_heuristic;
	sh->begin_traversal();

	while (!sh->past_end())
	{
		STATE_VALUE sv = get_state_value(sh->branch_var(), constrain);
		state.push_back(state_info(sh->branch_var(), sv));
		sh->next(sv);
	}

}

void AI_StateInspector_Impl::end_ai_step()
{
	prev_player_score = game->match.get_score(pnum);
	prev_opponent_score = game->match.get_score(opnum);
	prev_speed = get_speed();
	prev_angle = (get_angle() < -M_PI ? get_angle() + 2*M_PI : get_angle());
	prev_angle = (prev_angle > 0 ? prev_angle : - prev_angle);
	prev_distance = get_distance();
}

STATE_VAR_ID AI_StateInspector_Impl::get_state_id(const std::string& name)
{
	return map_to_id[name];
}

std::string AI_StateInspector_Impl::get_state_name(STATE_VAR_ID sid)
{
	return map_to_info[sid].name;
}

state_var_info& AI_StateInspector_Impl::get_state_info(const std::string& name)
{
	return get_state_info(get_state_id(name));
}

state_var_info& AI_StateInspector_Impl::get_state_info(STATE_VAR_ID sid)
{
	return map_to_info[sid];
}

STATE_VALUE AI_StateInspector_Impl::get_state_value(const std::string& name, bool constrain)
{
	return get_state_value(get_state_id(name), constrain);
}

STATE_VALUE AI_StateInspector_Impl::get_state_value(STATE_VAR_ID sid, bool constrain)
{
	if (constrain)
		return constrain_to_limits(sid, (this->*map_to_value[sid])());
	else
		return (this->*map_to_value[sid])();
}

STATE_VALUE AI_StateInspector_Impl::constrain_to_limits(STATE_VAR_ID sid, STATE_VALUE sv)
{
	state_var_info& info = map_to_info[sid];

	if (sv < info.minimum_value)
		return info.minimum_value;
	else if (sv > info.maximum_value)
		return info.maximum_value;
	else
		return sv;
}

STATE_VALUE AI_StateInspector_Impl::get_distance()
{
	Vector3 v = player->physics_player->frame().translation - opponent->physics_player->frame().translation;
	return v.magnitude();
}

STATE_VALUE AI_StateInspector_Impl::get_current_move()
{
	Move* move = player->physics_player->getCurrentMove();
	return (move != 0 ? move->getId() : -1);
}

STATE_VALUE AI_StateInspector_Impl::get_opp_current_move()
{
	Move* move = opponent->physics_player->getCurrentMove();
	return (move != 0 ? move->getId() : -1);
}

STATE_VALUE AI_StateInspector_Impl::get_obj_delta()
{
	STATE_VALUE cur_obj = game->match.get_score(pnum) - game->match.get_score(opnum);
	STATE_VALUE prev_obj = prev_player_score - prev_opponent_score;

	if (get_distance() < .5) return cur_obj - prev_obj;
	return (prev_distance - get_distance())*40 + (cur_obj - prev_obj);
}

STATE_VALUE AI_StateInspector_Impl::get_user_move_choice()
{
	return game->input.user_command(pnum);
}

STATE_VALUE AI_StateInspector_Impl::get_user_reinforcement()
{
	return game->input.user_reinforcement(pnum);
}

STATE_VALUE AI_StateInspector_Impl::get_angle()
{
	Vector3 dir = player->physics_player->frame().vectorToObjectSpace(opponent->physics_player->frame().translation);
	dir.y = 0;
	
	float angle;
	if (dir.z == 0)
	{
		if (dir.x > 0)
			angle = -M_PI/2;
		else
			angle = M_PI/2;
	}
	else
		angle = atan(dir.x/dir.z);

	if (dir.z > 0)
		if (dir.x > 0)
			angle -= M_PI;
		else
			angle += M_PI;
	
	return (angle > M_PI - get_state_info("ANGLE").resolution/2 ? angle - 2*M_PI : angle);
}

STATE_VALUE AI_StateInspector_Impl::get_opp_attacking()
{
	return opponent->physics_player->isAttacking();
}

STATE_VALUE AI_StateInspector_Impl::get_opp_attack_section()
{
	Move* move = opponent->physics_player->getCurrentMove();
	
	if (move == 0)
		return -1;
	else
		return attack_sections[(move->getTarget() != 0 ? move->getTarget()->name() : "NULL")];
}

STATE_VALUE AI_StateInspector_Impl::get_opp_extension()
{
	return 0;
}

STATE_VALUE AI_StateInspector_Impl::get_opp_approach_speed()
{
	Vector3 vel = opponent->physics_player->velocity(opponent->physics_player->name()+"Pelvis");
	Vector3 dir = (player->physics_player->frame().translation - opponent->physics_player->frame().translation).unit();
	return vel.dot(dir);
}

STATE_VALUE AI_StateInspector_Impl::get_speed()
{
	return player->physics_player->velocity(player->physics_player->name()+"Pelvis").magnitude();
}

void AI_StateInspector_Impl::state_var_init()
{
	info_iter it = map_to_info.begin();
	info_iter end = map_to_info.end();

	while (it != end)
	{
		state_var_info& info = (*it++).second;

		if (info.resolution == 0)
			continue;

		info.minimum_value = bit_shizzle(info.minimum_value, 4);
		info.maximum_value = bit_shizzle(info.maximum_value, 4);
		info.resolution = bit_shizzle(info.resolution, 4);

		STATE_VALUE rem = fmod(info.maximum_value - info.minimum_value, info.resolution);
		info.maximum_value += (rem > 0 ? info.resolution - rem : 0);
		rem = fmod(info.maximum_value - info.minimum_value, info.resolution);

		assert (rem == 0);
	}
}