#include "stage.hpp"

#include "main.hpp"

#include "stadium.hpp"
#include "playstate.hpp"
#include "spaces/space.hpp"
#include "tiles/tile.hpp"
#include "character/character.hpp"
#include "character/character_modelview.hpp"
#include "character/character_humancontroller.hpp"
#include "character/aicontrollers/coopbuddy.hpp"
#include "character/aicontrollers/grunt.hpp"
#include "character/aicontrollers/karateka.hpp"
#include "character/aicontrollers/thrower.hpp"
#include "stageentity.hpp"
#include "enemies/birdlike.hpp"
#include "enemies/grenade.hpp"
#include "kungflowutils.hpp"

#include <dlib/optimization/max_cost_assignment.h>
//using namespace dlib;

Stage::Stage(const KungFlowGame *game, const PlayState *playState) : game(game), playState(playState), stadium(nullptr), flag(nullptr) {//, id(id) {

  shadowSprite = new Sprite();
  shadowSprite->LoadFile("res/gfx/shadow32x16.png", e_FilterMode_Anisotropic);
  //shadowSprite->SetAlpha(0.4f);
  shadowObject = new Object(&playState->GetWorldNode(), shadowSprite);
  //shadowObject->spatial.position.Set(-0.2f, 0.0f);
  shadowObject->spatial.orientation = playState->GetViewOrientation();
  //shadowObject->spatial.scale.Set(1.6f, 0.8f);

  //triggeredEnemyCount = 0;

  assignmentMatrixUpdateInterval_ms = 2000;
  timeSinceLastAssignmentMatrixUpdate_ms = assignmentMatrixUpdateInterval_ms; // make sure it runs the first iteration

  personnelChangeConnection = sig_PersonnelChange.connect(boost::bind(&Stage::UpdateAssignmentMatrix, this));

}

Stage::~Stage() {

  personnelChangeConnection.disconnect();

  DeleteCharacters();
  DeleteEntities();

  assert(masterSlaveConnections.empty() == true);

  delete shadowObject;
  delete shadowSprite;

  if (stadium) delete stadium;
}

void Stage::InitLevel(int levelID) {

  if (levelID == 0) {

    stadium = new Stadium(game, playState, this);
    stadium->GenerateTriggeredSpaces();

    // - left - <X> + right +
    // -   up - <Y> + down  +
    // consider that ascii art ^ heh
    // x+ = southeast
    // y+ = southwest

    // your buddy!
    //SpawnAICharacter(e_Team_GoodGuys, stadium->GetGoodGuySpawnPoint(1), e_AIControllerType_CoopBuddy);
    //SpawnAICharacter(e_Team_GoodGuys, stadium->GetGoodGuySpawnPoint(2), e_AIControllerType_CoopBuddy);
    //SpawnAICharacter(e_Team_GoodGuys, stadium->GetGoodGuySpawnPoint(3), e_AIControllerType_CoopBuddy);

    // bad guy
    //SpawnAICharacter(e_Team_BadGuys, Vector2(stadium->GetDimensions()[0] * 0.5f, stadium->GetDimensions()[1] - 4));

    /* 16 bad guys!
    for (int i = 0; i < 16; i++) {
      Vector2 enemySpawnPos;
      while (true) {
        enemySpawnPos = Vector2(Random(2, stadium->GetDimensions()[0] - 2), Random(12, stadium->GetDimensions()[1] - 2));
        if (enemySpawnPos.GetDistance(stadium->GetGoodGuySpawnPoint(0)) > 4.0f) break;
      }
      SpawnAICharacter(1, enemySpawnPos);
    }
    */

  }

}

