#include "opt1d.h"

#include "monotonic_function_space.h"
#include "minimath.h"
#include "tnc/tnc.h"

#include <cassert>
#include <algorithm>
#include <vector>
#include <valarray>
#include <map>
#include <set>
#include <cmath>
#include <iostream>
#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 compute and use the hessian.
#define TNC_USE_HESSIAN 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 df_i() and d2f_i().
#define CACHE_NODE_DERIVATIVES 1
// Whether to cache the sparsity of df_i() and d2f_i().
// NOTE: This only has an effect if CACHE_NODE_DERIVATIVES is on.
#define CACHE_NODE_DERIVATIVES_SPARSITY 1

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

using namespace toposmooth;
using namespace toposmooth::minimath;

namespace
{
    std::ostream& dbg_out = std::cerr;
    
    const index_t kInvalidIndex = -1;
    
    typedef Eigen::SparseVector< real_t, Eigen::RowMajor > sparse_vector_t;
    typedef Eigen::DynamicSparseMatrix< real_t, Eigen::RowMajor > sparse_matrix_t;
    
    typedef sparse_matrix_t hessian_t;
}

namespace
{
    class TreeOps1DTNC : public tree_ops_t
    {
    public:
        template< typename floating_type >
        TreeOps1DTNC(
            const size_t num_values,
            const floating_type* original_values,
            const floating_type* values_spacing,
            const floating_type gradient0,
            const floating_type gradientN,
            const floating_type stay_close_to_original_values_weight = 0.,
            bool smooth_deformation_not_smooth_function = false
            )
            : m_callback_theta( 0 )
        {
            assert( num_values >= 3 );
            assert( original_values );
            assert( values_spacing );
            
            m_num_values = num_values;
            m_num_theta = num_values - 2;
            
            m_original_values.resize( num_values );
            std::copy( original_values, original_values + num_values, m_original_values.begin() );
            // Verify the values end-points are monotonically decreasing.
            assert( m_original_values.front() >= m_original_values.back() );
            
            m_values_spacing.resize( num_values - 1 );
            std::copy( values_spacing, values_spacing + num_values - 1, m_values_spacing.begin() );
            
            m_gradient0 = gradient0;
            m_gradientN = gradientN;
            
            m_laplacian_editing_not_laplacian = smooth_deformation_not_smooth_function;
            m_E_diff_weight = stay_close_to_original_values_weight;
            
#if CACHE_NODE_DERIVATIVES
            // Prepare the caches for df_i() and d2f_i().
            m_df_i.resize( num_values );
            m_df_i_dirty.resize( num_values, true );
            m_d2f_i.resize( num_values );
            m_d2f_i_dirty.resize( num_values, true );
#endif
        }
        
