// Copyright 2016 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "chrome/browser/android/vr_shell/vr_math.h"

#include <array>
#include <cmath>

#include "third_party/gvr-android-sdk/src/ndk-beta/include/vr/gvr/capi/include/gvr_types.h"

namespace vr_shell {

// Internal matrix layout:
//
//   m[0][0], m[0][1], m[0][2], m[0][3],
//   m[1][0], m[1][1], m[1][2], m[1][3],
//   m[2][0], m[2][1], m[2][2], m[2][3],
//   m[3][0], m[3][1], m[3][2], m[3][3],
//
// The translation component is in the right column m[i][3].
//
// The bottom row m[3][i] is (0, 0, 0, 1) for non-perspective transforms.
//
// These matrices are intended to be used to premultiply column vectors
// for transforms, so successive transforms need to be left-multiplied.

void SetIdentityM(gvr::Mat4f& mat) {
  float* m = (float*)mat.m;
  for (int i = 0; i < 16; i++) {
    m[i] = 0;
  }
  for (int i = 0; i < 16; i += 5) {
    m[i] = 1.0f;
  }
}

// Left multiply a translation matrix.
void TranslateM(gvr::Mat4f& tmat, gvr::Mat4f& mat, float x, float y, float z) {
  if (&tmat != &mat) {
    for (int i = 0; i < 4; ++i) {
      for (int j = 0; j < 4; ++j) {
        tmat.m[i][j] = mat.m[i][j];
      }
    }
  }
  tmat.m[0][3] += x;
  tmat.m[1][3] += y;
  tmat.m[2][3] += z;
}

// Right multiply a translation matrix.
void TranslateMRight(gvr::Mat4f& tmat,
                     gvr::Mat4f& mat,
                     float x,
                     float y,
                     float z) {
  if (&tmat != &mat) {
    for (int i = 0; i < 4; ++i) {
      for (int j = 0; j < 3; ++j) {
        tmat.m[i][j] = mat.m[i][j];
      }
    }
  }

  for (int i = 0; i < 4; i++) {
    tmat.m[i][3] =
        mat.m[i][0] * x + mat.m[i][1] * y + mat.m[i][2] * z + mat.m[i][3];
  }
}

// Left multiply a scale matrix.
void ScaleM(gvr::Mat4f& tmat, const gvr::Mat4f& mat,
            float x, float y, float z) {
  if (&tmat != &mat) {
    for (int i = 0; i < 4; ++i) {
      for (int j = 0; j < 3; ++j) {
        tmat.m[i][j] = mat.m[i][j];
      }
    }
  }
  // Multiply all rows including translation components.
  for (int j = 0; j < 4; ++j) {
    tmat.m[0][j] *= x;
    tmat.m[1][j] *= y;
    tmat.m[2][j] *= z;
  }
}

// Right multiply a scale matrix.
void ScaleMRight(gvr::Mat4f& tmat, const gvr::Mat4f& mat,
                 float x, float y, float z) {
  if (&tmat != &mat) {
    for (int i = 0; i < 4; ++i) {
      for (int j = 0; j < 3; ++j) {
        tmat.m[i][j] = mat.m[i][j];
      }
    }
  }
  // Multiply columns, don't change translation components.
  for (int i = 0; i < 3; ++i) {
    tmat.m[i][0] *= x;
    tmat.m[i][1] *= y;
    tmat.m[i][2] *= z;
  }
}

gvr::Mat4f MatrixTranspose(const gvr::Mat4f& mat) {
  gvr::Mat4f result;
  for (int i = 0; i < 4; ++i) {
    for (int k = 0; k < 4; ++k) {
      result.m[i][k] = mat.m[k][i];
    }
  }
  return result;
}

std::array<float, 4> MatrixVectorMul(const gvr::Mat4f& matrix,
                                     const std::array<float, 4>& vec) {
  std::array<float, 4> result;
  for (int i = 0; i < 4; ++i) {
    result[i] = 0;
    for (int k = 0; k < 4; ++k) {
      result[i] += matrix.m[i][k] * vec[k];
    }
  }
  return result;
}

std::array<float, 3> MatrixVectorMul(const gvr::Mat4f& matrix,
                                     const std::array<float, 3>& vec) {
  // Use homogeneous coordinates for the multiplication.
  std::array<float, 4> vec_h = {{vec[0], vec[1], vec[2], 1.0f}};
  std::array<float, 4> result;
  for (int i = 0; i < 4; ++i) {
    result[i] = 0;
    for (int k = 0; k < 4; ++k) {
      result[i] += matrix.m[i][k] * vec_h[k];
    }
  }
  // Convert back from homogeneous coordinates.
  float rw = 1.0f / result[3];
  return {{rw * result[0], rw * result[1], rw * result[2]}};
}

gvr::Vec3f MatrixVectorMul(const gvr::Mat4f& m, const gvr::Vec3f& v) {
  gvr::Vec3f res;
  res.x = m.m[0][0] * v.x + m.m[0][1] * v.y + m.m[0][2] * v.z + m.m[0][3];
  res.y = m.m[1][0] * v.x + m.m[1][1] * v.y + m.m[1][2] * v.z + m.m[1][3];
  res.z = m.m[2][0] * v.x + m.m[2][1] * v.y + m.m[2][2] * v.z + m.m[2][3];
  return res;
}

// Rotation only, ignore translation components.
gvr::Vec3f MatrixVectorRotate(const gvr::Mat4f& m, const gvr::Vec3f& v) {
  gvr::Vec3f res;
  res.x = m.m[0][0] * v.x + m.m[0][1] * v.y + m.m[0][2] * v.z;
  res.y = m.m[1][0] * v.x + m.m[1][1] * v.y + m.m[1][2] * v.z;
  res.z = m.m[2][0] * v.x + m.m[2][1] * v.y + m.m[2][2] * v.z;
  return res;
}

gvr::Mat4f MatrixMul(const gvr::Mat4f& matrix1, const gvr::Mat4f& matrix2) {
  gvr::Mat4f result;
  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 4; ++j) {
      result.m[i][j] = 0.0f;
      for (int k = 0; k < 4; ++k) {
        result.m[i][j] += matrix1.m[i][k] * matrix2.m[k][j];
      }
    }
  }
  return result;
}

