#include "opt_height_field.h"

#include "monotonic_function_space.h"
#include "minimath.h"

#define HAVE_TNC 1
#define HAVE_LBFGS 0

#if HAVE_TNC
#include "tnc/tnc.h"
#endif

#if HAVE_LBFGS
#include "lbfgs.h"
#endif

#include "halfedge.h"
#include "adt/cvec3t.h"
#include "laplacian_helpers.h"

#include <cassert>
#include <algorithm>
#include <vector>
#include <valarray>
#include <map>
#include <set>
#include <cmath>
#include <iostream>
#include <cstdio>
#include <limits>

// For some reason, on Tino's machine, M_PI isn't defined.
#ifndef M_PI
#define M_PI 3.14159265358979323846264338327950288
#endif

#include "Eigen/Eigen"
#include "Eigen/Sparse"
#include "Eigen/SVD"
// import most common Eigen types 
// USING_PART_OF_NAMESPACE_EIGEN

#define C_WRAPPER_FUNCTION 1

// Whether to perturb the initial values for a recovery test.
#define PERTURB 0

// Whether to verify the gradient with finite differencing (and only if kDebugHere is on below).
#define VERIFY_GRADIENT 0

// Whether to skip computing the energy when all vertices involved are fixed (the gradient
// contribution would be zero).
#define SKIP_FIXED_FIXED_SITUATION 1

// Whether to cache the answer to is_ancestor()
// TODO Q: Would it be more efficient to cache it in a sparse matrix?
#define CACHE_IS_ANCESTOR 0

// Whether to use a gradient-optimized function for gradient-only evaluations.
#define GRADIENT_ONLY_OPTIMIZATION 1

// Whether to print statistics about the monotonicity graph.
#define PRINT_GRAPH_STATISTICS 1

// Whether to check for cycles in the graph.
#define GRAPH_CYCLE_CHECK 1

// Whether to bound theta in [0, pi] when using TNC.
#define TNC_BOUNDS 0

/*
using toposmooth::real_t;
using toposmooth::tree_ops_t;
*/

using namespace toposmooth;
using namespace toposmooth::minimath;
using toposmooth::uint;

namespace
{
    std::ostream& dbg_out = std::cerr;
    
    const index_t kInvalidIndex = -1;
    
    typedef CVec3T< toposmooth::real_t > vec3_t;
    
    typedef Eigen::SparseVector< real_t, Eigen::RowMajor > sparse_vector_t;
    typedef Eigen::Matrix< real_t, Eigen::Dynamic, 1 > dense_vector_t;
    typedef Eigen::DynamicSparseMatrix< real_t, Eigen::RowMajor > assembly_matrix_t;
    typedef Eigen::SparseMatrix< real_t, Eigen::RowMajor > solve_matrix_t;
    
    typedef solve_matrix_t hessian_t;
    
    typedef std::set< index_t > children_t;
    typedef std::vector< children_t > parent_to_child_map_t;
}

namespace
{
    real_t DFS_set_max_leaf_value( const parent_to_child_map_t& all_node_children, const std::vector< real_t >& original_values, const index_t node, std::vector< real_t >& max_leaf_value_out )
    {
        /*
        // Leaf nodes might have no presence in the all_node_children map, so we have to
        // check for this case.
        // assert( all_node_children.find( node ) != all_node_children.end() );
        if( all_node_children.find( node ) == all_node_children.end() )
        {
            return original_values.at( node );
        }
        */
        
        // const children_t& children = all_node_children.find( node )->second;
        const children_t& children = all_node_children[ node ];
        if( children.empty() )
        {
            return original_values.at( node );
        }
        
        real_t max_leaf_value = -std::numeric_limits< real_t >::max();
        for( children_t::const_iterator child = children.begin(); child != children.end(); ++child )
        {
            max_leaf_value = std::max( max_leaf_value, DFS_set_max_leaf_value( all_node_children, original_values, *child, max_leaf_value_out ) );
        }
        
        max_leaf_value_out[ node ] = max_leaf_value;
        return max_leaf_value;
    }
    
    class TreeOpsHeightFieldTNC : public tree_ops_t
    {
    public:
        template< typename floating_type >
        TreeOpsHeightFieldTNC(
            
            const size_t num_vertices,
            const floating_type* vertices_xy,
            
            const size_t num_triangles,
            // NOTE: Triangles may or may not be copied internally!  Check below.
            const triangle_t* triangles,
            
            const floating_type* original_vertex_values,
            
            const size_t num_edges,
            const edge_t* directed_edges,
            
            const floating_type stay_close_to_original_values_weight = 0.,
            bool smooth_deformation_not_smooth_function = false
            )
            : m_last_hess_nnz( -1 ), m_callback_theta( 0 )
        {
            // TODO: To make the function more useful, copy the input to the output
            //       if there are no degrees of freedom after building the graph.
            /*
            if( m_num_theta <= 0 )
            {
                std::copy( original_vertex_values, original_vertex_values + num_vertices, new_vertex_values_out );
                return;
            }
            */
            
            // We need at least 3 rows and 5 columns of vertices to make
            // an interior chain of 3 vertices which would have one
            // degree of freedom.
            assert( num_vertices >= 3*5 );
            assert( vertices_xy );
            // 3 rows and 5 columns gives us two quads in each row and 4 in each column
            // (times 2 triangles per quad).
            assert( num_triangles >= 2*4 * 2 );
            assert( triangles );
#ifndef NDEBUG
            for( int t = 0; t < num_triangles; ++t )
            {
                for( int v = 0; v < 3; ++v )
                {
                    assert( triangles[t].v[v] >= 0 );
                    assert( triangles[t].v[v] < num_vertices );
                }
            }
#endif
            assert( original_vertex_values );
            
            // A chain of two edges gives us one degree of freedom.
            assert( num_edges >= 2 );
            assert( directed_edges );
#ifndef NDEBUG
            for( int e = 0; e < num_edges; ++e )
            {
                for( int v = 0; v < 2; ++v )
                {
                    assert( directed_edges[e].v[v] >= 0 );
                    assert( directed_edges[e].v[v] < num_vertices );
                }
                // Verify that edges are monotonically decreasing.
                // NOTE: This need not be true.  The only thing we require is that the root
                //       is greater than leaves.
                // assert( original_vertex_values[ directed_edges[e].v[0] ] >= original_vertex_values[ directed_edges[e].v[1] ] );
            }
#endif
            
            m_num_vertices = num_vertices;
            m_original_vertex_values.resize( num_vertices );
            std::copy( original_vertex_values, original_vertex_values + num_vertices, m_original_vertex_values.begin() );
            
            // This is used as a cache in each iteration.
            m_f_i.resize( num_vertices );
            std::copy( original_vertex_values, original_vertex_values + num_vertices, &m_f_i[0] );
            
            m_vertices_xy0.resize( num_vertices*3 );
            for( int i = 0; i < num_vertices; ++i )
            {
                m_vertices_xy0[ 3*i + 0 ] = vertices_xy[ 2*i + 0 ];
                m_vertices_xy0[ 3*i + 1 ] = vertices_xy[ 2*i + 1 ];
                m_vertices_xy0[ 3*i + 2 ] = 0.;
            }
            // std::copy( vertices_xy, vertices_xy + num_vertices*2, m_vertices_xy.begin() );
            
            m_laplacian_editing_not_laplacian = smooth_deformation_not_smooth_function;
            m_E_diff_weight = stay_close_to_original_values_weight;
            if( m_E_diff_weight > 0. )
            {
                m_area.resize( num_vertices, kBogusReal );
                m_area_dirty.resize( num_vertices, true );
            }
            
            
            // Store the mesh.
            
            // TODO Q: To copy or not to copy the triangles?
            // A1: Don't copy.  It's big, and we don't need to be wasteful.
            // m_num_triangles = num_triangles;
            // m_triangles = triangles;
            // A2: Copy it.  Unless it's a performance problem, be safe.
            m_triangles.resize( num_triangles );
            std::copy( triangles, triangles + num_triangles, m_triangles.begin() );
            
            mesh_gui::unordered_edges_from_triangles( num_triangles, triangles, m_edges );
            m_mesh.build( num_vertices, num_triangles, triangles, m_edges.size(), &m_edges[0] );
            
            // TODO Q: Should I find/replace m_num_vertices with m_mesh.n_vertices()?
            
            
            // Store the graph of values.
            // Generate: m_parent_node, m_is_ancestor, m_is_fixed, m_max_leaf_value
            
            // m_parent_node
            // NOTE: We will also generate a map of a node to its children, used in generating
            //       some structures.
            
            // We're using an array as a fast map.
            m_parent_node.clear();
            // Note that a root node should point to itself; we fix this at the end of the
            // constructor.
            m_parent_node.resize( num_vertices, kInvalidIndex );
            m_children.resize( num_vertices );
            
            // So we know who is a root and who is a leaf.
            std::valarray< int > incoming( 0, num_vertices );
            std::valarray< int > outgoing( 0, num_vertices );
            
            m_num_edges = num_edges;
            for( int e = 0; e < num_edges; ++e )
            {
                const index_t start = directed_edges[e].start();
                const index_t end = directed_edges[e].end();
                
                if( m_children[ end ].find( start ) != m_children[ end ].end() )
                {
                    std::cerr << "WARNING: 1-edge cycle detected!  Skipping...\n";
                    continue;
                }
                
                // Nodes can't have multiple incoming edges.
                // UPDATE: Leaf nodes can!
                // assert( kInvalidIndex == m_parent_node[ end ] );
                m_parent_node[ end ] = start;
                
                m_children[ start ].insert( end );
                // The next line ensures that leaf nodes are also present in the children map.
                // UPDATE: We don't need them to anymore, since we're using the DFS code
                //         to set max_leaf_values now (see below).
                // m_children[ end ];
                
                // A node should never have its parent as a child.
                assert( m_children[ end ].end() == m_children[ end ].find( start ) );
                
                incoming[ end ] += 1;
                outgoing[ start ] += 1;
            }
            
            // Fill m_is_fixed and m_max_leaf_value.
            // We're using arrays as a fast maps.
            m_is_fixed.clear();
            // All vertices are fixed except internal vertices.
            m_is_fixed.resize( num_vertices, true );
            m_is_root.clear();
            // No vertices are roots unless found otherwise.
            m_is_root.resize( num_vertices, false );
            m_max_leaf_value.clear();
            // All nodes start with a maximum leaf value of -(huge number).
            m_max_leaf_value.resize( num_vertices, -std::numeric_limits< real_t >::max() );
            
            // for( parent_to_child_map_t::const_iterator it = m_children.begin(); it != m_children.end(); ++it )
            for( index_t node = 0; node < m_children.size(); ++node )
            {
                // const index_t node = it->first;
                
                // A node cannot be both a leaf and a root.  This should never happen
                // because we take in only directed edges.
                // assert( !( m_children[ node ].empty() && m_parent_node[ node ] == kInvalidIndex ) );
                
                // Are we a leaf node?
                // if( m_children[ node ].empty() )
                if( incoming[ node ] > 0 && outgoing[ node ] == 0 )
                {
                    // std::cout << "leaf node: " << node << '\n';
                    
                    // This doesn't support leaves with multiple parents.
                    // (The code below in the "else if" statement does.)
                    // UPDATE: This code also won't work if leaf nodes have no presence in the
                    //         children map (see above).
                    /*
                    index_t parent = m_parent_node[ node ];
                    while( kInvalidIndex != parent && m_max_leaf_value[ parent ] < m_original_vertex_values.at( node ) )
                    {
                        m_max_leaf_value[ parent ] = m_original_vertex_values.at( node );
                        parent = m_parent_node[ parent ];
                    }
                    */
                }
                // Are we a root node?
                // else if( m_parent_node[ node ] == kInvalidIndex )
                else if( outgoing[ node ] > 0 && (incoming[ node ] == 0 || incoming[ node ] > 1) )
                {
                    // Interior fixed vertices will fail this assert.
                    // TODO: Handle them or guarantee they won't occur; at the least
                    //       DFS_set_max_leaf_value() will have to change to stop assuming a
                    //       node with children is free.
                    assert( m_parent_node[ node ] == kInvalidIndex );
                    // For now, just assert that we don't have interior fixed vertices like that.
                    assert( incoming[ node ] == 0 );
                    
                    m_is_root[ node ] = true;
                    
                    // std::cout << "root node: " << node << '\n';
                    
                    // This supports leaves with multiple parents.
                    // (The code above in the "if" statement doesn't.)
                    DFS_set_max_leaf_value( m_children, m_original_vertex_values, node, m_max_leaf_value );
                }
                // Are we a vanilla fixed node?
                else if( incoming[ node ] == 0 && outgoing[ node ] == 0 )
                {
                    assert( m_is_fixed[ node ] );
                }
                // Otherwise we are a vanilla free node.
                else
                {
                    assert( incoming[ node ] == 1 );
                    
                    // We must be a free node.
                    m_is_fixed[ node ] = false;
                    
                    assert( kInvalidIndex != m_parent_node[ node ] );
                    assert( !m_children[ node ].empty() );
                }
            }
            
            
            // m_is_ancestor is cached on demand.
            
            
            // Compute the number of degrees of freedom
            m_node2theta.clear();
            m_node2theta.resize( num_vertices, kInvalidIndex );
            m_theta2node.clear();
            m_theta2node.reserve( num_vertices );
            for( int i = 0; i < num_vertices; ++i )
            {
                if( !m_is_fixed[ i ] )
                {
                    m_node2theta[ i ] = m_theta2node.size();
                    m_theta2node.push_back( i );
                }
            }
            m_num_theta = m_theta2node.size();
            
            
#if SKIP_FIXED_FIXED_SITUATION
            // Make a vector of nodes to iterate over when calculating the energy.
            // These nodes are or have at least one neighbor that is free.
            std::vector< index_t > neighbors;
            for( int vertex_index = 0; vertex_index < num_vertices; ++vertex_index )
            {
                m_mesh.vertex_vertex_neighbors( vertex_index, neighbors );
                
                if( !is_fixed( vertex_index ) )
                {
                    m_not_fixed_fixed.push_back( vertex_index );
                }
                else
                {
                    for( uint n = 0; n < neighbors.size(); ++n )
                    {
                        if( !is_fixed( neighbors.at(n) ) )
                        {
                            m_not_fixed_fixed.push_back( vertex_index );
                            break;
                        }
                    }
                }
            }
#else
            for( int i = 0; i < num_vertices; ++i ) m_not_fixed_fixed.push_back( i );
#endif
            
            // Prepare the caches for df_i() and d2f_i().
            m_df_i.resize( num_vertices );
            // std::vector's and valarray's crash with a solve_matrix_t (but not a sparse_matrix_t).
            // m_d2f_i.resize( num_vertices );
            m_d2f_i = new solve_matrix_t[ num_vertices ];
            m_d2f_i_sparsity_set = false;
            
            
            // The spec requires that if a node is a root node, parent_node() returns itself.
            for( int i = 0; i < num_vertices; ++i )
                if( kInvalidIndex == m_parent_node[i] )
                    m_parent_node[i] = i;
            
            
#if GRAPH_CYCLE_CHECK
            // Checking for cycles and isolated vertices.
            std::vector< int > seen( num_vertices, 0 );
            for( int i = 0; i < num_vertices; ++i )
            {
                if( m_is_root[ i ] ) DFS_mark_seen_and_check_for_cycles( seen, i );
            }
            for( int i = 0; i < num_vertices; ++i )
            {
                if( !m_is_fixed[i] && !seen[i] )
                {
                    std::cerr << "WARNING: Unreachable vertex!\n";
                }
            }
#endif
            
            
#if PRINT_GRAPH_STATISTICS
            std::cout << "+++ Monotonicity graph statistics +++\n";
            assert( m_is_root.size() == num_vertices );
            std::vector< int > depths( num_vertices, -1 );
            int free_depths_sum = 0;
            int num_free = 0;
            int max_depth = 0;
            for( int i = 0; i < num_vertices; ++i )
            {
                calc_depth( depths, i );
                
                if( m_is_fixed[ i ] ) continue;
                
                const int d = depths[i];
                
                assert( d != 0 );
                // std::cout << "Internal node has depth: " << depths[i] << '\n';
                max_depth = std::max( d, max_depth );
                free_depths_sum += d;
                num_free += 1;
            }
            std::cout << "Total # free nodes: " << num_free << '\n';
            std::cout << "Depth of deepest free node: " << max_depth << '\n';
            std::cout << "Average free node depth (sum of all free node depths / # free nodes): " << float( free_depths_sum ) / float( num_free ) << '\n';
            std::cout << "--- Monotonicity graph statistics ---\n";
#endif
        }
        ~TreeOpsHeightFieldTNC()
        {
            delete [] m_d2f_i;
        }
        