        // 'new_values_out' must be an array of the same size as the 'original_values'
        // parameter passed to this class's constructor.
        template< typename floating_type >
        void Optimize( floating_type* new_values_out )
        {
            assert( new_values_out );
            
            assert( m_num_theta > 0 );
            std::vector< double > theta_inout( m_num_theta );
            GetInitialGuess( &theta_inout[0] );
            
            double f_out( kBogusReal );
            std::vector< double > gradient_out( m_num_theta, kBogusReal );
            int num_func_evaluations( 0 );
            // TODO: Try BFGS one-file optimization:
            // Fortran: http://www.ece.northwestern.edu/~nocedal/lbfgs.html
            // C: http://www.chokkan.org/software/liblbfgs/
            // One of these: http://www.alglib.net/optimization/lbfgs.php
            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)
                TreeOps1DTNC::tnc_function_callback, this,
                // We don't want upper and lower bounds, thetas are periodic
                NULL, NULL,
                // 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.
#if TNC_USE_HESSIAN
                -1, // 0,
#else
                -1,
#endif
                // 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_values_out );
        }
        
        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;
            // UPDATE: Lerp'ing .5 is far from linear.  Set this to the inverse of 1/( number of edges ).
            const double t0 = 1. - 1. / ( m_num_values - 1 );
            const double theta0 = acos( 2 * ( t0 - .5 ) );
            std::fill( initial_out, initial_out + m_num_theta, theta0 );
            // UPDATE 2: We can do better.  Let's use the t's which lead to a linear slope (a nonlinear sequence).
            // Let's begin:
            // We want the initial guess to be a linearly descending sequence from the first to the last.
            // Let s = m_original_values.front(), e = m_original_values.back(), N = m_original_values.size();
            const double s = m_original_values.front();
            const double e = m_original_values.back();
            const int N = m_original_values.size();
            // TODO: We shouldn't even run the optimization if the start and end values are the same, we should just set all values equal.
            if( s > e )
            for( int i = 1; i+1 < m_num_values; ++i )
            {
                // We want the value at i to be: s + i * ( e - s ) / ( N-1 ).
                // Thus we want t such that: e + t * ( s + (i-1) * ( e-s ) / ( N-1 ) - e ) = s + i * ( e - s ) / ( N-1 ).
                // Subtracting e from both sides and dividing by the factor t multiplies, we are left with t alone:
                const double t = ( s + i * (e-s) / double( N-1 ) - e ) / ( s + (i-1) * (e-s) / double( N-1 ) - e );
                
                // If the start and end values are the same, t_i will be nan.
                // C++ doesn't have isfinite(), it's a C99 thing.
                // if( !isfinite( t_i ) ) continue;
                assert( t >= -kEps && t <= 1. + kEps );
                
                // Use min/max to clamp to -1,1 in case we are floating point epsilon outside them,
                // which would cause acos() to complain.
                const double theta_i = acos( std::min( std::max( 2 * ( t - .5 ), -1. ), 1. ) );
                initial_out[ i-1 ] = theta_i;
            }
#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_original_values.size();
        }
        
        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_original_values.size(); ++i ) values_out[i] = f_i_nonrecursive( i, m_num_theta );
            m_callback_theta = 0;
        }
        
        void Evaluate( 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;
            
#if CACHE_NODE_DERIVATIVES
            // These derivatives depend on 'thetas', so we have to invalidate our cache when
            // we get new thetas.
            if( g || hess )
            {
                for( int i = 0; i < m_original_values.size(); ++i )
                {
#if CACHE_NODE_DERIVATIVES_SPARSITY
                    // if( m_df_i.at(i).size() > 0 ) m_df_i_dirty[i] = true;
                    if( m_d2f_i.at(i).rows() > 0 ) m_d2f_i_dirty[i] = true;
#else
                    // if( m_df_i.at(i).size() > 0 ) m_df_i.at(i) = sparse_vector_t(0);
                    if( m_d2f_i.at(i).rows() > 0 ) m_d2f_i.at(i) = sparse_matrix_t(0,0);
#endif
                }
            
                // We can calculate the gradient directly.
                // First allocate zero vectors.
                for( int i = 0; i < m_original_values.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 );
                    
                    m_df_i_dirty[i] = false;
                }
                
                // The first and last nodes are constant, so we skip them.
                // Each node builds from the previous, so seed it.
                {
                    sparse_vector_t& df_1 = m_df_i[ 1 ];
                    df_1.coeffRef( 0 ) = f_i_d_theta_j_nonconstant_nonrecursive( 1, 1 );
                }
                for( int i = 1; i < m_num_theta; ++i )
                {
                    const node_t node_i = 1 + i;
                    sparse_vector_t& df_i = m_df_i[ node_i ];
                    // Start with the previous node's gradient.
                    df_i = m_df_i[ node_i - 1 ];
                    // Multiply by the current t
                    df_i *= t_i( node_i );
                    // Put in the position for theta_i the diagonal element.
                    df_i.coeffRef( i ) = f_i_d_theta_j_nonconstant_nonrecursive( node_i, node_i );
                }
            }
