// Copyright 2009-2021 Intel Corporation
// SPDX-License-Identifier: Apache-2.0

#pragma once

#include "vec.isph"
#include "quaternion.isph"

struct LinearSpace3f 
{
  Vec3f vx;
  Vec3f vy;
  Vec3f vz;
};

struct LinearSpace3fa
{
  Vec3fa vx;
  Vec3fa vy;
  Vec3fa vz;
};

////////////////////////////////////////////////////////////////////////////////
/// Constructors
////////////////////////////////////////////////////////////////////////////////

inline uniform LinearSpace3f make_LinearSpace3f(const uniform Vec3f &x, const uniform Vec3f &y, const uniform Vec3f &z) { 
  uniform LinearSpace3f l; l.vx = x; l.vy = y; l.vz = z; return l; 
}

inline varying LinearSpace3f make_LinearSpace3f(const varying Vec3f &x, const varying Vec3f &y, const varying Vec3f &z) { 
  varying LinearSpace3f l; l.vx = x; l.vy = y; l.vz = z; return l; 
}

inline uniform LinearSpace3f make_LinearSpace3f(const uniform LinearSpace3fa &s) { 
  uniform LinearSpace3f l; l.vx = make_Vec3f(s.vx); l.vy = make_Vec3f(s.vy); l.vz = make_Vec3f(s.vz); return l; 
}

inline varying LinearSpace3f make_LinearSpace3f(const varying LinearSpace3fa &s) { 
  varying LinearSpace3f l; l.vx = make_Vec3f(s.vx); l.vy = make_Vec3f(s.vy); l.vz = make_Vec3f(s.vz); return l; 
}

/*! construction from quaternion */
inline uniform LinearSpace3f make_LinearSpace3f(const uniform Quaternion3f& q)
{
  uniform LinearSpace3f l;
  l.vx = make_Vec3f(q.r*q.r + q.i*q.i - q.j*q.j - q.k*q.k, 2.0f*(q.i*q.j + q.r*q.k), 2.0f*(q.i*q.k - q.r*q.j));
  l.vy = make_Vec3f(2.0f*(q.i*q.j - q.r*q.k), (q.r*q.r - q.i*q.i + q.j*q.j - q.k*q.k), 2.0f*(q.j*q.k + q.r*q.i));
  l.vz = make_Vec3f(2.0f*(q.i*q.k + q.r*q.j), 2.0f*(q.j*q.k - q.r*q.i), q.r*q.r - q.i*q.i - q.j*q.j + q.k*q.k);
  return l;
}

inline varying LinearSpace3f make_LinearSpace3f(const varying Quaternion3f& q)
{
  LinearSpace3f l;
  l.vx = make_Vec3f(q.r*q.r + q.i*q.i - q.j*q.j - q.k*q.k, 2.0f*(q.i*q.j + q.r*q.k), 2.0f*(q.i*q.k - q.r*q.j));
  l.vy = make_Vec3f(2.0f*(q.i*q.j - q.r*q.k), (q.r*q.r - q.i*q.i + q.j*q.j - q.k*q.k), 2.0f*(q.j*q.k + q.r*q.i));
  l.vz = make_Vec3f(2.0f*(q.i*q.k + q.r*q.j), 2.0f*(q.j*q.k - q.r*q.i), q.r*q.r - q.i*q.i - q.j*q.j + q.k*q.k);
  return l;
}

inline uniform LinearSpace3f make_LinearSpace3f_identity()
{
  return make_LinearSpace3f(make_Vec3f(1.f,0.f,0.f),
                            make_Vec3f(0.f,1.f,0.f),
                            make_Vec3f(0.f,0.f,1.f));
}

/*! return scale matrix */
inline uniform LinearSpace3f make_LinearSpace3f_scale(const uniform Vec3f& s) 
{
  return make_LinearSpace3f( make_Vec3f( s.x, 0.0f, 0.0f),
                             make_Vec3f(0.0f,  s.y, 0.0f),
                             make_Vec3f(0.0f, 0.0f,  s.z));
}