        void calc_depth( std::vector< int >& depths, node_t node_i )
        {
            const node_t parent = parent_node( node_i );
            assert( kInvalidIndex != parent );
            
            // Have we already been visited?
            if( -1 != depths[ node_i ] ) return;
            
            // Are we a root (or fixed)?
            if( parent == node_i )
            {
                depths[ node_i ] = 0;
            }
            else
            {
                // Make sure parent is calculated.
                calc_depth( depths, parent );
                assert( -1 != depths[ parent ] );
                depths[ node_i ] = depths[ parent ] + 1;
            }
        }
        void DFS_mark_seen_and_check_for_cycles( std::vector< int >& seen, node_t node )
        {
            if( seen[ node ] )
            {
                // It's OK if we've seen it if it's a leaf node.
                if( !m_children[ node ].empty() )
                {
                    std::cerr << "WARNING: Cycle detected!  You will die!\n";
                    assert( !"cycle detected!" );
                }
                
                return;
            }
            
            seen[ node ] = true;
            
            const children_t& children = m_children[ node ];
            for( children_t::const_iterator child = children.begin(); child != children.end(); ++child )
            {
                DFS_mark_seen_and_check_for_cycles( seen, *child );
            }
        }
        
#if HAVE_TNC
        // 'new_vertex_values_out' must be an array of the same size as the 'original_vertex_values'
        // parameter passed to this class's constructor.
        template< typename floating_type >
        void Optimize( floating_type* new_vertex_values_out )
        {
            assert( new_vertex_values_out );
            
            assert( m_num_theta > 0 );
            std::vector< double > theta_inout( m_num_theta );
            GetInitialGuess( &theta_inout[0] );
            
#if TNC_BOUNDS
            assert( *std::min_element( theta_inout.begin(), theta_inout.end() ) >= 0. );
            assert( *std::max_element( theta_inout.begin(), theta_inout.end() ) <= M_PI );
            std::vector< double > lower_bounds( m_num_theta, 0. );
            std::vector< double > upper_bounds( m_num_theta, M_PI );
#endif
            
            double f_out( kBogusReal );
            std::vector< double > gradient_out( m_num_theta, kBogusReal );
            int num_func_evaluations( 0 );
            const int result = tnc(
                // We have two less theta values than numbers in the chain
                m_num_theta,
                // The initial guess for theta as well as where to store the output values
                &theta_inout[0],
                // The final function evaluation is placed here
                &f_out,
                // The final gradient evaluation is placed here
                &gradient_out[0],
                // The function callback and user data (pass our static method and 'this' pointer)
                TreeOpsHeightFieldTNC::tnc_function_callback, this,
                // We don't want upper and lower bounds, thetas are periodic
                // UPDATE: Maybe we do, 0 and M_PI.
#if TNC_BOUNDS
                &lower_bounds[0], &upper_bounds[0],
#else
                NULL, NULL,
#endif
                // Let's use the default scale... It's what the python wrapper defaults to.  TODO Q: Is this right?  Is the default scale 1?
                NULL,
                // Let's use the default offset... It's what the python wrapper defaults to.  TODO Q: Is this right?  Is the default offset 0?
                NULL,
                // Be very verbose (use TNC_MSG_NONE for silence or TNC_MSG_EXIT for exit reasons only)
                TNC_MSG_ALL,
                // Number of hess*gradient evaulations per iteration; I'm using the python wrapper's default.
                -1,
                // Maximum number of function evaluations; I'm using the python wrapper's default.
                std::max(int( 100 ), int( 10*theta_inout.size() )),
                // Severity of line search; I'm using the default.
                -1,
                // Let's set our maximum step to 2*pi,
                2*M_PI,
                // Let's use the default accuracy,
                0,
                // The function minimum should be 0,
                0,
                // Let's use default tolerances,
                -1, -1, -1,
                // Let's use the default rescaling value,
                -1,
                &num_func_evaluations
                );
            
            dbg_out << "TNC returned with message: " << tnc_rc_string[result - TNC_MINRC] << '\n';
            
            GetValuesForThetas( &theta_inout[0], new_vertex_values_out );
        }
#elif HAVE_LBFGS
        template< typename floating_type >
        void Optimize( floating_type* new_vertex_values_out )
        {
            assert( new_vertex_values_out );
            
            assert( m_num_theta > 0 );
            std::vector< double > theta_inout( m_num_theta );
            GetInitialGuess( &theta_inout[0] );
            
            // Initialize the parameters to defaults.
            lbfgs_parameter_t params;
            lbfgs_parameter_init( &params );
            
            // BFGS optimization:
            // Fortran: http://www.ece.northwestern.edu/~nocedal/lbfgs.html
            // One of these: http://www.alglib.net/optimization/lbfgs.php
            // C: http://www.chokkan.org/software/liblbfgs/
            const int result = lbfgs(
                // The number of variables.
                m_num_theta,
                // The initial guess for theta as well as where to store the output values
                &theta_inout[0],
                // A pointer to the variable that stores the function value on output; we don't need it.
                NULL,
                // Callback functions
                TreeOpsHeightFieldTNC::lbfgs_evaluate_callback,
                TreeOpsHeightFieldTNC::lbfgs_progress_callback,
                // User data passed to our callback function,
                this,
                // A pointer to parameter structure (or NULL for defaults),
                &params
                );
            
            dbg_out << "lbfgs returned with value: " << result << '\n';
            
            GetValuesForThetas( &theta_inout[0], new_vertex_values_out );
        }
#endif
        
