#include "kungflowutils.hpp"

#include "stadium.hpp" // for tile
#include "spaces/space.hpp" // for tile
#include "tiles/tile.hpp"

#include "defines.hpp" // air density

#include <fmod_errors.h>

void FMOD_CHECK(FMOD_RESULT result) {
  if (result != FMOD_OK) {
    printf("FMOD error! (%d) %s\n", result, FMOD_ErrorString(result));
    exit(-1);
  }
}


Vector2 CalculatePhysicsMovement(const Vector2 &currentMovement, const Vector2 &desiredMovement, float maxChangeMPS, unsigned int elapsedTime_ms) {
  Vector2 desiredDelta = desiredMovement - currentMovement;
  if (desiredDelta.GetLength() > 0.001f) {
    desiredDelta.NormalizeMax(maxChangeMPS * elapsedTime_ms * 0.001f);
  } else desiredDelta = 0.0f;
  return currentMovement + desiredDelta;
}

void ApplyGravity(float &zMovement, float gravity, unsigned int timeStep_ms) {
  float gravityMovement = gravity * timeStep_ms * 0.001f;
  //physicsState.movement += gravity;
  zMovement -= gravityMovement;
}

// return true if hit floor, otherwise false
bool ApplyFloor(const Vector2 &position, float &zPosition, float &zMovement, const Stadium *stadium, float bounceFactor, bool *outOfBounds_ret) {
  const Tile *tile = stadium->GetTile(position);// int(round(physicsState.position.coords[0])
  if (tile) {
    if (outOfBounds_ret) *outOfBounds_ret = false;
    if (tile->GetZPosition() > zPosition) {// && tile->zPosition < physicsState.zPosition + 0.5f) {
      zPosition = tile->GetZPosition();// + 0.001f;//0.00001f;
      if (zMovement < 0.0f) zMovement = -zMovement * bounceFactor;
      return true;
    }
  } else {
    //printf("tile out of bounds in ApplyFloor()");
    //if (outOfBounds_ret) *outOfBounds_ret = true;
    //zPosition = 0;
    //zMovement = 0;
  }
  return false;
}

void ApplyFriction(Vector2 &movement, float crossSectionalArea, unsigned int timeStep_ms) {

  // air resistance
  // http://hyperphysics.phy-astr.gsu.edu/hbase/airfri.html
  float dragForce = 0.5 * _ATMOSPHEREFRICTIONCOEFFICIENT * crossSectionalArea * (movement.GetLength() * movement.GetLength());
  movement -= (movement.GetNormalized(0) * dragForce * (timeStep_ms * 0.001f)).GetNormalizedMax(movement.GetLength()); // normalize so we don't get a pingpong around 0 velo;

}

void CalculateTrajectory(const Vector2 &startPosition, float startZPosition, const Vector2 &targetPosition, float targetZPosition, float arcFactor, unsigned int iterations, std::vector<Position3D> &trajectory) {

  float distance = startPosition.GetDistance(targetPosition);

  for (unsigned int i = 0; i < iterations; i++) {

    //float timeFactor = (i + 1) / ((float)iterations + 1.0f); // skip first frame ( == start point )
    float timeFactor = i / (float)iterations; // skip first frame ( == start point )
    timeFactor = pow(timeFactor, 0.9f); // simulate air resistance (faster at the start);

    float arcHeight = sin(timeFactor * pi) * distance * arcFactor;

    Position3D position3D;
    position3D.position = startPosition * (1.0f - timeFactor) + targetPosition * timeFactor;
    position3D.zPosition = startZPosition * (1.0f - timeFactor) + targetZPosition * timeFactor + arcHeight;

    trajectory.push_back(position3D);

  }

}

Vector2 GetQuantizedDirection(const Vector2 &inputDirection, float bias) {

  // digitize input

  //inputDirection.Normalize(0);
  Vector2 inputDirectionNorm = inputDirection.GetNormalized(0);

  int directions = 8;//GetConfiguration()->GetInt("gameplay_quantizeddirectioncount", 8);

  radian angle = inputDirectionNorm.GetAngle();
  angle /= pi * 2.0f;
  angle = round(angle * directions);
  angle /= directions;
  angle *= pi * 2.0f;

  return (inputDirectionNorm * (1.0 - bias) + (Vector2(1, 0).GetRotated(angle) * bias)).GetNormalized(inputDirectionNorm) * inputDirection.GetLength();
}

/* todo
bool CheckRectangleCollision(const Vector2 &position1, const Vector2 &scale1, const Vector2 &position2, const Vector2 &scale2) {
}
*/

bool CheckCircleCollision(const Vector2 &position1, float radius1, const Vector2 &position2, float radius2) {
  //printf("%f, %f, %f\n", (position1 - position2).GetLength(), radius1, radius2);
  if ((position1 - position2).GetLength() < radius1 + radius2) return true; else return false;
}

radian GetCappedAngle(radian oldAngle, radian newAngle, unsigned int time_ms, radian maxRadiansPerSecond) {
  radian angleToNew = Vector2(1, 0).GetRotated(newAngle).GetAngle(Vector2(1, 0).GetRotated(oldAngle));
  radian maxAngle = time_ms * 0.001f * maxRadiansPerSecond;
  angleToNew = Clamp(angleToNew, -maxAngle, maxAngle);
  radian resultingAngle = Vector2(1, 0).GetRotated(oldAngle + angleToNew).GetAngle();
  return resultingAngle;
}


Vector2 GetForceFieldMovement(const std::vector<ForceSpot> &forceField, const Vector2 &currentPos) {

  // attractorDampingDistance: from this distance to attractor, dampen influence so we won't overshoot target

  Vector2 cumulVec;
  float cumulForce = 0.0f;

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

    const ForceSpot &forceSpot = forceField.at(i);

    float distance, intensity;

    //Vector2 toSpot = (forceSpot.origin - currentPos).GetNormalized(0);//Max(sprintVelocity);
    distance = (forceSpot.origin - currentPos).GetLength();
    if (forceSpot.decayType == e_DecayType_Constant) {
      intensity = 1.0f;
    } else {
      intensity = Clamp(1.0f - distance / forceSpot.scale, 0.0f, 1.0f);
      if (forceSpot.exp != 1.0f) intensity = pow(intensity, forceSpot.exp);
    }
    if (intensity > 0.0f) {
      Vector2 relativeOrigin = forceSpot.origin - currentPos;
      relativeOrigin.Normalize(0);
      if (forceSpot.magnetType == e_MagnetType_Repel) {
        relativeOrigin = -relativeOrigin;//currentPos - forceSpot.origin;
      } else {
        // attractors need damping (buggy, so disabled)
        //if (distance < attractorDampingDistance) relativeOrigin *= distance / attractorDampingDistance;// pow(distance / attractorDampingDistance, 1.0f);// curve(distance / attractorDampingDistance, 1.0f);
      }

      float force = forceSpot.power * intensity;

      cumulVec += relativeOrigin * force;
      cumulForce += force;
    }

  }

  // if (cumulForce == 0.0f) return currentPos;
  // else return currentPos + cumulVec / cumulForce;

  if (cumulForce == 0.0f) return 0; else return (cumulVec / cumulForce); // * max velocity?
}
