#include "coopbuddy.hpp"

#include "gaia/gaia.hpp"
using namespace gaia;

//#include "defines.hpp"
#include "playstate.hpp"
#include "stage.hpp"
#include "entities/mixtape.hpp"
#include "spaces/space.hpp" // for tile
#include "tiles/tile.hpp"
#include "kungflowutils.hpp" // forcefields etc

#include "../character.hpp"
#include "../character_modelview.hpp"

CoopBuddyController::CoopBuddyController(const KungFlowGame *game, const PlayState *playState, Character *character) : CharacterAIController(game, playState, character) {

  CharacterThrowMove *throwMove = new CharacterThrowMove(playState, character->GetModelView()->GetCharacterSprite(), character->GetModelView()->GetStats().throwStats); // todo: probably should put those default stats elsewhere
  character->GetModelView()->AddMoveOwnership( e_MoveType_Throw, throwMove );
  throwConnection = throwMove->sig_Throw.connect(boost::bind(&CoopBuddyController::OnThrow, this, _1, _2));
}

CoopBuddyController::~CoopBuddyController() {
  throwConnection.disconnect();
}

void CoopBuddyController::OnStepPhysicsTime(CharacterModelView *characterModelView) {

  CharacterAIController::OnStepPhysicsTime(characterModelView);

  // some base vars

  bool dummyMode = false;
  //float throwFrequencyFactor = 0.5f;
  // base distances to opponent
  float distanceFactor = 1.0f;
  float futurePredict_sec = 0.6f;

  Vector2 position = characterModelView->GetPosition();

  if (!assignedOpponent) {
    characterModelView->SetDesiredVelocity(0);
    return;
  }
  //float historyFactor = 0.1f; // reaction time
  Vector2 assignedOpponentPosition = assignedOpponent->GetPosition();
  Vector2 toAssignedOpponent = assignedOpponentPosition - position;// - opponent->GetMovement() * historyFactor;
  characterModelView->SetDesiredBodyAngle(toAssignedOpponent.GetNormalized(_DEFAULTVECTOR).GetAngle());


  // complex position management using force fields

  std::vector<ForceSpot> forceField;

  // don't fall!
  float maxFallHeight = 5.0f;
  ForceSpot dontFallSpot;
  dontFallSpot.magnetType = e_MagnetType_Repel;
  dontFallSpot.decayType = e_DecayType_Variable;
  dontFallSpot.exp = 1.0f;
  dontFallSpot.power = 2.0f;
  dontFallSpot.scale = 3.0f;
  for (unsigned int i = 0; i < 4; i++) {
    Vector2 offset;
    if (i == 0) offset = Vector2(-1,  0);
    if (i == 1) offset = Vector2( 1,  0);
    if (i == 2) offset = Vector2( 0,  1);
    if (i == 3) offset = Vector2( 0, -1);
    const Tile *tile = playState->GetStage()->GetStadium()->GetTile(position + offset);
    if (tile && fabs(tile->GetZPosition() - character->GetZPosition()) > maxFallHeight) {
      dontFallSpot.origin = tile->GetPosition();
      forceField.push_back(dontFallSpot);
    }
  }


  // TEAM

  // constant attraction towards our human protagonist buddies (keep the human players the center of all action)
  float maxDistanceToHuman = 8.0f;
  const std::vector<Character*> &humans = playState->GetStage()->GetHumanCharactersRef();
  for (unsigned int i = 0; i < humans.size(); i++) {
    if (character->GetTeam() == humans.at(i)->GetTeam()) {

      // team mates are drawn to humans more
      ForceSpot spot;
      spot.origin = humans.at(i)->GetPosition();// + opponents.at(i)->GetPosition().GetNormalized(0) * 0.4f;
      spot.magnetType = e_MagnetType_Attract;
      spot.decayType = e_DecayType_Constant;
      spot.power = 0.4f / (float)humans.size();
      if (humans.at(i)->GetPosition().GetDistance(position) > maxDistanceToHuman) spot.power = 1.0f;
      forceField.push_back(spot);

      // ..but not too close
      spot.origin = humans.at(i)->GetPosition();// + opponents.at(i)->GetPosition().GetNormalized(0) * 0.4f;
      spot.magnetType = e_MagnetType_Repel;
      spot.decayType = e_DecayType_Variable;
      spot.exp = 0.6f;
      spot.power = 0.4f;
      spot.scale = 5.0f;//2.2f * _PHYSICSSCALE;
      forceField.push_back(spot);

    } else {
      /*
      // even all opponents are more drawn to humans specifically
      ForceSpot spot;
      spot.origin = humans.at(i)->GetPosition();// + opponents.at(i)->GetPosition().GetNormalized(0) * 0.4f;
      spot.magnetType = e_MagnetType_Attract;
      spot.decayType = e_DecayType_Constant;
      spot.power = 0.1f / (float)humans.size();
      forceField.push_back(spot);
      */
    }

  }


  // enemy mechanics

  //if (character->GetTeamID() == 1) playState->DrawDebugLine(character->GetPosition(), closestOpponent->GetPosition(), e_CoordinateSystem_World, 0.6f, 1.0f, 0.2f, 0.1f);

  for (unsigned int i = 0; i < assignedOpponents.size(); i++) {

    // repel when too close
    ForceSpot spot;
    spot.origin = assignedOpponents.at(i)->GetPosition();// + assignedOpponents.at(i)->GetMovement().GetNormalizedMax(4.0f) * 0.2f;
    spot.magnetType = e_MagnetType_Repel;
    spot.decayType = e_DecayType_Variable;
    //spot.exp = 1.2f;//0.5f;//0.7f;
    //spot.scale = 4.5f * distanceFactor;//6.75f * _PHYSICSSCALE;//3.8f * _PHYSICSSCALE;
    if (assignedOpponents.at(i) == assignedOpponent) { // if this is our main opponent, maybe get closer

      spot.power = 1.5f;
      spot.exp = 1.0f;
      spot.scale = 2.0f * distanceFactor * (0.4f + 0.6f * _ATTACKDISTANCEFACTOR);
      forceField.push_back(spot);

    } else {

      // other (unfocussed) enemies. ignore them, but stay out of their way somewhat.
      //if (closestOpponents.at(i) != closestOpponent) {
      spot.power = 0.5f;
      spot.exp = 1.0f;
      spot.scale = 2.0f * distanceFactor * (0.4f + 0.6f * _ATTACKDISTANCEFACTOR);
      forceField.push_back(spot);
      //if (character->GetTeamID() == 1) playState->DrawDebugLine(character->GetPosition(), spot.origin, e_CoordinateSystem_World, 1.0f, 0.5f, 1.0f, 0.1f);
    }

  }


  // teammate web

  for (unsigned int i = 0; i < closestMates.size(); i++) {

    if (closestMates.at(i)->GetController() != this) {

      // repel when too close
      ForceSpot spot;
      spot.origin = closestMates.at(i)->GetPosition();
      spot.magnetType = e_MagnetType_Repel;
      spot.decayType = e_DecayType_Variable;
      spot.power = 0.8f;
      spot.exp = 0.8f;
      spot.scale = 5.0f;//1.8f * _PHYSICSSCALE;//1.2f * _PHYSICSSCALE;
      forceField.push_back(spot);

    }

  }


  // always push to flowline

  Vector2 flowLineAdaptedCharPos = position;
  int flowAxis = playState->GetStage()->GetStadium()->GetFlowAxis();
  flowLineAdaptedCharPos.coords[flowAxis] = playState->GetStage()->GetStadium()->GetFlowPosition().coords[flowAxis] + 3; // and beyond :)
  ForceSpot flowSpot;
  flowSpot.origin = flowLineAdaptedCharPos;
  flowSpot.magnetType = e_MagnetType_Attract;
  flowSpot.decayType = e_DecayType_Constant;
  flowSpot.power = 0.3f;
  forceField.push_back(flowSpot);


  // always push to flag

  const StageEntity *flag = playState->GetStage()->GetFlag();
  if (flag && playState->GetStage()->GetMaster(flag) == nullptr) {
    Vector2 flagPos = flag->GetPosition();
    ForceSpot flagSpot;
    flagSpot.origin = flagPos;
    flagSpot.magnetType = e_MagnetType_Attract;
    flagSpot.decayType = e_DecayType_Constant;
    flagSpot.power = 0.15f;
    forceField.push_back(flagSpot);

    flagSpot.origin = flagPos;
    flagSpot.magnetType = e_MagnetType_Attract;
    flagSpot.decayType = e_DecayType_Variable;
    flagSpot.power = 2.0f;
    flagSpot.exp = 0.5f;
    flagSpot.scale = 5.0f;
    forceField.push_back(flagSpot);
  }


  // calculate result!

  Vector2 forceFieldMovement = GetForceFieldMovement(forceField, position);// + characterModelView->GetMovement() * 0.02f);


  // 8 way!

  forceFieldMovement = GetQuantizedDirection(forceFieldMovement, playState->GetQuantizationFactor());
  characterModelView->SetDesiredDirection(forceFieldMovement.GetNormalized(0));//.GetNormalizedMax(1.0f));

  float walkPow = 0.5f; // lower: more active / lively. higher: more apathic / lazy
  //if (nthClosestToAssignedOpponent < GetFightingCharacterCount()) walkPow *= 0.7f;//0.5f; // lower: more active / lively. higher: more apathic / lazy
  //else if (nthClosestToAssignedOpponent < GetFightingCharacterCount() + GetQueuedCharacterCount()) walkPow *= 0.9f;//0.7f; // lower: more active / lively. higher: more apathic / lazy
  //walkPow *= 0.5f + 0.5f * NormalizedClamp(toAssignedOpponent.GetLength(), 2.5f, 6.0f); // farther away characters get progressively more relaxed
  characterModelView->SetDesiredVelocity(pow(std::min(forceFieldMovement.GetLength(), 1.0f), walkPow) * playState->GetMediumVelocity());

  if (dummyMode) return;


  //Vector2 desiredThrowDirection = toAssignedOpponent.GetNormalized(0); //GetQuantizedDirection(toOpponent.GetNormalized(0), playState->GetQuantizationFactor());
  //playState->DrawDebugCoordCross(assignedOpponent->GetPosition() - assignedOpponent->GetZPosition(), e_CoordinateSystem_World, 1.0f);

  // throw mixtape

  float throwThresholdMinimumDistance = 1.5f * distanceFactor * (0.4f + 0.6f * _ATTACKDISTANCEFACTOR);
  float throwThresholdMaximumDistance = character->GetModelView_const()->GetStats().throwStats.maxDistance;//8.0f;//2.2f;

  if (passRequest.active) {
  //if (humans.empty() == false) {

    StageEntity *slave = playState->GetStage()->GetSlave(this->character);
    if (passRequest.subject == slave) {
//      if (slave->GetType() == e_StageEntityType_Generic) {
//        if (static_cast<GenericEntity*>(slave)->GetGenericEntityType() == e_GenericEntityType_Mixtape) {

      const Character *targetHuman = passRequest.requester; //humans.at(int(floor(Random(0, humans.size() - 0.0001f))));
      Vector2 toTargetHuman = targetHuman->GetPosition() - position; //targetHuman->GetPosition().GetDistance(position);

      // only if buddy is close enough
      if (toTargetHuman.GetLength() < throwThresholdMaximumDistance &&// && Random(0, 100) < 1.0f) {
          toTargetHuman.GetLength() > throwThresholdMinimumDistance) {// && Random(0, 100) < 1.0f) {

        //if (Random(0.0f, 1.0f / (float)game->GetPhysicsTimeStep_ms()) < .0001f * throwFrequencyFactor) {
        //if (Random(0.0f, 1.0f / (float)game->GetPhysicsTimeStep_ms()) < .001f * throwFrequencyFactor) {
        bool success = characterModelView->RequestThrow(targetHuman->GetPosition() + targetHuman->GetMovement().GetNormalizedMax(3.0f) * futurePredict_sec);//desiredThrowDirection, Clamp(toAssignedOpponent.GetLength() / throwThresholdDistance, 0.0f, 1.0f));
        //}
        if (success) ResetPassRequest();

      }

    } // has correct slave
  } // passrequest

}