        template< typename floating_type >
        void GetInitialGuess( floating_type* initial_out ) const
        {
            // 'initial_out' must be large enough to store GetNumThetas() values.
            assert( initial_out );
            
            // The initial guess and the output value.
            // NOTE: We use double's here because that's what tnc wants.
            // Set initial thetas to pi/2, which corresponds to a lerp of .5 at each node.
            const double theta0 = .5*M_PI;
            // TODO: Do something better than this, by tracing down the tree.
            std::fill( initial_out, initial_out + m_num_theta, theta0 );
#if PERTURB
            // Recovery test: Perturb the first degree-of-freedom.
            initial_out[ 0 ] += .2;
            initial_out[ m_num_theta - 1 ] += .2;
#endif
        }
        
        size_t GetNumThetas() const
        {
            return m_num_theta;
        }
        
        size_t GetNumValues() const
        {
            return m_num_vertices;
        }
        
        template< typename floating_type >
        void GetValuesForThetas( double thetas[], floating_type* values_out ) const
        {
            // 'values_out' must be large enough to store GetNumValues() values.
            assert( values_out );
            
            assert( 0 == m_callback_theta );
            m_callback_theta = thetas;
            for( int i = 0; i < m_num_vertices; ++i ) values_out[i] = f_i( i );
            m_callback_theta = 0;
        }
        
        // TODO: Check if my hessian is rather dense; if so, make an Evaluate() that returns
        //       hess * vec for some vector rather than returning the entire hessian.
        void Evaluate( const double thetas[], double* f, double g[], hessian_t* hess ) const
        {
            const bool kDebugHere = false;
            
            if( kDebugHere ) dbg_out << "+++ int tnc_callback( double thetas[], double* f, double g[] )\n";
            
            assert( f );
            
            // We need to evaluate the function 'f' and gradient 'g' for the given thetas.
            m_callback_theta = thetas;
            
            ComputeAndCache_f_and_derivatives( g, hess );
            
            *f = 0;
            if( g ) std::fill( g, g + m_num_theta, 0. );
            if( hess ) hess->setZero();
            this->add_E_and_gradient_and_hessian( f, g, hess );
            
            // Debugging print outs.
            if( kDebugHere )
            {
                dbg_out << "Current function values:\n";
                for( int i = 0; i < m_num_vertices; ++i ) dbg_out << f_i( i ) << ' ';
                dbg_out << '\n';
                
                dbg_out << "Current thetas:\n";
                for( int i = 0; i < m_num_theta; ++i ) dbg_out << this->theta( m_theta2node[ i ] ) << ' ';
                dbg_out << '\n';
                
                dbg_out << "Current energy: " << *f << '\n';
                
                if( g )
                {
                    dbg_out << "Current gradient:\n";
                    for( int i = 0; i < m_num_theta; ++i ) dbg_out << g[i] << ' ';
                    dbg_out << '\n';
                    
#if VERIFY_GRADIENT
                    // Verify gradients with finite differencing energy.
                    std::vector< double > gradient_dont_care( m_num_theta );
                    std::vector< double > gradient_fin_diff( m_num_theta, 0. );
                    double old_theta = -31337;
                    const double h = 1e-7;
                    for( int i = 0; i < m_num_theta; ++i )
                    {
                        old_theta = m_callback_theta[i];
                        m_callback_theta[i] += h;
                        
                        this->add_E_and_gradient_and_hessian( &gradient_fin_diff[i], &gradient_dont_care[0], NULL );
                        
                        m_callback_theta[i] = old_theta;
                    }
                    add( -*f, gradient_fin_diff, gradient_fin_diff );
                    scale( 1./h, gradient_fin_diff, gradient_fin_diff );
                    dbg_out << "Current gradient (computed via finite differencing):\n";
                    for( int i = 0; i < m_num_theta; ++i ) dbg_out << gradient_fin_diff[i] << ' ';
                    dbg_out << '\n';
                    
                    // Let's use the finite differencing gradient for now.
                    // UPDATE: The main gradient seems fine, so let's use it.
                    // std::copy( gradient_fin_diff.begin(), gradient_fin_diff.end(), g );
#endif
                }
                
                if( hess )
                {
                    Eigen::MatrixXd hessian_dense( hess->rows(), hess->cols() );
                    hessian_dense.setZero();
                    for( int k = 0; k < hess->outerSize(); ++k )
                    for( solve_matrix_t::InnerIterator it( *hess, k ); it; ++it )
                    {
                        hessian_dense( it.row(), it.col() ) = it.value();
                    }
                    // I don't know how to use the SVD module...
                    Eigen::SVD< Eigen::MatrixXd > svd( hessian_dense );
                    svd.compute( hessian_dense );
                    dbg_out << "Hessian minimum singular value: " << svd.singularValues().minCoeff() << '\n';
                    dbg_out << "Hessian singular values:\n";
                    for( int i = 0; i < svd.singularValues().size(); ++i ) dbg_out << svd.singularValues()(i) << ' ';
                    dbg_out << '\n';
                    // Why doesn't this work?
                    // dbg_out << "Hessian determinant: " << hessian_dense.determinant() << '\n';
                    dbg_out << "Hessian number of non-zeros: " << hess->nonZeros() << '\n';
                }
            }
            
            m_callback_theta = 0;
            
            if( kDebugHere ) dbg_out << "--- int tnc_callback( double thetas[], double* f, double g[] )\n";
        }
        
    private:
        // The parameter determine whether we are computing the gradient (if g is true) or the
        // Hessian (hess is true) along with the energy.
        void ComputeAndCache_f_and_derivatives( const bool g, const bool hess ) const
        {
            // Cache f_i() lookup.
            // Don't duplicate work on ts.
            std::valarray< real_t > ts( 0., m_num_vertices );
            assert( m_is_fixed.size() == m_num_vertices );
            for( int i = 0; i < m_num_vertices; ++i )
            {
                if( m_is_fixed[i] ) continue;
                
                ts[i] = t_i(i);
            }
            // for( root in roots )
            // for( parent_to_child_map_t::const_iterator it = m_children.begin(); it != m_children.end(); ++it )
            assert( m_is_root.size() == m_num_vertices );
            for( index_t root = 0; root < m_is_root.size(); ++root )
            {
                // const index_t root = it->first;
                // Unfortunately, this will be true for every fixed node.
                // if( root != m_parent_node[ root ] ) continue;
                if( !m_is_root[ root ] ) continue;
                
                DFS_cache_f_i_root( root, &ts[0] );
            }
            
            // These derivatives depend on 'thetas', so we have to invalidate our cache when
            // we get new thetas.
            if( g || hess )
            {
                // We can calculate the gradient directly.
                // First allocate zero vectors.
                // We also need to allocate zero vectors for fixed nodes, so we can
                // get their gradients without extra checks.
                for( int i = 0; i < m_df_i.size(); ++i )
                {
                    sparse_vector_t& df_i = m_df_i[ i ];
                    if( df_i.size() == m_num_theta ) df_i.setZero();
                    else df_i = sparse_vector_t( m_num_theta );
                }
                
                // If we're not computing the hessian, just compute the gradient.
                if( !hess )
                {
                    // Fixed nodes are constant, so skip them.
                    // Each node builds from the previous, so seed it.
                    // Every node seeds its subtree.
                    for( int i = 0; i < m_num_vertices; ++i )
                    {
                        if( m_is_fixed[i] ) continue;
                        
                        DFS_build_df_vector_seed( i, &ts[0] );
                    }
                }
                // Otherwise, compute them both at the same time.
                else
                {
                    // Is this the first iteration?  We need to initialize the sparsity pattern using a DynamicSparseMatrix.
                    if( !m_d2f_i_sparsity_set )
                    {
                        // Create a bunch of m_num_theta x m_num_theta dynamic sparse matrices.
                        // std::vector< assembly_matrix_t > d2f_i_first( m_d2f_i.size() );
                        // m_d2f_i is a raw array now.
                        std::vector< assembly_matrix_t > d2f_i_first( m_df_i.size() );
                        // assert( d2f_i_first.size() == m_d2f_i.size() );
                        // for( int i = 0; i < m_d2f_i.size(); ++i )
                        for( int i = 0; i < m_df_i.size(); ++i )
                        {
                            // We also need to allocate zero vectors for fixed nodes, so we can
                            // get their gradients without extra checks.
                            // if( m_is_fixed[i] ) continue;
                            
                            d2f_i_first[i].resize( m_num_theta, m_num_theta );
                        }
                        
                        // Fill them with hessians.
                        assert( d2f_i_first.size() == m_num_vertices );
                        for( int i = 0; i < m_num_vertices; ++i )
                        {
                            if( m_is_fixed[i] ) continue;
                            
                            DFS_build_df_vector_and_d2f_matrix_seed( &d2f_i_first[0], i, &ts[0] );
                        }
                        
                        // Copy the result to our non-dynamic sparse matrices.
                        for( int i = 0; i < m_df_i.size(); ++i )
                        {
                            // We also need to allocate zero vectors for fixed nodes, so we can
                            // get their gradients without extra checks.
                            // if( m_is_fixed[i] ) continue;
                            
                            m_d2f_i[i] = d2f_i_first[i];
                        }
                        
                        m_d2f_i_sparsity_set = true;
                    }
                    else
                    {
                        // The sparsity pattern is set; just overwrite values.
                        assert( m_d2f_i_sparsity_set );
                        
                        // Fixed nodes are constant, so skip them.
                        // Each node builds from the previous, so seed it.
                        // Every node seeds its subtree.
                        for( int i = 0; i < m_num_vertices; ++i )
                        {
                            if( m_is_fixed[i] ) continue;
                            DFS_build_df_vector_and_d2f_matrix_seed( &m_d2f_i[0], i, &ts[0] );
                        }
                    }
                }
            }
// We pass this test, so turn it off for now.
#define VERIFY_DIRECT_GRADIENT_AND_HESSIAN_COMPUTATION 0
#if VERIFY_DIRECT_GRADIENT_AND_HESSIAN_COMPUTATION
#ifndef NDEBUG
            if( g || hess )
            {
                assert( m_df_i.size() == m_num_vertices );
                for( int i = 0; i < m_num_vertices; ++i )
                {
                    if( m_is_fixed.at(i) )
                    {
                        assert( m_df_i.at(i).nonZeros() == 0 );
                        continue;
                    }
                    
                    for( int dtheta = 0; dtheta < m_num_theta; ++dtheta )
                    {
                        assert(
                            fabs( f_i_d_theta_j( i, m_theta2node[ dtheta ] ) ) < kEps ||
                            fabs( m_df_i.at(i).coeff( dtheta ) - f_i_d_theta_j( i, m_theta2node[ dtheta ] ) ) < kEps
                            );
                    }
                }
            }
            if( hess )
            {
                // assert( m_d2f_i.size() == m_num_vertices );
                assert( m_is_fixed.size() == m_num_vertices );
                for( int i = 0; i < m_num_vertices; ++i )
                {
                    if( m_is_fixed[i] )
                    {
                        assert( m_d2f_i[i].nonZeros() == 0 );
                        continue;
                    }
                    
                    for( int dtheta_j = 0; dtheta_j < m_num_theta; ++dtheta_j )
                    for( int dtheta_k = 0; dtheta_k < m_num_theta; ++dtheta_k )
                    {
                        assert(
                            fabs( f_i_d_theta_j_theta_k( i, m_theta2node[ dtheta_j ], m_theta2node[ dtheta_k ] ) ) < kEps ||
                            fabs( m_d2f_i[i].coeff( dtheta_j, dtheta_k ) - f_i_d_theta_j_theta_k( i, m_theta2node[ dtheta_j ], m_theta2node[ dtheta_k ] ) ) < kEps
                            );
                    }
                }
            }
#endif
#endif
        }
        
