#include <Magnum/Math/Matrix3.h>

#include "arc_ball.h"

/* Project a point in NDC onto the arcball sphere */
Quaternion ndcToArcBall(const Vector2 &p) {
  const Float dist = Math::dot(p, p);

  /* Point is on sphere */
  if (dist <= 1.0f)
    return {{p.x(), p.y(), Math::sqrt(1.0f - dist)}, 0.0f};

  /* Point is outside sphere */
  else {
    const Vector2 proj = p.normalized();
    return {{proj.x(), proj.y(), 0.0f}, 0.0f};
  }
}

ArcBall::ArcBall(const Vector3 &eye, const Vector3 &viewCenter,
                 const Vector3 &upDir, Deg fov, const Vector2i &windowSize)
    : m_fov{fov}, m_windowSize{windowSize} {
  setViewParameters(eye, viewCenter, upDir);
}

void ArcBall::setViewParameters(const Vector3 &eye, const Vector3 &viewCenter,
                                const Vector3 &upDir) {
  const Vector3 dir = viewCenter - eye;
  Vector3 zAxis = dir.normalized();
  Vector3 xAxis = (Math::cross(zAxis, upDir.normalized())).normalized();
  Vector3 yAxis = (Math::cross(xAxis, zAxis)).normalized();
  xAxis = (Math::cross(zAxis, yAxis)).normalized();

  m_targetPosition = -viewCenter;
  m_targetZooming = -dir.length();
  m_targetQRotation =
      Quaternion::fromMatrix(Matrix3x3{xAxis, yAxis, -zAxis}.transposed())
          .normalized();

  m_positionT0 = m_currentPosition = m_targetPosition;
  m_zoomingT0 = m_currentZooming = m_targetZooming;
  m_qRotationT0 = m_currentQRotation = m_targetQRotation;

  updateInternalTransformations();
}

void ArcBall::reset() {
  m_targetPosition = m_positionT0;
  m_targetZooming = m_zoomingT0;
  m_targetQRotation = m_qRotationT0;
}

void ArcBall::setLagging(const Float lagging) {
  CORRADE_INTERNAL_ASSERT(lagging >= 0.0f && lagging < 1.0f);
  m_lagging = lagging;
}

void ArcBall::initTransformation(const Vector2i &mousePos) {
  m_prevMousePosNDC = screenCoordToNDC(mousePos);
}

void ArcBall::rotate(const Vector2i &mousePos) {
  const Vector2 mousePosNDC = screenCoordToNDC(mousePos);
  const Quaternion currentQRotation = ndcToArcBall(mousePosNDC);
  const Quaternion prevQRotation = ndcToArcBall(m_prevMousePosNDC);
  m_prevMousePosNDC = mousePosNDC;
  m_targetQRotation =
      (currentQRotation * prevQRotation * m_targetQRotation).normalized();
}

void ArcBall::translate(const Vector2i &mousePos) {
  const Vector2 mousePosNDC = screenCoordToNDC(mousePos);
  const Vector2 translationNDC = mousePosNDC - m_prevMousePosNDC;
  m_prevMousePosNDC = mousePosNDC;
  translateDelta(translationNDC);
}

void ArcBall::translateDelta(const Vector2 &translationNDC) {
  /* Half size of the screen viewport at the view center and perpendicular
     with the viewDir */
  const Float hh = Math::abs(m_targetZooming) * Math::tan(m_fov * 0.5f);
  const Float hw = hh * Vector2{m_windowSize}.aspectRatio();

  m_targetPosition += m_inverseView.transformVector(
      {translationNDC.x() * hw, translationNDC.y() * hh, 0.0f});
}

void ArcBall::zoom(const Float delta) { m_targetZooming += delta; }

bool ArcBall::updateTransformation() {
  const Vector3 diffViewCenter = m_targetPosition - m_currentPosition;
  const Quaternion diffRotation = m_targetQRotation - m_currentQRotation;
  const Float diffZooming = m_targetZooming - m_currentZooming;

  const Float dViewCenter = Math::dot(diffViewCenter, diffViewCenter);
  const Float dRotation = Math::dot(diffRotation, diffRotation);
  const Float dZooming = diffZooming * diffZooming;

  /* Nothing change */
  if (dViewCenter < 1.0e-10f && dRotation < 1.0e-10f && dZooming < 1.0e-10f) {
    return false;
  }

  /* Nearly done: just jump directly to the target */
  if (dViewCenter < 1.0e-6f && dRotation < 1.0e-6f && dZooming < 1.0e-6f) {
    m_currentPosition = m_targetPosition;
    m_currentQRotation = m_targetQRotation;
    m_currentZooming = m_targetZooming;

    /* Interpolate between the current transformation and the target
       transformation */
  } else {
    const Float t = 1 - m_lagging;
    m_currentPosition = Math::lerp(m_currentPosition, m_targetPosition, t);
    m_currentZooming = Math::lerp(m_currentZooming, m_targetZooming, t);
    m_currentQRotation =
        Math::slerpShortestPath(m_currentQRotation, m_targetQRotation, t);
  }

  updateInternalTransformations();
  return true;
}

void ArcBall::updateInternalTransformations() {
  m_view = DualQuaternion::translation(Vector3::zAxis(m_currentZooming)) *
          DualQuaternion{m_currentQRotation} *
          DualQuaternion::translation(m_currentPosition);
  m_inverseView = m_view.inverted();
}

Vector2 ArcBall::screenCoordToNDC(const Vector2i &mousePos) const {
  return {mousePos.x() * 2.0f / m_windowSize.x() - 1.0f,
          1.0f - 2.0f * mousePos.y() / m_windowSize.y()};
}