/*! return matrix for rotation around arbitrary axis */
inline uniform LinearSpace3f make_LinearSpace3f_rotate(const uniform Vec3f& _u, const uniform float& r)
{
  uniform Vec3f u = normalize(_u);
  uniform float s = sin(r), c = cos(r);
  return make_LinearSpace3f(make_Vec3f(u.x*u.x+(1-u.x*u.x)*c, u.x*u.y*(1-c)+u.z*s,  u.x*u.z*(1-c)-u.y*s),
                            make_Vec3f(u.x*u.y*(1-c)-u.z*s, u.y*u.y+(1-u.y*u.y)*c, u.y*u.z*(1-c)+u.x*s),
                            make_Vec3f(u.x*u.z*(1-c)+u.y*s, u.y*u.z*(1-c)-u.x*s, u.z*u.z+(1-u.z*u.z)*c));
}

/*! return matrix for rotation around arbitrary axis */
inline varying LinearSpace3f make_LinearSpace3f_rotate(const varying Vec3f& _u, const varying float& r)
{
  varying Vec3f u = normalize(_u);
  varying float s = sin(r), c = cos(r);
  return make_LinearSpace3f(make_Vec3f(u.x*u.x+(1-u.x*u.x)*c, u.x*u.y*(1-c)+u.z*s,  u.x*u.z*(1-c)-u.y*s),
                            make_Vec3f(u.x*u.y*(1-c)-u.z*s, u.y*u.y+(1-u.y*u.y)*c, u.y*u.z*(1-c)+u.x*s),
                            make_Vec3f(u.x*u.z*(1-c)+u.y*s, u.y*u.z*(1-c)-u.x*s, u.z*u.z+(1-u.z*u.z)*c));
}

/*! return matrix for rotation around arbitrary axis */
inline varying LinearSpace3f make_LinearSpace3f_rotate(const uniform Vec3f& _u, const varying float& r)
{
  varying Vec3f u = normalize(_u);
  varying float s = sin(r), c = cos(r);
  return make_LinearSpace3f(make_Vec3f(u.x*u.x+(1-u.x*u.x)*c, u.x*u.y*(1-c)+u.z*s,  u.x*u.z*(1-c)-u.y*s),
                            make_Vec3f(u.x*u.y*(1-c)-u.z*s, u.y*u.y+(1-u.y*u.y)*c, u.y*u.z*(1-c)+u.x*s),
                            make_Vec3f(u.x*u.z*(1-c)+u.y*s, u.y*u.z*(1-c)-u.x*s, u.z*u.z+(1-u.z*u.z)*c));
}
  
////////////////////////////////////////////////////////////////////////////////
// Unary Operators
////////////////////////////////////////////////////////////////////////////////

inline uniform LinearSpace3f neg(const uniform LinearSpace3f &l) { return make_LinearSpace3f(neg(l.vx),neg(l.vy),neg(l.vz)); }
inline varying LinearSpace3f neg(const varying LinearSpace3f &l) { return make_LinearSpace3f(neg(l.vx),neg(l.vy),neg(l.vz)); }

/*! compute the determinant of the matrix */
inline uniform float det(const uniform LinearSpace3f &l) { return dot(l.vx,cross(l.vy,l.vz)); }
inline varying float det(const varying LinearSpace3f &l) { return dot(l.vx,cross(l.vy,l.vz)); }

/*! compute transposed matrix */
inline uniform LinearSpace3f transposed(const uniform LinearSpace3f &l) { 
  return make_LinearSpace3f(make_Vec3f(l.vx.x,l.vy.x,l.vz.x),
                            make_Vec3f(l.vx.y,l.vy.y,l.vz.y),
                            make_Vec3f(l.vx.z,l.vy.z,l.vz.z));
}
inline varying LinearSpace3f transposed(const varying LinearSpace3f &l) { 
  return make_LinearSpace3f(make_Vec3f(l.vx.x,l.vy.x,l.vz.x),
                            make_Vec3f(l.vx.y,l.vy.y,l.vz.y),
                            make_Vec3f(l.vx.z,l.vy.z,l.vz.z));
}

