#include "common/entities/Entity.h"
#include "utils/Utils.h"

static constexpr float GRAVITY = -0.08f;

Entity::Entity()
    : lastLengthAngle(0.0f), lengthAngle(0.0f), lastWidthAngle(0.0f),
      widthAngle(0.0f), size(Vector3(0.5f, 1.8f, 0.5f)),
      drag(0.686f, 0.98f, 0.686f), speed(0.1f), jumpPower(0.42f),
      onGround(false), skip(false) {
}

void Entity::tick() {
    lastPosition = position;
    lastLengthAngle = lengthAngle;
    lastWidthAngle = widthAngle;

    velocity += acceleration;
    velocity[0] *= drag[0];
    velocity[1] *= drag[1];
    velocity[2] *= drag[2];
    acceleration = Vector3(0.0f, GRAVITY, 0.0f);
}

void Entity::addForce(const Vector3& force) {
    acceleration += force;
}

const Vector3& Entity::getVelocity() const {
    return velocity;
}

void Entity::move(const Vector3& v) {
    position += v;
    onGround = v[1] == 0.0f && velocity[1] < 0.0f;
    velocity = v;
}

CollisionBox Entity::getCollisionBox() const {
    return CollisionBox(size).offset(position);
}

bool Entity::isOnGround() const {
    return onGround;
}

void Entity::setPosition(const Vector3& pos) {
    setPosition(pos, lengthAngle, widthAngle);
}

void Entity::setPosition(const Vector3& pos, float lengthAngle,
                         float widthAngle) {
    Entity::lastPosition = pos;
    Entity::position = pos;
    Entity::lastLengthAngle = lengthAngle;
    Entity::lengthAngle = lengthAngle;
    Entity::lastWidthAngle = widthAngle;
    Entity::widthAngle = widthAngle;
    Entity::velocity = Vector3();
    Entity::acceleration = Vector3();
}

Vector3 Entity::getRenderPosition(float lag) const {
    return Utils::interpolate(lastPosition, position, lag) +
           Vector3(size[0] * 0.5f, size[1], size[2] * 0.5f);
}

Quaternion Entity::getRotation() const {
    return getRenderRotation(1.0f);
}

Quaternion Entity::getRenderRotation(float lag) const {
    float lAngle = Utils::interpolate(lastLengthAngle, lengthAngle, lag);
    float wAngle = Utils::interpolate(lastWidthAngle, widthAngle, lag);
    Quaternion q(Vector3(0.0f, 1.0f, 0.0f), lAngle);
    q *= Quaternion(Vector3(1.0f, 0.0f, 0.0f), wAngle);
    return q;
}

void Entity::addLengthAngle(float angle) {
    lengthAngle += angle;
    constexpr float full = 360.0f;
    while(lengthAngle < 0.0f) {
        lengthAngle += full;
        lastLengthAngle += full;
    }
    while(lengthAngle >= full) {
        lengthAngle -= full;
        lastLengthAngle -= full;
    }
}

void Entity::addWidthAngle(float angle) {
    constexpr float border = 89.9f;
    widthAngle += angle;
    widthAngle = std::min(std::max(widthAngle + angle, -border), border);
}

float Entity::getMaxXZVelocity() const {
    float f = std::min(drag[0], drag[2]);
    return f * speed / (1 - f);
}

void Entity::jump() {
    if(onGround) {
        acceleration[1] += (jumpPower / drag[1] - GRAVITY);
    }
}