        void DFS_cache_f_i_root( const index_t root, const real_t* ts ) const
        {
            assert( ts );
            assert( root >= 0 && root < m_num_vertices );
            // Roots must be fixed.
            assert( m_is_fixed[ root ] );
            assert( m_is_root[ root ] );
            
            const real_t current_val = m_original_vertex_values[ root ];
            m_f_i[ root ] = current_val;
            
            // for( child_node in children[ node_index ] )
            const children_t& children = m_children[ root ];
            // Root nodes must have children.
            assert( !children.empty() );
            for( children_t::const_iterator child = children.begin(); child != children.end(); ++child )
            {
                DFS_cache_f_i_child( *child, current_val, ts );
            }
        }
        void DFS_cache_f_i_child( const index_t node_i, real_t current_val, const real_t* ts ) const
        {
            assert( ts );
            
            // We're done once we reach a fixed node.
            if( is_fixed( node_i ) ) return;
            
            const real_t& t = ts[ node_i ];
            const real_t max_leaf_val = max_leaf_value( node_i );
            
            current_val = max_leaf_val + t * ( current_val - max_leaf_val );
            m_f_i[ node_i ] = current_val;
            
            // for( child_node in children[ node_index ] )
            const children_t& children = m_children[ node_i ];
            // Free nodes must have children.
            assert( !children.empty() );
            for( children_t::const_iterator child = children.begin(); child != children.end(); ++child )
            {
                DFS_cache_f_i_child( *child, current_val, ts );
            }
        }
        void DFS_build_df_vector_seed( const node_t seed_node, const real_t* ts ) const
        {
            assert( seed_node >= 0 && seed_node < m_num_vertices );
            assert( !is_fixed( seed_node ) );
            
            const index_t dtheta_index = m_node2theta[ seed_node ];
            assert( kInvalidIndex != dtheta_index );
            
            const real_t seed_val = f_i_d_theta_j_nonconstant( seed_node, seed_node );
            
            sparse_vector_t& df_seed = m_df_i[ seed_node ];
            df_seed.coeffRef( dtheta_index ) = seed_val;
            
            // for( child_node in children[ seed_node ] )
            // Free nodes must have children.
            // assert( m_children.find( seed_node ) != m_children.end() );
            // const children_t& children = m_children.find( seed_node )->second;
            const children_t& children = m_children[ seed_node ];
            assert( !children.empty() );
            for( children_t::const_iterator child = children.begin(); child != children.end(); ++child )
            {
                DFS_build_df_vector_child( dtheta_index, *child, seed_val, ts );
            }
        }
        void DFS_build_df_vector_child( const index_t dtheta_index, const node_t node_index, real_t current_val, const real_t* ts ) const
        {
            assert( dtheta_index >= 0 && dtheta_index < m_num_theta );
            assert( node_index >= 0 && node_index < m_num_vertices );
            
            if( m_is_fixed[ node_index ] ) return;
            
            current_val *= ts[ node_index ];
            m_df_i[ node_index ].coeffRef( dtheta_index ) = current_val;
            
            // for( child_node in children[ node_index ] )
            const children_t& children = m_children[ node_index ];
            // Free nodes must have children.
            assert( !children.empty() );
            for( children_t::const_iterator child = children.begin(); child != children.end(); ++child )
            {
                DFS_build_df_vector_child( dtheta_index, *child, current_val, ts );
            }
        }
        template< typename mat_t >
        void DFS_build_df_vector_and_d2f_matrix_seed( mat_t* matrices, const node_t seed_node, const real_t* ts ) const
        {
            assert( seed_node >= 0 && seed_node < m_num_vertices );
            assert( !is_fixed( seed_node ) );
            
            const index_t dtheta_index = m_node2theta[ seed_node ];
            assert( kInvalidIndex != dtheta_index );
            
            const real_t seed_gradient_val = f_i_d_theta_j_nonconstant( seed_node, seed_node );
            const real_t seed_hess_diag_val = f_i_d_theta_j_theta_k_nonconstant( seed_node, seed_node, seed_node );
            const real_t seed_hess_nondiag_val = seed_gradient_val;
            
            sparse_vector_t& df_seed = m_df_i[ seed_node ];
            df_seed.coeffRef( dtheta_index ) = seed_gradient_val;
            
            matrices[ seed_node ].coeffRef( dtheta_index, dtheta_index ) = seed_hess_diag_val;
            
            // for( child_node in children[ seed_node ] )
            // Free nodes must have children.
            // assert( m_children.find( seed_node ) != m_children.end() );
            // const children_t& children = m_children.find( seed_node )->second;
            const children_t& children = m_children[ seed_node ];
            assert( !children.empty() );
            for( children_t::const_iterator child = children.begin(); child != children.end(); ++child )
            {
                DFS_build_df_vector_and_d2f_matrix_child( matrices, dtheta_index, kInvalidIndex, *child, seed_gradient_val, seed_hess_diag_val, seed_hess_nondiag_val, ts );
            }
        }
        template< typename mat_t >
        void DFS_build_df_vector_and_d2f_matrix_child( mat_t* matrices, const index_t dtheta_j_index, index_t dtheta_k_index, const node_t node_index, const real_t parent_grad_val, real_t current_hess_diag_val, real_t current_hess_nondiag_val, const real_t* ts ) const
        {
            assert( dtheta_j_index >= 0 && dtheta_j_index < m_num_theta );
            assert( kInvalidIndex == dtheta_k_index || (dtheta_k_index >= 0 && dtheta_k_index < m_num_theta) );
            assert( node_index >= 0 && node_index < m_num_vertices );
            
            if( m_is_fixed[ node_index ] ) return;
            
            // for( child_node in children[ node_index ] )
            const children_t& children = m_children[ node_index ];
            // Free nodes must have children.
            assert( !children.empty() );
            
            // If we haven't pivoted yet on an off-diagonal derivative, compute the diagonal elements
            // and spawn an off-diagonal pivot.
            if( kInvalidIndex == dtheta_k_index )
            {
                const real_t current_grad_val = parent_grad_val * ts[ node_index ];
                m_df_i[ node_index ].coeffRef( dtheta_j_index ) = current_grad_val;
                
                current_hess_diag_val *= ts[ node_index ];
                matrices[ node_index ].coeffRef( dtheta_j_index, dtheta_j_index ) = current_hess_diag_val;
                
                current_hess_nondiag_val = parent_grad_val * dt_i( node_index );
                dtheta_k_index = m_node2theta[ node_index ];
                matrices[ node_index ].coeffRef( dtheta_j_index, dtheta_k_index ) = current_hess_nondiag_val;
                matrices[ node_index ].coeffRef( dtheta_k_index, dtheta_j_index ) = current_hess_nondiag_val;
                
                for( children_t::const_iterator child = children.begin(); child != children.end(); ++child )
                {
                    // The diagonal recursion
                    DFS_build_df_vector_and_d2f_matrix_child( matrices, dtheta_j_index, kInvalidIndex, *child, current_grad_val, current_hess_diag_val, -1., ts );
                    // The non-diagonal recursion
                    DFS_build_df_vector_and_d2f_matrix_child( matrices, dtheta_j_index, dtheta_k_index, *child, current_grad_val, current_hess_diag_val, current_hess_nondiag_val, ts );
                }
            }
            // Otherwise, just compute the non-diagonal element.
            else
            {
                current_hess_nondiag_val *= ts[ node_index ];
                matrices[ node_index ].coeffRef( dtheta_j_index, dtheta_k_index ) = current_hess_nondiag_val;
                matrices[ node_index ].coeffRef( dtheta_k_index, dtheta_j_index ) = current_hess_nondiag_val;
                
                for( children_t::const_iterator child = children.begin(); child != children.end(); ++child )
                {
                    // Only a non-diagonal recursion
                    DFS_build_df_vector_and_d2f_matrix_child( matrices, dtheta_j_index, dtheta_k_index, *child, -1., -1., current_hess_nondiag_val, ts );
                }
            }
        }
        
    private:
        bool is_fixed( node_t node ) const
        {
            assert( node >= 0 && node < m_num_vertices );
            return m_is_fixed[ node ];
        }
        real_t theta( node_t node ) const
        {
            assert( !is_fixed( node ) );
            assert( m_callback_theta );
            return m_callback_theta[ m_node2theta[ node ] ];
        }
        real_t fixed_value( node_t node ) const
        {
            assert( is_fixed( node ) );
            return m_original_vertex_values[ node ];
        }
        real_t max_leaf_value( node_t node ) const
        {
            assert( node >= 0 && node < m_num_vertices );
            return m_max_leaf_value[ node ];
        }
        node_t parent_node( node_t node ) const
        {
            assert( node >= 0 && node < m_num_vertices );
            return m_parent_node[ node ];
        }
        bool is_ancestor( node_t old, node_t young ) const
        {
            assert( old >= 0 && old < m_num_vertices );
            assert( young >= 0 && young < m_num_vertices );
            
#if CACHE_IS_ANCESTOR
            const std::pair< node_t, node_t > the_pair = std::make_pair( old, young );
            
            // This requires only one lookup if we have already cached the result.
            const is_ancestor_map_t::const_iterator found = m_is_ancestor.find( the_pair );
            if( found != m_is_ancestor.end() ) return found->second;
#endif
            
            bool result = false;
            
            index_t parent = young;
            while( m_parent_node[ parent ] != parent )
            {
                parent = m_parent_node[ parent ];
                if( old == parent )
                {
                    result = true;
                    break;
                }
            }
            
#if CACHE_IS_ANCESTOR
            m_is_ancestor[ the_pair ] = result;
#endif
            return result;
        }
        
