void CreateCamera(int width, int height, vector3df position, vector3df target)
{
  //Build camera
  ISceneManager* sm = g_GameGraph->GetSceneManager();
  WorldState* worldState =
      static_cast<WorldState*> (g_GameGraph->GetWorldState());
  HexMap* hm = worldState->GetCurrentMap();

  if (NULL != sm)
  {
    if (NULL != hm)
    {
      ICameraSceneNode* orthoCam = sm->addCameraSceneNode(
          sm->getRootSceneNode(), position, target);

      matrix4 projMat;
      projMat.buildProjectionMatrixOrthoLH(width, height, -5, 5);
      orthoCam->setProjectionMatrix(projMat, true);
      orthoCam->bindTargetAndRotation(true);
      if (orthoCam->isOrthogonal())
      {
        CameraAnimator* cameraAnim = new CameraAnimator(position, width,
            height, hm->GetMapDimensions().Height,
            hm->GetMapDimensions().Width, worldState->GetHero(),
            hm->GetCoordinateTranslator());
        orthoCam->addAnimator(cameraAnim);
        cameraAnim->drop();
        cameraAnim = NULL;

        if (width > height)
        {
          LOGI("Creating a landscape camera.");
          landscapeCamera = orthoCam;
        }
        else
        {
          LOGI("Creating a portrait camera");
          portraitCamera = orthoCam;
        }
      }
      else
      {
        LOGE("The created camera is not orthoganol, something is wrong with the perspective matrix.");
      }
    }
    else
    {
      LOGE("The hex map created in the WorldState is NULL.");
    }
  }
  else
  {
    LOGE("The scene manager cannot be loaded from the device.");
  }
}

    int GOAPAstar::calculateH(WorldState from, WorldState to)
    {
        auto care = to.GetCare();
        auto diff = ( ( from.GetFlags() & care ) ^ ( to.GetFlags() & care ) );
        
        int distance = 0;

        for (int i = 0; i < StateType::STATE_NUM; ++i)
            if ( ( diff & ( 1LL << i ) ) != 0 )
                ++distance;

        return distance;
    }

void WaveManager::InsertEnemy()
{
    WorldState* w = WorldState::Instance();
    Enemy* foe;
    Enemy* foe2;
    
    // Si hay dos rutas duplicamos los enemigos en cada ruta
    
    if(vEnemies.at(waveCounter-1).at(enemyCounter) == 1){ // Enemigo tipo 1
        foe= EntityFactory::CreateEnemyOne(*w->vPath->at(0));
        if(WorldState::Instance()->doublePath){
            foe2= EntityFactory::CreateEnemyOne(*w->vPathAux->at(0));
            foe2->alternativeRoute = true;
        }
    }
    else if(vEnemies.at(waveCounter-1).at(enemyCounter) == 2){ // Tipo 2
        foe= EntityFactory::CreateEnemyTwo(*w->vPath->at(0));
        if(WorldState::Instance()->doublePath){
            foe2= EntityFactory::CreateEnemyTwo(*w->vPathAux->at(0));
            foe2->alternativeRoute = true;
        }
    }
    else{
        foe= EntityFactory::CreateEnemyThree(*w->vPath->at(0));
        if(WorldState::Instance()->doublePath){
            foe2= EntityFactory::CreateEnemyThree(*w->vPathAux->at(0));
            foe2->alternativeRoute = true;
        }
    }
        
    w->AddColisionableEntity(foe);
    w->AddEnemy(foe);	
    
    if(WorldState::Instance()->doublePath){
        w->AddColisionableEntity(foe2);
        w->AddEnemy(foe2);
    }
    
    
    // Actualizamos contadores y comprobamos la situación
    enemyCounter++;     
    
    if(enemyCounter == vEnemies.at(waveCounter-1).size()){ // Fin de Racha
        state = Wave::State::Waiting;
        
        enemyCounter=0;
        
        if(waveCounter == vEnemies.size()) // Fin de Vector de Rachas
            state = Wave::State::Finished;
    }
}