void Stage::OnStepPhysicsTime() {

  if (_DISABLEFLOW == false) {
    // accelerate/decelerate level flow depending on player movement
    // these 3 should add up to 1.0f
    float movementInfluence = 0.5f;//0.2f; // accelerates movements
    float positionInfluence = 0.2f;//0.7f;
    float desiredDirectionInfluence = 0.3f;//0.1f; // adds directness
    float flowVelocityBase = 1.0f;//0.7f;//0.6f;
    float flowVelocityVariance = 0.5f;//0.4f;

    float averageVelocity = 0.0f;
    int humanCharsNum = GetHumanCharactersRef().size();
    for (int i = 0; i < humanCharsNum; i++) {

      averageVelocity += (NormalizedClamp(GetHumanCharactersRef().at(i)->GetMovement().coords[GetStadium()->GetFlowAxis()], -5.0f, 5.0f) * 2.0f - 1.0f) * movementInfluence;
      //printf("%f\n", averageVelocity);

      float toFlowLine = GetStadium()->GetFlowPosition().coords[GetStadium()->GetFlowAxis()] - GetHumanCharactersRef().at(i)->GetPosition().coords[GetStadium()->GetFlowAxis()];
      //toFlowLine = Clamp(toFlowLine, -_AXISMOVEMENT_BANDWIDTH, _AXISMOVEMENT_BANDWIDTH) / _AXISMOVEMENT_BANDWIDTH; // -1 to 1
      //toFlowLine = Clamp(toFlowLine, -1.0f, 1.0f) / 2.0f; // -0.5 to 0.5
      //toFlowLine = Clamp(toFlowLine, -2.0f, 2.0f) / 1.0f; // -2 to 2
      //toFlowLine = pow(fabs(toFlowLine) * 0.5f, 0.8f) * 2.0f * SignSide(toFlowLine); // make 0.5s higher (todo: rewrite)
      toFlowLine = Clamp(toFlowLine, -0.5f, 0.5f) / 0.5f; // -1 to 1
      //toFlowLine = pow(fabs(toFlowLine) * 0.5f, 0.8f) * 2.0f * SignSide(toFlowLine); // make 0.5s higher (todo: rewrite)
      averageVelocity -= toFlowLine * positionInfluence;

      averageVelocity += GetHumanCharactersRef().at(i)->GetModelView()->GetDesiredDirection().coords[GetStadium()->GetFlowAxis()] * desiredDirectionInfluence;

    }
    averageVelocity /= humanCharsNum;
    float flowVelocity = flowVelocityBase + averageVelocity * flowVelocityVariance;
    stadium->SetFlowTilesPerBeat(flowVelocity);

  } else { // if (_DISABLEFLOW) {

    float averageHumanZPos_dud;
    stadium->SetFlowTilesPerBeat(0.0f);
    stadium->SetFlowPosition(GetAverageHumanCharacterPosition(0.0f, 0.0f, 0.0f, averageHumanZPos_dud));

  }

  stadium->OnStepPhysicsTime();

  timeSinceLastAssignmentMatrixUpdate_ms += playState->GetPhysicsTimeStep_ms();
  if (timeSinceLastAssignmentMatrixUpdate_ms > assignmentMatrixUpdateInterval_ms) {
    if (_ENABLE_ASSIGNMENTMATRIX) UpdateAssignmentMatrix();
    //printf("UPDATE\n");
    timeSinceLastAssignmentMatrixUpdate_ms = 0;
  }

  //if (Random(0, 1) < 0.003f) SpawnBadGuy(1, Vector2(Random(-50, 50), Random(-50, 50)));

/* stage-wide input?
  for (unsigned int i = 0; i < humanGamersWithPlayerBody.size(); i++) {
    const KeyInput &keyInput = humanGamersWithPlayerBody.at(i).humanGamer->GetKeyInput();
  }
*/


  // SPAWN AI CHARS HERE

  //if (Random(0.0f, 1.0f / (float)game->GetPhysicsTimeStep_ms()) < .0005f) {
  //if (Random(0.0f, 1.0f / (float)game->GetPhysicsTimeStep_ms()) < .0001f) { // .0005f == competitive

  // spawn random grunts
/*
  if (Random(0.0f, 1.0f / (float)game->GetPhysicsTimeStep_ms()) < .0012f) {
    Vector2 position = Vector2(Random(7, 15) + stadium->GetFlowPosition().coords[0], Random(-5, 5));//GetStadium()->GetDimensions()[1] - 1));
    SpawnAICharacter(1, position, e_AIControllerType_Grunt);
  }
*/

/*
  int everyNthMeter = 1;
  //if (int(playState->GetStage()->GetStadium()->GetFlowPosition().coords[playState->GetStage()->GetStadium()->GetFlowAxis()]) % everyNthMeter == 0) {
  if (int(playState->GetStage()->GetStadium()->GetFlowPosition().coords[playState->GetStage()->GetStadium()->GetFlowAxis()]) > everyNthMeter * triggeredEnemyCount) {

    triggeredEnemyCount++;

    Vector2 position;
    position.coords[stadium->GetFlowAxis()] = Random(12, 14) + stadium->GetFlowPosition().coords[stadium->GetFlowAxis()];
    position.coords[abs(1 - stadium->GetFlowAxis())] = Random(-8, 8);//GetStadium()->GetDimensions()[1] - 1));
//    } else {
//      position = Vector2(Random(8, 10) + stadium->GetFlowPosition().coords[0], Random(-5, 5));//GetStadium()->GetDimensions()[1] - 1));
//    }
    SpawnAICharacter(1, position, e_AIControllerType_Grunt);
  }
*/

/*
  // slow, long spawn
  unsigned int gameDuration_ms = 1000 * 240;
  if (game->GetPhysicsTimeStepsDone() % 5000 == 0 && game->GetPhysicsTimeStep_ms() * game->GetPhysicsTimeStepsDone() < gameDuration_ms) {
  // fast, short spawn
  //unsigned int gameDuration_ms = 1000 * 6;
  //if (game->GetPhysicsTimeStepsDone() % 60 == 0 && game->GetPhysicsTimeStep_ms() * game->GetPhysicsTimeStepsDone() < gameDuration_ms) {
    unsigned int enemyNum = 4;
    for (unsigned int i = 0; i < enemyNum; i++) {
      //Vector2 enemySpawnPos(Random(2.0f, GetStadium()->GetDimensions()[0] - 3.0f), GetStadium()->GetDimensions()[1] - 3.0f - Random(0.0f, 2.0f));
      Vector2 enemySpawnPos(Random(2.0f, GetStadium()->GetDimensions()[0] - 3.0f), Random(3.0f, GetStadium()->GetDimensions()[1] - 3.0f));
      SpawnAICharacter(1, enemySpawnPos, e_AIControllerType_Karateka);
      if (Random(0, 1) < 0.5f) {
        // bonus char, sometimes
        Vector2 enemySpawnPos(Random(2.0f, GetStadium()->GetDimensions()[0] - 3.0f), Random(3.0f, GetStadium()->GetDimensions()[1] - 3.0f));
        SpawnAICharacter(1, enemySpawnPos, e_AIControllerType_Thrower);
      }
      //SpawnAICharacter(1, enemySpawnPos, e_AIControllerType_Thrower);
    }
  }
*/
/*
  // spawn random enemies
  //if (Random(0.0f, 6000.0f) < 1.0f) {
  if (Random(0.0f, 1.0f / (float)game->GetPhysicsTimeStep_ms()) < .0002f) {
    //EnemyBirdlike *birdy = new EnemyBirdlike(game, playState);
    //enemies.push_back(birdy);
    Vector2 position = Vector2(-10, Random(15, 90) + stadium->GetFlowPosition().coords[1]);//GetStadium()->GetDimensions()[1] - 1));
    float zPosition = 0;
    const Tile *tile = playState->GetStage()->GetStadium()->GetTile(Vector2(stadium->GetFlowPosition().coords[0], position.coords[1]));
    if (tile != nullptr) zPosition = tile->GetZPosition();

    SpawnEnemy(e_EnemyType_Birdlike, position, zPosition);
  }
*/
/*
  if (Random(0.0f, 1.0f / (float)game->GetPhysicsTimeStep_ms()) < .0008f) {
    //EnemyGrenade *grenade = new EnemyGrenade(game, playState);
    //enemies.push_back(grenade);
    SpawnEnemy(e_EnemyType_Grenade, Vector2(Random(5, 9), Random(2, 4)));
  }
*/

  for (unsigned int i = 0; i < characters.size(); i++) {
    characters.at(i)->OnStepPhysicsTime();
  }

  // remove entities
  float entityExpirationDistance = 40.0f;
  std::vector<StageEntity*>::iterator entityIter = entities.begin();
  while (entityIter != entities.end()) {
    (*entityIter)->OnStepPhysicsTime();
    if ((*entityIter)->HasExpired() || ((*entityIter)->GetPosition() - (*entityIter)->GetZPosition()).GetDistance(GetCameraFocusPositionProj()) > entityExpirationDistance) {

      // remove possible master/slave relationship
      const Character *master = GetMaster((*entityIter));
      if (master) {
        RemoveMasterSlaveConnection(master, (*entityIter));
      }

      delete (*entityIter);
      entityIter = entities.erase(entityIter);

    } else {

      entityIter++;

    }
  }

  CheckCollisions();


  for (unsigned int i = 0; i < characters.size(); i++) { // todo: maybe switch direction every frame to stop player 1 from having first actions?
    characters.at(i)->PostStepPhysicsTime();
  }
  for (unsigned int i = 0; i < entities.size(); i++) {
    entities.at(i)->PostStepPhysicsTime();
  }

  float expirationDistance = 0.0f;
  int flowAxis = playState->GetStage()->GetStadium()->GetFlowAxis();
  std::vector<Character*>::iterator charIter = characters.begin();
  while (charIter != characters.end()) {

    // survive falls!
    if ((*charIter)->GetZPosition() < 0 && (*charIter)->GetTeam() == e_Team_GoodGuys) {
      const Space *closestSpace = stadium->GetClosestSpace((*charIter)->GetPosition());
      if (closestSpace != nullptr) {
        (*charIter)->SetPosition(closestSpace->GetCenterPosition());
        (*charIter)->SetZPosition(stadium->GetBaseZPosition(closestSpace->GetCenterPosition()) + 5.0f);
        (*charIter)->SetZMovement(5.0f);
      }
    }

    // expire characters that are too far back
    //if ((*charIter)->GetTeamID() == 1 && (*charIter)->GetPosition().coords[flowAxis] + expirationDistance < GetStadium()->GetFlowPosition().coords[flowAxis]) (*charIter)->Expire();
    float averageHumanZPos_dud;
    Vector2 killLine = GetAverageHumanCharacterPosition(0.0f, -6.0f, 0.0f, averageHumanZPos_dud);
    if ((*charIter)->GetTeam() == e_Team_BadGuys && (*charIter)->GetPosition().coords[flowAxis] + expirationDistance < killLine.coords[flowAxis]) (*charIter)->Expire();

    // die :(
    if ( ((*charIter)->GetModelView_const()->GetHealthPoints() <= 0.0f && ((*charIter)->GetModelView_const()->GetActiveMoveType() != e_MoveType_Hit || (*charIter)->GetModelView_const()->GetActiveMove()->GetCurrentTimeFactor() > 0.3f)) || // death from violence :(
         ((*charIter)->GetZPosition() < 0 && (*charIter)->GetTeam() != e_Team_GoodGuys) || // death from falling :(
         (*charIter)->HasExpired()) { // death from.. expiration? :(
      //if ((*charIter)->GetTeamID() == 1) SpawnAICharacter(0, (*charIter)->GetPosition()); // converts bad guy into good guy
      //if ((*charIter)->GetTeamID() == 0) SpawnAICharacter(1, (*charIter)->GetPosition()); // converts good guy into bad guy
      //delete (*charIter);
      //charIter = characters.erase(charIter);
      //sig_PersonnelChange();
      DeleteCharacter(*charIter);
    } else {
      charIter++;
    }
  }

  for (unsigned int i = 0; i < masterSlaveConnections.size(); i++) {
    Vector2 slavePosition;
    float slaveZPosition;
    masterSlaveConnections.at(i).master->GetSlavePosition(slavePosition, slaveZPosition);
    masterSlaveConnections.at(i).slave->SetPosition(slavePosition);
    masterSlaveConnections.at(i).slave->SetZPosition(slaveZPosition);
    masterSlaveConnections.at(i).slave->SetMovement(masterSlaveConnections.at(i).master->GetMovement());
    masterSlaveConnections.at(i).slave->SetZMovement(masterSlaveConnections.at(i).master->GetZMovement());
  }

}