        void add_E_and_gradient_and_hessian( double* e, double g[], hessian_t* h ) const
        {
            assert( e );
            
            real_t* grad_df_coeffs = 0;
            if( g )
            {
                grad_df_coeffs = new real_t[ m_num_theta ];
                std::fill( grad_df_coeffs, grad_df_coeffs + m_num_theta, 0. );
            }
            
            real_t* hess_d2f_coeffs = 0;
            if( h )
            {
                hess_d2f_coeffs = new real_t[ m_num_theta ];
                std::fill( hess_d2f_coeffs, hess_d2f_coeffs + m_num_theta, 0. );
            }
            
            assembly_matrix_t* hess_dfTdf_coeffs = 0;
            if( h )
            {
                hess_dfTdf_coeffs = new assembly_matrix_t( m_num_theta, m_num_theta );
                // Every degree of freedom's one ring, squared.
                // TODO: Which is correct: .resizeNonZeros( nnz ) or { .startFill( nnz ); .endFill(); }?
                // A1: DynamicSparseMatrix's don't have .resizeNonZeros()
                // TODO Q: Who is using way too much memory?  Let's not reserve for now...
                // hess_dfTdf_coeffs->resizeNonZeros( m_num_theta * 6*6 );
                // hess_dfTdf_coeffs->startFill( m_num_theta * 6*6 );
                // hess_dfTdf_coeffs->endFill();
                // std::cerr << "hess_dfTdf_coeffs: " << ( m_num_theta * 6*6 ) << " entries.\n";
            }
            
            // Our energy will be the sum of laplacian editing energies at every node.
            // NOTE: We only consider nodes whose energy computation includes a degree-of-freedom,
            //       free nodes or neighbors of free nodes, but this short-circuiting occurs
            //       inside the call to add_E_laplacian_editing_i_and_gradient().
            //       We created a vector of non-fixed_fixed nodes 'm_not_fixed_fixed' in our constructor.
            // for( int i = 0; i < m_num_vertices; ++i )
            std::vector< index_t > neighbors;
            // reserve enough space for a high valence.
            neighbors.reserve( 20 );
            for( int i = 0; i < m_not_fixed_fixed.size(); ++i )
            {
                add_E_laplacian_i_and_gradient_and_hessian( e, grad_df_coeffs, hess_d2f_coeffs, hess_dfTdf_coeffs, m_not_fixed_fixed[i], neighbors, m_laplacian_editing_not_laplacian );
                
                if( 0. < m_E_diff_weight )
                {
                    add_E_diff_i_and_gradient_and_hessian( e, grad_df_coeffs, hess_d2f_coeffs, hess_dfTdf_coeffs, m_not_fixed_fixed[i], m_E_diff_weight );
                }
            }
            
            if( g )
            {
                assert( grad_df_coeffs );
                
                for( int i = 0; i < m_num_theta; ++i )
                {
                    const real_t coeff = grad_df_coeffs[i];
                    
                    for( sparse_vector_t::InnerIterator it( df_i( m_theta2node[ i ] ) ); it; ++it )
                    {
                        g[ it.index() ] += coeff * it.value();
                    }
                }
            }
            
            if( h )
            {
                assert( hess_d2f_coeffs );
                assert( hess_dfTdf_coeffs );
                
                assert( hess_dfTdf_coeffs->rows() == m_num_theta );
                assert( hess_dfTdf_coeffs->cols() == m_num_theta );
                
                int nnz = m_last_hess_nnz;
                if( -1 == m_last_hess_nnz )
                {
                    /*
                    // assert( m_num_vertices == m_d2f_i.size() );
                    for( int i = 0; i < m_num_vertices; ++i )
                    {
                        nnz += d2f_i( i ).nonZeros();
                    }
                    // I think the dfTdf will be a subset of these.
                    */
                    // nnz = m_num_theta * m_num_edges * 2;
                    // There will be an entry for every directed edge.
                    // There will also be entries for each free vertex's neighbors (df^T df).
                    nnz = m_num_edges + m_num_theta*6;
                    // nnz = m_num_edges + m_num_theta;
                    // I seem to be totally off.  Guess low, so I don't allocate gigabytes, and then
                    // re-use the value.
                }
                assert( nnz > 0 );
                
#if 1
                // All column values.
                sparse_vector_t col_vals( m_num_theta );
                // Reserve the maximum size, so we don't do any memory allocation with this.
                col_vals.reserve( m_num_theta );
                // TODO Q: Who is using way too much memory?  Let's not reserve for now...
                // std::cerr << "h: " << ( nnz ) << " entries.\n";
                // h->startFill( nnz );
                h->startFill();
                for( int row = 0; row < m_num_theta; ++row )
                {
                    col_vals.setZero();
                    
                    // Add the df^T*df rows.
                    assert( hess_dfTdf_coeffs->rows() == m_num_theta );
                    assert( hess_dfTdf_coeffs->cols() == m_num_theta );
                    for( int k = 0; k < hess_dfTdf_coeffs->outerSize(); ++k )
                    for( assembly_matrix_t::InnerIterator it( *hess_dfTdf_coeffs, k ); it; ++it )
                    {
                        const index_t v_row = m_theta2node[ it.row() ];
                        const index_t v_col = m_theta2node[ it.col() ];
                        assert( v_row >= 0 && v_row < m_num_vertices );
                        assert( v_col >= 0 && v_col < m_num_vertices );
                        
                        const sparse_vector_t& df_row = df_i( v_row );
                        const sparse_vector_t& df_col = df_i( v_col );
                        
                        const real_t df_row_row = df_row.coeff( row );
                        // This check tells us if there's anything at the entry.
                        if( df_row_row != real_t(0) )
                        {
                            for( sparse_vector_t::InnerIterator cit( df_col ); cit; ++cit )
                            {
                                col_vals.coeffRef( cit.index() ) += it.value() * df_row_row * cit.value();
                            }
                        }
                        
                        // If we're off-diagonal, also add the transpose.
                        if( v_row != v_col )
                        {
                            const real_t df_col_row = df_col.coeff( row );
                            // This check tells us if there's anything at the entry.
                            if( df_col_row != real_t(0) )
                            {
                                for( sparse_vector_t::InnerIterator cit( df_row ); cit; ++cit )
                                {
                                    col_vals.coeffRef( cit.index() ) += it.value() * df_col_row * cit.value();
                                }
                            }
                        }
                    }
                    
                    // Add the d2f rows.
                    for( int i = 0; i < m_num_theta; ++i )
                    {
                        const real_t d2f_coeff = hess_d2f_coeffs[i];
                        
                        for( solve_matrix_t::InnerIterator it( d2f_i( m_theta2node[ i ] ), row ); it; ++it )
                        {
                            assert( it.row() == row );
                            col_vals.coeffRef( it.index() ) += d2f_coeff * it.value();
                        }
                    }
                    
                    // Now insert the row into the Hessian matrix.
                    for( sparse_vector_t::InnerIterator it( col_vals ); it; ++it )
                    {
                        h->fill( row, it.index() ) = it.value();
                    }
                }
                h->endFill();
                
                // Trying to figure out how many nonzeros h will have.
                // std::cerr << "num_edges: " << m_num_edges << '\n';
                // std::cerr << "num_theta: " << m_num_theta << '\n';
                // std::cerr << "num_vertices: " << m_num_vertices << '\n';
                m_last_hess_nnz = h->nonZeros();
#else
                
                // TODO: Which is correct: .resizeNonZeros( nnz ) or { .startFill( nnz ); .endFill(); }?
                // A1: DynamicSparseMatrices don't have resizeNonZeros(); let's be consistent?
                // h->resizeNonZeros( nnz );
                h->startFill( nnz );
                h->endFill();
                
                for( int k = 0; k < hess_dfTdf_coeffs->outerSize(); ++k )
                for( assembly_matrix_t::InnerIterator it( *hess_dfTdf_coeffs, k ); it; ++it )
                {
                    const index_t v_row = m_theta2node.at( it.row() );
                    const index_t v_col = m_theta2node.at( it.col() );
                    assert( v_row >= 0 && v_row < m_num_vertices );
                    assert( v_col >= 0 && v_col < m_num_vertices );
                    
                    // (*h) += df_i( v_row ).transpose() * (it.value() * df_i( v_col ));
                    
                    // solve_matrix_t outer( df_i( v_row ).transpose() * df_i( v_col ) );
                    // outer *= it.value();
                    
                    
                    solve_matrix_t outer( df_i( v_row ).transpose() * ( it.value() * df_i( v_col ) ) );
                    (*h) += outer;
                    // I only store the upper triangle now, so I need to add the transpose.
                    if( v_row != v_col )
                    {
                        // I don't think this will work without a *second* copy.  I wish I had auto:
                        /*
                            auto outer = foo.transpose() * foo;
                            *h += outer;
                            *h += solve_matrix_t( outer.transpose() )
                        */
                        (*h) += outer.transpose();
                    }
                }
                
                for( int i = 0; i < m_num_theta; ++i )
                {
                    const real_t coeff = hess_d2f_coeffs[i];
                    // Hmm, actually, d2f_i aren't going to be used after this point...
                    // m_d2f_i[ m_theta2node[ i ] ] *= coeff;
                    // (*h) += m_d2f_i[ m_theta2node[ i ] ];
                    (*h) += coeff * d2f_i( m_theta2node[ i ] );
                }
#endif
            }
            
// The hessian returned will be the identity matrix, to isolate memory usage --- the above code, or subsequent copies.
#if 0
            if( h )
            {
                // h->setIdentity( m_num_theta, m_num_theta );
                h->setZero();
                const int N = m_num_theta;
                h->startFill( N );
                for( int i = 0; i < N; ++i ) h->fill( i,i ) = 1.;
                h->endFill();
            }
#endif
            
            delete [] grad_df_coeffs;
            delete [] hess_d2f_coeffs;
            delete hess_dfTdf_coeffs;
        }
        void add_E_diff_i_and_gradient_and_hessian( real_t* e, real_t* grad_df_coeffs, real_t* hess_d2f_coeffs, assembly_matrix_t* hess_dfTdf_coeffs, const node_t vertex_index, const real_t weight ) const
        {
            assert( e );
            assert( vertex_index >= 0 && vertex_index < m_num_vertices );
            assert( weight > 0. );
            assert( bool( hess_d2f_coeffs ) == bool( hess_dfTdf_coeffs ) );
            
            // If we're a fixed node, don't do anything.
            if( m_is_fixed.at( vertex_index ) ) return;
            
            const bool g = grad_df_coeffs;
            const bool hess = hess_d2f_coeffs;
            
            // We need to compute the area around the vertex.
            assert( m_area.size() == m_num_vertices );
            assert( m_area_dirty.size() == m_num_vertices );
            if( m_area_dirty[ vertex_index ] )
            {
                std::vector< index_t > neighbors;
                m_mesh.vertex_vertex_neighbors( vertex_index, neighbors );
                const uint valence = neighbors.size();
                
                const bool has_boundary = m_mesh.vertex_is_boundary( vertex_index );
                // Q: Does my halfedge data structure make sure that the outgoing boundary halfedge at a vertex is always first (and last)?
                // A: Yes!
                assert(
                    !has_boundary || (
                        m_mesh.halfedge( m_mesh.directed_edge2he_index( vertex_index, neighbors.at(0) ) ).face == -1 &&
                        m_mesh.halfedge( m_mesh.halfedge( m_mesh.directed_edge2he_index( vertex_index, neighbors.back() ) ).opposite_he ).face == -1
                        )
                    );
                
                const vec3_t p_center = vec3_t( &m_vertices_xy0[ 3*vertex_index ] );
                std::valarray< vec3_t > p( valence );
                for( uint i = 0; i < valence; ++i ) p[i] = vec3_t( &m_vertices_xy0[ 3*( neighbors[i] ) ] );
                
                // A is total area of one-ring / 3 (or something more complicated if AREA_MIXED is defined).
                // cot( alpha ) is the angle opposite the v[] vector in the increasing index direction.
                std::valarray< real_t > cot_alpha( valence );
                // cot( beta ) is the angle opposite the v[] vector in the decreasing index direction.
                std::valarray< real_t > cot_beta( valence );
                cotangentWeightsT( p_center, &p[0], valence, has_boundary, &cot_alpha[0], &cot_beta[0], &m_area[ vertex_index ] );
                m_area_dirty[ vertex_index ] = false;
            }
            const real_t A( m_area[ vertex_index ] );
            
            // Our surface preserving term: the difference between the current and original value.
            // E = weight * ( v[i] - v'[i] )^2
            const real_t diff = ( f_i( vertex_index ) - m_original_vertex_values[ vertex_index ] );
            // energy
            real_t E = weight * diff*diff * A;
            *e += E;
            
            const index_t theta_i = m_node2theta.at( vertex_index );
            assert( theta_i >= 0 && theta_i < m_num_theta );
            
            // gradient
            if( g )
            {
                grad_df_coeffs[ theta_i ] += (2.*weight*diff*A);
            }
            
            // hessian
            if( hess )
            {
                // d2E = (2.*weight*A) * ( solve_matrix_t( df.transpose() * df )  +  diff * d2f );
                
                const real_t hcoeff = (2.*weight*A);
                hess_d2f_coeffs[ theta_i ] += hcoeff * diff;
                hess_dfTdf_coeffs->coeffRef( theta_i, theta_i ) += hcoeff;
            }
        }
        void add_E_laplacian_i_and_gradient_and_hessian( real_t* e, real_t* grad_df_coeffs, real_t* hess_d2f_coeffs, assembly_matrix_t* hess_dfTdf_coeffs, const node_t vertex_index, std::vector< index_t >& neighbors, const bool laplacian_editing_not_laplacian = false ) const
        {
            assert( e );
            assert( vertex_index >= 0 && vertex_index < m_num_vertices );
            assert( bool( hess_d2f_coeffs ) == bool( hess_dfTdf_coeffs ) );
            
            const bool g = grad_df_coeffs;
            const bool hess = hess_d2f_coeffs;
            
            m_mesh.vertex_vertex_neighbors( vertex_index, neighbors );
            const uint valence = neighbors.size();
            neighbors.push_back( vertex_index );
            
            const bool has_boundary = m_mesh.vertex_is_boundary( vertex_index );
            // Q: Does my halfedge data structure make sure that the outgoing boundary halfedge at a vertex is always first (and last)?
            // A: Yes!
            assert(
                !has_boundary || (
                    m_mesh.halfedge( m_mesh.directed_edge2he_index( vertex_index, neighbors.at(0) ) ).face == -1 &&
                    m_mesh.halfedge( m_mesh.halfedge( m_mesh.directed_edge2he_index( vertex_index, neighbors.at( valence-1 ) ) ).opposite_he ).face == -1
                    )
                );
            
            const vec3_t p_center = vec3_t( &m_vertices_xy0[ 3*vertex_index ] );
            std::valarray< vec3_t > p( valence );
            for( uint i = 0; i < valence; ++i ) p[i] = vec3_t( &m_vertices_xy0[ 3*( neighbors[i] ) ] );
            
            // A is total area of one-ring / 3 (or something more complicated if AREA_MIXED is defined).
            real_t A( 0. );
            // cot( alpha ) is the angle opposite the v[] vector in the increasing index direction.
            std::valarray< real_t > cot_alpha( valence );
            // cot( beta ) is the angle opposite the v[] vector in the decreasing index direction.
            std::valarray< real_t > cot_beta( valence );
            cotangentWeightsT( p_center, &p[0], valence, has_boundary, &cot_alpha[0], &cot_beta[0], &A );
            
            const real_t pd_center = f_i( vertex_index );
            
            // gradient
            const int gradN = ( g || hess ) ? ( valence + 1 ) : 0;
            
            // hessian
            const int hessN = hess ? ( valence + 1 ) : 0;
            
            // Laplacian surface editing energy: the difference in Laplacian between
            // the deformed and undeformed configurations.
            // E = ( \sum -.5*( cot alpha_i + cot beta_i )*( v[i] - v'[i] ) )^2
            // NOTE: We only care about v[i].z - v'[i].z.  This is because v[i].xy = v'[i].xy,
            //       so the difference is always 0.
            real_t E_lse_sqrt = 0;
            // gradient
            // Coefficients for the df of each neighbor and the center vertex.
            // The center vertex coefficient comes at the end.
            std::valarray< real_t > dE_df_coeffs( 0., gradN );
            assert( dE_df_coeffs.size() == gradN );
            // hessian
            // Coefficients for the d2f of each neighbor and the center vertex.
            // The center vertex coefficient comes at the end.
            std::valarray< real_t > d2E_d2f_coeffs( 0., hessN );
            assert( d2E_d2f_coeffs.size() == hessN );
            // Coefficients for the df^T * df of each neighbor and the center vertex.
            // The center vertex coefficient comes at the end.
            std::valarray< real_t > d2E_dfTdf_coeffs( 0., hessN );
            assert( d2E_dfTdf_coeffs.size() == hessN );
            
            // The sum of 'coeff' for all vertices.  I think it won't be 1 if we are at a boundary.
            real_t coeff_sum = 0.;
            for( uint i = 0 ; i < valence ; ++i )
            {
                const real_t p_undeformed_diff =
                    laplacian_editing_not_laplacian
                    ? ( m_original_vertex_values[ neighbors[i] ] - m_original_vertex_values[ vertex_index ] )
                    : 0.
                    ;
                
                // E_lse_sqrt += -.5 * (cot_alpha[i] + cot_beta[i]) * ((pd[i] - pd_center) - (p[i] - p_center));
                const real_t coeff = -.5 * (cot_alpha[i] + cot_beta[i]);
                E_lse_sqrt += coeff * (( f_i( neighbors[i] ) - pd_center ) - p_undeformed_diff);
                
                // gradient
                if( g || hess )
                {
                    dE_df_coeffs[ i ] += coeff;
                    
                    // Add the center element once at the end.
                    coeff_sum += coeff;
                }
                
                // hessian
                if( hess )
                {
                    d2E_d2f_coeffs[ i ] += coeff;
                    // Add the center element once at the end.
                }
            }
            if( g || hess )
            {
                dE_df_coeffs[ valence ] = -coeff_sum;
            }
            if( hess )
            {
                d2E_d2f_coeffs[ valence ] = -coeff_sum;
            }
            
            // energy
            // real_t E_lse = dot( E_lse_sqrt, E_lse_sqrt ) / (A);
            real_t E_lse = ( E_lse_sqrt*E_lse_sqrt ) / (A);
            *e += E_lse;
            
            // hessian
            if( hess )
            {
                // d2/dx2 f(x)^2 = d/dx 2 f(x) f'(x) dx = 2 ( f'(x)^2 d2x + f(x) f''(x) d2x )
                // NOTE: We must compute the hessian before the gradient, because the gradient
                //       computation is in-place and we need to use the gradient current as of
                //       exiting the loop.
                // d2E = ( 2./A ) * ( solve_matrix_t( dE.transpose() * dE )  +  E_lse_sqrt * d2E );
                const real_t hcoeff = 2./A;
                d2E_d2f_coeffs *= hcoeff * E_lse_sqrt;
                assert( d2E_d2f_coeffs.size() == valence + 1 );
                // We have stashed 'vertex_index' at the end of neighbors.
                assert( d2E_d2f_coeffs.size() == neighbors.size() );
                for( int i = 0; i < neighbors.size(); ++i )
                {
                    const index_t theta_i = m_node2theta.at( neighbors.at( i ) );
                    // constant nodes have zero hessians.
                    if( kInvalidIndex == theta_i ) continue;
                    
                    assert( theta_i >= 0 && theta_i < m_num_theta );
                    hess_d2f_coeffs[ theta_i ] += d2E_d2f_coeffs[i];
                }
                
                // The outer product of dE with itself is the double for loop of the elements in the
                // gradient:
                assert( dE_df_coeffs.size() == neighbors.size() );
                assert( hess_dfTdf_coeffs->rows() == m_num_theta );
                assert( hess_dfTdf_coeffs->cols() == m_num_theta );
                for( int i = 0; i < neighbors.size(); ++i )
                {
                    const index_t theta_i = m_node2theta.at( neighbors.at( i ) );
                    // constant nodes have zero gradients.
                    if( kInvalidIndex == theta_i ) continue;
                    
                    for( int j = i+1; j < neighbors.size(); ++j )
                    {
                        const index_t theta_j = m_node2theta.at( neighbors.at( j ) );
                        // constant nodes have zero gradients.
                        if( kInvalidIndex == theta_j ) continue;
                        
                        assert( theta_i >= 0 && theta_i < m_num_theta );
                        assert( theta_j >= 0 && theta_j < m_num_theta );
                        
                        // Just store the upper triangle, and know that it is symmetric and
                        // each entry c = mat(i,j) should be multiplied
                        // c*df(i).T*df(j) as well as c*df(j).T*df(i).
                        const real_t val = hcoeff * dE_df_coeffs[i] * dE_df_coeffs[j];
                        hess_dfTdf_coeffs->coeffRef( theta_i, theta_j ) += val;
                        // hess_dfTdf_coeffs->coeffRef( theta_j, theta_i ) += val;
                    }
                }
                assert( dE_df_coeffs.size() == neighbors.size() );
                for( int i = 0; i < neighbors.size(); ++i )
                {
                    const index_t theta_i = m_node2theta.at( neighbors.at( i ) );
                    // constant nodes have zero gradients.
                    if( kInvalidIndex == theta_i ) continue;
                    
                    const real_t val = hcoeff * dE_df_coeffs[i] * dE_df_coeffs[i];
                    hess_dfTdf_coeffs->coeffRef( theta_i, theta_i ) += val;
                }
            }
            
            // gradient
            if( g )
            {
                dE_df_coeffs *= 2.*E_lse_sqrt / (A);
                
                // We have stashed 'vertex_index' at the end of neighbors.
                assert( dE_df_coeffs.size() == neighbors.size() );
                for( int i = 0; i < neighbors.size(); ++i )
                {
                    const index_t theta_i = m_node2theta.at( neighbors.at( i ) );
                    // constant nodes have zero gradients.
                    if( kInvalidIndex == theta_i ) continue;
                    
                    grad_df_coeffs[ theta_i ] += dE_df_coeffs[i];
                }
            }
        }
        void add_E_laplacian_i_and_gradient( double* e, double g[], const node_t vertex_index, const bool laplacian_editing_not_laplacian = false ) const
        {
            assert( e );
            assert( vertex_index >= 0 && vertex_index < m_num_vertices );
            
            std::vector< index_t > neighbors;
            m_mesh.vertex_vertex_neighbors( vertex_index, neighbors );
            const uint valence = neighbors.size();
            
            const bool has_boundary = m_mesh.vertex_is_boundary( vertex_index );
            // Q: Does my halfedge data structure make sure that the outgoing boundary halfedge at a vertex is always first (and last)?
            // A: Yes!
            assert(
                !has_boundary || (
                    m_mesh.halfedge( m_mesh.directed_edge2he_index( vertex_index, neighbors.at(0) ) ).face == -1 &&
                    m_mesh.halfedge( m_mesh.halfedge( m_mesh.directed_edge2he_index( vertex_index, neighbors.at( valence-1 ) ) ).opposite_he ).face == -1
                    )
                );
            
            const vec3_t p_center = vec3_t( &m_vertices_xy0[ 3*vertex_index ] );
            std::valarray< vec3_t > p( valence );
            for( uint i = 0; i < valence; ++i ) p[i] = vec3_t( &m_vertices_xy0[ 3*( neighbors[i] ) ] );
            
            // A is total area of one-ring / 3 (or something more complicated if AREA_MIXED is defined).
            real_t A( 0. );
            // cot( alpha ) is the angle opposite the v[] vector in the increasing index direction.
            std::valarray< real_t > cot_alpha( valence );
            // cot( beta ) is the angle opposite the v[] vector in the decreasing index direction.
            std::valarray< real_t > cot_beta( valence );
            cotangentWeightsT( p_center, &p[0], valence, has_boundary, &cot_alpha[0], &cot_beta[0], &A );
            
            const real_t pd_center = f_i( vertex_index );
            
            // Laplacian surface editing energy: the difference in Laplacian between
            // the deformed and undeformed configurations.
            // E = ( \sum -.5*( cot alpha_i + cot beta_i )*( v[i] - v'[i] ) )^2
            // NOTE: We only care about v[i].z - v'[i].z.  This is because v[i].xy = v'[i].xy,
            //       so the difference is always 0.
            real_t E_lse_sqrt = 0;
            // gradient
            const int gradNcomponents = ( g ) ? ( valence + 1 ) : 0;
            std::vector< const sparse_vector_t* > dE_vecs( gradNcomponents );
            std::valarray< real_t > dE_coeffs( gradNcomponents );
            
            real_t coeff_sum = 0.;
            for( uint i = 0 ; i < valence ; ++i )
            {
                const real_t p_undeformed_diff =
                    laplacian_editing_not_laplacian
                    ? ( m_original_vertex_values[ neighbors[i] ] - m_original_vertex_values[ vertex_index ] )
                    : 0.
                    ;
                
                // E_lse_sqrt += -.5 * (cot_alpha[i] + cot_beta[i]) * ((pd[i] - pd_center) - (p[i] - p_center));
                const real_t coeff = -.5 * (cot_alpha[i] + cot_beta[i]);
                E_lse_sqrt += coeff * (( f_i( neighbors[i] ) - pd_center ) - p_undeformed_diff);
                
                // gradient
                if( g )
                {
                    dE_vecs[ i ] = &df_i( neighbors[i] );
                    dE_coeffs[ i ] = coeff;
                    
                    coeff_sum += coeff;
                }
            }
            if( g )
            {
                dE_vecs[valence] = &df_i( vertex_index );
                dE_coeffs[valence] = -coeff_sum;
            }
            
            // energy
            // real_t E_lse = dot( E_lse_sqrt, E_lse_sqrt ) / (A);
            real_t E_lse = ( E_lse_sqrt*E_lse_sqrt ) / (A);
            
            // gradient
            if( g )
            {
                dE_coeffs *= 2.*E_lse_sqrt / (A);
            }
            
            *e += E_lse;
            
            // for( int theta_i = 0; theta_i < m_num_theta; ++theta_i ) g[theta_i] += dE[ theta_i ];
            /// I wish this worked:
            // Eigen::Map< Eigen::Matrix< real_t, Eigen::Dynamic, 1 > >( g, m_num_theta ) += dE;
            // if( g ) for( sparse_vector_t::InnerIterator it( dE ); it; ++it ) g[ it.index() ] += it.value();
            if( g )
            {
                assert( dE_vecs.size() == valence + 1 );
                assert( dE_coeffs.size() == valence + 1 );
                for( int i = 0; i < valence + 1; ++i )
                {
                    const real_t& coeff = dE_coeffs[i];
                    for( sparse_vector_t::InnerIterator it( *dE_vecs[i] ); it; ++it ) g[ it.index() ] += coeff * it.value();
                }
            }
        }
        