void Load(int resolutionWidth, int resolutionHeight)
{
  //The game is being freshly initialized, create a default graph.
  if (NULL == g_GameGraph)
  {
    srand(time(NULL));

    g_GameGraph = new GameGraph(g_Device, g_Driver, g_OhrResourceManager);
    g_GameGraph->SetResolutionWidth(resolutionWidth);
    g_GameGraph->SetResolutionHeight(resolutionHeight);

    //TODO: This is for testing, should actually display a title screen and prompt user
    //for action loading world either from a save file or creating anew.
    WorldState* worldState = new WorldState();
    RunningState* runningState = new RunningState();
    g_GameGraph->SetWorldState(worldState);
    g_GameGraph->AddGameState("running", runningState);
    g_GameGraph->SetCurrentState(runningState);
    worldState->OnEnter(g_GameGraph);
    runningState->OnEnter(g_GameGraph);
  }
  else
  {
    //The game graph has already been loaded. Update its resolution settings and
    //tell it to reload with the new device and driver.
    g_GameGraph->SetResolutionWidth(resolutionWidth);
    g_GameGraph->SetResolutionHeight(resolutionHeight);
    g_GameGraph->Reload(g_Device, g_Driver);
  }

  //Initialze SceneManager position
  g_GameGraph->GetSceneManager()->getRootSceneNode()->setPosition(
      vector3df(0.0f, 0.0f, 0.0f));

  vector3df position(0.000000f, 0.000000f, -4.000000f);
  vector3df target(0.000000f, 0.000000f, -1.000000f);

  //Create portrait and landscape camera
  CreateCamera(resolutionWidth, resolutionHeight, position, target);

  //Select the appropriate one for use
  if (resolutionWidth > resolutionHeight)
  {
    g_GameGraph->GetSceneManager()->setActiveCamera(landscapeCamera);
  }
  else
  {
    g_GameGraph->GetSceneManager()->setActiveCamera(portraitCamera);
  }
}

	void renderLoop(sf::Window & window)
	{
		sf::Clock time;
		WorldState state;
		
		while (state.isRunning())
		{
			this->handleEvents(window, state);
			state.timeStep( time.getElapsedTime().asSeconds() );
			engine.display(state);
			window.display();
		}
		window.close();
	}

std::string VAL::EndGameWorldStateFormatter::asString(const VecInt& gameHistory, WorldState & ws,
		const VecVecVecKey& kb) {
	ostringstream os;
	int scoreIndex = proc->getFunctionId("score");

	int cardCount = proc->typeCardinalities[proc->typeNameIds["card"]];
//	int slotCount = proc->typeCardinalities[proc->typeNameIds["slot"]];

	VecInt cardRow(cardCount, -1);
	NumScalar p1Score = ws.getFluentValue(scoreIndex, 1);
	NumScalar p2Score = ws.getFluentValue(scoreIndex, 2);
	os << "Score 1: " << p1Score << " Score 2: " << p2Score << "\n";
	//assert((unsigned)cardCount <= gameHistory.size());
	if ((unsigned) cardCount < gameHistory.size()) {
		for (unsigned i = 1; i <= (int) cardRow.size(); i++) {
			int index = gameHistory[i];
			VecInt args = proc->operatorIndexToVector(index);
//		os << proc->getOperatorName(args[0]) << " " << proc->asString(args) << "\n";
			cardRow[i - 1] = args[3] + 1; // c0=1 c1=2, etc.
		}
	}
	os << proc->asString(cardRow, LIST) << "\n";

	return os.str();

}

 void handleEvents(sf::Window & window, WorldState & state)
 {
     sf::Event event;
     while (window.pollEvent(event))
     {
         // Close window : exit
         if (event.type == sf::Event::Closed)
             state.setRunning(false);
         
         // Escape key : exit
         if ((event.type == sf::Event::KeyPressed) && (event.key.code == sf::Keyboard::Escape))
             state.setRunning(false);
         
         if (event.type == sf::Event::MouseMoved) {
             state.updateMousePos(event.mouseMove.x, event.mouseMove.y);
         }
     }
 }