#endif
            
            *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_original_values.size(); ++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( 1+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( sparse_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:
        bool is_fixed( node_t node ) const
        {
            assert( node >= 0 && node < m_num_values );
            return node == 0 || node + 1 == m_num_values;
        }
        real_t theta( node_t node ) const
        {
            assert( !is_fixed( node ) );
            assert( m_callback_theta );
            return m_callback_theta[ node - 1 ];
        }
        real_t fixed_value( node_t node ) const
        {
            assert( is_fixed( node ) );
            return m_original_values[ node ];
        }
        real_t max_leaf_value( node_t node ) const
        {
            // The node must never be a leaf node.
            assert( node >= 0 && node + 1 < m_num_values );
            // We have a monotonically decreasing 1d function; the minimum value is the last value.
            return m_original_values[ m_num_values - 1 ];
        }
        node_t parent_node( node_t node ) const
        {
            assert( node >= 0 && node < m_num_values );
            // The root node should return the root node (so says the docs).
            if( 0 == node ) return 0;
            // The parent in our monotonically decreasing chain is the preceding node.
            return node - 1;
        }
        bool is_ancestor( node_t old, node_t young ) const
        {
            assert( old >= 0 && old < m_num_values );
            assert( young >= 0 && young < m_num_values );
            
            // A node is not considered its own ancestor.
            return old < young;
        }
        
        void add_E_and_gradient_and_hessian( double* e, double g[], hessian_t* h ) const
        {
            assert( e );
            
            // 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_original_values.size(); ++i )
            {
                add_E_laplacian_i_and_gradient_and_hessian( e, g, h, i, m_laplacian_editing_not_laplacian );
                if( 0. < m_E_diff_weight )
                {
                    add_E_diff_i_and_gradient_and_hessian( e, g, h, i, m_E_diff_weight );
                }
            }
        }
        void add_E_diff_i_and_gradient_and_hessian( double* e, double g[], hessian_t* hess, const node_t vertex_index, const real_t weight ) const
        {
            assert( e );
            assert( vertex_index >= 0 && vertex_index < m_original_values.size() );
            assert( weight > 0. );
            
            const node_t a = vertex_index-1;
            const node_t b = vertex_index;
            // const node_t c = vertex_index+1;
            // Re-use the spacing at the end for the gradient one-past.
            const real_t delta_ab = m_values_spacing.at( std::max( a, node_t(0) ) );
            assert( !m_values_spacing.empty() );
            const real_t delta_bc = m_values_spacing.at( std::min( b, node_t( m_values_spacing.size()-1 ) ) );
            const real_t A = .5 * ( delta_ab + delta_bc );
            
            // gradient
            const int gradN = ( g || hess ) ? m_num_theta : 0;
            sparse_vector_t df( gradN );
            if( g || hess ) df_i( df, vertex_index );
            
            // hessian
            // TODO: When building the system, use two vectors for i and j and an Eigen dense vector for values.
            const int hessN = hess ? m_num_theta : 0;
            sparse_matrix_t d2f( hessN, hessN );
            if( hess ) d2f_i( d2f, 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_nonrecursive( vertex_index, m_num_theta ) - m_original_values[ vertex_index ] );
            // energy
            real_t E = weight * diff*diff * A;
            
            // gradient
            sparse_vector_t dE;
            if( g || hess ) dE = (2.*weight*diff*A) * df;
            
            // hessian
            sparse_matrix_t d2E;
            if( hess ) d2E = (2.*weight*A) * ( sparse_matrix_t( df.transpose() * df )  +  diff * d2f );
            
            
            // Add our results to the output variables.
            *e += E;
            
            assert( dE.size() == m_num_theta );
            // 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( hess ) *hess += d2E;
        }
        void add_E_laplacian_i_and_gradient_and_hessian( double* e, double g[], hessian_t* hess, const node_t vertex_index, const bool laplacian_editing_not_laplacian = false ) const
        {
            assert( e );
            assert( vertex_index >= 0 && vertex_index < m_num_values );
            
            const node_t a = vertex_index-1;
            const node_t b = vertex_index;
            const node_t c = vertex_index+1;
            // Re-use the spacing at the end for the gradient one-past.
            const real_t delta_ab = m_values_spacing.at( std::max( a, node_t(0) ) );
            assert( !m_values_spacing.empty() );
            const real_t delta_bc = m_values_spacing.at( std::min( b, node_t( m_values_spacing.size()-1 ) ) );
            const real_t A = .5 * ( delta_ab + delta_bc );
            
            // Our energy at node 'i' is (fxx(i) - fxx_undeformed(i))**2.
            // f_xx(b)
            //   = ( ( ( (f(c) - f(b)) / delta_bc ) ) - ( (f(b) - f(a)) / delta_ab ) ) / ( .5 * (delta_ab + delta_bc) )
            //   = ( ( delta_ab * (f(c) - f(b)) / (delta_ab*delta_bc) ) - ( delta_bc * (f(b) - f(a)) / (delta_ab*delta_bc) ) ) / ( .5 * (delta_ab + delta_bc) )
            //   = ( ( delta_bc * f(a) - (delta_ab + delta_bc)*f(b) + delta_ab*f(c) ) / ( (delta_ab*delta_bc) * .5 * (delta_ab + delta_bc) )
            //   = ( ( delta_bc * f(a) - (delta_ab + delta_bc)*f(b) + delta_ab*f(c) ) / ( .5 * (delta_ab*delta_bc) * (delta_ab + delta_bc) )
            
            // gradient
            const int gradN = ( g || hess ) ? m_num_theta : 0;
            sparse_vector_t df_a( gradN ), df_b( gradN ), df_c( gradN );
            if( g || hess )
            {
                if( a >= 0 ) df_i( df_a, a );
                df_i( df_b, b );
                if( c < m_num_values ) df_i( df_c, c );
            }
            
            // hessian
            // TODO: If this is slow, use two vectors for i and j and an Eigen dense vector for values.
            const int hessN = hess ? m_num_theta : 0;
            sparse_matrix_t d2f_a( hessN, hessN ), d2f_c( hessN, hessN ), d2f_b( hessN, hessN );
            if( hess )
            {
                if( a >= 0 ) d2f_i( d2f_a, a );
                d2f_i( d2f_b, b );
                if( c < m_num_values ) d2f_i( d2f_c, c );
            }
            
            // const real_t fxx_i = ( f_bc - f_ab ) / ( .5 * (delta_ab + delta_bc) );
            real_t fxx_i = 0.;
            sparse_vector_t dfxx_i( gradN );
            sparse_matrix_t d2fxx_i( hessN, hessN );
            
            if( m_num_values == c )
            {
                fxx_i = m_gradientN;
            }
            else
            {
                fxx_i = ( f_i_nonrecursive(c,m_num_theta) - f_i_nonrecursive(b,m_num_theta) ) / delta_bc;
                if( g || hess )
                {
                    dfxx_i = (1./delta_bc) * ( df_c - df_b );
                    d2fxx_i = (1./delta_bc) * ( d2f_c - d2f_b );
                }
            }
            
            if( -1 == a )
            {
                fxx_i -= m_gradient0;
            }
            else
            {
                fxx_i -= ( f_i_nonrecursive(b,m_num_theta) - f_i_nonrecursive(a,m_num_theta) ) / delta_ab;
                if( g || hess )
                {
                    dfxx_i -= (1./delta_ab) * ( df_b - df_a );
                    d2fxx_i -= (1./delta_ab) * ( d2f_b - d2f_a );
                }
            }
            
            fxx_i *= 1./( .5 * (delta_ab + delta_bc) );
            if( g || hess )
            {
                dfxx_i *= 1./( .5 * (delta_ab + delta_bc) );
                d2fxx_i *= 1./( .5 * (delta_ab + delta_bc) );
            }
            
            
            // In laplacian editing energy, we are interested in the difference in laplacian between
            // the current and underformed configuration.
            real_t fxx_undeformed_i = 0.;
            if( laplacian_editing_not_laplacian )
            {
                // Store the 'function pointer' to the undeformed values (really it's an array lookup, so []'s, not ()'s).
                const real_t* f_u = &m_original_values[0];
                const real_t f_u_ab =
                    -1 == a
                    ? m_gradient0
                    : ( f_u[b] - f_u[a] ) / delta_ab
                    ;
                const real_t f_u_bc =
                    m_num_values == c
                    ? m_gradientN
                    : ( f_u[c] - f_u[b] ) / delta_bc
                    ;
                fxx_undeformed_i = ( f_u_bc - f_u_ab ) / ( .5 * (delta_ab + delta_bc) );
            }
            
            
            // The energy is the square of fxx times i's area.
            const real_t delta_fxx_i = ( fxx_i - fxx_undeformed_i );
            // *e += A * ( fxx_i - fxx_undeformed_i ) ** 2
            *e += A * delta_fxx_i * delta_fxx_i;
            
            // 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 final
                //       gradient computation is in-place and we need to use its current value.
                *hess += ( 2.*A ) * ( sparse_matrix_t( dfxx_i.transpose() * dfxx_i )  +  delta_fxx_i * d2fxx_i );
            }
            
            // gradient
            if( g )
            {
                // The gradient is the gradient of A * ( fxx_i - fxx_undeformed_i ) ** 2,
                // so
                // gradient( A * ( fxx_i - fxx_undeformed_i ) ** 2 )
                //   = A * 2 * gradient( fxx_i - fxx_undeformed_i ) **2
                //   = A*2*( fxx_i - fxx_undeformed_i ) * gradient( fxx_i - fxx_undeformed_i )
                //   = A*2*( fxx_i - fxx_undeformed_i ) * gradient( fxx_i )
                dfxx_i *= 2.*A*delta_fxx_i;
                
                assert( !g || dfxx_i.size() == m_num_theta );
                // for( int theta_i = 0; theta_i < m_num_theta; ++theta_i ) g[theta_i] += dfxx_i[ theta_i ];
                /// I wish this worked:
                // Eigen::Map< Eigen::Matrix< real_t, Eigen::Dynamic, 1 > >( g, m_num_theta ) += dfxx_i;
                if( g ) for( sparse_vector_t::InnerIterator it( dfxx_i ); it; ++it ) g[ it.index() ] += it.value();
            }
        }
        void df_i( sparse_vector_t& out, node_t i ) const
        {
            assert( out.size() == m_num_theta );
            
#if CACHE_NODE_DERIVATIVES
            // If we have the result cached, return it.
            sparse_vector_t& cached = m_df_i[ i ];
            if( cached.size() == m_num_theta )
            {
                if( m_df_i_dirty[ i ] )
                {
                    for( sparse_vector_t::InnerIterator it( cached ); it; ++it )
                    {
                        it.valueRef() = f_i_d_theta_j_nonconstant_nonrecursive( i, 1 + it.index(), m_num_theta );
                    }
                    
                    m_df_i_dirty[ i ] = false;
                }
                
                out = cached;
                
                return;
            }
#endif
            
            out.setZero();
            for( int dtheta = 0; dtheta < m_num_theta; ++dtheta )
            {
                bool constant;
                const real_t val = f_i_d_theta_j( i, 1 + dtheta, &constant );
                if( !constant )
                {
                    out.coeffRef( dtheta ) = val;
                }
            }
            
#if CACHE_NODE_DERIVATIVES
            // Cache the result
            cached = out;
            m_df_i_dirty[ i ] = false;
#endif
        }
        void d2f_i( sparse_matrix_t& out, node_t i ) const
        {
            assert( out.rows() == m_num_theta );
            assert( out.cols() == m_num_theta );
            
#if CACHE_NODE_DERIVATIVES
            // If we have the result cached, return it.
            sparse_matrix_t& cached = m_d2f_i[ i ];
            if( cached.rows() == m_num_theta )
            {
                if( m_d2f_i_dirty[ i ] )
                {
                    for( int k = 0; k < cached.outerSize(); ++k )
                    for( sparse_matrix_t::InnerIterator it( cached, k ); it; ++it )
                    {
                        // Skip the lower triangle; it will be set when we touch upper triangle
                        // values.
                        if( it.col() < it.row() )
                        {
                            continue;
                        }
                        
                        const real_t val = f_i_d_theta_j_theta_k_nonconstant_nonrecursive( i, 1 + it.row(), 1 + it.col(), m_num_theta );
                        cached.coeffRef( it.row(), it.col() ) = val;
                        
                        // Also set the lower triangle value.
                        // Verify that the transpose coefficient already exists
                        // (this unused access will bail otherwise).
#ifndef NDEBUG
                        cached.coeff( it.col(), it.row() );
#endif
                        cached.coeffRef( it.col(), it.row() ) = val;
                    }
                    
                    m_d2f_i_dirty[ i ] = false;
                }
                
                out = cached;
                
                return;
            }
#endif
            
            out.setZero();
            // The output is symmetric, so compute one triangle and store it twice.
            // TODO: Don't ever copy, just use one triangle throughout all my hessian computations.
            // Optimization: Completely skip the loops when node 'i' is fixed.
            if( !is_fixed(i) )
            for( int dtheta_j = 0; dtheta_j < m_num_theta; ++dtheta_j )
            {
                // Optimization: Skip an entire row if node_j is not an ancestor of i (or i itself).
                const node_t node_j = 1 + dtheta_j;
                if( !( i == node_j || is_ancestor( node_j, i ) ) ) continue;
                
                for( int dtheta_k = dtheta_j; dtheta_k < m_num_theta; ++dtheta_k )
                {
                    // Optimization: Skip the repeated is_fixed() and is_ancestor() check we've just
                    //               performed by calling the nonconstant version of
                    //               f_i_d_theta_j_theta_k() after we check that node_k is an
                    //               ancestor of i (or i itself).
                    const node_t node_k = 1 + dtheta_k;
                    if( !( i == node_k || is_ancestor( node_k, i ) ) ) continue;
                    
#ifndef NDEBUG
                    bool constant;
                    const real_t val = f_i_d_theta_j_theta_k( i, node_j, node_k, &constant );
                    // Our checks above mean that this value shouldn't be constant.
                    assert( !constant );
                    if( !constant )
#else
                    const real_t val = f_i_d_theta_j_theta_k_nonconstant_nonrecursive( i, node_j, node_k, m_num_theta );
#endif
                    {
                        out.coeffRef( dtheta_j, dtheta_k ) = val;
                        out.coeffRef( dtheta_k, dtheta_j ) = val;
                    }
                }
            }
            
#if CACHE_NODE_DERIVATIVES
            // Cache the result
            cached = out;
            m_d2f_i_dirty[ i ] = false;
#endif
        }
        
        int tnc_callback( double thetas[], double* f, double g[] )
        {
            const bool kDebugHere = true;
            
#if !TNC_USE_HESSIAN
            Evaluate( thetas, f, g, NULL );
#else
            sparse_matrix_t hessian( m_num_theta, m_num_theta );
            Evaluate( thetas, f, g, &hessian );
            
            // TODO: set gradient to hessian^{-1} * gradient.
            // TODO: my own newton's method, x_{i+1} = x_i - hessian^{-1} gradient
            typedef Eigen::SparseMatrix< real_t, Eigen::ColMajor > solve_matrix_t;
            solve_matrix_t hessian_compressed( hessian );
            Eigen::SparseLDLT< solve_matrix_t > solver( hessian_compressed );
            Eigen::VectorXd g_eigen( m_num_theta );
            g_eigen = Eigen::Map< Eigen::VectorXd >( g, m_num_theta );
            solver.solveInPlace( g_eigen );
            Eigen::Map< Eigen::VectorXd >( g, m_num_theta ) = g_eigen;
            
            if( kDebugHere )
            {
                dbg_out << "Hessian^{-1} * gradient:\n";
                for( int i = 0; i < m_num_theta; ++i ) dbg_out << g[i] << ' ';
                dbg_out << '\n';
            }
#endif
            
            // 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< TreeOps1DTNC* >( state )->tnc_callback( x, f, g );
        }
    
    private:
        size_t m_num_values;
        size_t m_num_theta;
        std::vector< real_t > m_original_values;
        std::vector< real_t > m_values_spacing;
        real_t m_gradient0;
        real_t m_gradientN;
        
#if CACHE_NODE_DERIVATIVES
        // These are caches.
        mutable std::vector< sparse_vector_t > m_df_i;
        mutable std::valarray< bool > m_df_i_dirty;
        mutable std::vector< sparse_matrix_t > m_d2f_i;
        mutable std::valarray< bool > m_d2f_i_dirty;
#endif
        
        bool m_laplacian_editing_not_laplacian;
        real_t m_E_diff_weight;
        
        mutable double* m_callback_theta;
    };
}