        real_t f_i( node_t i ) const
        {
            assert( m_callback_theta );
            assert( i >= 0 && i < m_f_i.size() );
            assert( m_f_i.size() == m_num_vertices );
            
            return m_f_i[i];
        }
        
        // Always returns the cached vector.
        const sparse_vector_t& df_i( node_t i ) const
        {
            assert( m_df_i[ i ].size() == m_num_theta );
            return m_df_i[ i ];
        }
        
        void df_i( sparse_vector_t& out, node_t i ) const
        {
            // The assignment will resize as needed.
            // assert( out.size() == m_num_theta );
            
            out = df_i( i );
        }
        
        void df_i( dense_vector_t& out, node_t i ) const
        {
            out.setZero( m_num_theta );
            
            for( sparse_vector_t::InnerIterator it( df_i(i) ); it; ++it )
            {
                out[ it.index() ] = it.value();
            }
        }
        
        // Always returns the cached matrix.
        // TODO: I want this to be a solve_matrix_t, but a vector of them seems to cause crashes.
        const solve_matrix_t& d2f_i( node_t i ) const
        {
            return m_d2f_i[ i ];
        }
        
        void d2f_i( solve_matrix_t& out, node_t i ) const
        {
            // The assignment will resize as needed.
            // assert( out.rows() == m_num_theta );
            // assert( out.cols() == m_num_theta );
            
            out = d2f_i( i );
        }
        
#if HAVE_TNC
        int tnc_callback( double thetas[], double* f, double g[] )
        {
            const bool kDebugHere = true;
            
            Evaluate( thetas, f, g, NULL );
            
            // Continue iteration (return 0 to continue, 1 to terminate).
            return 0;
        }
        