/*! compute adjoint matrix */
inline uniform LinearSpace3f adjoint(const uniform LinearSpace3f &l) { 
  return transposed(make_LinearSpace3f(cross(l.vy,l.vz),cross(l.vz,l.vx),cross(l.vx,l.vy))); 
}
inline varying LinearSpace3f adjoint(const varying LinearSpace3f &l) { 
  return transposed(make_LinearSpace3f(cross(l.vy,l.vz),cross(l.vz,l.vx),cross(l.vx,l.vy))); 
}

/*! calculates orthogonal coordinate frame with z-Vector pointing towards N */
inline uniform LinearSpace3f frame(const uniform Vec3f &N) 
{
  const uniform Vec3f dx0 = make_Vec3f(0.0f,N.z,-N.y);
  const uniform Vec3f dx1 = make_Vec3f(-N.z,0.0f,N.x);
  const uniform Vec3f dx = normalize(dot(dx0,dx0) > dot(dx1,dx1) ? dx0 : dx1);
  const uniform Vec3f dy = normalize(cross(N,dx));
  return make_LinearSpace3f(dx,dy,N);
}

inline varying LinearSpace3f frame(const varying Vec3f &N) 
{
  const varying Vec3f dx0 = make_Vec3f(0.0f,N.z,-N.y);
  const varying Vec3f dx1 = make_Vec3f(-N.z,0.0f,N.x);
  const varying Vec3f dx = normalize(dot(dx0,dx0) > dot(dx1,dx1) ? dx0 : dx1);
  const varying Vec3f dy = normalize(cross(N,dx));
  return make_LinearSpace3f(dx,dy,N);
}

////////////////////////////////////////////////////////////////////////////////
/// Binary Operators
////////////////////////////////////////////////////////////////////////////////

inline uniform LinearSpace3f operator+(const uniform LinearSpace3f &a, const uniform LinearSpace3f &b) { return make_LinearSpace3f(a.vx+b.vx, a.vy+b.vy, a.vz+b.vz); }
inline varying LinearSpace3f operator+(const varying LinearSpace3f &a, const varying LinearSpace3f &b) { return make_LinearSpace3f(a.vx+b.vx, a.vy+b.vy, a.vz+b.vz); }

inline uniform LinearSpace3f operator-(const uniform LinearSpace3f &a, const uniform LinearSpace3f &b) { return make_LinearSpace3f(a.vx-b.vx, a.vy-b.vy, a.vz-b.vz); }
inline varying LinearSpace3f operator-(const varying LinearSpace3f &a, const varying LinearSpace3f &b) { return make_LinearSpace3f(a.vx-b.vx, a.vy-b.vy, a.vz-b.vz); }

inline uniform Vec3f         operator*(const uniform LinearSpace3f &l, const uniform Vec3f &v) { return v.x*l.vx + v.y*l.vy + v.z*l.vz; }
inline varying Vec3f         operator*(const uniform LinearSpace3f &l, const varying Vec3f &v) { return v.x*l.vx + v.y*l.vy + v.z*l.vz; }
inline varying Vec3f         operator*(const varying LinearSpace3f &l, const varying Vec3f &v) { return v.x*l.vx + v.y*l.vy + v.z*l.vz; }

inline uniform LinearSpace3f operator*(const uniform float         &a, const uniform LinearSpace3f &b) { return make_LinearSpace3f(a*b.vx, a*b.vy, a*b.vz); }
inline uniform LinearSpace3f operator*(const uniform LinearSpace3f &a, const uniform float         &b) { return make_LinearSpace3f(a.vx*b, a.vy*b, a.vz*b); }
inline uniform LinearSpace3f operator*(const uniform LinearSpace3f &a, const uniform LinearSpace3f &b) { return make_LinearSpace3f(a*b.vx, a*b.vy, a*b.vz); }

