//============================================================================
//  Copyright (c) Kitware, Inc.
//  All rights reserved.
//  See LICENSE.txt for details.
//  This software is distributed WITHOUT ANY WARRANTY; without even
//  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
//  PURPOSE.  See the above copyright notice for more information.
//
//  Copyright 2016 Sandia Corporation.
//  Copyright 2016 UT-Battelle, LLC.
//  Copyright 2016 Los Alamos National Security.
//
//  Under the terms of Contract DE-AC04-94AL85000 with Sandia Corporation,
//  the U.S. Government retains certain rights in this software.
//
//  Under the terms of Contract DE-AC52-06NA25396 with Los Alamos National
//  Laboratory (LANL), the U.S. Government retains certain rights in
//  this software.
//============================================================================
#ifndef vtk_m_rendering_MatrixHelpers_h
#define vtk_m_rendering_MatrixHelpers_h

#include <vtkm/Matrix.h>
#include <vtkm/VectorAnalysis.h>

namespace vtkm {
namespace rendering {

struct MatrixHelpers
{
  static VTKM_CONT
  void CreateOGLMatrix(const vtkm::Matrix<vtkm::Float32,4,4> &mtx,
                              vtkm::Float32 *oglM)
  {
    oglM[ 0] = mtx[0][0];
    oglM[ 1] = mtx[1][0];
    oglM[ 2] = mtx[2][0];
    oglM[ 3] = mtx[3][0];
    oglM[ 4] = mtx[0][1];
    oglM[ 5] = mtx[1][1];
    oglM[ 6] = mtx[2][1];
    oglM[ 7] = mtx[3][1];
    oglM[ 8] = mtx[0][2];
    oglM[ 9] = mtx[1][2];
    oglM[10] = mtx[2][2];
    oglM[11] = mtx[3][2];
    oglM[12] = mtx[0][3];
    oglM[13] = mtx[1][3];
    oglM[14] = mtx[2][3];
    oglM[15] = mtx[3][3];
  }

  static VTKM_CONT
  vtkm::Matrix<vtkm::Float32,4,4> ViewMatrix(const vtkm::Vec<vtkm::Float32,3> &position,
                                             const vtkm::Vec<vtkm::Float32,3> &lookAt,
                                             const vtkm::Vec<vtkm::Float32,3> &up)
  {
    vtkm::Vec<vtkm::Float32,3> viewDir = position-lookAt;
    vtkm::Vec<vtkm::Float32,3> right = vtkm::Cross(up,viewDir);
    vtkm::Vec<vtkm::Float32,3> ru = vtkm::Cross(viewDir,right);

    vtkm::Normalize(viewDir);
    vtkm::Normalize(right);
    vtkm::Normalize(ru);

    vtkm::Matrix<vtkm::Float32,4,4> matrix;
    vtkm::MatrixIdentity(matrix);

    matrix(0,0) = right[0];
    matrix(0,1) = right[1];
    matrix(0,2) = right[2];
    matrix(1,0) = ru[0];
    matrix(1,1) = ru[1];
    matrix(1,2) = ru[2];
    matrix(2,0) = viewDir[0];
    matrix(2,1) = viewDir[1];
    matrix(2,2) = viewDir[2];

    matrix(0,3) = -vtkm::dot(right,position);
    matrix(1,3) = -vtkm::dot(ru,position);
    matrix(2,3) = -vtkm::dot(viewDir,position);

    return matrix;
  }

  static VTKM_CONT
  vtkm::Matrix<vtkm::Float32,4,4> WorldMatrix(const vtkm::Vec<vtkm::Float32,3> &neworigin,
                                              const vtkm::Vec<vtkm::Float32,3> &newx,
                                              const vtkm::Vec<vtkm::Float32,3> &newy,
                                              const vtkm::Vec<vtkm::Float32,3> &newz)
  {
    vtkm::Matrix<vtkm::Float32,4,4> matrix;
    vtkm::MatrixIdentity(matrix);

    matrix(0,0) = newx[0];
    matrix(0,1) = newy[0];
    matrix(0,2) = newz[0];
    matrix(1,0) = newx[1];
    matrix(1,1) = newy[1];
    matrix(1,2) = newz[1];
    matrix(2,0) = newx[2];
    matrix(2,1) = newy[2];
    matrix(2,2) = newz[2];

    matrix(0,3) = neworigin[0];
    matrix(1,3) = neworigin[1];
    matrix(2,3) = neworigin[2];

    return matrix;
  }

  static VTKM_CONT
  vtkm::Matrix<vtkm::Float32,4,4> TrackballMatrix(vtkm::Float32 p1x,
                                                  vtkm::Float32 p1y,
                                                  vtkm::Float32 p2x,
                                                  vtkm::Float32 p2y)
  {
    const vtkm::Float32 RADIUS = 0.80f; //z value lookAt x = y = 0.0
    const vtkm::Float32 COMPRESSION = 3.5f; // multipliers for x and y.
    const vtkm::Float32 AR3 = RADIUS*RADIUS*RADIUS;

    vtkm::Matrix<vtkm::Float32,4,4> matrix;

    vtkm::MatrixIdentity(matrix);
    if (p1x==p2x && p1y==p2y) { return matrix; }

    vtkm::Vec<vtkm::Float32, 3> p1(p1x,p1y, AR3/((p1x*p1x+p1y*p1y)*COMPRESSION+AR3));
    vtkm::Vec<vtkm::Float32, 3> p2(p2x,p2y, AR3/((p2x*p2x+p2y*p2y)*COMPRESSION+AR3));
    vtkm::Vec<vtkm::Float32, 3> axis = vtkm::Normal(vtkm::Cross(p2,p1));

    vtkm::Vec<vtkm::Float32, 3> p2_p1(p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2]);
    vtkm::Float32 t = vtkm::Magnitude(p2_p1);
    t = vtkm::Min(vtkm::Max(t, -1.0f), 1.0f);
    vtkm::Float32 phi = static_cast<vtkm::Float32>(-2.0f*asin(t/(2.0f*RADIUS)));
    vtkm::Float32 val = static_cast<vtkm::Float32>(sin(phi/2.0f));
    axis[0] *= val;
    axis[1] *= val;
    axis[2] *= val;

    //quaternion
    vtkm::Float32 q[4] = {axis[0], axis[1], axis[2], static_cast<vtkm::Float32>(cos(phi/2.0f))};

    // normalize quaternion to unit magnitude
    t =  1.0f / static_cast<vtkm::Float32>(sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]));
    q[0] *= t;
    q[1] *= t;
    q[2] *= t;
    q[3] *= t;

    matrix(0,0) = 1 - 2 * (q[1]*q[1] + q[2]*q[2]);
    matrix(0,1) = 2 * (q[0]*q[1] + q[2]*q[3]);
    matrix(0,2) = (2 * (q[2]*q[0] - q[1]*q[3]) );

    matrix(1,0) = 2 * (q[0]*q[1] - q[2]*q[3]);
    matrix(1,1) = 1 - 2 * (q[2]*q[2] + q[0]*q[0]);
    matrix(1,2) = (2 * (q[1]*q[2] + q[0]*q[3]) );

    matrix(2,0) = (2 * (q[2]*q[0] + q[1]*q[3]) );
    matrix(2,1) = (2 * (q[1]*q[2] - q[0]*q[3]) );
    matrix(2,2) = (1 - 2 * (q[1]*q[1] + q[0]*q[0]) );

    return matrix;
  }

};

}} //namespace vtkm::rendering

#endif // vtk_m_rendering_MatrixHelpers_h
