// This file is part of libigl, a simple c++ geometry processing library.
// 
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
// 
// This Source Code Form is subject to the terms of the Mozilla Public License 
// v. 2.0. If a copy of the MPL was not distributed with this file, You can 
// obtain one at http://mozilla.org/MPL/2.0/.
#ifndef IGL_ARAP_ENERGY_TYPE_DOF_H
#define IGL_ARAP_ENERGY_TYPE_DOF_H
#include "igl_inline.h"

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include "ARAPEnergyType.h"
#include <vector>

/// @file arap_dof.h
/// @brief "Fast Automatic Skinning Transformations" [Jacobson et al.\ 2012]
///
/// Arap DOF precomputation consists of two parts the computation. The first is
/// that which depends solely on the mesh (V,F), the linear blend skinning
/// weights (M) and the groups G. Then there's the part that depends on the
/// previous precomputation and the list of free and fixed vertices. 
///
///
/// #### Caller example:
///
///     Once:
///       arap_dof_precomputation(...)
///
///     Each frame:
///       while(not satisfied)
///         arap_dof_update(...)
///       end
/// The code and variables differ from the description in Section 3 of "Fast
/// Automatic Skinning Transformations" by [Jacobson et al. 2012]
/// 
/// Here is a useful conversion table:
///
///     [article]                             [code]
///     S = \tilde{K} T                       S = CSM * Lsep
///     S --> R                               S --> R --shuffled--> Rxyz
///     Gamma_solve RT = Pi_1 \tilde{K} RT    L_part1xyz = CSolveBlock1 * Rxyz 
///     Pi_1 \tilde{K}                        CSolveBlock1
///     Peq = [T_full; P_pos]                 
///     T_full                                B_eq_fix <--- L0
///     P_pos                                 B_eq
///     Pi_2 * P_eq =                         Lpart2and3 = Lpart2 + Lpart3
///       Pi_2_left T_full +                  Lpart3 = M_fullsolve(right) * B_eq_fix
///       Pi_2_right P_pos                    Lpart2 = M_fullsolve(left) * B_eq
///     T = [Pi_1 Pi_2] [\tilde{K}TRT P_eq]   L = Lpart1 + Lpart2and3
///


namespace igl
{
  
  template <typename LbsMatrixType, typename SSCALAR>
  struct ArapDOFData;
  /// Precomputes the system to optimize for "Fast Automatic Skinning
  /// Transformations" [Jacobson et al.\ 2012] skinning degrees of freedom
  /// optimization using as-rigid-as-possible energy. This consists of building
  /// constructor matrices (to compute covariance matrices from transformations
  /// and to build the poisson solve right hand side from rotation matrix entries)
  /// and also prefactoring the poisson system.
  ///
  /// @param[in] V  #V by dim list of vertex positions
  /// @param[in] F  #F by {3|4} list of face indices
  /// @param[in] M  #V * dim by #handles * dim * (dim+1) matrix such that
  ///     new_V(:) = LBS(V,W,A) = reshape(M * A,size(V)), where A is a column
  ///     vectors formed by the entries in each handle's dim by dim+1 
  ///     transformation matrix. Specifcally, A =
  ///       reshape(permute(Astack,[3 1 2]),n*dim*(dim+1),1)
  ///     or A = [Lxx;Lyx;Lxy;Lyy;tx;ty], and likewise for other dim
  ///     if Astack(:,:,i) is the dim by (dim+1) transformation at handle i
  ///     handles are ordered according to P then BE (point handles before bone
  ///     handles)
  /// @param[in] G  #V list of group indices (1 to k) for each vertex, such that vertex i 
  ///     is assigned to group G(i)
  /// @param[out] data  structure containing all necessary precomputation for calling
  ///     arap_dof_update
  /// @return true on success, false on error
  ///
  /// \see lbs_matrix_column
  ///
  /// \fileinfo
  template <typename LbsMatrixType, typename SSCALAR>
  IGL_INLINE bool arap_dof_precomputation(
    const Eigen::MatrixXd & V, 
    const Eigen::MatrixXi & F,
    const LbsMatrixType & M,
    const Eigen::Matrix<int,Eigen::Dynamic,1> & G,
    ArapDOFData<LbsMatrixType, SSCALAR> & data);
  
  /// Should always be called after arap_dof_precomputation, but may be called in
  /// between successive calls to arap_dof_update, recomputes precomputation
  /// given that there are only changes in free and fixed
  ///
  /// @param[in]  fixed_dim  list of transformation element indices for fixed (or partailly
  ///   fixed) handles: not necessarily the complement of 'free'
  ///    NOTE: the constraints for fixed transformations still need to be
  ///    present in A_eq
  /// @param[in] A_eq  dim*#constraint_points by m*dim*(dim+1)  matrix of linear equality
  ///     constraint coefficients. Each row corresponds to a linear constraint,
  ///     so that A_eq * L = Beq says that the linear transformation entries in
  ///     the column L should produce the user supplied positional constraints
  ///     for each handle in Beq. The row A_eq(i*dim+d) corresponds to the
  ///     constrain on coordinate d of position i
  /// @param[out] data  structure containing all necessary precomputation for calling
  ///     arap_dof_update
  /// @return true on success, false on error
  ///
  /// \see lbs_matrix_column
  ///
  /// \fileinfo
  template <typename LbsMatrixType, typename SSCALAR>
  IGL_INLINE bool arap_dof_recomputation(
    const Eigen::Matrix<int,Eigen::Dynamic,1> & fixed_dim,
    const Eigen::SparseMatrix<double> & A_eq,
    ArapDOFData<LbsMatrixType, SSCALAR> & data);
  