inline varying LinearSpace3f operator*(const varying float         &a, const varying LinearSpace3f &b) { return make_LinearSpace3f(a*b.vx, a*b.vy, a*b.vz); }
inline varying LinearSpace3f operator*(const varying LinearSpace3f &a, const varying float         &b) { return make_LinearSpace3f(a.vx*b, a.vy*b, a.vz*b); }
inline varying LinearSpace3f operator*(const varying LinearSpace3f &a, const varying LinearSpace3f &b) { return make_LinearSpace3f(a*b.vx, a*b.vy, a*b.vz); }

inline varying LinearSpace3f operator*(const varying float         &a, const uniform LinearSpace3f &b) { return make_LinearSpace3f(a*b.vx, a*b.vy, a*b.vz); }
inline varying LinearSpace3f operator*(const uniform LinearSpace3f &a, const varying float         &b) { return make_LinearSpace3f(a.vx*b, a.vy*b, a.vz*b); }

inline uniform Vec3f xfmVector(const uniform LinearSpace3f l, const uniform Vec3f v) { return v.x*l.vx + v.y*l.vy + v.z*l.vz; }
inline varying Vec3f xfmVector(const uniform LinearSpace3f l, const varying Vec3f v) { return v.x*l.vx + v.y*l.vy + v.z*l.vz; }
inline varying Vec3f xfmVector(const varying LinearSpace3f l, const varying Vec3f v) { return v.x*l.vx + v.y*l.vy + v.z*l.vz; }

////////////////////////////////////////////////////////////////////////////////
/// Comparison Operators
////////////////////////////////////////////////////////////////////////////////

inline uniform bool eq(const uniform LinearSpace3f &a, const uniform LinearSpace3f &b) { return eq(a.vx,b.vx) & eq(a.vy,b.vy) & eq(a.vz,b.vz); }
inline varying bool eq(const varying LinearSpace3f &a, const varying LinearSpace3f &b) { return eq(a.vx,b.vx) & eq(a.vy,b.vy) & eq(a.vz,b.vz); }

inline uniform bool ne(const uniform LinearSpace3f &a, const uniform LinearSpace3f &b) { return ne(a.vx,b.vx) | ne(a.vy,b.vy) | ne(a.vz,b.vz); }
inline varying bool ne(const varying LinearSpace3f &a, const varying LinearSpace3f &b) { return ne(a.vx,b.vx) | ne(a.vy,b.vy) | ne(a.vz,b.vz); }

////////////////////////////////////////////////////////////////////////////////
// Unary Operators
////////////////////////////////////////////////////////////////////////////////

/*! compute inverse matrix */
inline uniform LinearSpace3f rcp(const uniform LinearSpace3f &l) { return rcp(det(l))*adjoint(l); }
inline varying LinearSpace3f rcp(const varying LinearSpace3f &l) { return rcp(det(l))*adjoint(l); }

inline uniform Vec3f xfmNormal(const uniform LinearSpace3f l, const uniform Vec3f v) { return xfmVector(transposed(rcp(l)),v); }
inline varying Vec3f xfmNormal(const uniform LinearSpace3f l, const varying Vec3f v) { return xfmVector(transposed(rcp(l)),v); }
inline varying Vec3f xfmNormal(const varying LinearSpace3f l, const varying Vec3f v) { return xfmVector(transposed(rcp(l)),v); }


////////////////////////////////////////////////////////////////////////////////
// Interpolation
////////////////////////////////////////////////////////////////////////////////

inline uniform LinearSpace3f lerp(uniform float factor, const uniform LinearSpace3f& a,  const uniform LinearSpace3f& b) {
  return make_LinearSpace3f(lerp(factor, a.vx, b.vx), lerp(factor, a.vy, b.vy), lerp(factor, a.vz, b.vz));
}

inline varying LinearSpace3f lerp(varying float factor, const uniform LinearSpace3f& a,  const uniform LinearSpace3f& b) {
  return make_LinearSpace3f(lerp(factor, a.vx, b.vx), lerp(factor, a.vy, b.vy), lerp(factor, a.vz, b.vz));
}