gvr::Mat4f PerspectiveMatrixFromView(const gvr::Rectf& fov,
                                     float z_near,
                                     float z_far) {
  gvr::Mat4f result;
  const float x_left = -std::tan(fov.left * M_PI / 180.0f) * z_near;
  const float x_right = std::tan(fov.right * M_PI / 180.0f) * z_near;
  const float y_bottom = -std::tan(fov.bottom * M_PI / 180.0f) * z_near;
  const float y_top = std::tan(fov.top * M_PI / 180.0f) * z_near;

  assert(x_left < x_right && y_bottom < y_top && z_near < z_far &&
         z_near > 0.0f && z_far > 0.0f);
  const float X = (2 * z_near) / (x_right - x_left);
  const float Y = (2 * z_near) / (y_top - y_bottom);
  const float A = (x_right + x_left) / (x_right - x_left);
  const float B = (y_top + y_bottom) / (y_top - y_bottom);
  const float C = (z_near + z_far) / (z_near - z_far);
  const float D = (2 * z_near * z_far) / (z_near - z_far);

  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 4; ++j) {
      result.m[i][j] = 0.0f;
    }
  }
  result.m[0][0] = X;
  result.m[0][2] = A;
  result.m[1][1] = Y;
  result.m[1][2] = B;
  result.m[2][2] = C;
  result.m[2][3] = D;
  result.m[3][2] = -1;

  return result;
}

gvr::Vec3f getForwardVector(const gvr::Mat4f& matrix) {
  gvr::Vec3f forward;
  float* fp = &forward.x;
  // Same as multiplying the inverse of the rotation component of the matrix by
  // (0, 0, -1, 0).
  for (int i = 0; i < 3; ++i) {
    fp[i] = -matrix.m[2][i];
  }
  return forward;
}