        static int tnc_function_callback( double x[], double* f, double g[], void* state )
        {
            return static_cast< TreeOpsHeightFieldTNC* >( state )->tnc_callback( x, f, g );
        }
#endif
        
#if HAVE_LBFGS
        lbfgsfloatval_t lbfgs_evaluate( const lbfgsfloatval_t *x, lbfgsfloatval_t *g, const int n, const lbfgsfloatval_t step )
        {
            assert( n == m_num_theta );
            assert( x );
            assert( g );
            
            real_t f;
            Evaluate( x, &f, g, NULL );
            return f;
        }
        int lbfgs_progress( const lbfgsfloatval_t *x, const lbfgsfloatval_t *g, const lbfgsfloatval_t fx, const lbfgsfloatval_t xnorm, const lbfgsfloatval_t gnorm, const lbfgsfloatval_t step, int n, int k, int ls )
        {
            assert( n == m_num_theta );
            assert( x );
            assert( g );
            
            if( ( ( k - 1 ) % 100 ) == 0 )
            {
                std::cout << "Iteration " << k << ", " << ls << " evaluations: f = " << fx << ", |g| = " << gnorm << std::endl;
            }
            
            // continue optimization
            return 0;
        }
        
        static lbfgsfloatval_t lbfgs_evaluate_callback( void* user_data, const lbfgsfloatval_t *x, lbfgsfloatval_t *g, const int n, const lbfgsfloatval_t step )
        {
            return static_cast< TreeOpsHeightFieldTNC* >( user_data )->lbfgs_evaluate( x, g, n, step );
        }
        static int lbfgs_progress_callback( void* user_data, const lbfgsfloatval_t *x, const lbfgsfloatval_t *g, const lbfgsfloatval_t fx, const lbfgsfloatval_t xnorm, const lbfgsfloatval_t gnorm, const lbfgsfloatval_t step, int n, int k, int ls )
        {
            return static_cast< TreeOpsHeightFieldTNC* >( user_data )->lbfgs_progress( x, g, fx, xnorm, gnorm, step, n, k, ls );
        }
#endif
        
    private:
        size_t m_num_theta;
        size_t m_num_vertices;
        size_t m_num_edges;
        std::vector< index_t > m_node2theta;
        std::vector< index_t > m_theta2node;
        
        std::vector< real_t > m_vertices_xy0;
        
        std::vector< real_t > m_original_vertex_values;
        std::vector< real_t > m_values_spacing;
        
        std::vector< triangle_t > m_triangles;
        std::vector< edge_t > m_edges;
        
        mesh_gui::trimesh_t m_mesh;
        
        parent_to_child_map_t m_children;
        std::vector< index_t > m_parent_node;
        typedef std::map< std::pair< node_t, node_t >, bool > is_ancestor_map_t;
        mutable is_ancestor_map_t m_is_ancestor;
        std::vector< int > m_is_fixed;
        std::vector< int > m_is_root;
        std::vector< real_t > m_max_leaf_value;
        
        std::vector< index_t > m_not_fixed_fixed;
        