inline varying LinearSpace3f lerp(varying float factor, const varying LinearSpace3f& a,  const varying LinearSpace3f& b) {
  return make_LinearSpace3f(lerp(factor, a.vx, b.vx), lerp(factor, a.vy, b.vy), lerp(factor, a.vz, b.vz));
}

////////////////////////////////////////////////////////////////////////////////
// Norms
////////////////////////////////////////////////////////////////////////////////

inline uniform float norm_1(const uniform LinearSpace3f& l) 
{
  uniform float sum = 0;
  sum += reduce_add(abs(l.vx));
  sum += reduce_add(abs(l.vy));
  sum += reduce_add(abs(l.vz));
  return sum / 9.f;
}

inline uniform Quaternion3f make_Quaternion3f(const uniform LinearSpace3f & l)
{
  uniform Quaternion3f q;
  const uniform float tr = l.vx.x + l.vy.y + l.vz.z;
  if (tr > 0) 
  { 
    const uniform float S = sqrt(tr+1.0) * 2;
    q.r = 0.25 * S;
    q.i = (l.vy.z - l.vz.y) / S;
    q.j = (l.vz.x - l.vx.z) / S; 
    q.k = (l.vx.y - l.vy.x) / S; 
  }
  else if ((l.vx.x > l.vy.y)&(l.vx.x > l.vz.z)) 
  { 
    const uniform float S = sqrt(1.0 + l.vx.x - l.vy.y - l.vz.z) * 2;
    q.r = (l.vy.z - l.vz.y) / S;
    q.i = 0.25 * S;
    q.j = (l.vy.x + l.vx.y) / S; 
    q.k = (l.vz.x + l.vx.z) / S; 
  }
  else if (l.vy.y > l.vz.z) 
  { 
    const uniform float S = sqrt(1.0 + l.vy.y - l.vx.x - l.vz.z) * 2;
    q.r = (l.vz.x - l.vx.z) / S;
    q.i = (l.vy.x + l.vx.y) / S; 
    q.j = 0.25 * S;
    q.k = (l.vz.y + l.vy.z) / S; 
  } 
  else
  { 
    const uniform float S = sqrt(1.0 + l.vz.z - l.vx.x - l.vy.y) * 2;
    q.r = (l.vx.y - l.vy.x) / S;
    q.i = (l.vz.x + l.vx.z) / S;
    q.j = (l.vz.y + l.vy.z) / S;
    q.k = 0.25 * S;
  }
  return q;
}

inline varying Quaternion3f make_Quaternion3f(const varying LinearSpace3f & l)
{
  Quaternion3f q;
  const float tr = l.vx.x + l.vy.y + l.vz.z;
  if (tr > 0) 
  { 
    const float S = sqrt(tr+1.0) * 2;
    q.r = 0.25 * S;
    q.i = (l.vy.z - l.vz.y) / S;
    q.j = (l.vz.x - l.vx.z) / S; 
    q.k = (l.vx.y - l.vy.x) / S; 
  }
  else if ((l.vx.x > l.vy.y)&(l.vx.x > l.vz.z)) 
  { 
    const float S = sqrt(1.0 + l.vx.x - l.vy.y - l.vz.z) * 2;
    q.r = (l.vy.z - l.vz.y) / S;
    q.i = 0.25 * S;
    q.j = (l.vy.x + l.vx.y) / S; 
    q.k = (l.vz.x + l.vx.z) / S; 
  }
  else if (l.vy.y > l.vz.z) 
  { 
    const float S = sqrt(1.0 + l.vy.y - l.vx.x - l.vz.z) * 2;
    q.r = (l.vz.x - l.vx.z) / S;
    q.i = (l.vy.x + l.vx.y) / S; 
    q.j = 0.25 * S;
    q.k = (l.vz.y + l.vy.z) / S; 
  } 
  else
  { 
    const float S = sqrt(1.0 + l.vz.z - l.vx.x - l.vy.y) * 2;
    q.r = (l.vx.y - l.vy.x) / S;
    q.i = (l.vz.x + l.vx.z) / S;
    q.j = (l.vz.y + l.vy.z) / S;
    q.k = 0.25 * S;
  }
  return q;
}