const bool SortPhysicsObjectDepth(const PhysicsObject *e1, const PhysicsObject *e2) {

  // height layers (imperfect, but nice enough for now)

//  if (e1->GetPosition().coords[0] + e1->GetPosition().coords[1] + (e1->GetZPosition() + e1->GetHeight() * 0.5f) * 1000 <
//      e2->GetPosition().coords[0] + e2->GetPosition().coords[1] + (e2->GetZPosition() + e2->GetHeight() * 0.5f) * 1000) return true;
  if (e1->GetPosition().coords[0] + e1->GetPosition().coords[1] + (e1->GetZPosition() + e1->GetHeight() * 0.5f) * 15 <
      e2->GetPosition().coords[0] + e2->GetPosition().coords[1] + (e2->GetZPosition() + e2->GetHeight() * 0.5f) * 15) return true;
  else return false;

/* distance to viewer
  float e1viewdistance = e1->GetPosition().coords[0] * 0.707f + e1->GetPosition().coords[1] * 0.707f + e1->GetZPosition() + e1->GetHeight() * 0.5f;
  float e2viewdistance = e2->GetPosition().coords[0] * 0.707f + e2->GetPosition().coords[1] * 0.707f + e2->GetZPosition() + e2->GetHeight() * 0.5f;
  return e1viewdistance < e2viewdistance;
*/

/*
  // only when sprites overlap

  Vector2 center1, radius1;
  e1->GetSpriteBounds(center1, radius1);
  Vector2 center2, radius2;
  e2->GetSpriteBounds(center2, radius2);

// no access to playstate here :(
//  playState->DrawDebugLine(center1 - radius1.GetRotated(playState->GetViewOrientation()), center2 + radius2.GetRotated(playState->GetViewOrientation()), e_CoordinateSystem_World, 0.2f, 0.6f, 0.2f, 0.1f);
//  playState->DrawDebugLine(center2 - radius2.GetRotated(playState->GetViewOrientation()), center1 + radius1.GetRotated(playState->GetViewOrientation()), e_CoordinateSystem_World, 0.4f, 0.2f, 0.4f, 0.1f);

  if ( center1.coords[0] - radius1.coords[0] > center2.coords[0] + radius2.coords[0] ||
       center1.coords[1] - radius1.coords[1] > center2.coords[1] + radius2.coords[1] ||
       center1.coords[0] + radius1.coords[0] < center2.coords[0] - radius2.coords[0] ||
       center1.coords[1] + radius1.coords[1] < center2.coords[1] - radius2.coords[1] ) {

    // doesn't collide, no preferred sort
    //printf("bip%i\n", e1);
    return false;

  } else {

    //if (e1->GetPosition().coords[0] + e1->GetPosition().coords[1] + e1->GetZPosition() < e2->GetPosition().coords[0] + e2->GetPosition().coords[1] + e2->GetZPosition()) return true;
    if (e1->GetPosition().coords[0] + e1->GetPosition().coords[1] + e1->GetZPosition() < e2->GetPosition().coords[0] + e2->GetPosition().coords[1] + e2->GetZPosition()) return true;
    else return false;

  }
*/

}