        // These are caches.
        mutable std::vector< sparse_vector_t > m_df_i;
        // mutable std::vector< solve_matrix_t > m_d2f_i;
        mutable solve_matrix_t* m_d2f_i;
        mutable bool m_d2f_i_sparsity_set;
        mutable size_t m_last_hess_nnz;
        
        // These are caches.
        mutable std::valarray< real_t > m_area;
        mutable std::valarray< bool > m_area_dirty;
        
        bool m_laplacian_editing_not_laplacian;
        real_t m_E_diff_weight;
        
        mutable const double* m_callback_theta;
        mutable std::valarray< real_t > m_f_i;
    };
}

namespace toposmooth
{
    template< typename floating_type >
    void smooth_mesh2d_monotonically_decreasing(
        const size_t num_vertices,
        const floating_type* vertices_xy,
        const size_t num_triangles,
        const triangle_t* triangles,
        const floating_type* original_vertex_values,
        floating_type* new_vertex_values_out,
        const size_t num_edges,
        const edge_t* directed_edges,
        const floating_type stay_close_to_original_values_weight,
        bool smooth_deformation_not_smooth_function
        )
    {
        assert( vertices_xy );
        assert( triangles );
        assert( original_vertex_values );
        assert( new_vertex_values_out );
        assert( directed_edges );
        
#if 0
        {
            FILE* f = fopen( "prepare_smooth_mesh2d_monotonically_decreasing.inputs", "wb" );
            
            fprintf( f, "%d\n", int( num_vertices ) );
            for( int i = 0; i < num_vertices; ++i )
            {
                fprintf( f, "%g %g\n", vertices_xy[ 2*i + 0 ], vertices_xy[ 2*i + 1 ] );
            }
            
            fprintf( f, "%d\n", int( num_triangles ) );
            for( int i = 0; i < num_triangles; ++i )
            {
                fprintf( f, "%d %d %d\n", int( triangles[i].v[0] ), int( triangles[i].v[1] ), int( triangles[i].v[2] ) );
            }
            
            for( int i = 0; i < num_vertices; ++i )
            {
                fprintf( f, "%g\n", original_vertex_values[ i ] );
            }
            
            fprintf( f, "%d\n", int( num_edges ) );
            for( int i = 0; i < num_edges; ++i )
            {
                fprintf( f, "%d %d\n", int( directed_edges[i].v[0] ), int( directed_edges[i].v[1] ) );
            }
            
            fprintf( f, "%g\n", stay_close_to_original_values_weight );
            
            fprintf( f, "%d\n", int( smooth_deformation_not_smooth_function ) );
            
            fclose( f );
        }
#endif
        
        // Create a TreeOpsHeightFieldTNC and call Optimize() on it.
        TreeOpsHeightFieldTNC opt( num_vertices, vertices_xy, num_triangles, triangles, original_vertex_values, num_edges, directed_edges, stay_close_to_original_values_weight, smooth_deformation_not_smooth_function );
        opt.Optimize( new_vertex_values_out );
    }
    
    // These next two lines forward declare both the float and double version of
    // this function, to ensure that we actually compile them both.
    // See "C++ FAQ Lite: [35.15] How can I avoid linker errors with my template classes?"
    // http://www.parashift.com/c++-faq-lite/templates.html#faq-35.15
    template void smooth_mesh2d_monotonically_decreasing< float >(
        const size_t num_vertices,
        const float* vertices_xy,
        const size_t num_triangles,
        const triangle_t* triangles,
        const float* original_vertex_values,
        float* new_vertex_values_out,
        const size_t num_edges,
        const edge_t* directed_edges,
        const float stay_close_to_original_values_weight,
        bool smooth_deformation_not_smooth_function
        );
    template void smooth_mesh2d_monotonically_decreasing< double >(
        const size_t num_vertices,
        const double* vertices_xy,
        const size_t num_triangles,
        const triangle_t* triangles,
        const double* original_vertex_values,
        double* new_vertex_values_out,
        const size_t num_edges,
        const edge_t* directed_edges,
        const double stay_close_to_original_values_weight,
        bool smooth_deformation_not_smooth_function
        );
}

#if C_WRAPPER_FUNCTION
extern "C" {

typedef struct
{
    long v[2];
} c_edge_t;

typedef struct
{
    long v[3];
} c_triangle_t;

void smooth_mesh2d_monotonically_decreasing_f(
    const long num_vertices,
    const float* vertices_xy,
    const long num_triangles,
    const c_triangle_t* triangles,
    const float* original_vertex_values,
    float* new_vertex_values_out,
    const long num_edges,
    const c_edge_t* directed_edges,
    const float stay_close_to_original_values_weight,
    int smooth_deformation_not_smooth_function
    )
{
    toposmooth::smooth_mesh2d_monotonically_decreasing(
        num_vertices,
        vertices_xy,
        num_triangles,
        ( const triangle_t* )( triangles ),
        original_vertex_values,
        new_vertex_values_out,
        num_edges,
        ( const edge_t* )( directed_edges ),
        stay_close_to_original_values_weight,
        smooth_deformation_not_smooth_function
    );
}

void smooth_mesh2d_monotonically_decreasing_d(
    const long num_vertices,
    const double* vertices_xy,
    const long num_triangles,
    const c_triangle_t* triangles,
    const double* original_vertex_values,
    double* new_vertex_values_out,
    const long num_edges,
    const c_edge_t* directed_edges,
    const double stay_close_to_original_values_weight,
    int smooth_deformation_not_smooth_function
    )
{
    toposmooth::smooth_mesh2d_monotonically_decreasing(
        num_vertices,
        vertices_xy,
        num_triangles,
        ( const triangle_t* )( triangles ),
        original_vertex_values,
        new_vertex_values_out,
        num_edges,
        ( const edge_t* )( directed_edges ),
        stay_close_to_original_values_weight,
        smooth_deformation_not_smooth_function
    );
}


// For calling from C.
void* prepare_smooth_mesh2d_monotonically_decreasing_d(
    const long num_vertices,
    const double* vertices_xy,
    const long num_triangles,
    const c_triangle_t* triangles,
    const double* original_vertex_values,
    const long num_edges,
    const c_edge_t* directed_edges,
    const double stay_close_to_original_values_weight,
    int smooth_deformation_not_smooth_function
    )
{
    assert( vertices_xy );
    assert( triangles );
    assert( original_vertex_values );
    assert( directed_edges );
    
    // Create a TreeOpsHeightFieldTNC.
    TreeOpsHeightFieldTNC* opt = new TreeOpsHeightFieldTNC(
        num_vertices,
        vertices_xy,
        num_triangles,
        ( const triangle_t* )( triangles ),
        original_vertex_values,
        num_edges,
        ( const edge_t* )( directed_edges ),
        stay_close_to_original_values_weight,
        smooth_deformation_not_smooth_function
        );
    return opt;
}
void release_smooth_mesh2d_monotonically_decreasing_d( void* prepared )
{
    TreeOpsHeightFieldTNC* opt = static_cast< TreeOpsHeightFieldTNC* >( prepared );
    delete opt;
}

long num_dofs_smooth_mesh2d_monotonically_decreasing_d( void* prepared )
{
    TreeOpsHeightFieldTNC* opt = static_cast< TreeOpsHeightFieldTNC* >( prepared );
    
    return opt->GetNumThetas();
}

long num_values_smooth_mesh2d_monotonically_decreasing_d( void* prepared )
{
    TreeOpsHeightFieldTNC* opt = static_cast< TreeOpsHeightFieldTNC* >( prepared );
    
    return opt->GetNumValues();
}

double* initial_dofs_smooth_mesh2d_monotonically_decreasing_d( void* prepared )
{
    TreeOpsHeightFieldTNC* opt = static_cast< TreeOpsHeightFieldTNC* >( prepared );
    
    const long N = opt->GetNumThetas();
    assert( N >= 0 );
    
    double* initial = new double[ N ];
    opt->GetInitialGuess( initial );
    return initial;
}
void release_initial_dofs_smooth_mesh2d_monotonically_decreasing_d( void* prepared, double* initial )
{
    delete [] initial;
}

double* values_smooth_mesh2d_monotonically_decreasing_d( void* prepared, double* x )
{
    TreeOpsHeightFieldTNC* opt = static_cast< TreeOpsHeightFieldTNC* >( prepared );
    
    const long N = opt->GetNumValues();
    assert( N >= 0 );
    
    double* values = new double[ N ];
    opt->GetValuesForThetas( x, values );
    return values;
}
void release_values_smooth_mesh2d_monotonically_decreasing_d( void* prepared, double* values )
{
    delete [] values;
}

double func_smooth_mesh2d_monotonically_decreasing_d( void* prepared, double* x )
{
    TreeOpsHeightFieldTNC* opt = static_cast< TreeOpsHeightFieldTNC* >( prepared );
    
    double e;
    opt->Evaluate( x, &e, NULL, NULL );
    return e;
}

double* gradient_smooth_mesh2d_monotonically_decreasing_d( void* prepared, double* x )
{
    TreeOpsHeightFieldTNC* opt = static_cast< TreeOpsHeightFieldTNC* >( prepared );
    
    const long N = opt->GetNumThetas();
    assert( N >= 0 );
    
    double e;
    double* gradient = new double[ N ];
    
    opt->Evaluate( x, &e, gradient, NULL );
    
    return gradient;
}
void release_gradient_smooth_mesh2d_monotonically_decreasing_d( void* prepared, double* gradient )
{
    delete [] gradient;
}

typedef struct
{
    long num_entries;
    long* row;
    long* col;
    double* val;
} c_hess_t;
c_hess_t hessian_smooth_mesh2d_monotonically_decreasing_d( void* prepared, double* x )
{
    TreeOpsHeightFieldTNC* opt = static_cast< TreeOpsHeightFieldTNC* >( prepared );
    
    c_hess_t result;
    result.num_entries = 0;
    result.row = 0;
    result.col = 0;
    result.val = 0;
    
    const long N = opt->GetNumThetas();
    assert( N >= 0 );
    double e;
    hessian_t hessian( N, N );
    opt->Evaluate( x, &e, NULL, &hessian );
    
    const int nnz = hessian.nonZeros();
    assert( nnz >= 0 );
    
    result.num_entries = nnz;
    result.row = new long[ nnz ];
    result.col = new long[ nnz ];
    result.val = new double[ nnz ];
    
    int o = 0;
    for( int k = 0; k < hessian.outerSize(); ++k )
    for( solve_matrix_t::InnerIterator it( hessian, k ); it; ++it )
    {
        result.row[o] = it.row();
        result.col[o] = it.col();
        result.val[o] = it.value();
        
        o += 1;
    }
    
    return result;
}
void release_hessian_smooth_mesh2d_monotonically_decreasing_d( void* prepared, c_hess_t hess )
{
    delete [] hess.row;
    delete [] hess.col;
    delete [] hess.val;
}

}
#endif /* C_WRAPPER */