  /// Optimizes the transformations attached to each weight function based on
  /// precomputed system.
  ///
  /// @param[in] data  precomputation data struct output from arap_dof_precomputation
  /// @param[in] Beq  dim*#constraint_points constraint values.
  /// @param[in] L0  #handles * dim * dim+1 list of initial guess transformation entries,
  ///     also holds fixed transformation entries for fixed handles
  /// @param[in] max_iters  maximum number of iterations
  /// @param[in] tol  stopping criteria parameter. If variables (linear transformation
  ///     matrix entries) change by less than 'tol' the optimization terminates,
  ///       0.75 (weak tolerance)
  ///       0.0 (extreme tolerance)
  /// @param[out] L  #handles * dim * dim+1 list of final optimized transformation entries,
  ///     allowed to be the same as L
  ///
  /// \fileinfo
  template <typename LbsMatrixType, typename SSCALAR>
  IGL_INLINE bool arap_dof_update(
    const ArapDOFData<LbsMatrixType,SSCALAR> & data,
    const Eigen::Matrix<double,Eigen::Dynamic,1> & B_eq,
    const Eigen::MatrixXd & L0,
    const int max_iters,
    const double tol,
    Eigen::MatrixXd & L
    );
  
  /// Structure that contains fields for all precomputed data or data that needs
  /// to be remembered at update
  ///
  /// \fileinfo
  template <typename LbsMatrixType, typename SSCALAR>
  struct ArapDOFData
  {
    /// Matrix with SSCALAR type
    typedef Eigen::Matrix<SSCALAR, Eigen::Dynamic, Eigen::Dynamic> MatrixXS;
    /// Type of arap energy we're solving
    igl::ARAPEnergyType energy;
    /// List of indices of fixed transformation entries
    Eigen::Matrix<int,Eigen::Dynamic,1> fixed_dim;
    /// List of precomputed covariance scatter matrices multiplied by lbs
    /// matrices
    std::vector<Eigen::MatrixXd> CSM_M;
    /// @private
    LbsMatrixType M_KG;
    /// Number of mesh vertices
    int n;
    /// Number of weight functions
    int m;
    /// Number of dimensions
    int dim;
    /// Effective dimensions
    int effective_dim;
    /// List of indices into C of positional constraints
    Eigen::Matrix<int,Eigen::Dynamic,1> interpolated;
    /// Mask of free variables
    std::vector<bool> free_mask;
    /// Full quadratic coefficients matrix before lagrangian (should be dense)
    LbsMatrixType Q;
  
  
    //// Solve matrix for the global step
    //Eigen::MatrixXd M_Solve; // TODO: remove from here
  
    /// Full solve matrix that contains also conversion from rotations to the right hand side, 
    /// i.e., solves Poisson transformations just from rotations and positional constraints
    MatrixXS M_FullSolve;
  
    /// Precomputed condensed matrices (3x3 commutators folded to 1x1):
    MatrixXS CSM;
    /// @private
    MatrixXS CSolveBlock1;
  
    /// Print timings at each update
    bool print_timings;
  
    /// dynamics
    bool with_dynamics;
    // I'm hiding the extra dynamics stuff in this struct, which sort of defeats
    // the purpose of this function-based coding style...
  
    /// Time step
    double h;
  
    /// #handles * dim * dim+1 list of transformation entries from
    /// previous solve
    MatrixXS L0;
    //// Lm1  #handles * dim * dim+1 list of transformation entries from
    //// previous-previous solve
    //MatrixXS Lm1;
    /// "Velocity"
    MatrixXS Lvel0;
  
    /// #V by dim matrix of external forces
    MatrixXS fext;
  
    /// Mass_tilde: MT * Mass * M
    LbsMatrixType Mass_tilde;
  
    /// Force due to gravity (premultiplier)
    Eigen::MatrixXd fgrav;
    /// Direction of gravity
    Eigen::Vector3d grav_dir;
    /// Magnitude of gravity
    double grav_mag;
    
    /// Π1 from the paper
    MatrixXS Pi_1;
  
    // @private Default values
    ArapDOFData(): 
      energy(igl::ARAP_ENERGY_TYPE_SPOKES), 
      with_dynamics(false),
      h(1),
      grav_dir(0,-1,0),
      grav_mag(0)
    {
    }
  };
}

#ifndef IGL_STATIC_LIBRARY
#  include "arap_dof.cpp"
#endif

#endif