/**
 * Provides the relative translation of the head as a 3x1 vector.
 *
 */
gvr::Vec3f getTranslation(const gvr::Mat4f& matrix) {
  gvr::Vec3f translation;
  float* tp = &translation.x;
  // Same as multiplying the matrix by (0, 0, 0, 1).
  for (int i = 0; i < 3; ++i) {
    tp[i] = matrix.m[i][3];
  }
  return translation;
}

float VectorLength(const gvr::Vec3f& vec) {
  return sqrt(vec.x * vec.x + vec.y * vec.y + vec.z * vec.z);
}

void NormalizeVector(gvr::Vec3f& vec) {
  float len = VectorLength(vec);
  vec.x /= len;
  vec.y /= len;
  vec.z /= len;
}

float VectorDot(const gvr::Vec3f& a, const gvr::Vec3f& b) {
  return a.x * b.x + a.y * b.y + a.z * b.z;
}

void NormalizeQuat(gvr::Quatf& quat) {
  float len = sqrt(quat.qx * quat.qx + quat.qy * quat.qy + quat.qz * quat.qz +
                   quat.qw * quat.qw);
  quat.qx /= len;
  quat.qy /= len;
  quat.qz /= len;
  quat.qw /= len;
}

gvr::Quatf QuatFromAxisAngle(float x, float y, float z, float angle) {
  gvr::Quatf res;
  float s = sin(angle / 2);
  res.qx = x * s;
  res.qy = y * s;
  res.qz = z * s;
  res.qw = cos(angle / 2);
  return res;
}

gvr::Quatf QuatMultiply(const gvr::Quatf& a, const gvr::Quatf& b) {
  gvr::Quatf res;
  res.qw = a.qw * b.qw - a.qx * b.qx - a.qy * b.qy - a.qz * b.qz;
  res.qx = a.qw * b.qx + a.qx * b.qw + a.qy * b.qz - a.qz * b.qy;
  res.qy = a.qw * b.qy - a.qx * b.qz + a.qy * b.qw + a.qz * b.qx;
  res.qz = a.qw * b.qz + a.qx * b.qy - a.qy * b.qx + a.qz * b.qw;
  return res;
}

gvr::Mat4f QuatToMatrix(const gvr::Quatf& quat) {
  const float x2 = quat.qx * quat.qx;
  const float y2 = quat.qy * quat.qy;
  const float z2 = quat.qz * quat.qz;
  const float xy = quat.qx * quat.qy;
  const float xz = quat.qx * quat.qz;
  const float xw = quat.qx * quat.qw;
  const float yz = quat.qy * quat.qz;
  const float yw = quat.qy * quat.qw;
  const float zw = quat.qz * quat.qw;

  const float m11 = 1.0f - 2.0f * y2 - 2.0f * z2;
  const float m12 = 2.0f * (xy - zw);
  const float m13 = 2.0f * (xz + yw);
  const float m21 = 2.0f * (xy + zw);
  const float m22 = 1.0f - 2.0f * x2 - 2.0f * z2;
  const float m23 = 2.0f * (yz - xw);
  const float m31 = 2.0f * (xz - yw);
  const float m32 = 2.0f * (yz + xw);
  const float m33 = 1.0f - 2.0f * x2 - 2.0f * y2;

  float ret[16] = {m11, m12, m13, 0.0f, m21,  m22,  m23,  0.0f,
                   m31, m32, m33, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f};

  return *((gvr::Mat4f*)&ret);
}

gvr::Vec3f GetRayPoint(const gvr::Vec3f& rayOrigin,
                       const gvr::Vec3f& rayVector,
                       float scale) {
  gvr::Vec3f v;
  v.x = rayOrigin.x + scale * rayVector.x;
  v.y = rayOrigin.y + scale * rayVector.y;
  v.z = rayOrigin.z + scale * rayVector.z;
  return v;
}

}  // namespace vr_shell