void Stage::Draw(const Viewport &viewport, const Camera &camera) const {

  stadium->DrawBackground(viewport, camera);

  // remember, lights don't have to be drawn in order, so we can draw them here before the objects. they will be overlaid at the end of the rendering pipeline
  stadium->DrawLights();
  //stadium->Draw(viewport, camera);
  stadium->DrawBackLayer(viewport, camera);

  // flow line
  if (_DEBUGVISUALS) {
    Vector2 p1 = stadium->GetFlowPosition() + stadium->GetFlowDirection().GetRotated( 0.5f * pi) * 10.0f;
    Vector2 p2 = stadium->GetFlowPosition() + stadium->GetFlowDirection().GetRotated(-0.5f * pi) * 10.0f;
    float z = 0;
    const Tile *tile = stadium->GetTile(stadium->GetFlowPosition());
    if (tile != nullptr) z = tile->GetZPosition();
    playState->DrawDebugLine(p1 - z, p2 - z, e_CoordinateSystem_World, 0, 1, 1, 0.1f);
  }

  std::vector<PhysicsObject*> drawQueue;
  stadium->GetPhysicsObjects(drawQueue);
  drawQueue.insert(drawQueue.end(), characters.begin(), characters.end());
  drawQueue.insert(drawQueue.end(), entities.begin(), entities.end());
  //printf("enemies %i\n", enemies.size());

  std::sort(drawQueue.begin(), drawQueue.end(), SortPhysicsObjectDepth);

  //CreateTopologyDAG(drawQueue.begin(), drawQueue.end(), SortPhysicsObjectDepth);

  for (unsigned int i = 0; i < drawQueue.size(); i++) {
    //goodGuys.at(i)->GetModelView()->Draw(viewport, camera);
    drawQueue.at(i)->Draw(viewport, camera);

/*
    // debug
    Vector2 center, radius;
    drawQueue.at(i)->GetSpriteBounds(center, radius);
    playState->DrawDebugLine(center - radius.GetRotated(playState->GetViewOrientation()), center + radius.GetRotated(playState->GetViewOrientation()), e_CoordinateSystem_World, 0.2f, 0.6f, 0.2f, 0.4f);
    playState->DrawDebugLine(center - radius.GetRotated(playState->GetViewOrientation() + 0.5f * pi), center + radius.GetRotated(playState->GetViewOrientation() + 0.5f * pi), e_CoordinateSystem_World, 0.2f, 0.6f, 0.2f, 0.2f);
*/
  }

  stadium->DrawFrontLayer(viewport, camera);

  // draw 3d help lines for close entities, to better assess height/angle
  float minHelperDistance = 2.0f;
  float minHelperZDistance = 4.0f;
  float r = 0.4f;
  float g = 1.0f;
  float b = 0.4f;
  float a = 0.6f;
  for (unsigned int i = 0; i < _humanCharacters.size(); i++) {
    for (unsigned int j = 0; j < entities.size(); j++) {
      float horizDistance = (entities.at(j)->GetPosition() + entities.at(j)->GetMovement() * 0.4f).GetDistance(_humanCharacters.at(i)->GetPosition());
      float zDistance = fabs((entities.at(j)->GetZPosition() + entities.at(j)->GetZMovement() * 0.4f) - _humanCharacters.at(i)->GetZPosition());
      if (horizDistance < minHelperDistance && zDistance < minHelperZDistance) {

        float horizDistanceFactor = 1.0f - horizDistance / minHelperDistance;
        float vertDistanceFactor = 1.0f - zDistance / minHelperZDistance;
        float distanceFactor = pow(horizDistanceFactor * vertDistanceFactor, 0.5f);

        // horiz lines
        playState->DrawLine(_humanCharacters.at(i)->GetPosition() - _humanCharacters.at(i)->GetZPosition(), entities.at(j)->GetPosition() - _humanCharacters.at(i)->GetZPosition(), e_CoordinateSystem_World, r, g, b, a * 0.7f * distanceFactor);
        playState->DrawLine(_humanCharacters.at(i)->GetPosition() - entities.at(j)->GetZPosition(), entities.at(j)->GetPosition() - entities.at(j)->GetZPosition(), e_CoordinateSystem_World, r, g, b, a * 1.0f * distanceFactor);

        // vert lines
        playState->DrawLine(_humanCharacters.at(i)->GetPosition() - _humanCharacters.at(i)->GetZPosition(), _humanCharacters.at(i)->GetPosition(), e_CoordinateSystem_World, r, g, b, a * 0.7f * distanceFactor);
        playState->DrawLine(entities.at(j)->GetPosition() - entities.at(j)->GetZPosition(), entities.at(j)->GetPosition(), e_CoordinateSystem_World, r, g, b, a * 0.7f * distanceFactor);

        // diagonal direct from-to line
        playState->DrawLine(_humanCharacters.at(i)->GetPosition() - _humanCharacters.at(i)->GetZPosition(), entities.at(j)->GetPosition() - entities.at(j)->GetZPosition(), e_CoordinateSystem_World, r * 0.5f + 0.5f, g * 0.5f + 0.5f, b * 0.5f + 0.5f, a * 0.7f * distanceFactor);

      }
    }
  }

}

void Stage::DrawLight(const Vector2 &worldPosition, float worldZPosition, const Vector2 &radius, float r, float g, float b) const {

  Spatial worldSpatial;
  worldSpatial.position = worldPosition;
  worldSpatial.position -= worldZPosition;// * 0.707f;
  //worldSpatial.z = z;
  worldSpatial.scale = radius;
  worldSpatial.scale.coords[0] /= 0.707f;
  worldSpatial.scale.coords[1] *= 0.707f;
  Spatial screenSpatial = WorldToScreenSpatial(worldSpatial, game->GetMainViewport(), playState->GetCamera());
  Vector2 screenPosition = screenSpatial.position;
  Vector2 screenRadiusVec = screenSpatial.scale;

  float intensity = cos(playState->GetCurrentMusic()->GetBeatOffset() * 2.0f * pi) * 0.5f + 0.5f;
  intensity = intensity * 0.3f + 0.7f * (1.0f - playState->GetCurrentMusic()->GetBeatOffset());
  intensity = pow(intensity, 0.8f);
  intensity = 0.8f + intensity * 0.4f;
  //intensity = 0.5f + intensity * 1.0f;
  playState->DrawLight(screenPosition, screenRadiusVec, std::min(1.0f, r * intensity), std::min(1.0f, g * intensity), std::min(1.0f, b * intensity));
}

void Stage::DrawShadow(const Vector2 &position, float zPosition, float scale, float intensity) const {

  shadowSprite->SetAlpha(intensity * _SHADOWINTENSITY);
  shadowObject->spatial.scale.Set(1.6f * scale, 0.8f * scale);

  const Tile *tile = playState->GetStage()->GetStadium()->GetTile(position);
  if (tile != nullptr) {
    float tileZ = tile->GetZPosition();
    //playState->DrawLine(position - tileZ, position - zPosition, e_CoordinateSystem_World, 0.0f, 0.5f, 1.0f, 0.3f);
    //playState->GetStage()->DrawLight(GetPosition(), tileZ, 0.8f, 0.35f, 0.4f, 0.55f);
    shadowObject->spatial.position = position - tileZ;
    shadowObject->Draw(game->GetMainViewport(), playState->GetCamera());
  }

}


void Stage::SpawnHumanCharacter(e_Team team, const Vector2 &position, const HumanGamer *humanGamer) {
  // create character for human player
  Character *humanCharacter = new Character(game, playState, team, position);
  CharacterHumanController *controller = new CharacterHumanController(game, playState, humanCharacter, humanGamer);
  humanCharacter->SetControllerOwnership(controller);

  characters.push_back(humanCharacter);
  _humanCharacters.push_back(humanCharacter);
  sig_PersonnelChange();
}

void Stage::SpawnAICharacter(e_Team team, const Vector2 &position, e_AIControllerType aiControllerType) {

  // create character for ai player

  Character *aiCharacter = new Character(game, playState, team, position);
  CharacterAIController *controller;

  switch (aiControllerType) {
    case e_AIControllerType_CoopBuddy:
      controller = new CoopBuddyController(game, playState, aiCharacter);
      break;
    case e_AIControllerType_Karateka:
      controller = new KaratekaController(game, playState, aiCharacter);
      break;
    case e_AIControllerType_Thrower:
      controller = new ThrowerController(game, playState, aiCharacter);
      break;
    case e_AIControllerType_Grunt:
      controller = new GruntController(game, playState, aiCharacter, e_GruntType_Rusher);
      break;
    default:
      controller = nullptr;
      break;
  }
  if (controller) aiCharacter->SetControllerOwnership(controller);

  characters.push_back(aiCharacter);
  sig_PersonnelChange();
}

Enemy *Stage::SpawnEnemy(e_EnemyType enemyType, e_Team team, const Vector2 &position, float zPosition, const Vector2 &momentum, float zMomentum) {

  Enemy *enemy;
  /*
  const Tile *tile = GetStadium()->GetTile(position);
  //float zPosition = 0;
  if (tile != nullptr) {
    zPosition = tile->zPosition;
  } else {
    printf("out of bounds!\n");
  }
  */

  switch (enemyType) {
    case e_EnemyType_Birdlike:
      enemy = new EnemyBirdlike(game, playState, team, position, zPosition);
      break;
    case e_EnemyType_Grenade:
      enemy = new EnemyGrenade(game, playState, team, position, zPosition, momentum, zMomentum);
      break;
    default:
      return nullptr;
  }
  entities.push_back(enemy);
  return enemy;
}