void CoopBuddyController::OnThrow(const Vector2 &desiredMovement, const Vector2 &toTargetTranslation) {
  //Vector2 throwDirection = toTargetTranslation.GetNormalized(0);

  /* hard throw
  float throwPower = std::min(toTargetTranslation.GetLength() * 1.1f, character->GetModelView_const()->GetStats().throwStats.maxDistance);
  playState->GetStage()->SpawnEnemy(e_EnemyType_Grenade, character->GetPosition() + _DEFAULTVECTOR.GetRotated(character->GetModelView_const()->GetBodyAngle()) * 0.4f, character->GetZPosition() + character->GetHeight() * 1.0f, throwDirection * (3.0f + 0.5f * throwPower), 0.6f * throwPower);
  */

  //float throwPower = NormalizedClamp(toTargetTranslation.GetLength(), 0, character->GetModelView_const()->GetStats().throwStats.maxDistance);


  StageEntity *slave = playState->GetStage()->GetSlave(this->character);
  if (slave) {
    playState->GetStage()->RemoveMasterSlaveConnection(this->character, slave);

    Vector2 throwDirection = toTargetTranslation.GetNormalized(playState->GetStage()->GetStadium()->GetFlowDirection());
    float safeDistance = (character->GetRadius() + slave->GetRadius());
    float distanceBeforeTarget = 0.0f;//0.8f; // aim before player, so they can properly hit it

    //Vector2 startPosition = character->GetPosition() + _DEFAULTVECTOR.GetRotated(character->GetModelView_const()->GetBodyAngle()) * safeDistance;
    Vector2 startPosition = character->GetPosition() + throwDirection * safeDistance;
    float startZPosition = character->GetZPosition() + character->GetHeight() * 1.5f;
    Vector2 targetPosition = character->GetPosition() - character->GetDirection() * 0.35f + toTargetTranslation; // corrects for throw move translation
    targetPosition += -toTargetTranslation.GetNormalized(0) * distanceBeforeTarget;
    float targetZPosition = character->GetZPosition() + character->GetHeight() * 0.5f;
    //if (Random(0, 1) < 0.1f) targetZPosition += int(floor(Random(0.0f, 3.0f) * 0.7f)) * 1.2f; // height levels: ground, jump, double jump
    //float unroundedNumBeats = std::max(1.0f, startPosition.GetDistance(targetPosition) * 0.2f);
    //int numBeats = std::max(1, (int)(startPosition.GetDistance(targetPosition) * 0.2f) );
    //float remainder = unroundedNumBeats - numBeats;
    float numBeatsUnrounded = startPosition.GetDistance(targetPosition) * 0.4f;
    float numBeats = round(startPosition.GetDistance(targetPosition) * 0.4f);
    float remainder = numBeatsUnrounded - numBeats; // lower value for remainder == slower ball (than intended) == make arc higher to make up for that
    //printf("%f\n", remainder);
    float arcFactor = 0.25f - remainder * 0.45f;
    numBeats += 1.0f - playState->GetCurrentMusic()->GetBeatOffset(); // what's left of the current beat
    int duration_ms = std::max(1.5f, numBeats) * playState->GetCurrentMusic()->GetBeatDuration_ms();
    std::vector<Position3D> trajectory;
    CalculateTrajectory(startPosition, startZPosition, targetPosition, targetZPosition, arcFactor, duration_ms / playState->GetPhysicsTimeStep_ms(), trajectory);

    playState->DrawDebugCoordCross(targetPosition - targetZPosition, e_CoordinateSystem_World, 1.0f);

    //float throwPower = toTargetTranslation.GetLength();//10.0f;//14.0f;//12.0f;
    //float zMovement = 2.5f;

    slave->SetPosition(character->GetPosition() + throwDirection * safeDistance);
    if (slave->GetType() == e_StageEntityType_Generic && static_cast<GenericEntity*>(slave)->GetGenericEntityType() == e_GenericEntityType_Mixtape) {
      static_cast<Mixtape*>(slave)->SetTrajectory(trajectory);
    }

    //playState->SoundPlay(e_GlobalSound_SoundBlast, 0.4f);
  }

  // physics version
  /*
  if (Random(0, 1) < 0.3f) {
    // high ball
    //playState->GetStage()->SpawnEnemy(e_EnemyType_Grenade, character->GetPosition() + _DEFAULTVECTOR.GetRotated(character->GetModelView_const()->GetBodyAngle()) * 0.4f, character->GetZPosition() + character->GetHeight() * 1.0f, throwDirection * (2.0f + 2.0f * throwPower), 8.0f * throwPower);
    playState->GetStage()->SpawnEnemy(e_EnemyType_Grenade, character->GetPosition() + _DEFAULTVECTOR.GetRotated(character->GetModelView_const()->GetBodyAngle()) * 0.4f, character->GetZPosition() + character->GetHeight() * 1.0f, throwDirection * (2.0f + 2.0f * throwPower), 8.0f * throwPower);
  } else { // low hard pass
    //playState->GetStage()->SpawnEnemy(e_EnemyType_Grenade, character->GetPosition() + _DEFAULTVECTOR.GetRotated(character->GetModelView_const()->GetBodyAngle()) * 0.4f, character->GetZPosition() + character->GetHeight() * 1.0f, throwDirection * (3.0f + 3.0f * throwPower), 3.0f * throwPower);
    playState->GetStage()->SpawnEnemy(e_EnemyType_Grenade, character->GetPosition() + _DEFAULTVECTOR.GetRotated(character->GetModelView_const()->GetBodyAngle()) * 0.4f, character->GetZPosition() + character->GetHeight() * 1.0f, throwDirection * (5.0f + 5.0f * throwPower), 2.0f * throwPower);
  }
  */

}