void BulletManager::createPlayerBody()
{
	btCollisionShape* bodyShape = new btCapsuleShape(.3f,2.0f);
	WorldState *worldState = (WorldState *) GameState::GAMESTATE;
	Camera *camera = worldState->getPhysicsManager()->getWorldCameras()->getPlayerCamera();
	btTransform bodyTransform;
	bodyTransform.setIdentity();
	float *translate = camera->getEye();
	bodyTransform.setOrigin(btVector3(translate[0],translate[1]-0.79f,translate[2]));
	btScalar mass(10.);
	btVector3 localInertia(0,0,0);
	bodyShape->calculateLocalInertia(mass,localInertia);
	btDefaultMotionState* myMotionState = new btDefaultMotionState(bodyTransform);
	btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,bodyShape,localInertia);
	m_playerBody = new btRigidBody(rbInfo);
	m_playerBody->setAngularFactor(0);
	m_dynamicsWorld->addRigidBody(m_playerBody);
}

    GLBox()
    {
        //sf::err().rdbuf(NULL); //hide errors
        
#ifdef __APPLE__
        //int nullFD = open("/dev/null", O_WRONLY);
        //int oldFD = dup(2); // Duplicate the old file descriptor, so it can be restored
        //dup2(nullFD, 2); // Redirect
#endif
        
        sf::VideoMode mode(RESOLUTION, RESOLUTION, 32);
#ifdef __linux__
        sf::ContextSettings settings(32, 0, 0, 3, 3);
#else
        sf::ContextSettings settings(0, 0, 0, 3, 3);
#endif
        sf::Window window(mode, "glver", sf::Style::Default, settings);
        float ver = getVer();
        
#ifdef __APPLE__
        //dup2(oldFD, 2); // Redirect back
        //close(oldFD); // Not needed anymore
#endif
        
        if( ver < 1.0f ) {
            printf("OpenGL is not supported.\n");
            exit(1);
        }
        printf("OpenGL version %.1f is supported.\n", ver);
        
        
        sf::Clock time;
        WorldState state;
        graphicsInit();
        
        while (state.isRunning())
        {
            this->handleEvents(window, state);
            state.timeStep( time.getElapsedTime().asSeconds() );
            display(state);
            window.display();
        }
        window.close();
    }

	void handleEvents(WorldState & state, RenderEngine & render)
	{
		sf::Event event;
		
		while (window->pollEvent(event))
		{
			if (event.type == sf::Event::Closed)
				state.setRunning(false);
			if ((event.type == sf::Event::KeyPressed) && (event.key.code == sf::Keyboard::Escape))
				state.setRunning(false);
			
			if((event.type == sf::Event::TextEntered) && (event.text.unicode == 'r'))
				state.toggleModelRotate();
			if((event.type == sf::Event::TextEntered) && (event.text.unicode == 'q'))
				state.toggleIncreaseVelocity();
			
			if(event.type == sf::Event::Resized) {
				resize(event.size.width, event.size.height);
			}

			if(event.type == sf::Event::MouseButtonPressed)
			{
				state.lastClickPos[0] = event.mouseButton.x;
				state.lastClickPos[1] = (state.currentRes[1]-event.mouseButton.y);
				state.lastFrameDragPos[0] = event.mouseButton.x;
				state.lastFrameDragPos[1] = (state.currentRes[1]-event.mouseButton.y);
				state.mouseButtonDown = true;
			}

			if(event.type == sf::Event::MouseButtonReleased)
				state.mouseButtonDown = false;

			if(event.type == sf::Event::MouseMoved && state.mouseButtonDown)
			{
				state.cursorDragAmount[0] += state.lastFrameDragPos[0] - event.mouseMove.x;
				state.cursorDragAmount[1] += state.lastFrameDragPos[1] - (state.currentRes[1]-event.mouseMove.y);
				state.lastFrameDragPos[0] = event.mouseMove.x;
				state.lastFrameDragPos[1] = (state.currentRes[1]-event.mouseMove.y);
			}

			if(event.type == sf::Event::MouseWheelMoved)
			{
				state.zoomCamera(event.mouseWheel.delta);
				state.cursorScrollAmount += event.mouseWheel.delta;
			}

			if(event.type == sf::Event::MouseMoved)
			{
				state.cursorAbsolutePos[0] = event.mouseMove.x;
				state.cursorAbsolutePos[1] = (state.currentRes[1]-event.mouseMove.y);
			}

			if ((event.type == sf::Event::KeyPressed) && (event.key.code == sf::Keyboard::Up))
				state.zoomCamera(1);
			if ((event.type == sf::Event::KeyPressed) && (event.key.code == sf::Keyboard::Down))
				state.zoomCamera(-1);

		}
	}

    int GOAPAstar::nodeInClosed(WorldState ws)
    {
        for (uint i = 0, n = _closed.size(); i < n; ++i)
        {
            if (_closed[i].ws.GetFlags() == ws.GetFlags())
                return i;
        }

        return -1;
    }

    int GOAPAstar::nodeInOpened(WorldState ws)
    {
        for (uint i = 0, n = _open.size(); i < n; ++i)
        {
            if (_open[i].ws.GetFlags() == ws.GetFlags())
                return i;
        }

        return -1;
    }

	Program4()
	{
		getWindowContext();

		WorldState state;
		render.init(state);
		
		previousPos = glm::vec2(0);
		buttonDown[0]=false;
		buttonDown[1]=false;
		buttonDown[2]=false;
		
		sf::Clock c;
		float lastFrame = c.restart().asSeconds();
		float lastPrint = lastFrame;
		float targetFrameTime = 1.0f/(float)TARGET_FPS;
		
		while (state.isRunning())
		{			
			App->setActive();
			float currentTime = c.getElapsedTime().asSeconds();
			float sinceLastFrame = currentTime - lastFrame;
			float sleepTime = targetFrameTime - sinceLastFrame;
			if(sleepTime > 0)
				sf::sleep(sf::seconds(sleepTime));
			
			currentTime = c.getElapsedTime().asSeconds();
			lastFrame = currentTime;
			float sinceLastPrint = currentTime - lastPrint;
            
			handleEvents(state, render);
			state.timeStep(currentTime);
            
			if(sinceLastPrint > PRINT_FPS_INTERVAL) {
				lastPrint = currentTime;
				state.printFPS();
			}
            
			render.display(state);
			App->display();
		}
	}

	/**
	*Draws this block to the screen
	*
	*@param state = The current world state, which we pass to the underlying rendering function
	*/
	void BlockEntity::Render(WorldState& curWorldState)
	{
		auto values = Find("value");
		if (values->Get<int>(mState))
		{
			std::string color = Find("BlockColor")->Get<std::string>();
			if (color == "Blue")
			{
				mRenderer.RenderOnScreen(*(curWorldState.GetWorld()->GetWindow()), TetrominoRender::TextureType::BlueBlock, sf::Vector2f(mX, mY));
			}
			else if (color == "Green")
			{
				mRenderer.RenderOnScreen(*(curWorldState.GetWorld()->GetWindow()), TetrominoRender::TextureType::GreenBlock, sf::Vector2f(mX, mY));
			}
			else if (color == "Pink")
			{
				mRenderer.RenderOnScreen(*(curWorldState.GetWorld()->GetWindow()), TetrominoRender::TextureType::PinkBlock, sf::Vector2f(mX, mY));
			}
			else if (color == "Purple")
			{
				mRenderer.RenderOnScreen(*(curWorldState.GetWorld()->GetWindow()), TetrominoRender::TextureType::PurpleBlock, sf::Vector2f(mX, mY));
			}
			else if (color == "Yellow")
			{
				mRenderer.RenderOnScreen(*(curWorldState.GetWorld()->GetWindow()), TetrominoRender::TextureType::YellowBlock, sf::Vector2f(mX, mY));
			}
			else if (color == "Orange")
			{
				mRenderer.RenderOnScreen(*(curWorldState.GetWorld()->GetWindow()), TetrominoRender::TextureType::OrangeBlock, sf::Vector2f(mX, mY));
			}
		}
	}