void Stage::AddEntityOwnership(StageEntity *entity) {
  entities.push_back(entity);
}

void Stage::DeleteCharacters() {//UnregisterAllHumanGamers() {
  for (unsigned int i = 0; i < characters.size(); i++) {
    delete characters.at(i);
  }
  characters.clear();
  _humanCharacters.clear();
  masterSlaveConnections.clear();
  sig_PersonnelChange();
}

void Stage::DeleteCharacter(Character *character) {

  std::vector<Character*>::iterator charIter = characters.begin();
  while (charIter != characters.end()) {
    if ((*charIter) == character) {
      delete (*charIter);
      characters.erase(charIter);
      break;
    }
    charIter++;
  }

  // remove master/slave relationship (oh la la) if it exists
  const StageEntity *slave = GetSlave(character);
  if (slave) {
    RemoveMasterSlaveConnection(character, slave);
  }

  // rebuild _humanCharacters cache
  std::vector<Character*>::iterator humanCharIter = _humanCharacters.begin();
  while (humanCharIter != _humanCharacters.end()) {
    if ((*humanCharIter) == character) {
      humanCharIter = _humanCharacters.erase(humanCharIter);
      break;
    }
    humanCharIter++;
  }

  sig_PersonnelChange();
}

/*
void Stage::DeleteBadGuys() {
  for (unsigned int i = 0; i < badGuys.size(); i++) {
    delete badGuys.at(i);
  }
  badGuys.clear();
}
*/

void Stage::DeleteEntities() {
  for (unsigned int i = 0; i < entities.size(); i++) {
    delete entities.at(i);
  }
  entities.clear();
  masterSlaveConnections.clear();
}

void Stage::CheckCollisions() {

  // now check stage entities for overlap, and reset their positions if needed (because some entities aren't allowed to overlap)

  std::vector<StageEntity*> stageEntities;
  GetAllStageEntities(stageEntities);

  // remove slaved entities from collision list
  std::vector<StageEntity*>::iterator entityIter = stageEntities.begin();
  while (entityIter != stageEntities.end()) {
    const Character *master = GetMaster(*entityIter);
    if (master) {
      entityIter = stageEntities.erase(entityIter);
    } else {
      entityIter++;
    }
  }

  // calculate all desired new positions first, then in the end, apply the netto (average) force (because there can be multiple collisions on one object)
  std::vector<Vector2> cachedPositions[stageEntities.size()];

  struct EntityCouple {
    EntityCouple(StageEntity *e1, StageEntity *e2) : e1(e1), e2(e2) {
    }
    StageEntity *e1;
    StageEntity *e2;
  };
  std::list<EntityCouple> collisionList;

  float radiusFactor = 1.0f;
  unsigned int stageEntitySize = stageEntities.size();
  for (unsigned int i = 0; i < stageEntitySize - 1; i++) {
    for (unsigned int j = i + 1; j < stageEntitySize; j++) {

      if (// entities can't hit each other
          (stageEntities[i]->GetType() == e_StageEntityType_Character || stageEntities[j]->GetType() == e_StageEntityType_Character) &&
          /*
          // enemies can't hit good guy (wut?! yes, this is testing. need to introduce teams here or something)
          ( !( ((stageEntities[i)->GetType() == e_StageEntityType_Character && static_cast<Character*>(stageEntities[i])->GetTeamID() == 0) && stageEntities[j)->GetType() == e_StageEntityType_Enemy) ||
               ((stageEntities[j)->GetType() == e_StageEntityType_Character && static_cast<Character*>(stageEntities[j])->GetTeamID() == 0) && stageEntities[i)->GetType() == e_StageEntityType_Enemy) ) ) &&
          */
          // touching on z axis?
          ( stageEntities[i]->GetZPosition() + stageEntities[i]->GetHeight() > stageEntities[j]->GetZPosition() &&
            stageEntities[j]->GetZPosition() + stageEntities[j]->GetHeight() > stageEntities[i]->GetZPosition() )) {

        //printf("%i - %i\n", i, j);
        //Vector2 position1, scale1;
        //stageEntities[i]->GetSpriteBounds(position1, scale1);
        Vector2 position1 = stageEntities[i]->GetPosition();
        Vector2 scale1 = Vector2(stageEntities[i]->GetRadius(), stageEntities[i]->GetRadius());
        //Vector2 position2, scale2;
        //stageEntities[j)->GetSpriteBounds(position2, scale2);
        Vector2 position2 = stageEntities[j]->GetPosition();
        Vector2 scale2 = Vector2(stageEntities[j]->GetRadius(), stageEntities[j]->GetRadius());

        float radius1 = fabs(scale1.coords[0]) * 0.5f + fabs(scale1.coords[1]) * 0.5f; // atm we can only collide round objects, not elliptic
        float radius2 = fabs(scale2.coords[0]) * 0.5f + fabs(scale2.coords[1]) * 0.5f; // atm we can only collide round objects, not elliptic
        radius1 *= radiusFactor;
        radius2 *= radiusFactor;

        float minimalDistance = radius1 + radius2;
        Vector2 p1_to_p2 = position2 - position1;
        if ((position2 - position1).GetLength() < minimalDistance) {

          float weightRatio = stageEntities[i]->GetWeight() / (stageEntities[i]->GetWeight() + stageEntities[j]->GetWeight());

          // force entities to stand on top of each other
          if (stageEntities[i]->GetType() == e_StageEntityType_Character && stageEntities[j]->GetType() == e_StageEntityType_Character) {
            if ( stageEntities[i]->GetZPosition() >= stageEntities[j]->GetZPosition() + stageEntities[j]->GetHeight() * 0.8f ) {
              if (stageEntities[i]->GetType() == e_StageEntityType_Character) {
                static_cast<Character*>(stageEntities[i])->SetQueuedZPosition(stageEntities[j]->GetZPosition() + stageEntities[j]->GetHeight());
                //static_cast<Character*>(stageEntities[i))->SetQueuedPosition(stageEntities[j)->GetPosition());
                static_cast<Character*>(stageEntities[i])->SetQueuedZMovement(stageEntities[i]->GetZMovement() * weightRatio + stageEntities[j]->GetZMovement() * (1.0f - weightRatio));
              } else {
                stageEntities[i]->SetZPosition(stageEntities[j]->GetZPosition() + stageEntities[j]->GetHeight());
                stageEntities[i]->SetZMovement(stageEntities[i]->GetZMovement() * weightRatio + stageEntities[j]->GetZMovement() * (1.0f - weightRatio));
              }
              continue;
            }
            else if ( stageEntities[j]->GetZPosition() >= stageEntities[i]->GetZPosition() + stageEntities[i]->GetHeight() * 0.8f ) {
              if (stageEntities[j]->GetType() == e_StageEntityType_Character) {
                static_cast<Character*>(stageEntities[j])->SetQueuedZPosition(stageEntities[i]->GetZPosition() + stageEntities[i]->GetHeight());
                //static_cast<Character*>(stageEntities[j))->SetQueuedPosition(stageEntities[i)->GetPosition());
                static_cast<Character*>(stageEntities[j])->SetQueuedZMovement(stageEntities[j]->GetZMovement() * (1.0f - weightRatio) + stageEntities[i]->GetZMovement() * weightRatio);
              } else {
                stageEntities[j]->SetZPosition(stageEntities[i]->GetZPosition() + stageEntities[i]->GetHeight());
                stageEntities[j]->SetZMovement(stageEntities[j]->GetZMovement() * weightRatio + stageEntities[i]->GetZMovement() * (1.0f - weightRatio));
              }
              continue;
            }
          }

          Vector2 center = (position1 + position2) * 0.5f;
          Vector2 spacedPosition1 = center - (p1_to_p2.GetNormalized(Vector2(0, 1)) * 0.5f * minimalDistance);
          Vector2 spacedPosition2 = center + (p1_to_p2.GetNormalized(Vector2(0, 1)) * 0.5f * minimalDistance);
          Vector2 newPosition1 = stageEntities[i]->GetPosition() * weightRatio          + spacedPosition1 * (1.0f - weightRatio);
          Vector2 newPosition2 = stageEntities[j]->GetPosition() * (1.0f - weightRatio) + spacedPosition2 * weightRatio;
          //Vector2 newPosition1 = spacedPosition1 * weightRatio + spacedPosition2 * (1.0f - weightRatio);
          //Vector2 newPosition2 = spacedPosition1 * (1.0f - weightRatio) + spacedPosition2 * weightRatio;

          bool alterMovement = false; // too beta yet
          float stageEntity1velocityCache = stageEntities[i]->GetVelocity(); // because it may be changed by SetMovement halfway

          cachedPositions[i].push_back(newPosition1);
          if (stageEntities[i]->GetType() == e_StageEntityType_Character) {
            //static_cast<Character*>(stageEntities[i))->SetQueuedPosition(newPosition1);
//          stageEntities[i)->SetQueuedMovement( (newPosition1 - position1) / ((float)playState->GetPhysicsTimeStep_ms() * 0.001f) * 2 );
            //stageEntities[i)->SetQueuedMovement( stageEntities[i)->GetMovement() * weightRatio + stageEntities[j)->GetMovement() * (1.0f - weightRatio) );
            if (alterMovement) static_cast<Character*>(stageEntities[i])->SetQueuedMovement( stageEntities[i]->GetMovement() * weightRatio + (-p1_to_p2.GetNormalized(0) * stageEntities[j]->GetVelocity()) * (1.0f - weightRatio) );
          } else {
            //stageEntities[i)->SetPosition( newPosition1 );
            //stageEntity1velocityCache = stageEntities[i)->GetVelocity();
            if (alterMovement) stageEntities[i]->SetMovement( stageEntities[i]->GetMovement() * weightRatio + (-p1_to_p2.GetNormalized(0) * stageEntities[j]->GetVelocity()) * (1.0f - weightRatio) );
          }

          cachedPositions[j].push_back(newPosition2);
          if (stageEntities[j]->GetType() == e_StageEntityType_Character) {
            //static_cast<Character*>(stageEntities[j))->SetQueuedPosition(newPosition2);
//          stageEntities[j)->SetQueuedMovement( (newPosition2 - position2) / ((float)playState->GetPhysicsTimeStep_ms() * 0.001f) * 2 );
          //stageEntities[j)->SetQueuedMovement( stageEntities[i)->GetMovement() * weightRatio + stageEntities[i)->GetMovement() * (1.0f - weightRatio) );
            if (alterMovement) static_cast<Character*>(stageEntities[j])->SetQueuedMovement( stageEntities[j]->GetMovement() * (1.0f - weightRatio) + (p1_to_p2.GetNormalized(0) * stageEntity1velocityCache) * weightRatio );
          } else {
            //stageEntities[j)->SetPosition( newPosition2 );
            if (alterMovement) stageEntities[j]->SetMovement( stageEntities[j]->GetMovement() * (1.0f - weightRatio) + (p1_to_p2.GetNormalized(0) * stageEntity1velocityCache) * weightRatio );
          }

          collisionList.push_back(EntityCouple(stageEntities[j], stageEntities[i]));
//          stageEntities[i]->OnCollide(stageEntities[j]);
//          stageEntities[j]->OnCollide(stageEntities[i]);

        }
      }
    }
  }

  for (unsigned int i = 0; i < stageEntities.size(); i++) {
    if (!cachedPositions[i].empty()) {

      Vector2 resultPos;
      for (unsigned int f = 0; f < cachedPositions[i].size(); f++) {
        resultPos += cachedPositions[i].at(f);
      }
      resultPos /= cachedPositions[i].size();

      // set position
      if (stageEntities[i]->GetType() == e_StageEntityType_Character) {
        static_cast<Character*>(stageEntities[i])->SetQueuedPosition(resultPos);
      } else {
        stageEntities[i]->SetPosition(resultPos);
      }
    }
  }

  std::list<EntityCouple>::iterator iter = collisionList.begin();
  while (iter != collisionList.end()) {
    (*iter).e1->OnCollide((*iter).e2);
    (*iter).e2->OnCollide((*iter).e1);
    iter++;
  }

}

