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

Player::Player()
    : lastLengthAngle(0.0f), lengthAngle(0.0f), lastWidthAngle(0.0f),
      widthAngle(0.0f), size(Vector3(0.5f, 1.8f, 0.5f)) {
}

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

    constexpr float drag = 0.48f;
    acceleration += Vector3(-velocity[0] * drag, 0.0f, -velocity[2] * drag);
    velocity += acceleration;

    acceleration = Vector3(0.0f, -0.08f, 0.0f);
}

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

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

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

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

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

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

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

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

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

Quaternion Player::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 Player::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 Player::addWidthAngle(float angle) {
    constexpr float border = 89.9f;
    widthAngle += angle;
    widthAngle = std::min(std::max(widthAngle + angle, -border), border);
}