void BulletManager::updateDynamicsWorld(float nSpeed)
{
	Profiler::getInstance()->startProfile("Update Dynamics World");
	// move camera
	WorldState *worldState = (WorldState *) GameState::GAMESTATE;
	WorldCamera *camera = worldState->getPhysicsManager()->getWorldCameras()->getPlayerCamera();
	float nOldEye[3];
	nOldEye[0] = camera->getEyeX();
	nOldEye[1] = camera->getEyeY();
	nOldEye[2] = camera->getEyeZ();
	if (InputManager::getInstance()->isKeyDown('w'))
	{
		camera->moveForward(nSpeed*0.1f);
	}
	if (InputManager::getInstance()->isKeyDown('s'))
	{
		camera->moveBackward(nSpeed*0.1f);
	}
	if (InputManager::getInstance()->isKeyDown('a'))
	{
		camera->moveLeft(nSpeed*0.1f);
	}
	if (InputManager::getInstance()->isKeyDown('d'))
	{
		camera->moveRight(nSpeed*0.1f);
	}
	float *nNewEye = camera->getEye();
	m_playerBody->activate(true);
	m_playerBody->setLinearVelocity(100.0f*btVector3(nNewEye[0]-nOldEye[0],-0.04f,nNewEye[2]-nOldEye[2]));

	// check physics
	m_dynamicsWorld->stepSimulation(1.f/60.f,10);

	// update camera
	btVector3 camPos = m_playerBody->getWorldTransform().getOrigin();
	camera->setPosition(camPos[0],camPos[1]+0.8,camPos[2]);
	Profiler::getInstance()->endProfile();
}

    void GOAPAstar::getPossibleStateTransitions(GOAPlanner *ap, WorldState state)
    {
        _transitions.clear();

        for (auto &pair : ap->_actions)
        {
            auto action = pair.second;

            auto pre = action.GetPreWorld();
            auto care = pre.GetCare();
            bool met = (pre.GetFlags() & care) == (state.GetFlags() & care);

            if (met)
            {
                // compute the future world
                WorldState to = state;
                to.ApplyAction(action);

                // add the action and the future world to the transitions array
                _transitions.emplace_back(std::make_pair(action, to));
            }
        }
    }

	/**
	The update function of ActionAdopt calls the setSector method of
	the entity that contains this action on the provided target sector.
	This should produce the behavior of orphaning the entity and moving it
	into a new sector

	@param curState the current worldstate to reference during update
	*/
	void ActionAdopt::Update(const WorldState& curState)
	{
		//Double check to ensure that our target sector isn't the one
		//that our containing entity already exists in
		std::string targetName = Find("Target")->Get<std::string>();
		Sector* curSector = curState.GetSector();
		if (targetName == curSector->Name())
		{
			return;
		}

		//Grab the entity that contains us
		Entity* curEntity = curState.GetEntity();
		
		//Grab the world
		World* curWorld = curState.GetWorld();

		//Set up the sector
		Datum* targetDatum = curWorld->Sectors()->Find(targetName);
		if (targetDatum == nullptr || targetDatum->GetType() != Datum::TABLE)
		{
			throw std::exception("Sector could not be found");
		}
		else		
		{
			Sector* targetSector = targetDatum->Get<Scope*>()->As<Sector>();
			if (targetSector == nullptr)
			{
				throw std::exception("Malformed sector");
			}
			else
			{
				curEntity->SetSector(targetSector);
				return;
			}
		}
	}