Vector2 Stage::CollideNewPosition(const StageEntity *entity, const Vector2 &currentPosition, const Vector2 &desiredPosition, float desiredZPosition) const {

  // works best for entities smaller than 1x1m, because else the estimatedEntityCollisionPoint's sudden shift after stopping colliding with a tile may re-collide the same tile just moments after

  float traversableHeightDelta = 0.26f;//0.51f
  float entityRadius = entity->GetRadius();
  entityRadius -= 0.05f; // allow 'overhang'
  entityRadius = std::max(entityRadius, 0.1f); // no infinitely small objects

  // find non-traversable tile closest to entity 'front' (estimatedEntityCollisionPoint)
  Vector2 estimatedEntityCollisionPoint = desiredPosition + (desiredPosition - currentPosition).GetNormalized(0) * entityRadius;
  std::vector<Vector2> candidateTilePositions;
  candidateTilePositions.reserve(4);
  candidateTilePositions.push_back( Vector2(int(floor(estimatedEntityCollisionPoint.coords[0])), int(floor(estimatedEntityCollisionPoint.coords[1]))) );
  candidateTilePositions.push_back( Vector2(int(floor(estimatedEntityCollisionPoint.coords[0])), int( ceil(estimatedEntityCollisionPoint.coords[1]))) );
  candidateTilePositions.push_back( Vector2(int( ceil(estimatedEntityCollisionPoint.coords[0])), int(floor(estimatedEntityCollisionPoint.coords[1]))) );
  candidateTilePositions.push_back( Vector2(int( ceil(estimatedEntityCollisionPoint.coords[0])), int( ceil(estimatedEntityCollisionPoint.coords[1]))) );
  int closestIndex = -1;
  float closestDistance = 100000.0f;
  for (unsigned int c = 0; c < candidateTilePositions.size(); c++) {
    const Tile *tile = GetStadium()->GetTile(candidateTilePositions.at(c));
    if (GetStadium()->IsTraversable(candidateTilePositions.at(c)) == false || (tile != nullptr && tile->GetZPosition() > desiredZPosition + traversableHeightDelta)) {
      float distance = estimatedEntityCollisionPoint.GetDistance(candidateTilePositions.at(c));
      if (distance < closestDistance) {
        closestDistance = distance;
        closestIndex = (signed int)c;
      }
    }
  }

  if (closestIndex != -1) {
    // found a neighboring tile that is non-traversable. time for some collision checking fun

    // http://stackoverflow.com/questions/401847/circle-rectangle-collision-detection-intersection
    Vector2 tileCenterPos = candidateTilePositions.at(closestIndex);
    // point on edge of tile that's closest to sprite center
    Vector2 pointOnTile( Clamp(desiredPosition.coords[0], tileCenterPos.coords[0] - 0.5f, tileCenterPos.coords[0] + 0.5f),
                         Clamp(desiredPosition.coords[1], tileCenterPos.coords[1] - 0.5f, tileCenterPos.coords[1] + 0.5f) );

    // Calculate the distance between the circle's center and this closest point
    float distance = pointOnTile.GetDistance(desiredPosition);
    Vector2 resultingPosition = desiredPosition;
    if (distance < entityRadius) {
      resultingPosition = pointOnTile + (desiredPosition - pointOnTile).GetNormalized(0) * entityRadius;
    }

    return resultingPosition;

  } else {

    return desiredPosition;

  }

  /*
  if (GetStadium()->IsTraversable(currentPosition) == false || // if we are already on non-traversable terrain (somehow), accept any new position
      GetStadium()->IsTraversable(desiredPosition) == true) {
    return desiredPosition;
  }
  */
}


