#include "Object.hpp"
#include "Logic.hpp"
#include <math/Plane.hpp>
#include <algorithm>
#include <iomanip>

std::vector<Object*> Object::objects;

Object::Object() {
  objects.push_back(this);
}

Object::~Object() {
  std::vector<Object*>::iterator i = std::find(
    objects.begin(),
    objects.end(),
    this
  );
  if (
    i != objects.end()
  ) {
    objects.erase(i);
  }
}

const bool Object::isFront(const Vector& position) const {
  static Plane p;
  return p.set(
    matrix._vector.direction,
    matrix._vector.position
  ).isFront(position);
}

const bool Object::isLeft(const Vector& position) const {
  static Plane p;
  return p.set(
    matrix._vector.side,
    matrix._vector.position
  ).isFront(position);
}

const double Object::getDistance(const Vector& position) const {
  return matrix._vector.position.getDistance(position);
}

const bool Object::isActive() const {
  return active;
}

Object& Object::setActive(const bool active) {
  speed.zero();
  this->active = active;
  return *this;
}

const Vector& Object::getPosition() const {
  return matrix._vector.position;
}

Object& Object::setPosition(const Vector& position) {
  matrix._vector.position = position;
  return *this;
}

Object& Object::setPosition(
  const double x,
  const double y,
  const double z
) {
  matrix._vector.position.set(
    x,
    y,
    z
  );
  return *this;
}

Object& Object::setPosition(
  const double x,
  const double z
) {
  return setPosition(
    x,
    getRy(),
    z
  );
}

const Vector& Object::getDirection() const {
  return matrix._vector.direction;
}

Object& Object::setDirection(const Vector& direction) {
  (matrix._vector.direction = direction).y = 0;
  (
    matrix._vector.side = matrix._vector.direction.normalize()
  ).rotateY(-Math::PIdiv2).normalize();
  matrix._vector.up = Vector::Y;
  return *this;
}

Object& Object::lookAt(const Vector& position) {
  static Vector v;
  return setDirection(
    (v = position) -= getPosition()
  );
}

const Vector& Object::getSpeed() const {
  return speed;
}

Object& Object::setSpeed(const Vector& v) {
  speed = v;
  return *this;
}

Object& Object::setSpeed(
  const double x,
  const double z
) {
  static Vector v;
  return setSpeed(
    v.set(
      x,
      speed.y,
      z
    )
  );
}

const double Object::getRy() const {
  return getRxz();
}

const bool Object::isStatic() const {
  return false;
}

const bool Object::isTransparent() const {
  return false;
}

const bool Object::isInAir() const {
  return inAir;
}

const unsigned long Object::getCollisions(
  std::vector<Object*>& collisions,
  const Vector& position
) const {
  for (
    std::vector<Object*>::iterator i = objects.begin();
    i < objects.end();
    i++
  ) {
    if (
      *i != this && (*i)->active && !(*i)->isTransparent()
    ) {
      static Vector v;
      if (
        std::abs(
          (
            (v = position) -= (*i)->getPosition()
          ).y
        ) < (*i)->getRy() + getRy()
      ) {
        v.y = 0;
        if (
          v.getLength() < (*i)->getRxz() + getRxz()
        ) {
          collisions.push_back(*i);
        }
      }
    }
  }
  return collisions.size();
}

const Vector* Object::getAcceleration() const {
  return &Logic::g;
}

Object& Object::simulatePosition(
  Vector& position,
  const Time dt
) {
  static Vector v;
  matrix._vector.position += (
    (v = speed) *= dt
  );
  if (
    const Vector* a = getAcceleration()
  ) {
    speed += (
      (v = *a) *= dt
    );
    matrix._vector.position += (
      (v *= dt) /= 2
    );
  }
  return *this;
}

Object& Object::simulate(const Time dt) {
  if (
    !isStatic() && active
  ) {
    static Vector old;
    old = matrix._vector.position;
    simulatePosition(
      matrix._vector.position,
      dt
    );
    inAir = true;
    std::vector<Object*> collisions;
    if (
      getCollisions(
        collisions,
        matrix._vector.position
      ) > 0
    ) {
      static Vector v;
      (v = speed).y = 0;
      for (
        std::vector<Object*>::iterator i = collisions.begin();
        active && i < collisions.end();
        i++
      ) {
        if (
          (*i)->active
        ) {
          const double d = (*i)->getRy() + getRy();
          const volatile double top = (*i)->getPosition().y + d;
          const volatile double bottom = (*i)->getPosition().y - d;
          if (speed.y < 0 && old.y >= top) {
            matrix._vector.position.y = top;
            speed.y = 0;
            inAir = false;
          } else if (speed.y > 0 && old.y <= bottom) {
            matrix._vector.position.y = bottom;
            speed.y = 0;
          } else {
            static Vector normal;
            (
              (
                normal = (*i)->getPosition()
              ) -= old
            ).y = 0;
            if (
              normal.getLength() > 0
            ) {
              static Plane plane;
              v = plane.set(
                normal.normalize()
              ).getProjection(v);
            }
          }
          onCollision(*i);
          (*i)->onCollision(this);
        }
      }
      matrix._vector.position.set(
        old.x,
        matrix._vector.position.y,
        old.z
      ) += (
        v *= dt
      );
    }
    if (active) {
      if (
        matrix._vector.position.y < getRy()
      ) {
        matrix._vector.position.y = (std::max)(
          matrix._vector.position.y,
          getRy()
        );
        if (speed.y < 0) {
          speed.y = 0;
        }
        inAir = false;
        onCollision();
      }
      if (active) {
        static Vector v;
        const double maxR = Graphics::get()->R - getRxz();
        (
          v = matrix._vector.position
        ).y = 0;
        if (
          v.getLength() > maxR
        ) {
          (
            v.normalize() *= maxR
          ).y = matrix._vector.position.y;
          matrix._vector.position = v;
          onCollision();
        }
      }
    }
  }
  return *this;
}

Object& Object::onCollision(Object* object) {
  return *this;
}

std::ostream& Object::operator >> (std::ostream& os) const {
  return os << std::setw(8) << getRxz() << " " << std::setw(8) << getRy();
}

std::ostream& operator << (
  std::ostream& os,
  const Object& o
) {
  return o >> os;
}

const double MeshObject::getRxz() const {
  if (rXZ == 0) {
    static Vector v;
    (
      v = getMesh().getBox().getSize()
    ).y = 0;
    rXZ = v.getLength() / 2;
  }
  return rXZ;
}

const double MeshObject::getRy() const {
  if (rY == 0) {
    rY = getMesh().getBox().getSize().y / 2;
  }
  return rY;
}

const bool MeshObject::useMatrix() const {
  return !isStatic();
}

const MeshObject& MeshObject::renderMesh() const {
  getMesh().render();
  return *this;
}

const Object& MeshObject::render() const {
  if (
    useMatrix()
  ) {
    Graphics::get()->apply(matrix);
  }
  return renderMesh();
}

std::ostream& MeshObject::operator >> (std::ostream& os) const {
  return Object::operator >> (
    os << getMesh() << std::endl << std::endl
  );
}