bool Tackler::MayDangerousIfTackle(const PlayerState & tackler, const WorldState & world_state)
{
	if (tackler.GetTackleProb(false) < FLOAT_EPS && tackler.GetTackleProb(true) < FLOAT_EPS) { //不可铲
		return false;
	}

	const Vector tackler_pos = tackler.GetPos();
	const Vector ball_pos = world_state.GetBall().GetPos();
	const Vector ball_2_tackler = ball_pos - tackler_pos;
	const double ball_dist2 = ball_2_tackler.Mod2();
	const AngleDeg ball_dir = ball_2_tackler.Dir();

	for (unsigned i = 0; i < world_state.GetPlayerList().size(); ++i) {
		const PlayerState & opp = *world_state.GetPlayerList()[i];

		if (!opp.IsAlive()) continue; //dead
		if (opp.GetUnum() * tackler.GetUnum() > 0) continue; //not opp
		if (opp.IsIdling()) continue; //idling
		if (!opp.IsKickable()) continue; //not kickable

		//cond. 1. opp no dashing -- 无从知晓，这里认为对手一定会dash
		//cond. 2. ball near -- 球离自己更近，不构成危险情况
		Vector opp_2_tackler = opp.GetPos() - tackler_pos;
		if (opp_2_tackler.Mod2() > ball_dist2) continue;
		//cond. 3. behind or big y_diff -- 对手在自己与球连线方向上在自己身后或错开位置太大，不构成危险情况
		opp_2_tackler = opp_2_tackler.Rotate(-ball_dir);
		if (opp_2_tackler.X() < 0.0 || std::fabs( opp_2_tackler.Y() ) > opp.GetPlayerSize() + tackler.GetPlayerSize()) continue;
		//cond. 4. over body_diff -- 对手身体角度跟自己与球连线方向的夹角大于90度，不构成危险情况
		AngleDeg body_diff = std::fabs( GetNormalizeAngleDeg( opp.GetBodyDir() - ball_dir ) );
		if (body_diff > 90.0) continue;

		return true;
	}

	return false;
}

	bool Goal::satisfied(const WorldState& worldState)
	{
		bool satisfied = true;

		for (const auto& state : mConditions)
		{
			if (worldState.at(state.first) != state.second)
			{
				satisfied = false;
				break;
			}
		}

		return satisfied;
	}

	void handleEvents(WorldState & state, RenderEngine & render)
	{
		sf::Event event;
		
		while (App->pollEvent(event))
		{
			if (event.type == sf::Event::Closed)
				state.setRunning(false);
			if ((event.type == sf::Event::KeyPressed) && (event.key.code == sf::Keyboard::Escape))
				state.setRunning(false);
			
            if((event.type == sf::Event::TextEntered) && (event.text.unicode == 'q'))
                state.setShadingMode(0);
            if((event.type == sf::Event::TextEntered) && (event.text.unicode == 'w'))
                state.setShadingMode(1);
            if((event.type == sf::Event::TextEntered) && (event.text.unicode == 'e'))
                state.setShadingMode(2);
			if((event.type == sf::Event::TextEntered) && (event.text.unicode == 'r'))
                state.toggleModelRotate();
			if((event.type == sf::Event::TextEntered) && (event.text.unicode == 't'))
                state.toggleLightRotate();
		}
	}