// utility functions

void Stage::GetAllStageEntities(std::vector<StageEntity*> &targetContainer) const {
  targetContainer.insert(targetContainer.end(), characters.begin(), characters.end());
  targetContainer.insert(targetContainer.end(), entities.begin(), entities.end());
}

Character *Stage::GetClosestCharacter(e_Team team, const Vector2 &focus, const Character *ignore) const {
  float closestDistance = 100000;
  int closestID = -1;
  for (unsigned int i = 0; i < characters.size(); i++) {
    if (characters.at(i)->GetTeam() == team && characters.at(i) != ignore) {
      float distance = (characters.at(i)->GetPosition() - focus).GetLength();
      if (distance < closestDistance) {
        closestDistance = distance;
        closestID = (signed int)i;
      }
    }
  }
  if (closestID == -1) return nullptr;
  else return characters.at(closestID);
}

std::vector<Character*> Stage::GetClosestCharacters(e_Team team, const Vector2 &focus, unsigned int num, const Character *ignore) const {

  /* moved to character.hpp
  struct CharacterProximity {
    Character *character;
    float distance;
    bool operator < (const CharacterProximity &comp) const {
      return distance < comp.distance;
    }
  };
  */

  std::vector<CharacterProximity> proximities;
  for (unsigned int i = 0; i < characters.size(); i++) {
    if (characters.at(i)->GetTeam() == team && characters.at(i) != ignore) {
      CharacterProximity prox;
      prox.character = characters.at(i);
      prox.distance = focus.GetDistance(characters.at(i)->GetPosition());
      proximities.push_back(prox);
    }
  }

  std::sort(proximities.begin(), proximities.end());

  std::vector<Character*> result;
  for (unsigned int i = 0; i < std::min(num, (unsigned int)proximities.size()); i++) {
    //printf("%f\n", proximities.at(i).distance);
    result.push_back(proximities.at(i).character);
  }
  //SDL_Delay(1000);

  return result;
}

std::vector<Character*> Stage::GetCharacters(e_Team team, const Character *ignore) const {
  std::vector<Character*> chars;
  for (unsigned int i = 0; i < characters.size(); i++) {
    if (characters.at(i)->GetTeam() == team && characters.at(i) != ignore) chars.push_back(characters.at(i));
  }
  return chars;
}

const std::vector<Character*> &Stage::GetAllCharactersRef() const {
  return characters;
}

const std::vector<Character*> &Stage::GetHumanCharactersRef() const {
  return _humanCharacters;
}

const std::vector<StageEntity*> &Stage::GetAllEntitiesRef() const {
  return entities;
}

Vector2 Stage::GetAverageHumanCharacterPosition(float movementFactor, float flowDirectionFactor, float flowMovementFactor, float &averageHumanZPos_ret) const {

  Vector2 averageHumanPos = Vector2(0, 0);
  averageHumanZPos_ret = 0;
  const std::vector<Character*> &focusCharacters = GetHumanCharactersRef();
  for (unsigned int i = 0; i < focusCharacters.size(); i++) {

    // flow off
    //if (_DISABLEFLOW) {
    //  averageHumanPos += focusCharacters.at(i)->GetModelView()->GetPosition();// + focusCharacters.at(i)->GetModelView()->GetMovement() * 0.1f;
    //} else {
      // sharp, techy
      //averageHumanPos += focusCharacters.at(i)->GetModelView()->GetPosition() + focusCharacters.at(i)->GetModelView()->GetMovement() * 0.0f + GetStadium()->GetFlowDirection() * 1.6f + GetStadium()->GetFlowMovement() * 1.6f;
      averageHumanPos += focusCharacters.at(i)->GetModelView()->GetPosition() + focusCharacters.at(i)->GetModelView()->GetMovement() * movementFactor + GetStadium()->GetFlowDirection() * flowDirectionFactor + GetStadium()->GetFlowMovement() * flowMovementFactor;// * 0.4f + focusPos * 0.6f;
    //}

    averageHumanZPos_ret += focusCharacters.at(i)->GetModelView()->GetZPosition();
  }

  if (focusCharacters.size() > 0) {
    averageHumanPos /= focusCharacters.size();
    averageHumanZPos_ret /= focusCharacters.size();
  }

  return averageHumanPos;
}

Vector2 Stage::GetCameraFocusPositionProj() const {

  // follow average human character pos

  float averageHumanZPos = 0;
  //Vector2 averageHumanPos = GetAverageHumanCharacterPosition(0.0f, 2.2f, 2.0f, averageHumanZPos);
  // good for flow, hero in upper left
  //Vector2 averageHumanPos = GetAverageHumanCharacterPosition(0.0f, 4.5f, 0.2f, averageHumanZPos);
  // more centered hero
  Vector2 averageHumanPos = GetAverageHumanCharacterPosition(0.0f, 0.5f * _CAMBASEDISTANCE, 0.0f, averageHumanZPos);

  // mix with close enemy's average pos
  float maxEnemyFocusDistance = 8.0f * (0.5f + 0.15f * _CAMBASEDISTANCE);
  Vector2 averageEnemyPos = Vector2(0);
  std::vector<Character*> oppChars = GetClosestCharacters(e_Team_BadGuys, averageHumanPos, 5, nullptr);
  float totalWeight = 0.0f;
  if (oppChars.size() > 0) {
    for (unsigned int i = 0; i < oppChars.size(); i++) {
      float oppDistance = oppChars.at(i)->GetPosition().GetDistance(averageHumanPos);
      if (oppDistance < maxEnemyFocusDistance) {
        float weight = pow(1.0f - (oppDistance / maxEnemyFocusDistance), 0.8f);
        averageEnemyPos += oppChars.at(i)->GetPosition() * weight;
        totalWeight += weight;
      }
    }
  }

  float enemyFocusBias = 0.6f;

  if (totalWeight > 0.001f) {
    averageEnemyPos /= totalWeight;
    enemyFocusBias *= Clamp(totalWeight, 0.0f, 1.0f);
  } else {
    averageEnemyPos = averageHumanPos; // no enemies :(
  }

  Vector2 focusPos = averageHumanPos * (1.0f - enemyFocusBias) + averageEnemyPos * enemyFocusBias;

  // follow flowline
  Vector2 flowlinePos = GetStadium()->GetFlowPosition() + GetStadium()->GetFlowMovement() * 1.0f;

/*
  // flowpos height
  float zPosition = 0;
  const Tile *tile = GetStadium()->GetTile(GetStadium()->GetFlowPosition());
  if (tile != nullptr) {
    zPosition = tile->GetZPosition();
  }
*/

  int flowAxis = GetStadium()->GetFlowAxis();
  if (_DISABLEFLOW) {
    flowlinePos.coords[flowAxis]         *= 0.0f;
    flowlinePos.coords[1 - flowAxis]     *= 0.0f;
    averageHumanPos.coords[flowAxis]     *= 1.0f;
    averageHumanPos.coords[1 - flowAxis] *= 1.0f;
  } else {
    flowlinePos.coords[flowAxis]         *= 0.7f;
    flowlinePos.coords[1 - flowAxis]     *= 0.5f;
    averageHumanPos.coords[flowAxis    ] *= 0.3f;
    averageHumanPos.coords[1 - flowAxis] *= 0.5f;
  }
  Vector2 combine = flowlinePos + focusPos;
  return combine - averageHumanZPos;

}