namespace toposmooth
{
    template< typename floating_type >
    void smooth1d_monotonically_decreasing(
        const size_t num_values,
        const floating_type* original_values,
        floating_type* new_values_out,
        const floating_type* values_spacing,
        const floating_type gradient0,
        const floating_type gradientN,
        const floating_type stay_close_to_original_values_weight,
        bool smooth_deformation_not_smooth_function
        )
    {
        assert( original_values );
        assert( new_values_out );
        
        // Both end-points are fixed, and we need something in between.
        // Q: Should we just copy the input to the output if num_values <= 2?
        // A1: Why not?  It makes the function more useful.
        if( num_values <= 2 )
        {
            std::copy( original_values, original_values + num_values, new_values_out );
            return;
        }
        
        assert( num_values > 2 );
        
        // Create a TreeOps1DTNC() and call Optimize() on it.
        TreeOps1DTNC opt( num_values, original_values, values_spacing, gradient0, gradientN, stay_close_to_original_values_weight, smooth_deformation_not_smooth_function );
        opt.Optimize( new_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 smooth1d_monotonically_decreasing< float >(
        const size_t num_values,
        const float* original_values,
        float* new_values_out,
        const float* values_spacing,
        const float gradient0,
        const float gradientN,
        const float stay_close_to_original_values_weight,
        bool smooth_deformation_not_smooth_function
        );
    template void smooth1d_monotonically_decreasing< double >(
        const size_t num_values,
        const double* original_values,
        double* new_values_out,
        const double* values_spacing,
        const double gradient0,
        const double gradientN,
        const double stay_close_to_original_values_weight,
        bool smooth_deformation_not_smooth_function
        );
}

#if C_WRAPPER_FUNCTION
extern "C" {

void smooth1d_monotonically_decreasing_f(
    const long num_values,
    const float* original_values,
    float* new_values_out,
    const float* values_spacing,
    const float gradient0,
    const float gradientN,
    const float stay_close_to_original_values_weight,
    const int smooth_deformation_not_smooth_function
    )
{
    toposmooth::smooth1d_monotonically_decreasing(
        num_values,
        original_values,
        new_values_out,
        values_spacing,
        gradient0,
        gradientN,
        stay_close_to_original_values_weight,
        smooth_deformation_not_smooth_function
    );
}

void smooth1d_monotonically_decreasing_d(
    const long num_values,
    const double* original_values,
    double* new_values_out,
    const double* values_spacing,
    const double gradient0,
    const double gradientN,
    const double stay_close_to_original_values_weight,
    const int smooth_deformation_not_smooth_function
    )
{
    toposmooth::smooth1d_monotonically_decreasing(
        num_values,
        original_values,
        new_values_out,
        values_spacing,
        gradient0,
        gradientN,
        stay_close_to_original_values_weight,
        smooth_deformation_not_smooth_function
    );
}

}
#endif /* C_WRAPPER */