void Stage::AddMasterSlaveConnection(const Character *master, StageEntity *slave) {
  MasterSlaveConnection con;
  con.master = master;
  con.slave = slave;
  masterSlaveConnections.push_back(con);
}

void Stage::RemoveMasterSlaveConnection(const Character *master, const StageEntity *slave) {
  for (unsigned int i = 0; i < masterSlaveConnections.size(); i++) {
    if (masterSlaveConnections.at(i).master == master && masterSlaveConnections.at(i).slave == slave) {
      masterSlaveConnections.erase(masterSlaveConnections.begin() + i);
      break;
    }
  }
}

const Character *Stage::GetMaster(const StageEntity *slave) const {
  for (unsigned int i = 0; i < masterSlaveConnections.size(); i++) {
    if (masterSlaveConnections.at(i).slave == slave)
      return masterSlaveConnections.at(i).master;
  }
  return nullptr;
}

StageEntity *Stage::GetSlave(const Character *master) const {
  for (unsigned int i = 0; i < masterSlaveConnections.size(); i++) {
    if (masterSlaveConnections.at(i).master == master)
      return masterSlaveConnections.at(i).slave;
  }
  return nullptr;
}

void Stage::SetFlag(StageEntity *flag) {
  this->flag = flag;
}

StageEntity *Stage::GetFlag() const {
  return flag;
}

void Stage::GetAssignedOpponentCharacters(const Character *character, std::vector<Character*> &opponents_ret) const {
  for (unsigned int i = 0; i < tacticalCouples.size(); i++) {
    if (tacticalCouples.at(i).p1 == character) {
      opponents_ret.push_back(tacticalCouples.at(i).p2);
    }
    if (tacticalCouples.at(i).p2 == character) {
      opponents_ret.push_back(tacticalCouples.at(i).p1);
    }
  }
}

bool Stage::AreCharactersAssigned(const Character *p1, const Character *p2) const {
  for (unsigned int i = 0; i < tacticalCouples.size(); i++) {
    if ( (tacticalCouples.at(i).p1 == p1 && tacticalCouples.at(i).p2 == p2) ||
         (tacticalCouples.at(i).p1 == p2 && tacticalCouples.at(i).p2 == p1) ) return true;
  }
  return false;
}

void Stage::UpdateAssignmentMatrix() {

  //printf("x\n");

  // make couples between the fighters. if teams have different number of fighters, some fighters get multiple opponents assigned.
  std::vector<Character*> goodGuys = GetCharacters(e_Team_GoodGuys);
  std::vector<Character*> badGuys = GetCharacters(e_Team_BadGuys);
//  unsigned int smallestGroupSize = std::min(goodGuys.size(), badGuys.size());

  // tmp: keep only human chars on the good guys team
  for (int i = 0; i < (signed int)goodGuys.size(); i++) {
    if (goodGuys.at(i)->GetController_const()->GetControllerType() != e_ControllerType_Human) {
      goodGuys.erase(goodGuys.begin() + (unsigned int)i);
      i--;
    }
  }

  if (goodGuys.empty() || badGuys.empty()) {
    tacticalCouples.clear();
    return; // no party :(
  }

  unsigned int largestGroupSize = std::max(goodGuys.size(), badGuys.size());
  int idMatrix1[largestGroupSize];
  int idMatrix2[largestGroupSize];
  //dlib::matrix<int> costMatrix(goodGuys.size(), badGuys.size());
  dlib::matrix<int> costMatrix(largestGroupSize, largestGroupSize);
  //dlib::matrix<int>::iterator matIter = costMatrix.begin();
  //dlib::matrix<int[2]>::iterator idIter = idMatrix.begin();
  for (unsigned int x = 0; x < largestGroupSize; x++) {
    for (unsigned int y = 0; y < largestGroupSize; y++) {

      unsigned int i = x % (unsigned int)goodGuys.size();
      unsigned int j = y % (unsigned int)badGuys.size();
/*
      printf("x, y: %i, %i\n", x, y);
      printf("i: %i\n", i);
      printf("j: %i\n", j);
*/
      idMatrix1[x] = i;//goodGuys.at(i)->GetUniqueID();
      idMatrix2[y] = j;//badGuys.at(j)->GetUniqueID();

      int distanceValue = std::max(0, (int)(round( 10000.0f - 100.0f * (std::max(0.0f, (goodGuys.at(i)->GetPosition() - badGuys.at(j)->GetPosition()).GetLength())) )));
      //printf("%i\n", AreCharactersAssigned(goodGuys.at(i), badGuys.at(j)) == true);
      int previousAssignmentValue = 0; // use this to make opponents stick to the same person more (AreCharactersAssigned(goodGuys.at(i), badGuys.at(j)) == true) ? 100000 : 0;
      costMatrix(x, y) = distanceValue + previousAssignmentValue;
      //distanceMatrix(x, y) = pow( (goodGuys.at(i)->GetPosition() - badGuys.at(j)->GetPosition()).GetLength() * 10 , 1.5f);

    }
  }

  // clear previous assignments
  tacticalCouples.clear();

  // go hungarian on their ass!
  std::vector<long> assignmentMatrix = dlib::max_cost_assignment(costMatrix);
  for (unsigned int z = 0; z < assignmentMatrix.size(); z++) {
    //printf("%lu, ", assignment[z]);

    TacticalCouple couple;
    //couple.p1UniqueID = idMatrix1[ z ];
    couple.p1 = goodGuys.at(idMatrix1[ z ]);
    //couple.p2UniqueID = idMatrix2[ assignmentMatrix[z] ];
    couple.p2 = badGuys.at(idMatrix2[ assignmentMatrix[z] ]);
    tacticalCouples.push_back(couple);
    //printf("%i - %i\n", couple.p1, couple.p2);

  }
  //printf("\n");

}
