/*  This file is part of libDAI - http://www.libdai.org/
 *
 *  Copyright (c) 2006-2011, The libDAI authors. All rights reserved.
 *
 *  Use of this source code is governed by a BSD-style license that can be found in the LICENSE file.
 */

#include <iostream>
#include <fstream>
#include <vector>
#include <iterator>
#include <algorithm>
#include <boost/program_options.hpp>
#include <dai/factorgraph.h>
#include <dai/weightedgraph.h>
#include <dai/util.h>
#include <dai/graph.h>
#include <dai/enum.h>
#include <dai/properties.h>

using namespace std;
using namespace dai;
namespace po = boost::program_options;

/// Possible factor types
DAI_ENUM(FactorType,ISINGGAUSS,ISINGUNIFORM,EXPGAUSS,POTTS);

/// Creates a factor graph from a pairwise interactions graph
/** \param G  Graph describing interactions between variables
 *  \param ft Type of factors to use for interactions
 *  \param states Number of states of the variables
 *  \param props Additional properties for generating the interactions
 */
FactorGraph createFG( const GraphAL &G, FactorType ft, size_t states, const PropertySet &props ) {
    size_t N = G.nrNodes();

    DAI_ASSERT(states <= 2 || ft != FactorType::ISINGGAUSS && ft != FactorType::ISINGUNIFORM );

    // Get inverse temperature
    Real beta = 1.0;
    if( ft != FactorType::ISINGGAUSS && ft != FactorType::ISINGUNIFORM )
        beta = props.getAs<Real>("beta");

    // Get properties for Ising factors
    Real mean_h = 0.0;
    Real sigma_h = 0.0;
    Real mean_J = 0.0;
    Real sigma_J = 0.0;
    if( ft == FactorType::ISINGGAUSS ) {
        mean_h = props.getAs<Real>("mean_th");
        sigma_h = props.getAs<Real>("sigma_th");
        mean_J = props.getAs<Real>("mean_w");
        sigma_J = props.getAs<Real>("sigma_w");
    }
    Real min_h = 0.0;
    Real min_J = 0.0;
    Real max_h = 0.0;
    Real max_J = 0.0;
    if( ft == FactorType::ISINGUNIFORM ) {
        min_h = props.getAs<Real>("min_th");
        min_J = props.getAs<Real>("min_w");
        max_h = props.getAs<Real>("max_th");
        max_J = props.getAs<Real>("max_w");
    }

    // Create variables
    vector<Var> vars;
    vars.reserve( N );
    for( size_t i = 0; i < N; i++ )
        vars.push_back( Var( i, states ) );

    // Create factors
    vector<Factor> factors;
    factors.reserve( G.nrEdges() + N );
    // Pairwise factors
    for( size_t i = 0; i < N; i++ )
        for( const Neighbor &j : G.nb(i) )
            if( i < j ) {
                if( ft == FactorType::POTTS )
                    factors.push_back( createFactorPotts( vars[i], vars[j], beta ) );
                else if( ft == FactorType::EXPGAUSS )
                    factors.push_back( createFactorExpGauss( VarSet( vars[i], vars[j] ), beta ) );
                else if( ft == FactorType::ISINGGAUSS ) {
                    Real J = rnd_stdnormal() * sigma_J + mean_J;
                    factors.push_back( createFactorIsing( vars[i], vars[j], J ) );
                } else if( ft == FactorType::ISINGUNIFORM ) {
                    Real J = min_J + rnd_uniform() * (max_J - min_J);
                    factors.push_back( createFactorIsing( vars[i], vars[j], J ) );
                }
            }
    // Unary factors
    if( ft == FactorType::ISINGGAUSS )
        for( size_t i = 0; i < N; i++ ) {
            Real h = rnd_stdnormal() * sigma_h + mean_h;
            factors.push_back( createFactorIsing( vars[i], h ) );
        }
    else if( ft == FactorType::ISINGUNIFORM )
        for( size_t i = 0; i < N; i++ ) {
            Real h = min_h + rnd_uniform() * (max_h - min_h);
            factors.push_back( createFactorIsing( vars[i], h ) );
        }

    return FactorGraph( factors.begin(), factors.end(), vars.begin(), vars.end(), factors.size(), vars.size() );
}

/// Return a random factor graph with higher-order interactions
/** \param N number of variables
 *  \param M number of factors
 *  \param k number of variables that each factor depends on
 *  \param beta standard-deviation of Gaussian log-factor entries
 */
FactorGraph createHOIFG( size_t N, size_t M, size_t k, Real beta ) {
    vector<Var> vars;
    vector<Factor> factors;

    vars.reserve(N);
    for( size_t i = 0; i < N; i++ )
        vars.push_back(Var(i,2));

    for( size_t I = 0; I < M; I++ ) {
        VarSet vars;
        while( vars.size() < k ) {
            do {
                size_t newind = (size_t)(N * rnd_uniform());
                Var newvar = Var(newind, 2);
                if( !vars.contains( newvar ) ) {
                    vars |= newvar;
                    break;
                }
            } while( 1 );
        }
        factors.push_back( createFactorExpGauss( vars, beta ) );
    }

    return FactorGraph( factors.begin(), factors.end(), vars.begin(), vars.end(), factors.size(), vars.size() );
}

/// Creates a regular random bipartite graph
/** \param N1 = number of nodes of type 1
 *  \param d1 = size of neighborhoods of nodes of type 1
 *  \param N2 = number of nodes of type 2
 *  \param d2 = size of neighborhoods of nodes of type 2
 *  \note asserts that N1 * d1 == N2 * d2
 */
BipartiteGraph createRandomBipartiteGraph( size_t N1, size_t N2, size_t d1, size_t d2 ) {
    BipartiteGraph G;

    DAI_ASSERT( N1 * d1 == N2 * d2 );

    // build lists of degree-repeated vertex numbers
    std::vector<size_t> stubs1( N1*d1, 0 );
    for( size_t n1 = 0; n1 < N1; n1++ )
        for( size_t t = 0; t < d1; t++ )
            stubs1[n1*d1 + t] = n1;

    // build lists of degree-repeated vertex numbers
    std::vector<size_t> stubs2( N2*d2, 0 );
    for( size_t n2 = 0; n2 < N2; n2++ )
        for( size_t t = 0; t < d2; t++ )
            stubs2[n2*d2 + t] = n2;

    // shuffle lists
    random_shuffle( stubs1.begin(), stubs1.end(), rnd );
    random_shuffle( stubs2.begin(), stubs2.end(), rnd );

    // add edges
    vector<Edge> edges;
    edges.reserve( N1*d1 );
    for( size_t e = 0; e < N1*d1; e++ )
        edges.push_back( Edge(stubs1[e], stubs2[e]) );

    // finish construction
    G.construct( N1, N2, edges.begin(), edges.end() );

    return G;
}

/// Returns x**n % p, assuming p is prime
int powmod (int x, int n, int p) {
    int y = 1;
    for( int m = 0; m < n; m++ )
        y = (x * y) % p;
    return y;
}

/// Returns order of x in GF(p) with p prime
size_t order (int x, int p) {
    x = x % p;
    DAI_ASSERT( x != 0 );
    size_t n = 0;
    size_t prod = 1;
    do {
        prod = (prod * x) % p;
        n++;
    } while( prod != 1 );
    return n;
}

/// Returns whether n is a prime number
bool isPrime (size_t n) {
    bool result = true;
    for( size_t k = 2; (k < n) && result; k++ )
        if( n % k == 0 )
            result = false;
    return result;
}

/// Constructs a regular LDPC graph with N=6, j=2, K=4, k=3
BipartiteGraph createSmallLDPCGraph() {
    BipartiteGraph G;
    size_t N=4, j=3, K=4; // k=3;

    vector<Edge> edges;
    edges.reserve( N*j );
    edges.push_back( Edge(0,0) ); edges.push_back( Edge(1,0) ); edges.push_back( Edge(2,0) );
    edges.push_back( Edge(0,1) ); edges.push_back( Edge(1,1) ); edges.push_back( Edge(3,1) );
    edges.push_back( Edge(0,2) ); edges.push_back( Edge(2,2) ); edges.push_back( Edge(3,2) );
    edges.push_back( Edge(1,3) ); edges.push_back( Edge(2,3) ); edges.push_back( Edge(3,3) );

    // finish construction
    G.construct( N, K, edges.begin(), edges.end() );

    return G;
}

/// Creates group-structured LDPC code
/** Use construction described in "A Class of Group-Structured LDPC Codes"
 *  by R. M. Tanner, D. Sridhara and T. Fuja
 *  Proceedings of ICSTA, 2001
 *
 *  Example parameters: (p,j,k) = (31,3,5)
 *                      (p,j,k) = (37,3,4)
 *                      (p,j,k) = (7,2,4)
 *                      (p,j,k) = (29,2,4)
 *
 *  j and k must be divisors of p-1
 */
BipartiteGraph createGroupStructuredLDPCGraph( size_t p, size_t j, size_t k ) {
    BipartiteGraph G;

    size_t n = j;
    size_t N = p * k;
    size_t K = p * j;

    size_t a, b;
    for( a = 2; a < p; a++ )
        if( order(a,p) == k )
            break;
    DAI_ASSERT( a != p );
    for( b = 2; b < p; b++ )
        if( order(b,p) == j )
            break;
    DAI_ASSERT( b != p );
    // cout << "# order(a=" << a << ") = " << order(a,p) << endl;
    // cout << "# order(b=" << b << ") = " << order(b,p) << endl;

    DAI_ASSERT( N * n == K * k );

    vector<Edge> edges;
    edges.reserve( N * n );

    for( size_t s = 0; s < j; s++ )
        for( size_t t = 0; t < k; t++ ) {
            size_t P = (powmod(b,s,p) * powmod(a,t,p)) % p;
            for( size_t m = 0; m < p; m++ )
                edges.push_back( Edge(t*p + m, s*p + ((m + P) % p)) );
        }

    // finish construction
    G.construct( N, K, edges.begin(), edges.end() );

    return G;
}

// Constructs a parity check table
void createParityCheck( Real *result, size_t n, Real eps ) {
    size_t N = 1 << n;
    for( size_t i = 0; i < N; i++ ) {
        size_t c = 0;
        for( size_t t = 0; t < n; t++ )
            if( i & (1 << t) )
                c ^= 1;
        if( c )
            result[i] = eps;
        else
            result[i] = 1.0 - eps;
    }
    return;
}

/// Predefined names of various factor graph types
const char *FULL_TYPE        = "FULL";
const char *DREG_TYPE        = "DREG";
const char *LOOP_TYPE        = "LOOP";
const char *TREE_TYPE        = "TREE";
const char *GRID_TYPE        = "GRID";
const char *GRID3D_TYPE      = "GRID3D";
const char *HOI_TYPE         = "HOI";
const char *LDPC_TYPE        = "LDPC";

/// Possible LDPC structures
DAI_ENUM(LDPCType,SMALL,GROUP,RANDOM);

/// Main function
int main( int argc, char *argv[] ) {
    try {
        // Variables for storing command line arguments
        size_t seed;
        size_t states = 2;
        string type;
        size_t d, N, K, k, j, n1, n2, n3, prime;
        bool periodic = false;
        FactorType ft;
        LDPCType ldpc;
        Real beta, sigma_w, sigma_th, mean_w, mean_th, min_w, min_th, max_w, max_th, noise;

        // Declare the supported options.
        po::options_description opts("General command line options");
        opts.add_options()
            ("help",     "produce help message")
            ("seed",     po::value<size_t>(&seed),   "random number seed (tries to read from /dev/urandom if not specified)")
            ("states",   po::value<size_t>(&states), "number of states of each variable (default=2 for binary variables)")
        ;

        // Graph structure options
        po::options_description opts_graph("Options for specifying graph structure");
        opts_graph.add_options()
            ("type",     po::value<string>(&type),   "factor graph type (one of 'FULL', 'DREG', 'LOOP', 'TREE', 'GRID', 'GRID3D', 'HOI', 'LDPC')")
            ("d",        po::value<size_t>(&d),      "variable connectivity (only for type=='DREG');\n\t<d><N> should be even")
            ("N",        po::value<size_t>(&N),      "number of variables (not for type=='GRID','GRID3D')")
            ("n1",       po::value<size_t>(&n1),     "width of grid (only for type=='GRID','GRID3D')")
            ("n2",       po::value<size_t>(&n2),     "height of grid (only for type=='GRID','GRID3D')")
            ("n3",       po::value<size_t>(&n3),     "length of grid (only for type=='GRID3D')")
            ("periodic", po::value<bool>(&periodic), "periodic grid? (only for type=='GRID','GRID3D'; default=0)")
            ("K",        po::value<size_t>(&K),      "number of factors (only for type=='HOI','LDPC')")
            ("k",        po::value<size_t>(&k),      "number of variables per factor (only for type=='HOI','LDPC')")
        ;

        // Factor options
        po::options_description opts_factors("Options for specifying factors");
        opts_factors.add_options()
            ("factors",  po::value<FactorType>(&ft), "factor type (one of 'EXPGAUSS','POTTS','ISINGGAUSS','ISINGUNIFORM')")
            ("beta",     po::value<Real>(&beta),     "inverse temperature (ignored for factors=='ISINGGAUSS','ISINGUNIFORM')")
            ("mean_w",   po::value<Real>(&mean_w),   "mean of pairwise interactions w_{ij} (only for factors=='ISINGGAUSS')")
            ("mean_th",  po::value<Real>(&mean_th),  "mean of unary interactions th_i (only for factors=='ISINGGAUSS')")
            ("sigma_w",  po::value<Real>(&sigma_w),  "stddev of pairwise interactions w_{ij} (only for factors=='ISINGGAUSS')")
            ("sigma_th", po::value<Real>(&sigma_th), "stddev of unary interactions th_i (only for factors=='ISINGGAUSS'")
            ("min_w",    po::value<Real>(&min_w),    "minimum of pairwise interactions w_{ij} (only for factors=='ISINGUNIFORM')")
            ("min_th",   po::value<Real>(&min_th),   "minimum of unary interactions th_i (only for factors=='ISINGUNIFORM')")
            ("max_w",    po::value<Real>(&max_w),    "maximum of pairwise interactions w_{ij} (only for factors=='ISINGUNIFORM')")
            ("max_th",   po::value<Real>(&max_th),   "maximum of unary interactions th_i (only for factors=='ISINGUNIFORM')")
        ;

        // LDPC options
        po::options_description opts_ldpc("Options for specifying LDPC code factor graphs");
        opts_ldpc.add_options()
            ("ldpc",     po::value<LDPCType>(&ldpc), "type of LDPC code (one of 'SMALL','GROUP','RANDOM')")
            ("j",        po::value<size_t>(&j),      "number of parity checks per bit (only for type=='LDPC')")
            ("noise",    po::value<Real>(&noise),    "bitflip probability for binary symmetric channel (only for type=='LDPC')")
            ("prime",    po::value<size_t>(&prime),  "prime number for construction of LDPC code (only for type=='LDPC' with ldpc='GROUP'))")
        ;

        // All options
        opts.add(opts_graph).add(opts_factors).add(opts_ldpc);

        // Parse command line arguments
        po::variables_map vm;
        po::store(po::parse_command_line(argc, argv, opts), vm);
        po::notify(vm);

        // Display help message if necessary
        if( vm.count("help") || !vm.count("type") ) {
            cout << "This program is part of libDAI - http://www.libdai.org/" << endl << endl;
            cout << "Usage: ./createfg [options]" << endl << endl;
            cout << "Creates a factor graph according to the specified options." << endl << endl;

            cout << endl << opts << endl;

            cout << "The following factor graph types with pairwise interactions can be created:" << endl;
            cout << "\t'FULL':   fully connected graph of <N> variables" << endl;
            cout << "\t'DREG':   random regular graph of <N> variables where each variable is connected with <d> others" << endl;
            cout << "\t'LOOP':   a single loop of <N> variables" << endl;
            cout << "\t'TREE':   random tree-structured (acyclic, connected) graph of <N> variables" << endl;
            cout << "\t'GRID':   2D grid of <n1>x<n2> variables" << endl;
            cout << "\t'GRID3D': 3D grid of <n1>x<n2>x<n3> variables" << endl;
            cout << "The following higher-order interactions factor graphs can be created:" << endl;
            cout << "\t'HOI':    random factor graph consisting of <N> variables and <K> factors," << endl;
            cout << "\t          each factor being an interaction of <k> variables." << endl;
            cout << "The following LDPC code factor graphs can be created:" << endl;
            cout << "\t'LDPC':   simulates LDPC decoding problem, using an LDPC code of <N> bits and <K>" << endl;
            cout << "\t          parity checks, with <k> bits per check and <j> checks per bit, transmitted" << endl;
            cout << "\t          on a binary symmetric channel with probability <noise> of flipping a bit." << endl;
            cout << "\t          The transmitted codeword has all bits set to zero. The argument 'ldpc'" << endl;
            cout << "\t          determines how the LDPC code is constructed: either using a group structure," << endl;
            cout << "\t          or randomly, or a fixed small code with (N,K,k,j) = (4,4,3,3)." << endl << endl;

            cout << "For all types except type=='LDPC', the factors have to be specified as well." << endl << endl;

            cout << "EXPGAUSS factors (the default) are created by drawing all log-factor entries" << endl;
            cout << "independently from a Gaussian with mean 0 and standard deviation <beta>." << endl << endl;

            cout << "In case of pairwise interactions, one can also choose POTTS factors, for which" << endl;
            cout << "the log-factors are simply delta functions multiplied by the strength <beta>." << endl << endl;

            cout << "For pairwise interactions and binary variables, one can also use ISINGGAUSS factors." << endl;
            cout << "Here variables x1...xN are assumed to be +1/-1--valued, and unary interactions" << endl;
            cout << "are of the form exp(th*xi) with th drawn from a Gaussian distribution with mean" << endl;
            cout << "<mean_th> and standard deviation <sigma_th>, and pairwise interactions are of the" << endl;
            cout << "form exp(w*xi*xj) with w drawn from a Gaussian distribution with mean <mean_w>" << endl;
            cout << "and standard deviation <sigma_w>." << endl;
            cout << "Alternatively, one can use ISINGUNIFORM factors: here th is drawn from a uniform" << endl;
            cout << "distribution on [<min_th>, <max_th>), and w is drawn from a uniform distribution" << endl;
            cout << "on [<min_w>, <max_w>)." << endl;
            return 1;
        }

        // Set default number of states
        if( !vm.count("states") )
            states = 2;

        // Set default factor type
        if( !vm.count("factors") )
            ft = FactorType::EXPGAUSS;
        // Check validness of factor type
        if( ft == FactorType::POTTS )
            if( type == HOI_TYPE )
                throw "For factors=='POTTS', interactions should be pairwise (type!='HOI')";
        if( ft == FactorType::ISINGGAUSS )
            if( ((states != 2) || (type == HOI_TYPE)) )
                throw "For factors=='ISINGGAUSS', variables should be binary (states==2) and interactions should be pairwise (type!='HOI')";
        if( ft == FactorType::ISINGUNIFORM )
            if( ((states != 2) || (type == HOI_TYPE)) )
                throw "For factors=='ISINGUNIFORM', variables should be binary (states==2) and interactions should be pairwise (type!='HOI')";

        // Read random seed
        if( !vm.count("seed") ) {
            ifstream infile;
            bool success;
            infile.open( "/dev/urandom" );
            success = infile.is_open();
            if( success ) {
                infile.read( (char *)&seed, sizeof(size_t) / sizeof(char) );
                success = infile.good();
                infile.close();
            }
            if( !success )
                throw "Please specify random number seed.";
        }
        rnd_seed( seed );

        // Set default periodicity
        if( !vm.count("periodic") )
            periodic = false;

        // Store some options in a PropertySet object
        PropertySet options;
        if( vm.count("mean_th") )
            options.set("mean_th", mean_th);
        if( vm.count("sigma_th") )
            options.set("sigma_th", sigma_th);
        if( vm.count("mean_w") )
            options.set("mean_w", mean_w);
        if( vm.count("sigma_w") )
            options.set("sigma_w", sigma_w);
        if( vm.count("beta") )
            options.set("beta", beta);
        if( vm.count("min_w") )
            options.set("min_w", min_w);
        if( vm.count("min_th") )
            options.set("min_th", min_th);
        if( vm.count("max_w") )
            options.set("max_w", max_w);
        if( vm.count("max_th") )
            options.set("max_th", max_th);

        // Output some comments
        cout << "# Factor graph made by " << argv[0] << endl;
        cout << "# type = " << type << endl;
        cout << "# states = " << states << endl;
        cout << "# factor type = " << ft << endl;

        // The factor graph to be constructed
        FactorGraph fg;

#define NEED_ARG(name, desc) do { if(!vm.count(name)) throw "Please specify " desc " with --" name; } while(0);

        if( type == FULL_TYPE || type == DREG_TYPE || type == LOOP_TYPE || type == TREE_TYPE || type == GRID_TYPE || type == GRID3D_TYPE ) {
            // Pairwise interactions

            // Check command line options
            if( type == GRID_TYPE ) {
                NEED_ARG("n1", "width of grid");
                NEED_ARG("n2", "height of grid");
                N = n1 * n2;
            } else if( type == GRID3D_TYPE ) {
                NEED_ARG("n1", "width of grid");
                NEED_ARG("n2", "height of grid");
                NEED_ARG("n3", "depth of grid");
                N = n1 * n2 * n3;
            } else
                NEED_ARG("N", "number of variables");

            if( ft == FactorType::ISINGGAUSS ) {
                NEED_ARG("mean_w", "mean of pairwise interactions");
                NEED_ARG("mean_th", "mean of unary interactions");
                NEED_ARG("sigma_w", "stddev of pairwise interactions");
                NEED_ARG("sigma_th", "stddev of unary interactions");
            } else if( ft == FactorType::ISINGUNIFORM ) {
                NEED_ARG("min_w", "minimum of pairwise interactions");
                NEED_ARG("min_th", "minimum of unary interactions");
                NEED_ARG("max_w", "maximum of pairwise interactions");
                NEED_ARG("max_th", "maximum of unary interactions");
            } else
                NEED_ARG("beta", "stddev of log-factor entries");

            if( type == DREG_TYPE )
                NEED_ARG("d", "connectivity (number of neighboring variables of each variable)");

            // Build pairwise interaction graph
            GraphAL G;
            if( type == FULL_TYPE )
                G = createGraphFull( N );
            else if( type == DREG_TYPE )
                G = createGraphRegular( N, d );
            else if( type == LOOP_TYPE )
                G = createGraphLoop( N );
            else if( type == TREE_TYPE )
                G = createGraphTree( N );
            else if( type == GRID_TYPE )
                G = createGraphGrid( n1, n2, periodic );
            else if( type == GRID3D_TYPE )
                G = createGraphGrid3D( n1, n2, n3, periodic );

            // Construct factor graph from pairwise interaction graph
            fg = createFG( G, ft, states, options );

            // Output some additional comments
            if( type == GRID_TYPE || type == GRID3D_TYPE ) {
                cout << "# n1 = " << n1 << endl;
                cout << "# n2 = " << n2 << endl;
                if( type == GRID3D_TYPE )
                    cout << "# n3 = " << n3 << endl;
            }
            if( type == DREG_TYPE )
                cout << "# d = " << d << endl;
            cout << "# options = " << options << endl;
        } else if( type == HOI_TYPE ) {
            // Higher order interactions

            // Check command line arguments
            NEED_ARG("N", "number of variables");
            NEED_ARG("K", "number of factors");
            NEED_ARG("k", "number of variables per factor");
            NEED_ARG("beta", "stddev of log-factor entries");

            // Create higher-order interactions factor graph
            do {
                fg = createHOIFG( N, K, k, beta );
            } while( !fg.isConnected() );

            // Output some additional comments
            cout << "# K = " << K << endl;
            cout << "# k = " << k << endl;
            cout << "# beta = " << beta << endl;
        } else if( type == LDPC_TYPE ) {
            // LDPC codes

            // Check command line arguments
            NEED_ARG("ldpc", "type of LDPC code");
            NEED_ARG("noise", "bitflip probability for binary symmetric channel");

            // Check more command line arguments (seperately for each LDPC type)
            if( ldpc == LDPCType::RANDOM ) {
                NEED_ARG("N", "number of variables");
                NEED_ARG("K", "number of factors");
                NEED_ARG("k", "number of variables per factor");
                NEED_ARG("j", "number of parity checks per bit");
                if( N * j != K * k )
                    throw "Parameters should satisfy N * j == K * k";
            } else if( ldpc == LDPCType::GROUP ) {
                NEED_ARG("prime", "prime number");
                NEED_ARG("k", "number of variables per factor");
                NEED_ARG("j", "number of parity checks per bit");

                if( !isPrime(prime) )
                    throw "Parameter <prime> should be prime";
                if( !((prime-1) % j == 0 ) )
                    throw "Parameters should satisfy (prime-1) % j == 0";
                if( !((prime-1) % k == 0 ) )
                    throw "Parameters should satisfy (prime-1) % k == 0";

                N = prime * k;
                K = prime * j;
            } else if( ldpc == LDPCType::SMALL ) {
                N = 4;
                K = 4;
                j = 3;
                k = 3;
            }

            // Output some additional comments
            cout << "# N = " << N << endl;
            cout << "# K = " << K << endl;
            cout << "# j = " << j << endl;
            cout << "# k = " << k << endl;
            if( ldpc == LDPCType::GROUP )
                cout << "# prime = " << prime << endl;
            cout << "# noise = " << noise << endl;

            // Construct likelihood and paritycheck factors
            Real likelihood[4] = {1.f - noise, noise, noise, 1.f - noise};
            Real *paritycheck = new Real[1 << k];
            createParityCheck(paritycheck, k, 0.0);

            // Create LDPC structure
            BipartiteGraph ldpcG;
            bool regular;
            do {
                if( ldpc == LDPCType::GROUP )
                    ldpcG = createGroupStructuredLDPCGraph( prime, j, k );
                else if( ldpc == LDPCType::RANDOM )
                    ldpcG = createRandomBipartiteGraph( N, K, j, k );
                else if( ldpc == LDPCType::SMALL )
                    ldpcG = createSmallLDPCGraph();

                regular = true;
                for( size_t i = 0; i < N; i++ )
                    if( ldpcG.nb1(i).size() != j )
                        regular = false;
                for( size_t I = 0; I < K; I++ )
                    if( ldpcG.nb2(I).size() != k )
                        regular = false;
            } while( !regular && !ldpcG.isConnected() );

            // Convert to FactorGraph
            vector<Factor> factors;
            for( size_t I = 0; I < K; I++ ) {
                VarSet vs;
                for( size_t _i = 0; _i < k; _i++ ) {
                    size_t i = ldpcG.nb2(I)[_i];
                    vs |= Var( i, 2 );
                }
                factors.push_back( Factor( vs, paritycheck ) );
            }
            delete paritycheck;

            // Generate noise vector
            vector<char> noisebits(N,0);
            size_t bitflips = 0;
            for( size_t i = 0; i < N; i++ ) {
                if( rnd_uniform() < noise ) {
                    noisebits[i] = 1;
                    bitflips++;
                }
            }
            cout << "# bitflips = " << bitflips << endl;

            // Simulate transmission of all-zero codeword
            vector<char> input(N,0);
            vector<char> output(N,0);
            for( size_t i = 0; i < N; i++ )
                output[i] = (input[i] + noisebits[i]) & 1;

            // Add likelihoods
            for( size_t i = 0; i < N; i++ )
               factors.push_back( Factor(Var(i,2), likelihood + output[i]*2) );

            // Construct Factor Graph
            fg = FactorGraph( factors );
        } else
            throw "Invalid type";

        // Output additional comments
        cout << "# N = " << fg.nrVars() << endl;
        cout << "# seed = " << seed << endl;

        // Output factor graph
        cout << fg;
    } catch( const char *e ) {
        /// Display error message
        cerr << "Error: " << e << endl;
        return 1;
    }

    return 0;
}


// On-the-Fly Algorithm for Emptyness of NBAs [Yannakakis et. al].
// Model Checking, Institute of Computer Science, University of Innsbruck.
// Written by Daniel Strigl.

//          Copyright Daniel Strigl 2008.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <boost/graph/adjacency_list.hpp>
#include <iostream>
#include <vector>
#include <string>
#include <sstream>
using namespace boost;

// Typedefs and defines
typedef adjacency_list<vecS, vecS, directedS> graph_t;
typedef graph_traits<graph_t>::vertex_descriptor vertex_t;
#define _countof(array) (sizeof(array) / sizeof(array[0]))

///////////////////////////////////////////////////////////////////////////////

#if 1

// ----------------------------------------------------------------------------
//  Sample 1
//    [http://cl-informatik.uibk.ac.at/teaching/ss08/mc/exercises.pdf, Ex. 1]
// ----------------------------------------------------------------------------

// Set up the vertex names
enum { _0, _1, _2, _3, _4, _5, _6, _7, _8, _9, _10, _11, N };
char* name[] = {
    "0", "1", "2", "3", "4", "5", "6", "7", "8", "9", "10", "11" };

// Specify the edges in the graph
typedef std::pair<int, int> E;
E edge_array[] = {
    E( _0,  _1), E( _0,  _2), E( _0,  _3),
    E( _1,  _0),
    E( _2,  _3), E( _2,  _8),
    E( _3,  _4),
    E( _4,  _5), E( _4,  _6), E( _4,  _7),
    E( _5,  _7),
    E( _6,  _2), E( _6,  _7),
    E( _7, _11),
    E( _8,  _9),
    E( _9,  _3), E( _9,  _6), E( _9, _10),
    E(_10,  _6), E(_10,  _7), E(_10,  _9), E(_10, _11),
    E(_11,  _7)
};

// Specify the final (accepting) states
vertex_t F[] = { _0, _4, _5, _9, _10 };

// Specify the initial state
vertex_t S = _0;

#else /////////////////////////////////////////////////////////////////////////

// ----------------------------------------------------------------------------
//  Sample 2
//    [http://cl-informatik.uibk.ac.at/teaching/ss08/mc/ohp/1x1.pdf, Slide 23]
// ----------------------------------------------------------------------------

// Set up the vertex names
enum { _q0, _q1, _q2, _q3, N };
char* name[] = { "q0", "q1", "q2", "q3" };

// Specify the edges in the graph
typedef std::pair<int, int> E;
E edge_array[] = {
    E(_q0, _q1), E(_q0, _q2),
    E(_q1, _q2), E(_q1, _q3),
    E(_q2, _q3),
    E(_q3, _q2),
};

// Specify the final (accepting) states
vertex_t F[] = { _q1, _q2 };

// Specify the initial state
vertex_t S = _q0;

#endif

///////////////////////////////////////////////////////////////////////////////

static std::vector<vertex_t> finalStates(F, F + _countof(F));
static graph_t g(edge_array, edge_array + sizeof(edge_array) / sizeof(E), N);
static std::vector<bool> marked(num_vertices(g), false);
static std::vector<bool> flagged(num_vertices(g), false);
static std::vector<vertex_t> outer_dfs_stack;
static std::vector<vertex_t> inner_dfs_stack;
static std::basic_ostringstream<char> word;

static void plot_list(std::vector<bool>& l)
{
    std::vector<vertex_t> tmp;
    for (size_t i = 0; i < l.size(); ++i)
        if (l[i])
            tmp.push_back(i);

    if (tmp.empty())
    {
        std::cout << "$\\varnothing$";
        return;
    }

    std::cout << "\\{";

    for (size_t i = 0; i < tmp.size(); ++i)
    {
        std::cout << name[tmp[i]];
        if (i < tmp.size() - 1)
            std::cout << ", ";
    }

    std::cout << "\\}";
}

static void plot_stack(std::vector<vertex_t>& s)
{
    if (s.empty())
    {
        std::cout << "$\\varepsilon$";
        return;
    }

    for (size_t i = s.size(); i > 0; --i)
    {
        std::cout << name[s[i-1]];
        if (i > 1)
            std::cout << ", ";
    }
}

static void plot_stat()
{
    plot_stack(outer_dfs_stack);
    std::cout << " & ";
    plot_stack(inner_dfs_stack);
    std::cout << " & ";
    plot_list(marked);
    std::cout << " & ";
    plot_list(flagged);
    std::cout << " \\\\" << std::endl;
}

static bool inner_dfs(const graph_t& g, vertex_t q)
{
    inner_dfs_stack.push_back(q);

    flagged[q] = true;

    plot_stat();

    graph_traits<graph_t>::adjacency_iterator vi, vi_end;

    for (tie(vi, vi_end) = adjacent_vertices(q, g); vi != vi_end; ++vi)
    {
        std::vector<vertex_t>::const_iterator res;

        if ((res = find(outer_dfs_stack.begin(), outer_dfs_stack.end(), *vi))
            != outer_dfs_stack.end())
        {
            for (std::vector<vertex_t>::const_iterator
                itr = outer_dfs_stack.begin();
                itr != outer_dfs_stack.end(); ++itr)
            {
                word << name[*itr] << "\\:";
            }

            word << "\\left(\\:";

            for (std::vector<vertex_t>::const_iterator
                itr = inner_dfs_stack.begin() + 1;
                itr != inner_dfs_stack.end(); ++itr)
            {
                word << name[*itr] << "\\:";
            }

            for (std::vector<vertex_t>::const_iterator itr = res;
                itr != outer_dfs_stack.end(); ++itr)
            {
                word << name[*itr] << "\\:";
            }

            word << "\\right)" << "^{\\omega}";

            return false;
        }
        else if (!flagged[*vi])
        {
            if (!inner_dfs(g, *vi))
                return false;
        }
    }

    inner_dfs_stack.pop_back();

    return true;
}

static bool outer_dfs(const graph_t& g, vertex_t q)
{
    outer_dfs_stack.push_back(q);

    marked[q] = true;

    plot_stat();

    graph_traits<graph_t>::adjacency_iterator vi, vi_end;

    for (tie(vi, vi_end) = adjacent_vertices(q, g); vi != vi_end; ++vi)
    {
        if (!marked[*vi])
        {
            if (!outer_dfs(g, *vi))
                return false;
        }
    }

    if (find(finalStates.begin(), finalStates.end(), q) != finalStates.end())
        if (!inner_dfs(g, q))
            return false;

    outer_dfs_stack.pop_back();

    return true;
}

int main(int argc, char* argv[])
{
    std::cout << "\\documentclass[landscape]{article}" << std::endl;
    std::cout << "\\usepackage{lscape}" << std::endl;
    std::cout << "\\usepackage[latin1]{inputenc}" << std::endl;
    std::cout << "\\usepackage{amssymb}" << std::endl;
    std::cout << std::endl;
    std::cout << "\\begin{document}" << std::endl;
    std::cout << std::endl;
    std::cout << "\\begin{table}[htb]" << std::endl;
    std::cout << "\\begin{center}" << std::endl;
    std::cout << "\\begin{tabular}{cccc}" << std::endl;
    std::cout << "\\textbf{outer\\_dfs-stack} & \\textbf{inner\\_dfs-stack} &"\
        " \\textbf{marked} & \\textbf{flagged} \\\\" << std::endl;
    std::cout << "\\hline" << std::endl;
    std::cout << "$\\varepsilon$ & $\\varepsilon$ & $\\varnothing$ &"\
        " $\\varnothing$ \\\\" << std::endl;

    const bool result = outer_dfs(g, S);

    std::cout << "\\end{tabular}" << std::endl;
    std::cout << "\\end{center}" << std::endl;
    std::cout << "\\end{table}" << std::endl;
    std::cout << std::endl;

    std::cout << "\\begin{center}" << std::endl;
    if (result)
        std::cout << "\\textbf{terminate}(true)" << std::endl;
    else
        std::cout << "\\textbf{terminate}(false)" << std::endl;
    std::cout << "\\end{center}" << std::endl;

    if (!result)
    {
        std::cout << std::endl;
        std::cout << "\\begin{center}" << std::endl;
        std::cout << "$word = " << word.str() << "$" << std::endl;
        std::cout << "\\end{center}" << std::endl;
    }

    std::cout << std::endl;
    std::cout << "\\end{document}" << std::endl;

    return 0;
}


/*
// Copyright (c) 2000-2009, Texas Engineering Experiment Station (TEES), a
// component of the Texas A&M University System.

// All rights reserved.

// The information and source code contained herein is the exclusive
// property of TEES and may not be disclosed, examined or reproduced
// in whole or in part without explicit written authorization from TEES.
*/

// This define to disable bounds checking in boost::multi_array
#define BOOST_DISABLE_ASSERTS

#include <cfloat>
#include <algorithm>
#include <iostream>
#include <fstream>

#include <boost/multi_array.hpp>
#include <boost/array.hpp>
#include <boost/iterator/zip_iterator.hpp>
#include <boost/tuple/tuple.hpp>

#include "confint.hpp"

using namespace std;

#define TIMER 1

////////////////////////////////////////////////////////////////////////////////
inline double euclid_dist_sq(double x, double y)
{
  const double temp = x - y;
  return temp * temp;
}

////////////////////////////////////////////////////////////////////////////////
// read in the data points
// select initial cluster centroids
////////////////////////////////////////////////////////////////////////////////
template<typename Stream, typename Points, typename Centroids>
void kmeans_input(Stream& in_str, Points& points, Centroids& centroids)
{
  typedef decltype(*points.begin()) point_ref;

  std::for_each(points.begin(), points.end(), [&](point_ref&& point) {
    in_str.read((char*) &point[0], sizeof(double) * point.size());

    if (in_str.fail())
    {
      cerr << "Incomplete file read" << endl;
      exit(1);
    }
  });

  std::copy(points.begin(), points.begin() + centroids.size(),
            centroids.begin());
}

////////////////////////////////////////////////////////////////////////////////
// display results to standard output
////////////////////////////////////////////////////////////////////////////////
template<typename Centroids>
void kmeans_output(int k_clusters, int point_count, int dimensions,
                   Centroids& centroids)
{
  cout << "k= " << k_clusters << ", n= " << point_count << ", ";
  cout << "d= " << dimensions << endl;
  cout << "CENTROIDS" << endl;

  typedef decltype(*centroids.begin()) centroid_ref;

  std::for_each(centroids.begin(), centroids.end(),[&](centroid_ref&& centroid){
    std::for_each(centroid.begin(), centroid.end(),
                  [&](double const& dimension) { cout << dimension << " "; });
    cout << endl; });
}

////////////////////////////////////////////////////////////////////////////////
// k-means computation
////////////////////////////////////////////////////////////////////////////////
template<typename Points, typename Centroids, typename Counts>
void kmeans_seq(Counts& old_assign_counts, Centroids& old_centroids,
                Counts& new_assign_counts, Centroids& new_centroids,
                Points& points, double& mean_sq_err, int& iterations) {

  std::vector<double> distance(old_centroids.size());
  double              old_mean_sq_err;
  double              new_mean_sq_err;
  std::plus<double>   plus_wf;

  typedef decltype(*(old_centroids.begin())) point_ref;

  do {
    old_mean_sq_err = mean_sq_err;
    new_mean_sq_err = 0.0;

    // initialize for another iteration
    std::fill(new_assign_counts.begin(), new_assign_counts.end(), 0);

    for_each(new_centroids.begin(), new_centroids.end(),
             [](point_ref&& centroid) {
               std::fill(centroid.begin(), centroid.end(), 0.0); });

    // for all points
    for_each(points.begin(), points.end(), [&](point_ref&& point) {
      // compute squared Euclidean distance from all centroids to this point
      std::transform(
        old_centroids.begin(), old_centroids.end(), distance.begin(),
        [&](point_ref&& centroid) {
          return std::inner_product(point.begin(),point.end(),centroid.begin(),
                                    0, plus_wf, euclid_dist_sq); });

      // find the nearest centroid to this point
      auto iter         = std::min_element(distance.begin(), distance.end());
      const int nearest = std::distance(distance.begin(), iter);

      // update the new owner centroid to include this point
      std::transform(new_centroids[nearest].begin(),
                     new_centroids[nearest].end(), point.begin(),
                     new_centroids[nearest].begin(), plus_wf);

      ++new_assign_counts[nearest];

      new_mean_sq_err += std::inner_product(point.begin(), point.end(),
                                            old_centroids[nearest].begin(), 0,
                                            plus_wf, euclid_dist_sq);
    });

    // re-estimate the centroids as the average of the assigned points
    std::transform(new_assign_counts.begin(), new_assign_counts.end(),
                   old_assign_counts.begin(),
                   [](int& new_count) { return std::max(new_count, 1); });

    auto beg_iter = boost::make_zip_iterator(boost::make_tuple(
      old_centroids.begin(), new_centroids.begin(), old_assign_counts.begin()));

    auto end_iter = boost::make_zip_iterator(boost::make_tuple(
      old_centroids.end(), new_centroids.end(), old_assign_counts.end()));

    typedef decltype(*beg_iter) tuple_ref;

    std::for_each(beg_iter, end_iter, [](tuple_ref&& t) {
      std::transform(boost::get<1>(t).begin(), boost::get<1>(t).end(),
                     boost::get<0>(t).begin(), [&](double const& dimension) {
                       return dimension / boost::get<2>(t); }); });

    ++iterations;

    mean_sq_err = new_mean_sq_err;
  } while (mean_sq_err < old_mean_sq_err);
}

////////////////////////////////////////////////////////////////////////////////
// driver
////////////////////////////////////////////////////////////////////////////////
void kmeans(int k_clusters, int point_count, int dimensions, ifstream& in_str)
{
#ifdef TIMER
  counter_t timer;
  timer.reset();
  timer.start();
#endif

  typedef boost::multi_array<double, 2> points_ct_t;
  typedef std::vector<int>              counts_ct_t;

  const auto points_extents    = boost::extents[point_count][dimensions];
  const auto centroids_extents = boost::extents[ k_clusters][dimensions];

  points_ct_t points(points_extents);

#ifdef TIMER
  points_ct_t initial_centroids(centroids_extents);
#endif

  points_ct_t old_centroids(centroids_extents);
  points_ct_t new_centroids(centroids_extents);

  counts_ct_t old_assign_counts(k_clusters);
  counts_ct_t new_assign_counts(k_clusters);

  double mean_sq_err = DBL_MAX;
  int iterations     = 0;

  kmeans_input(in_str, points, old_centroids);

  initial_centroids = old_centroids;

#ifdef TIMER
  double io_time = timer.stop();

  confidence_interval_controller iter_control(32, 100, 0.05);

  while (iter_control.iterate())
  {
    old_centroids = initial_centroids;

    timer.reset();
    timer.start();
#endif

  kmeans_seq(old_assign_counts, old_centroids, new_assign_counts, new_centroids,
             points, mean_sq_err, iterations);

#ifdef TIMER
    iter_control.push_back(timer.stop());
  }
#endif

  kmeans_output(k_clusters, point_count, dimensions, old_centroids);

#ifdef TIMER
 iter_control.report("km_seq");
 cout << "io= " << io_time << ",";
 cout << "mse= " << mean_sq_err << ", iter= " << iterations << endl;
#endif
}

////////////////////////////////////////////////////////////////////////////////
int main(int argc, char **argv)
{
  if ( argc < 2 ) {
    cerr << "Cluster count must be specified on command line" << endl;
    exit(1);
  }

  int k_clusters = atoi( argv[1] );

  if ( k_clusters < 2 ) {
    cerr << "Cluster count must be greater than 1" << endl;
    exit(1);
  }

  if ( argc < 3 ) {
    cerr << "Point count must be specified on command line" << endl;
    exit(1);
  }

  int point_count = atoi( argv[2] );

  if ( point_count < 1 ) {
    cerr << "Point count must be greater than 0" << endl;
    exit(1);
  }

  if ( argc < 4 ) {
    cerr << "Dimension count must be specified on command line" << endl;
    exit(1);
  }

  int dimensions = atoi( argv[3] );

  if ( point_count < 2 ) {
    cerr << "Dimensions must be greater than 1" << endl;
    exit(1);
  }

  if ( argc < 5 ) {
    cerr << "Input file must be specified on command line" << endl;
    exit(1);
  }

  ifstream in_str;
  in_str.open(argv[4], ios::binary|ios::in );

  if ( !in_str.is_open() ) {
    cerr << "Input file must be specified on command line" << endl;
    exit(1);
  }

  kmeans(k_clusters, point_count, dimensions, in_str);

  in_str.close();
}


#include <Engine/MeshEdit/Paramaterize.h>
#include <Engine/MeshEdit/MinSurf.h>
#include <Engine/Primitive/TriMesh.h>
#include <Eigen/Sparse>
#include <Eigen/SparseQR>

using namespace Ubpa;
using namespace Eigen;
using namespace std;

Paramaterize::Paramaterize(Ptr<TriMesh> triMesh, bool uSquare, bool uUniform)
    : heMesh(make_shared<HEMesh<V>>()), useSquare(uSquare), useUniform(uUniform) {
	Init(triMesh);
}

void Paramaterize::Clear() {
	heMesh->Clear();
	triMesh = nullptr;
}

bool Paramaterize::Init(Ptr<TriMesh> triMesh) {
	Clear();

	if (triMesh == nullptr)
		return true;

	if (triMesh->GetType() == TriMesh::INVALID) {
		printf("ERROR::Parameterize::Init:\n"
			"\t""trimesh is invalid\n");
		return false;
	}

	// init half-edge structure
	size_t nV = triMesh->GetPositions().size();
	vector<vector<size_t>> triangles;
	triangles.reserve(triMesh->GetTriangles().size());
	for (auto triangle : triMesh->GetTriangles())
		triangles.push_back({ triangle->idx[0], triangle->idx[1], triangle->idx[2] });
	heMesh->Reserve(nV);
	heMesh->Init(triangles);

	if (!heMesh->IsTriMesh() || !heMesh->HaveBoundary()) {
		printf("ERROR::Parameterize::Init:\n"
			"\t""trimesh is not a triangle mesh or hasn't a boundaries\n");
		heMesh->Clear();
		return false;
	}

	// triangle mesh's positions ->  half-edge structure's positions
	for (int i = 0; i < nV; i++) {
		auto v = heMesh->Vertices().at(i);
		v->pos = triMesh->GetPositions()[i].cast_to<vecf3>();
	}

	this->triMesh = triMesh;
	return true;
}

void Paramaterize::DoPara() {
	// First, detect and fix boundary
	random_set<V*> boundary_points;
	random_set<V*> inner_points;

	auto boundaries = this->heMesh->Boundaries();
	if (boundaries.size() != 1) {
		cout << "ERROR::Parameterize::DoPara:" << endl
			 << "\t" << "got boundaries = " << boundaries.size()
			 << " (expect 1)" << endl;
		return;
	}

	for (auto v: boundaries[0]) {
		boundary_points.insert(v->Origin());
	}

	for (auto v: heMesh->Vertices()) {
		if (!boundary_points.contains(v)) {
			inner_points.insert(v);
		}
	}

	//const float boost_factor = 100;
	const float boost_factor = 1;
	// Fix our boundary
	if (useSquare) {
		int points_total = boundary_points.size();
		float step = 4.0f / points_total;

		float curr = 0;
		for (auto v: boundary_points) {
			vecf3 new_pos;
			if (curr >= 0 && curr < 1) {
				new_pos[0] = curr;
				new_pos[1] = new_pos[2] = 0;
			} else if (curr >= 1 && curr < 2) {
				new_pos[0] = 1;
				new_pos[1] = curr - 1;
				new_pos[2] = 0;
			} else if (curr >= 2 && curr < 3) {
				new_pos[0] = 1 - (curr - 2);
				new_pos[1] = 1;
				new_pos[2] = 0;
			} else { // curr >= 3; remember to cut off as fp precision is an issue
				new_pos[0] = 0;
				new_pos[1] = curr > 4 ? 4 : 4 - curr;
				new_pos[2] = 0;
			}
			v->pos = new_pos * boost_factor;
			curr += step;
		}

	} else {
		int points_total = boundary_points.size();
		float step = 2 * 3.1415926f / points_total;

		float curr = 0;
		for (auto v: boundary_points) {
			vecf3 new_pos;
			new_pos[0] = cosf(curr);
			new_pos[1] = sinf(curr);
			new_pos[2] = 0;

			v->pos = new_pos * boost_factor;
			curr += step;
		}
	}

	// Build sparse matrix
	size_t n = inner_points.size();
	SparseMatrix<float> coeff_mat(n, n);
	coeff_mat.setZero();
	VectorXf b_vec_x = VectorXf::Zero(n);
	VectorXf b_vec_y = VectorXf::Zero(n);
	VectorXf b_vec_z = VectorXf::Zero(n);

	cout << "coeff mat build start" << endl;

	if (useUniform) {
		int current_row = 0;
		for (auto v: inner_points) {
			// vidx CERTAINLY follows order (and it's redundant)
			size_t vidx = inner_points.idx(v);
			auto adj = v->AdjVertices();
			size_t degree = v->Degree();
			for (auto adjv : adj) {
				// check type
				if (boundary_points.contains(adjv)) { // this set is usually smaller
					b_vec_x(current_row) += (1.0f / degree) * adjv->pos[0];
					b_vec_y(current_row) += (1.0f / degree) * adjv->pos[1];
					b_vec_z(current_row) += (1.0f / degree) * adjv->pos[2];
				} else { // inner
					assert(inner_points.contains(adjv));
					size_t adjidx = inner_points.idx(adjv);
					// todo add assert = 0
					coeff_mat.insert(current_row, adjidx) = - 1.0f / degree;
				}
			}

			// add itself
			// todo add assert
			coeff_mat.insert(current_row, vidx) = 1;
			current_row++;
		}
	} else { // use cotangent weight

		int fallback_met = 0;
		int current_row = 0;
		for (auto v: inner_points) {
			// vidx CERTAINLY follows order (and it's redundant)
			size_t vidx = inner_points.idx(v);
			auto adj = v->AdjVertices();
			size_t degree = v->Degree();

			float weight_sum = 0;
			bool needFallback = false;
			for (auto adjv : adj) {
				// check type

				float weight = 0;  // this is required to be positive
				auto adjadjv = adjv->AdjVertices();

				decltype(adjadjv) intersect;
				for (auto vv : adjadjv) {
					for (auto vvv : adj) {
						if (vv == vvv && vv != v) {
							intersect.push_back(vv);
						}
					}
				}
				assert(intersect.size() == 2);

				// calculate angels of two
				// (v_i - adjadjv[0]) & (adjadjv[0] - adjv)
				// float cos_alpha = (v->pos - adjadjv[0]->pos).cos_theta(adjadjv[0]->pos - adjv->pos)
				// 				/ (v->pos - adjadjv[0]->pos).sin_theta(adjadjv[0]->pos - adjv->pos);
				float ctheta_alpha = -1 * (v->pos - intersect[0]->pos).cos_theta(intersect[0]->pos - adjv->pos);
				float ctg_alpha = ctheta_alpha / (sqrtf(1 - ctheta_alpha * ctheta_alpha) + 1e-5);

				float ctheta_beta = -1 * (v->pos - intersect[1]->pos).cos_theta(intersect[1]->pos - adjv->pos);
				float ctg_beta = ctheta_beta / (sqrtf(1 - ctheta_beta * ctheta_beta) + 1e-5);

				// needFallback = true;
				// break;
				if (ctg_alpha + ctg_beta <= 0) {
					needFallback = true;
					break;
				} else {
					weight = ctg_alpha + ctg_beta;
					weight_sum += weight;
				}

				if (boundary_points.contains(adjv)) { // this set is usually smaller
					b_vec_x(current_row) += weight * adjv->pos[0];
					b_vec_y(current_row) += weight * adjv->pos[1];
					b_vec_z(current_row) += weight * adjv->pos[2];
				} else {
					assert(inner_points.contains(adjv));
					size_t adjidx = inner_points.idx(adjv);

					coeff_mat.insert(current_row, adjidx) = - weight;
				}
			}

			if (needFallback) {
				// update all b_vec, reassign coeff_mat and update weight_sum
				b_vec_x(current_row) = 0;
				b_vec_y(current_row) = 0;
				b_vec_z(current_row) = 0;

				for (auto adjv : adj) {
					// check type
					if (boundary_points.contains(adjv)) { // this set is usually smaller
						b_vec_x(current_row) += (1.0f / degree) * adjv->pos[0];
						b_vec_y(current_row) += (1.0f / degree) * adjv->pos[1];
						b_vec_z(current_row) += (1.0f / degree) * adjv->pos[2];
					} else { // inner
						assert(inner_points.contains(adjv));
						size_t adjidx = inner_points.idx(adjv);
						// todo add assert = 0
						coeff_mat.coeffRef(current_row, adjidx) = - 1.0f / degree; // since no one knows whether it's inserted or not
					}
				}

				weight_sum = 1;
				fallback_met++;
			}

			// add itself
			// todo add assert
			coeff_mat.insert(current_row, vidx) = weight_sum;
			current_row++;
		}
		cout << "met " << fallback_met << " fallback(s)." << endl;
	}

	cout << "coeff mat build complete" << endl;

	// Solve
	SparseQR<SparseMatrix<float>, COLAMDOrdering<int>> solver;

	cout << "begin makeCompressed()" << endl;
	coeff_mat.makeCompressed();

	cout << "begin compute()" << endl;
	solver.compute(coeff_mat);
	if (solver.info() != Eigen::Success) {
		cout << "solver: decomposition was not successful." << endl;
		return;
	}

	cout << "begin solve() for x" << endl;
	VectorXf res_x = solver.solve(b_vec_x);

	cout << "begin solve() for y" << endl;
	VectorXf res_y = solver.solve(b_vec_y);

	cout << "begin solve() for z" << endl;
	VectorXf res_z = solver.solve(b_vec_z);

	// Update vertex coordinates
	for (int i = 0; i < n; i++) {
		// find the corresponding point
		auto v = inner_points[i];
		vecf3 new_pos = { res_x(i), res_y(i), res_z(i) }; // works?

		//cout << new_pos << endl;
		v->pos = new_pos;
	}

}

bool Paramaterize::Run() {
	if (heMesh->IsEmpty() || !triMesh) {
		printf("ERROR::Parameterize::Run\n"
			"\t""heMesh->IsEmpty() || !triMesh\n");
		return false;
	}

	DoPara();

	// half-edge structure -> triangle mesh
	size_t nV = heMesh->NumVertices();
	size_t nF = heMesh->NumPolygons();
	vector<pointf3> positions;
	vector<unsigned> indice;
	positions.reserve(nV);
	indice.reserve(3 * nF);
	for (auto v : heMesh->Vertices())
		positions.push_back(v->pos.cast_to<pointf3>());
	for (auto f : heMesh->Polygons()) { // f is triangle
		for (auto v : f->BoundaryVertice()) // vertices of the triangle
			indice.push_back(static_cast<unsigned>(heMesh->Index(v)));
	}

	triMesh->Init(indice, positions);

	return true;
}


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file sigma_point_additive_uncorrelated_update_policy.hpp
 * \date July 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

#include <fl/util/meta.hpp>
#include <fl/util/traits.hpp>
#include <fl/util/descriptor.hpp>
#include <fl/filter/gaussian/transform/point_set.hpp>
#include <fl/filter/gaussian/quadrature/sigma_point_quadrature.hpp>

namespace fl
{

// Forward declarations
template <typename...> class SigmaPointUpdatePolicy;

template <
    typename SigmaPointQuadrature,
    typename AdditiveUncorrelatedObsrvFunction
>
class SigmaPointUpdatePolicy<
          SigmaPointQuadrature,
          AdditiveUncorrelated<AdditiveUncorrelatedObsrvFunction>>
    : public Descriptor
{
public:
    typedef typename AdditiveUncorrelatedObsrvFunction::State State;
    typedef typename AdditiveUncorrelatedObsrvFunction::Obsrv Obsrv;

    enum : signed int
    {
        NumberOfPoints =
            SigmaPointQuadrature::number_of_points(SizeOf<State>::Value)
    };

    // static_assert(false, "Just implementing this ........");

    typedef PointSet<State, NumberOfPoints> StatePointSet;
    typedef PointSet<Obsrv, NumberOfPoints> ObsrvPointSet;

    template <
        typename Belief
    >
    void operator()(const AdditiveUncorrelatedObsrvFunction& obsrv_function,
                    const SigmaPointQuadrature& quadrature,
                    const Belief& prior_belief,
                    const Obsrv& obsrv,
                    Belief& posterior_belief)
    {
        auto&& h = [&](const State& x)
        {
           return obsrv_function.expected_observation(x);
        };

        quadrature.propergate_gaussian(h, prior_belief, X, Z);

        auto R_inv =
            obsrv_function
                .noise_diagonal_covariance()
                .diagonal()
                .cwiseInverse()
                .eval();

        auto W_inv =
            X.covariance_weights_vector()
                .cwiseInverse()
                .eval();

        auto&& prediction = Z.center();
        auto&& Y_c = Z.points();
        auto&& X_c = X.centered_points();

        auto innovation = (obsrv - prediction).eval();

        auto C = (Y_c.transpose() * R_inv.asDiagonal() * Y_c).eval();
        C += W_inv.asDiagonal();
        C = C.inverse();

        auto correction = (
           X_c * C * Y_c.transpose() *  R_inv.asDiagonal() * innovation).eval();

        posterior_belief.dimension(prior_belief.dimension());
        posterior_belief.mean(X.mean() + correction);
        posterior_belief.covariance(X_c * C * X_c.transpose());
    }

    virtual std::string name() const
    {
        return "SigmaPointUpdatePolicy<"
            + this->list_arguments(
                 "SigmaPointQuadrature",
                 "AdditiveUncorrelated<AdditiveUncorrelatedSensorFunction>")
            + ">";
    }

    virtual std::string description() const
    {
        return "Sigma Point based filter update policy for observation model"
               " with additive uncorrelated noise";
    }

protected:
    StatePointSet X;
    ObsrvPointSet Z;
};

}


#include "../../include/IntrinsicFormula/WrinkleEditingProcess.h"
#include "../../include/Optimization/NewtonDescent.h"
#include "../../include/IntrinsicFormula/KnoppelStripePattern.h"
#include <igl/cotmatrix_entries.h>
#include <igl/cotmatrix.h>
#include <igl/doublearea.h>
#include <igl/boundary_loop.h>
#include <Eigen/SPQRSupport>

using namespace IntrinsicFormula;

WrinkleEditingProcess::WrinkleEditingProcess(const Eigen::MatrixXd& pos, const MeshConnectivity& mesh, const std::vector<int>& selectedVids, const Eigen::VectorXi& faceFlag, int quadOrd, double spatialRatio)
{
	_pos = pos;
	_mesh = mesh;
	_faceFlag = faceFlag;
	igl::cotmatrix_entries(pos, mesh.faces(), _cotMatrixEntries);
	igl::doublearea(pos, mesh.faces(), _faceArea);
	_faceArea /= 2.0;
	_quadOrd = quadOrd;
	_spatialRatio = spatialRatio;
	_selectedVids = selectedVids;

	int nverts = pos.rows();
	int nfaces = mesh.nFaces();
	int nedges = mesh.nEdges();

	_vertFlag.resize(nverts);
	_vertFlag.setConstant(-1);

	_edgeFlag.resize(nedges);
	_edgeFlag.setConstant(-1);

	_vertArea.setZero(nverts);
	buildVertexNeighboringInfo(_mesh, _pos.rows(), _vertNeiEdges, _vertNeiFaces);

	_edgeCotCoeffs.setZero(nedges);

	_effectiveVids.clear();
	_effectiveEids.clear();
	_effectiveVids.clear();

	std::vector<int> bnds;
	igl::boundary_loop(_mesh.faces(), bnds);

	std::set<int> edgeset;
	std::set<int> vertset;
	_nInterfaces = 0;

	for (int i = 0; i < nfaces; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			int vid = _mesh.faceVertex(i, j);
			int eid = _mesh.faceEdge(i, j);

			_vertArea(vid) += _faceArea(i) / _vertNeiFaces.size() / 3.0;
			_edgeCotCoeffs(eid) += _cotMatrixEntries(i, j);

			if (faceFlag(i) != -1)
			{
				_vertFlag(vid) = faceFlag(i);
				_edgeFlag(eid) = faceFlag(i);
			}
			else
			{
				_effectiveFids.push_back(i);
				//                if(std::find(bnds.begin(), bnds.end(), vid) == bnds.end() && vertset.count(vid) == 0)     // not on boundary
				if (vertset.count(vid) == 0)
					vertset.insert(vid);
				if (edgeset.count(eid) == 0)
					edgeset.insert(eid);
				_nInterfaces++;
			}
		}
	}
	std::copy(vertset.begin(), vertset.end(), std::back_inserter(_effectiveVids));
	std::copy(edgeset.begin(), edgeset.end(), std::back_inserter(_effectiveEids));

	_faceVertMetrics.resize(nfaces);
	for (int i = 0; i < nfaces; i++)
	{
		_faceVertMetrics[i].resize(3);
		for (int j = 0; j < 3; j++)
		{
			int vid = _mesh.faceVertex(i, j);
			int vidj = _mesh.faceVertex(i, (j + 1) % 3);
			int vidk = _mesh.faceVertex(i, (j + 2) % 3);

			Eigen::Vector3d e0 = _pos.row(vidj) - _pos.row(vid);
			Eigen::Vector3d e1 = _pos.row(vidk) - _pos.row(vid);

			Eigen::Matrix2d I;
			I << e0.dot(e0), e0.dot(e1), e1.dot(e0), e1.dot(e1);
			_faceVertMetrics[i][j] = I.inverse();
		}
	}
	std::cout << "number of interfaces: " << _nInterfaces << std::endl;

}

void WrinkleEditingProcess::initialization(const std::vector<Eigen::VectorXd>& initRefAmpList, const std::vector<Eigen::MatrixXd>& initRefOmegaList, const std::vector<Eigen::VectorXd>& refAmpList, const std::vector<Eigen::MatrixXd>& refOmegaList)
{

	std::vector<std::complex<double>> initZvals, initZvalsBeforeEdition;
	std::vector<std::complex<double>> tarZvals, tarZvalsBeforeEdition;

	int nFrames = refAmpList.size() - 2;

	Eigen::VectorXi bndVertsFlag = _vertFlag;
	for (int i = 0; i < bndVertsFlag.rows(); i++)
	{
		if (bndVertsFlag(i) != -1)
			bndVertsFlag(i) = 1;
		else
			bndVertsFlag(i) = 0;
	}

	Eigen::VectorXi firstStepFlags = Eigen::VectorXi::Ones(_vertFlag.rows());
	for (int i = 0; i < firstStepFlags.rows(); i++)
	{
		if (bndVertsFlag(i) != -1)
			firstStepFlags(i) = 1;
		else
			firstStepFlags(i) = 0;
	}

	for (int i = 0; i < _selectedVids.size(); i++)
	{
		firstStepFlags(_selectedVids[i]) = 0;
	}

	std::cout << "initialize bnd zvals. " << std::endl;

	roundVertexZvalsFromHalfEdgeOmegaVertexMag(_mesh, initRefOmegaList[0], initRefAmpList[0], _faceArea, _cotMatrixEntries, _pos.rows(), initZvalsBeforeEdition);
	roundVertexZvalsFromHalfEdgeOmegaVertexMag(_mesh, initRefOmegaList[nFrames + 1], initRefAmpList[nFrames + 1], _faceArea, _cotMatrixEntries, _pos.rows(), tarZvalsBeforeEdition);

	if (!_nInterfaces)
	{
		_combinedRefOmegaList = initRefOmegaList;
		_combinedRefAmpList = initRefAmpList;
		initZvals = initZvalsBeforeEdition;
		tarZvals = tarZvalsBeforeEdition;
	}
	else
	{
		std::cout << "compute reference omega." << std::endl;
		computeCombinedRefOmegaList(refOmegaList);
		std::cout << "compute reference amplitude." << std::endl;
		computeCombinedRefAmpList(refAmpList, &_combinedRefOmegaList);

		initZvals = initZvalsBeforeEdition;
		tarZvals = tarZvalsBeforeEdition;

		roundZvalsForSpecificDomainWithGivenMag(_mesh, _combinedRefOmegaList[0], _combinedRefAmpList[0], bndVertsFlag, _faceArea, _cotMatrixEntries, _pos.rows(), initZvals);
		roundZvalsForSpecificDomainWithBndValues(_pos, _mesh, _combinedRefOmegaList[0], bndVertsFlag, _faceArea, _cotMatrixEntries, _pos.rows(), initZvals);

		roundZvalsForSpecificDomainWithGivenMag(_mesh, _combinedRefOmegaList[nFrames + 1], _combinedRefAmpList[nFrames + 1], bndVertsFlag, _faceArea, _cotMatrixEntries, _pos.rows(), tarZvals);
		roundZvalsForSpecificDomainWithBndValues(_pos, _mesh, _combinedRefOmegaList[nFrames + 1], bndVertsFlag, _faceArea, _cotMatrixEntries, _pos.rows(), tarZvals);

		/*roundZvalsForSpecificDomainWithBndValues(_pos, _mesh, _combinedRefOmegaList[0], firstStepFlags, _faceArea, _cotMatrixEntries, _pos.rows(), initZvals);

		for (int i = 0; i < initZvals.size(); i++)
		{
			if (firstStepFlags[i] == 0)
			{
				double arg = std::arg(initZvals[i]);
				initZvals[i] = refAmpList[0][i] * std::complex<double>(std::cos(arg), std::sin(arg));
			}

		}

		roundZvalsForSpecificDomainWithBndValues(_pos, _mesh, _combinedRefOmegaList[0], bndVertsFlag, _faceArea, _cotMatrixEntries, _pos.rows(), initZvals);

		roundZvalsForSpecificDomainWithBndValues(_pos, _mesh, _combinedRefOmegaList[nFrames + 1], firstStepFlags, _faceArea, _cotMatrixEntries, _pos.rows(), tarZvals);

		for (int i = 0; i < tarZvals.size(); i++)
		{
			if (firstStepFlags[i] == 0)
			{
				double arg = std::arg(tarZvals[i]);
				tarZvals[i] = refAmpList[nFrames + 1][i] * std::complex<double>(std::cos(arg), std::sin(arg));
			}

		}

		roundZvalsForSpecificDomainWithBndValues(_pos, _mesh, _combinedRefOmegaList[nFrames + 1], bndVertsFlag, _faceArea, _cotMatrixEntries, _pos.rows(), tarZvals);*/
	}

	/*if (_nInterfaces)
	{
		roundZvalsForSpecificDomainWithGivenMag(_mesh, _combinedRefOmegaList[0], _combinedRefAmpList[0], bndVertsFlag, _faceArea, _cotMatrixEntries, _pos.rows(), initZvals);
		roundZvalsForSpecificDomainWithBndValues(_pos, _mesh, _combinedRefOmegaList[0], bndVertsFlag, _faceArea, _cotMatrixEntries, _pos.rows(), initZvals);

		roundZvalsForSpecificDomainWithGivenMag(_mesh, _combinedRefOmegaList[nFrames + 1], _combinedRefAmpList[nFrames + 1], bndVertsFlag, _faceArea, _cotMatrixEntries, _pos.rows(), tarZvals);
		roundZvalsForSpecificDomainWithBndValues(_pos, _mesh, _combinedRefOmegaList[nFrames + 1], bndVertsFlag, _faceArea, _cotMatrixEntries, _pos.rows(), tarZvals);
	}*/

	_edgeOmegaList = _combinedRefOmegaList;
	_zvalsList.resize(nFrames + 2);

	_zvalsList[0] = initZvals;
	_zvalsList[nFrames + 1] = tarZvals;

	double dt = 1.0 / (nFrames + 1);

	std::cout << "initialize the intermediate frames." << std::endl;
	for (int i = 1; i <= nFrames; i++)
	{
		 double t = i * dt;

		 _zvalsList[i] = tarZvals;

		 for(int j = 0; j < tarZvals.size(); j++)
		 {
			 _zvalsList[i][j] = (1 - t) * initZvals[j] + t * tarZvals[j];
		 }

//		if (_nInterfaces)
//		{
//			roundZvalsForSpecificDomainWithGivenMag(_mesh, _combinedRefOmegaList[i], _combinedRefAmpList[i], bndVertsFlag, _faceArea, _cotMatrixEntries, _pos.rows(), _zvalsList[i]);
//			roundZvalsForSpecificDomainWithBndValues(_pos, _mesh, _combinedRefOmegaList[i], bndVertsFlag, _faceArea, _cotMatrixEntries, _pos.rows(), _zvalsList[i]);
//		}
//		else
//		{
//			roundVertexZvalsFromHalfEdgeOmegaVertexMag(_mesh, _combinedRefOmegaList[i], _combinedRefAmpList[i], _faceArea, _cotMatrixEntries, _pos.rows(), _zvalsList[i]);
//		}

	}

//	for (int i = 0; i <= nFrames + 1; i++)
//	{
//		for (int j = 0; j < _zvalsList[i].size(); j++)
//		{
//			_combinedRefAmpList[i][j] = std::abs(_zvalsList[i][j]);
//		}
//	}

	_zdotModel = ComputeZdotFromHalfEdgeOmega(_mesh, _faceArea, _quadOrd, dt);

	//	_model = IntrinsicKnoppelDrivenFormula(_mesh, _faceArea, _cotMatrixEntries, _combinedRefOmegaList, _combinedRefAmpList, initZvals, tarZvals, _combinedRefOmegaList[0], _combinedRefOmegaList[nFrames + 1], nFrames, 1.0, _quadOrd,true);

}

void WrinkleEditingProcess::convertList2Variable(Eigen::VectorXd& x)
{
	int nverts = _zvalsList[0].size();
	int nedges = _edgeOmegaList[0].rows();

	int numFrames = _zvalsList.size() - 2;

	int DOFsPerframe = (2 * nverts + 2 * nedges);

	int DOFs = numFrames * DOFsPerframe;

	x.setZero(DOFs);

	for (int i = 0; i < numFrames; i++)
	{
		for (int j = 0; j < nverts; j++)
		{
			x(i * DOFsPerframe + 2 * j) = _zvalsList[i + 1][j].real();
			x(i * DOFsPerframe + 2 * j + 1) = _zvalsList[i + 1][j].imag();
		}

		for (int j = 0; j < nedges; j++)
		{
			x(i * DOFsPerframe + 2 * nverts + 2 * j) = _edgeOmegaList[i + 1](j, 0);
			x(i * DOFsPerframe + 2 * nverts + 2 * j + 1) = _edgeOmegaList[i + 1](j, 1);
		}
	}
}

void WrinkleEditingProcess::convertVariable2List(const Eigen::VectorXd& x)
{
	int nverts = _zvalsList[0].size();
	int nedges = _edgeOmegaList[0].rows();

	int numFrames = _zvalsList.size() - 2;

	int DOFsPerframe = (2 * nverts + 2 * nedges);

	for (int i = 0; i < numFrames; i++)
	{
		for (int j = 0; j < nverts; j++)
		{
			_zvalsList[i + 1][j] = std::complex<double>(x(i * DOFsPerframe + 2 * j), x(i * DOFsPerframe + 2 * j + 1));
		}

		for (int j = 0; j < nedges; j++)
		{
			_edgeOmegaList[i + 1](j, 0) = x(i * DOFsPerframe + 2 * nverts + 2 * j);
			_edgeOmegaList[i + 1](j, 1) = x(i * DOFsPerframe + 2 * nverts + 2 * j + 1);
		}
	}
}

double WrinkleEditingProcess::amplitudeEnergyWithGivenOmegaPerface(const Eigen::VectorXd& amp, const Eigen::MatrixXd& w,
	int fid, Eigen::Vector3d* deriv,
	Eigen::Matrix3d* hess)
{
	double energy = 0;

	double curlSq = curlFreeEnergyPerface(w, fid, NULL, NULL);
	Eigen::Vector3d wSq;
	wSq.setZero();

	/*for(int i = 0; i < 3; i++)
	{
		Eigen::Vector2d dtheta;

		int eidij = _mesh.faceEdge(fid, (i + 2) % 3);
		int eidik = _mesh.faceEdge(fid, (i + 1) % 3);
		int vid = _mesh.faceVertex(fid, i);

		if (vid == _mesh.edgeVertex(eidij, 0))
		{
			dtheta(0) = w(eidij, 0);
		}
		else
		{
			dtheta(0) = w(eidij, 1);
		}

		if (vid == _mesh.edgeVertex(eidik, 0))
		{
			dtheta(1) = w(eidik, 0);
		}
		else
		{
			dtheta(1) = w(eidik, 1);
		}

		wSq(i) = dtheta.dot(_faceVertMetrics[fid][i] * dtheta);
	}*/

	if (deriv)
		deriv->setZero();
	if (hess)
		hess->setZero();

	for (int i = 0; i < 3; i++)
	{
		int vid = _mesh.faceVertex(fid, i);
		energy += 0.5 * amp(vid) * amp(vid) / 3 * (wSq(i) * _faceArea(fid) + curlSq);

		if (deriv)
			(*deriv)(i) += amp(vid) * (wSq(i) * _faceArea(fid) + curlSq) / 3;
		if (hess)
			(*hess)(i, i) += (wSq(i) * _faceArea(fid) + curlSq) / 3;
	}

	return energy;
}

double WrinkleEditingProcess::amplitudeEnergyWithGivenOmega(const Eigen::VectorXd& amp, const Eigen::MatrixXd& w,
	Eigen::VectorXd* deriv,
	std::vector<Eigen::Triplet<double>>* hessT)
{
	double energy = 0;

	int nverts = _pos.rows();
	int nEffectiveFaces = _effectiveFids.size();

	std::vector<double> energyList(nEffectiveFaces);
	std::vector<Eigen::Vector3d> derivList(nEffectiveFaces);
	std::vector<Eigen::Matrix3d> hessList(nEffectiveFaces);

	auto computeEnergy = [&](const tbb::blocked_range<uint32_t>& range) {
		for (uint32_t i = range.begin(); i < range.end(); ++i)
		{
			int fid = _effectiveFids[i];
			energyList[i] = amplitudeEnergyWithGivenOmegaPerface(amp, w, fid, deriv ? &derivList[i] : NULL, hessT ? &hessList[i] : NULL);
		}
	};

	tbb::blocked_range<uint32_t> rangex(0u, (uint32_t)nEffectiveFaces, GRAIN_SIZE);
	tbb::parallel_for(rangex, computeEnergy);

	if (deriv)
		deriv->setZero(nverts);
	if (hessT)
		hessT->clear();

	for (int efid = 0; efid < nEffectiveFaces; efid++)
	{
		energy += energyList[efid];
		int fid = _effectiveFids[efid];

		if (deriv)
		{
			for (int j = 0; j < 3; j++)
			{
				int vid = _mesh.faceVertex(fid, j);
				(*deriv)(vid) += derivList[efid](j);
			}
		}

		if (hessT)
		{
			for (int j = 0; j < 3; j++)
			{
				int vid = _mesh.faceVertex(fid, j);
				for (int k = 0; k < 3; k++)
				{
					int vid1 = _mesh.faceVertex(fid, k);
					hessT->push_back({ vid, vid1, hessList[efid](j, k) });
				}
			}
		}
	}
	return energy;
}

void WrinkleEditingProcess::computeCombinedRefAmpList(const std::vector<Eigen::VectorXd>& refAmpList, std::vector<Eigen::MatrixXd>* combinedOmegaList)
{
	int nverts = _pos.rows();
	int nfaces = _mesh.nFaces();
	int nFrames = refAmpList.size();

	_combinedRefAmpList.resize(nFrames);

	double c = std::min(1.0 / (nFrames * nFrames), 1e-3);

	std::vector<Eigen::Triplet<double>> T;
	// projection matrix
	std::vector<int> freeVid;
	for (int i = 0; i < nverts; i++)
	{
		if (_vertFlag(i) == -1)
		{
			freeVid.push_back(i);
		}
	}

	for (int i = 0; i < freeVid.size(); i++)
	{
		T.push_back(Eigen::Triplet<double>(i, freeVid[i], 1.0));
	}

	Eigen::SparseMatrix<double> projM(freeVid.size(), nverts);
	projM.setFromTriplets(T.begin(), T.end());

	Eigen::SparseMatrix<double> unProjM = projM.transpose();

	auto projVar = [&](const int frameId)
	{
		Eigen::MatrixXd fullX = Eigen::VectorXd::Zero(nverts);
		for (int i = 0; i < nverts; i++)
		{
			if (_vertFlag(i) != -1)
			{
				fullX(i) = refAmpList[frameId](i);
			}
		}
		Eigen::VectorXd x0 = projM * fullX;
		return x0;
	};

	auto unProjVar = [&](const Eigen::VectorXd& x, const int frameId)
	{
		Eigen::VectorXd fullX = unProjM * x;

		for (int i = 0; i < nverts; i++)
		{
			if (_vertFlag(i) != -1)
			{
				fullX(i) = refAmpList[frameId](i);
			}
		}
		return fullX;
	};

	Eigen::SparseMatrix<double> L;
	igl::cotmatrix(_pos, _mesh.faces(), L);

	for (int i = 0; i < nFrames; i++)
	{
		std::cout << "Frame " << std::to_string(i) << ": free vertices: " << freeVid.size() << std::endl;;
		auto funVal = [&](const Eigen::VectorXd& x, Eigen::VectorXd* grad, Eigen::SparseMatrix<double>* hess, bool isProj) {
			Eigen::VectorXd deriv, deriv1;
			std::vector<Eigen::Triplet<double>> T;
			Eigen::SparseMatrix<double> H;

			Eigen::VectorXd fullx = unProjVar(x, i);
			double E = -0.5 * fullx.dot(L * fullx);

			if (combinedOmegaList)
			{
				E += amplitudeEnergyWithGivenOmega(fullx, (*combinedOmegaList)[i], grad ? &deriv1 : NULL, hess ? &T : NULL);
			}

			if (grad)
			{
				deriv = -L * fullx;
				if (combinedOmegaList)
					deriv += deriv1;
				(*grad) = projM * deriv;
			}

			if (hess)
			{
				if (combinedOmegaList)
				{
					H.resize(fullx.rows(), fullx.rows());
					H.setFromTriplets(T.begin(), T.end());
					(*hess) = projM * (H - L) * unProjM;
				}

				else
					(*hess) = projM * (-L) * unProjM;

			}

			return E;
		};
		auto maxStep = [&](const Eigen::VectorXd& x, const Eigen::VectorXd& dir) {
			return 1.0;
		};

		Eigen::VectorXd x0 = projVar(i);
		if (_nInterfaces && freeVid.size())
		{
			OptSolver::newtonSolver(funVal, maxStep, x0, 1000, 1e-6, 1e-10, 1e-15, false);

			Eigen::VectorXd deriv;
			double E = funVal(x0, &deriv, NULL, false);
			std::cout << "terminated with energy : " << E << ", gradient norm : " << deriv.norm() << std::endl << std::endl;
		}

		_combinedRefAmpList[i] = unProjVar(x0, i);
	}
}

double WrinkleEditingProcess::curlFreeEnergyPerface(const Eigen::MatrixXd& w, int faceId, Eigen::Matrix<double, 6, 1>* deriv, Eigen::Matrix<double, 6, 6>* hess)
{
	double E = 0;

	double diff0, diff1;
	Eigen::Matrix<double, 6, 1> select0, select1;
	select0.setZero();
	select1.setZero();

	Eigen::Matrix<double, 6, 1> edgews;

	for (int i = 0; i < 3; i++)
	{
		int eid = _mesh.faceEdge(faceId, i);
		edgews(2 * i) = w(eid, 0);
		edgews(2 * i + 1) = w(eid, 1);

		if (_mesh.faceVertex(faceId, (i + 1) % 3) == _mesh.edgeVertex(eid, 0))
		{
			select0(2 * i) = 1;
			select1(2 * i + 1) = 1;
		}

		else
		{
			select0(2 * i + 1) = 1;
			select1(2 * i) = 1;
		}
	}
	diff0 = select0.dot(edgews);
	diff1 = select1.dot(edgews);

	E = 0.5 * (diff0 * diff0 + diff1 * diff1);
	if (deriv)
	{
		*deriv = select0 * diff0 + select1 * diff1;
	}
	if (hess)
	{
		*hess = select0 * select0.transpose() + select1 * select1.transpose();
	}

	return E;
}

double WrinkleEditingProcess::curlFreeEnergy(const Eigen::MatrixXd& w, Eigen::VectorXd* deriv, std::vector<Eigen::Triplet<double>>* hessT)
{
	double E = 0;
	int nedges = _mesh.nEdges();
	int nEffectiveFaces = _effectiveFids.size();

	std::vector<double> energyList(nEffectiveFaces);
	std::vector<Eigen::Matrix<double, 6, 1>> derivList(nEffectiveFaces);
	std::vector<Eigen::Matrix<double, 6, 6>> hessList(nEffectiveFaces);

	auto computeEnergy = [&](const tbb::blocked_range<uint32_t>& range) {
		for (uint32_t i = range.begin(); i < range.end(); ++i)
		{
			int fid = _effectiveFids[i];
			energyList[i] = curlFreeEnergyPerface(w, fid, deriv ? &derivList[i] : NULL, hessT ? &hessList[i] : NULL);
		}
	};

	tbb::blocked_range<uint32_t> rangex(0u, (uint32_t)nEffectiveFaces, GRAIN_SIZE);
	tbb::parallel_for(rangex, computeEnergy);

	if (deriv)
		deriv->setZero(2 * nedges);
	if (hessT)
		hessT->clear();

	for (int efid = 0; efid < nEffectiveFaces; efid++)
	{
		E += energyList[efid];
		int fid = _effectiveFids[efid];

		if (deriv)
		{
			for (int j = 0; j < 3; j++)
			{
				int eid = _mesh.faceEdge(fid, j);
				(*deriv)(2 * eid) += derivList[efid](2 * j);
				(*deriv)(2 * eid + 1) += derivList[efid](2 * j + 1);
			}
		}

		if (hessT)
		{
			for (int j = 0; j < 3; j++)
			{
				int eid = _mesh.faceEdge(fid, j);
				for (int k = 0; k < 3; k++)
				{
					int eid1 = _mesh.faceEdge(fid, k);
					hessT->push_back({ 2 * eid, 2 * eid1, hessList[efid](2 * j, 2 * k) });
					hessT->push_back({ 2 * eid, 2 * eid1 + 1, hessList[efid](2 * j, 2 * k + 1) });
					hessT->push_back({ 2 * eid + 1, 2 * eid1, hessList[efid](2 * j + 1, 2 * k) });
					hessT->push_back({ 2 * eid + 1, 2 * eid1 + 1, hessList[efid](2 * j + 1, 2 * k + 1) });
				}
			}
		}
	}

	return E;
}

double WrinkleEditingProcess::divFreeEnergyPervertex(const Eigen::MatrixXd& w, int vertId, Eigen::VectorXd* deriv,
	Eigen::MatrixXd* hess)
{
	double energy = 0;
	int neiEdges = _vertNeiEdges[vertId].size();

	Eigen::VectorXd selectedVec0, selectedVec1;
	selectedVec0.setZero(2 * neiEdges);
	selectedVec1.setZero(2 * neiEdges);

	Eigen::VectorXd edgew;
	edgew.setZero(2 * neiEdges);

	for (int i = 0; i < neiEdges; i++)
	{
		int eid = _vertNeiEdges[vertId][i];
		if (_mesh.edgeVertex(eid, 0) == vertId)
		{
			selectedVec0(2 * i) = _edgeCotCoeffs(eid);
			selectedVec1(2 * i + 1) = _edgeCotCoeffs(eid);
		}
		else
		{
			selectedVec0(2 * i + 1) = _edgeCotCoeffs(eid);
			selectedVec1(2 * i) = _edgeCotCoeffs(eid);
		}

		edgew(2 * i) = w(eid, 0);
		edgew(2 * i + 1) = w(eid, 1);
	}
	double diff0 = selectedVec0.dot(edgew);
	double diff1 = selectedVec1.dot(edgew);

	energy = 0.5 * (diff0 * diff0 + diff1 * diff1);
	if (deriv)
	{
		(*deriv) = (diff0 * selectedVec0 + diff1 * selectedVec1);
	}
	if (hess)
	{
		(*hess) = (selectedVec0 * selectedVec0.transpose() + selectedVec1 * selectedVec1.transpose());
	}

	return energy;
}

double WrinkleEditingProcess::divFreeEnergy(const Eigen::MatrixXd& w, Eigen::VectorXd* deriv,
	std::vector<Eigen::Triplet<double>>* hessT)
{
	double energy = 0;
	int nedges = _mesh.nEdges();
	int nEffectiveVerts = _effectiveVids.size();

	std::vector<double> energyList(nEffectiveVerts);
	std::vector<Eigen::VectorXd> derivList(nEffectiveVerts);
	std::vector<Eigen::MatrixXd> hessList(nEffectiveVerts);

	auto computeEnergy = [&](const tbb::blocked_range<uint32_t>& range) {
		for (uint32_t i = range.begin(); i < range.end(); ++i)
		{
			int vid = _effectiveVids[i];
			energyList[i] = divFreeEnergyPervertex(w, vid, deriv ? &derivList[i] : NULL, hessT ? &hessList[i] : NULL);
		}
	};

	tbb::blocked_range<uint32_t> rangex(0u, (uint32_t)nEffectiveVerts, GRAIN_SIZE);
	tbb::parallel_for(rangex, computeEnergy);

	if (deriv)
		deriv->setZero(2 * nedges);
	if (hessT)
		hessT->clear();

	for (int efid = 0; efid < nEffectiveVerts; efid++)
	{
		int vid = _effectiveVids[efid];
		energy += energyList[efid];

		if (deriv)
		{
			for (int j = 0; j < _vertNeiEdges[vid].size(); j++)
			{
				int eid = _vertNeiEdges[vid][j];
				(*deriv)(2 * eid) += derivList[efid](2 * j);
				(*deriv)(2 * eid + 1) += derivList[efid](2 * j + 1);
			}
		}

		if (hessT)
		{
			for (int j = 0; j < _vertNeiEdges[vid].size(); j++)
			{
				int eid = _vertNeiEdges[vid][j];
				for (int k = 0; k < _vertNeiEdges[vid].size(); k++)
				{
					int eid1 = _vertNeiEdges[vid][k];
					hessT->push_back({ 2 * eid, 2 * eid1, hessList[efid](2 * j, 2 * k) });
					hessT->push_back({ 2 * eid, 2 * eid1 + 1, hessList[efid](2 * j, 2 * k + 1) });
					hessT->push_back({ 2 * eid + 1, 2 * eid1, hessList[efid](2 * j + 1, 2 * k) });
					hessT->push_back({ 2 * eid + 1, 2 * eid1 + 1, hessList[efid](2 * j + 1, 2 * k + 1) });
				}
			}
		}
	}

	return energy;
}

void WrinkleEditingProcess::computeCombinedRefOmegaList(const std::vector<Eigen::MatrixXd>& refOmegaList)
{
	int nedges = _mesh.nEdges();
	int nFrames = refOmegaList.size();

	_combinedRefOmegaList.resize(nFrames);

	std::vector<Eigen::Triplet<double>> T;
	// projection matrix
	std::vector<int> freeEid;
	for (int i = 0; i < nedges; i++)
	{
		if (_edgeFlag(i) == -1)
		{
			freeEid.push_back(i);
		}
	}

	for (int i = 0; i < freeEid.size(); i++)
	{
		T.push_back(Eigen::Triplet<double>(2 * i, 2 * freeEid[i], 1.0));
		T.push_back(Eigen::Triplet<double>(2 * i + 1, 2 * freeEid[i] + 1, 1.0));
	}

	Eigen::SparseMatrix<double> projM(2 * freeEid.size(), 2 * nedges);
	projM.setFromTriplets(T.begin(), T.end());

	Eigen::SparseMatrix<double> unProjM = projM.transpose();

	auto projVar = [&](const int frameId)
	{
		Eigen::MatrixXd fullX = Eigen::VectorXd::Zero(nedges * 2);
		for (int i = 0; i < nedges; i++)
		{
			if (_edgeFlag(i) != -1)
			{
				fullX(2 * i) = refOmegaList[frameId](i, 0);
				fullX(2 * i + 1) = refOmegaList[frameId](i, 1);
			}
		}
		Eigen::VectorXd x0 = projM * fullX;
		return x0;
	};

	auto unProjVar = [&](const Eigen::VectorXd& x, const int frameId)
	{
		Eigen::VectorXd fullX = unProjM * x;

		Eigen::MatrixXd w(nedges, 2);

		for (int i = 0; i < nedges; i++)
		{
			if (_edgeFlag(i) != -1)
			{
				fullX(2 * i) = refOmegaList[frameId](i, 0);
				fullX(2 * i + 1) = refOmegaList[frameId](i, 1);
			}
			w(i, 0) = fullX(2 * i);
			w(i, 1) = fullX(2 * i + 1);
		}
		return w;
	};
	auto mat2vec = [&](const Eigen::MatrixXd& w)
	{
		Eigen::VectorXd x(2 * w.rows());
		for (int i = 0; i < w.rows(); i++)
		{
			x(2 * i) = w(i, 0);
			x(2 * i + 1) = w(i, 1);
		}
		return x;
	};

	Eigen::MatrixXd prevw(nedges, 2);
	prevw.setZero();
	for (int i = 0; i < nedges; i++)
	{
		if (_edgeFlag(i) != -1)
		{
			prevw(i, 0) = refOmegaList[0](i, 0);
			prevw(2 * i + 1) = refOmegaList[0](i, 1);
		}
	}
	Eigen::VectorXd prefullx = mat2vec(prevw);

	for (int k = 0; k < nFrames; k++)
	{
		std::cout << "Frame " << std::to_string(k) << ": free edges: " << freeEid.size() << std::endl;
		auto funVal = [&](const Eigen::VectorXd& x, Eigen::VectorXd* grad, Eigen::SparseMatrix<double>* hess, bool isProj) {
			Eigen::VectorXd deriv, deriv1;
			std::vector<Eigen::Triplet<double>> T, T1;
			Eigen::SparseMatrix<double> H;
			Eigen::MatrixXd w = unProjVar(x, k);

			double E = curlFreeEnergy(w, grad ? &deriv : NULL, hess ? &T : NULL);
			E += divFreeEnergy(w, grad ? &deriv1 : NULL, hess ? &T1 : NULL);

			if (grad)
				deriv += deriv1;
			if (hess)
			{
				std::copy(T1.begin(), T1.end(), std::back_inserter(T));
				H.resize(2 * w.rows(), 2 * w.rows());
				H.setFromTriplets(T.begin(), T.end());
			}

			// we need some reg to remove the singularity, where we choose some kinetic energy (||w - prevw||^2), which coeff = 1e-3
			double c = std::min(1.0 / (nFrames * nFrames), 1e-3);
			E += c / 2.0 * (w - prevw).squaredNorm();

			if (grad)
			{
				Eigen::VectorXd fullx = mat2vec(w);
				(*grad) = projM * (deriv + c * (fullx - prefullx));
			}

			if (hess)
			{
				Eigen::SparseMatrix<double> idMat(2 * w.rows(), 2 * w.rows());
				idMat.setIdentity();
				(*hess) = projM * (H + c * idMat) * unProjM;
			}

			return E;
		};
		auto maxStep = [&](const Eigen::VectorXd& x, const Eigen::VectorXd& dir) {
			return 1.0;
		};

		Eigen::VectorXd x0 = projVar(k);

		if (_nInterfaces && freeEid.size())
		{
			OptSolver::newtonSolver(funVal, maxStep, x0, 1000, 1e-6, 1e-10, 1e-15, false);

			Eigen::VectorXd deriv;
			double E = funVal(x0, &deriv, NULL, false);
			std::cout << "terminated with energy : " << E << ", gradient norm : " << deriv.norm() << std::endl << std::endl;
		}

		prevw = unProjVar(x0, k);
		_combinedRefOmegaList[k] = prevw;
		prefullx = mat2vec(prevw);
	}
}

double WrinkleEditingProcess::computeEnergy(const Eigen::VectorXd& x, Eigen::VectorXd* deriv, Eigen::SparseMatrix<double>* hess, bool isProj)
{
	int nverts = _zvalsList[0].size();
	int nedges = _edgeOmegaList[0].rows();

	int numFrames = _zvalsList.size() - 2;

	int DOFsPerframe = (2 * nverts + 2 * nedges);
	int DOFs = numFrames * DOFsPerframe;

	convertVariable2List(x);

	Eigen::VectorXd curDeriv;
	std::vector<Eigen::Triplet<double>> T, curT;

	double energy = 0;
	if (deriv)
	{
		deriv->setZero(DOFs);
	}

	for (int i = 0; i < _zvalsList.size() - 1; i++)
	{
		energy += _zdotModel.computeZdotIntegration(_zvalsList[i], _edgeOmegaList[i], _zvalsList[i + 1], _edgeOmegaList[i + 1], deriv ? &curDeriv : NULL, hess ? &curT : NULL, isProj);

		if (deriv)
		{

			if (i == 0)
				deriv->segment(0, DOFsPerframe) += curDeriv.segment(DOFsPerframe, DOFsPerframe);
			else if (i == _zvalsList.size() - 2)
				deriv->segment((i - 1) * DOFsPerframe, DOFsPerframe) += curDeriv.segment(0, DOFsPerframe);
			else
			{
				deriv->segment((i - 1) * DOFsPerframe, 2 * DOFsPerframe) += curDeriv;
			}

		}

		if (hess)
		{
			for (auto& it : curT)
			{

				if (i == 0)
				{
					if (it.row() >= DOFsPerframe && it.col() >= DOFsPerframe)
						T.push_back({ it.row() - DOFsPerframe, it.col() - DOFsPerframe, it.value() });
				}
				else if (i == _zvalsList.size() - 2)
				{
					if (it.row() < DOFsPerframe && it.col() < DOFsPerframe)
						T.push_back({ it.row() + (i - 1) * DOFsPerframe, it.col() + (i - 1) * DOFsPerframe, it.value() });
				}
				else
				{
					T.push_back({ it.row() + (i - 1) * DOFsPerframe, it.col() + (i - 1) * DOFsPerframe, it.value() });
				}

			}
			curT.clear();
		}
	}

	for (int i = 0; i < numFrames; i++) {
		int id = i + 1;

		// vertex amp diff
		double aveAmp = 0;
		for (int j = 0; j < nverts; j++)
		{
			aveAmp += _combinedRefAmpList[id][j] / nverts;
		}
		for (int j = 0; j < nverts; j++) {
			double ampSq = _zvalsList[id][j].real() * _zvalsList[id][j].real() +
				_zvalsList[id][j].imag() * _zvalsList[id][j].imag();
			double refAmpSq = _combinedRefAmpList[id][j] * _combinedRefAmpList[id][j];

			energy += _spatialRatio * (ampSq - refAmpSq) * (ampSq - refAmpSq) / (aveAmp * aveAmp);

			if (deriv) {
				(*deriv)(i * DOFsPerframe + 2 * j) += 2.0 * _spatialRatio / (aveAmp * aveAmp) * (ampSq - refAmpSq) *
					(2.0 * _zvalsList[id][j].real());
				(*deriv)(i * DOFsPerframe + 2 * j + 1) += 2.0 * _spatialRatio / (aveAmp * aveAmp) * (ampSq - refAmpSq) *
					(2.0 * _zvalsList[id][j].imag());
			}

			if (hess) {
				Eigen::Matrix2d tmpHess;
				tmpHess << 2.0 * _zvalsList[id][j].real() * 2.0 * _zvalsList[id][j].real(), 2.0 * _zvalsList[id][j].real() * 2.0 * _zvalsList[id][j].imag(),
					2.0 * _zvalsList[id][j].real() * 2.0 * _zvalsList[id][j].imag(), 2.0 * _zvalsList[id][j].imag() * 2.0 * _zvalsList[id][j].imag();

				tmpHess *= 2.0 * _spatialRatio / (aveAmp * aveAmp);
				tmpHess += 2.0 * _spatialRatio / (aveAmp * aveAmp) * (ampSq - refAmpSq) * (2.0 * Eigen::Matrix2d::Identity());

				if (isProj)
					tmpHess = SPDProjection(tmpHess);

				for (int k = 0; k < 2; k++)
					for (int l = 0; l < 2; l++)
						T.push_back({ i * DOFsPerframe + 2 * j + k, i * DOFsPerframe + 2 * j + l, tmpHess(k, l) });

			}
		}

		// edge omega difference
		for (int j = 0; j < nedges; j++) {
			energy += _spatialRatio * (aveAmp * aveAmp) * (_edgeOmegaList[id] - _combinedRefOmegaList[id]).row(j).dot(
				(_edgeOmegaList[id] - _combinedRefOmegaList[id]).row(j));

			if (deriv) {
				(*deriv)(i * DOFsPerframe + 2 * nverts + 2 * j) += 2 * _spatialRatio * (aveAmp * aveAmp) *
					(_edgeOmegaList[id] - _combinedRefOmegaList[id])(j,
						0);
				(*deriv)(i * DOFsPerframe + 2 * nverts + 2 * j + 1) += 2 * _spatialRatio * (aveAmp * aveAmp) *
					(_edgeOmegaList[id] -
						_combinedRefOmegaList[id])(j, 1);
			}

			if (hess) {
				T.push_back({ i * DOFsPerframe + 2 * nverts + 2 * j, i * DOFsPerframe + 2 * nverts + 2 * j,
							 2 * _spatialRatio * (aveAmp * aveAmp) });
				T.push_back({ i * DOFsPerframe + 2 * nverts + 2 * j + 1, i * DOFsPerframe + 2 * nverts + 2 * j + 1,
							 2 * _spatialRatio * (aveAmp * aveAmp) });
			}
		}

		// knoppel part
		Eigen::VectorXd kDeriv;
		std::vector<Eigen::Triplet<double>> kT;

		double knoppel = IntrinsicFormula::KnoppelEnergyGivenMag(_mesh, _combinedRefOmegaList[id],
			_combinedRefAmpList[id] / aveAmp, _faceArea, _cotMatrixEntries,
			_zvalsList[id], deriv ? &kDeriv : NULL,
			hess ? &kT : NULL);
		energy += _spatialRatio * knoppel;

		if (deriv) {
			deriv->segment(i * DOFsPerframe, kDeriv.rows()) += _spatialRatio * kDeriv;
		}

		if (hess) {
			for (auto& it : kT) {
				T.push_back({ i * DOFsPerframe + it.row(), i * DOFsPerframe + it.col(), _spatialRatio * it.value() });
			}
		}
	}

	if (hess)
	{
		//std::cout << "num of triplets: " << T.size() << std::endl;
		hess->resize(DOFs, DOFs);
		hess->setFromTriplets(T.begin(), T.end());
	}
	return energy;
}

////////////////////////////////////////////// test functions ///////////////////////////////////////////////////////////////////////////
void WrinkleEditingProcess::testCurlFreeEnergy(const Eigen::MatrixXd& w)
{
	Eigen::VectorXd deriv;
	std::vector<Eigen::Triplet<double>> T;
	Eigen::SparseMatrix<double> hess;
	double E = curlFreeEnergy(w, &deriv, &T);
	hess.resize(2 * w.rows(), 2 * w.rows());
	hess.setFromTriplets(T.begin(), T.end());

	std::cout << "tested curl free energy: " << E << ", gradient norm: " << deriv.norm() << std::endl;

	Eigen::VectorXd dir = deriv;
	dir.setRandom();

	Eigen::VectorXd x(2 * w.rows());
	for (int i = 0; i < w.rows(); i++)
	{
		x(2 * i) = w(i, 0);
		x(2 * i + 1) = w(i, 1);
	}
	for (int i = 3; i < 10; i++)
	{
		double eps = std::pow(0.1, i);
		Eigen::VectorXd deriv1;
		Eigen::MatrixXd w1 = w;
		for (int j = 0; j < w.rows(); j++)
		{
			w1(j, 0) += eps * dir(2 * j);
			w1(j, 1) += eps * dir(2 * j + 1);
		}
		double E1 = curlFreeEnergy(w1, &deriv1, NULL);

		std::cout << "\neps: " << eps << std::endl;
		std::cout << "gradient check: " << std::abs((E1 - E) / eps - dir.dot(deriv)) << std::endl;
		std::cout << "hess check: " << ((deriv1 - deriv) / eps - hess * dir).norm() << std::endl;
	}
}

void WrinkleEditingProcess::testCurlFreeEnergyPerface(const Eigen::MatrixXd& w, int faceId)
{
	Eigen::Matrix<double, 6, 1> deriv;
	Eigen::Matrix<double, 6, 6> hess;
	double E = curlFreeEnergyPerface(w, faceId, &deriv, &hess);
	Eigen::Matrix<double, 6, 1> dir = deriv;
	dir.setRandom();

	std::cout << "tested curl free energy for face: " << faceId << ", energy: " << E << ", gradient norm: " << deriv.norm() << std::endl;

	for (int i = 3; i < 10; i++)
	{
		double eps = std::pow(0.1, i);
		Eigen::MatrixXd w1 = w;
		for (int j = 0; j < 3; j++)
		{
			int eid = _mesh.faceEdge(faceId, j);
			w1(eid, 0) += eps * dir(2 * j);
			w1(eid, 1) += eps * dir(2 * j + 1);
		}
		Eigen::Matrix<double, 6, 1> deriv1;
		double E1 = curlFreeEnergyPerface(w1, faceId, &deriv1, NULL);

		std::cout << "\neps: " << eps << std::endl;
		std::cout << "gradient check: " << std::abs((E1 - E) / eps - dir.dot(deriv)) << std::endl;
		std::cout << "hess check: " << ((deriv1 - deriv) / eps - hess * dir).norm() << std::endl;
	}

}

void WrinkleEditingProcess::testDivFreeEnergy(const Eigen::MatrixXd& w)
{
	Eigen::VectorXd deriv;
	std::vector<Eigen::Triplet<double>> T;
	Eigen::SparseMatrix<double> hess;
	double E = divFreeEnergy(w, &deriv, &T);
	hess.resize(2 * w.rows(), 2 * w.rows());
	hess.setFromTriplets(T.begin(), T.end());

	std::cout << "tested div free energy: " << E << ", gradient norm: " << deriv.norm() << std::endl;

	Eigen::VectorXd dir = deriv;
	dir.setRandom();

	for (int i = 3; i < 10; i++)
	{
		double eps = std::pow(0.1, i);
		Eigen::VectorXd deriv1;
		Eigen::MatrixXd w1 = w;
		for (int j = 0; j < w.rows(); j++)
		{
			w1(j, 0) += eps * dir(2 * j);
			w1(j, 1) += eps * dir(2 * j + 1);
		}
		double E1 = divFreeEnergy(w1, &deriv1, NULL);

		std::cout << "\neps: " << eps << std::endl;
		std::cout << "gradient check: " << std::abs((E1 - E) / eps - dir.dot(deriv)) << std::endl;
		std::cout << "hess check: " << ((deriv1 - deriv) / eps - hess * dir).norm() << std::endl;
	}
}

void WrinkleEditingProcess::testDivFreeEnergyPervertex(const Eigen::MatrixXd& w, int vertId)
{
	Eigen::VectorXd deriv;
	Eigen::MatrixXd hess;
	double E = divFreeEnergyPervertex(w, vertId, &deriv, &hess);
	Eigen::VectorXd dir = deriv;
	dir.setRandom();

	std::cout << "tested div free energy for vertex: " << vertId << ", energy: " << E << ", gradient norm: " << deriv.norm() << std::endl;

	for (int i = 3; i < 10; i++)
	{
		double eps = std::pow(0.1, i);
		Eigen::MatrixXd w1 = w;
		for (int j = 0; j < _vertNeiEdges[vertId].size(); j++)
		{
			int eid = _vertNeiEdges[vertId][j];
			w1(eid, 0) += eps * dir(2 * j);
			w1(eid, 1) += eps * dir(2 * j + 1);
		}
		Eigen::VectorXd deriv1;
		double E1 = divFreeEnergyPervertex(w1, vertId, &deriv1, NULL);

		std::cout << "\neps: " << eps << std::endl;
		std::cout << "gradient check: " << std::abs((E1 - E) / eps - dir.dot(deriv)) << std::endl;
		std::cout << "hess check: " << ((deriv1 - deriv) / eps - hess * dir).norm() << std::endl;
	}

}

void WrinkleEditingProcess::testAmpEnergyWithGivenOmega(const Eigen::VectorXd& amp, const Eigen::MatrixXd& w)
{
	Eigen::VectorXd deriv;
	std::vector<Eigen::Triplet<double>> T;
	Eigen::SparseMatrix<double> hess;
	double E = amplitudeEnergyWithGivenOmega(amp, w, &deriv, &T);
	hess.resize(amp.rows(), amp.rows());
	hess.setFromTriplets(T.begin(), T.end());

	std::cout << "tested amp energy: " << E << ", gradient norm: " << deriv.norm() << std::endl;

	Eigen::VectorXd dir = deriv;
	dir.setRandom();

	for (int i = 3; i < 10; i++)
	{
		double eps = std::pow(0.1, i);
		Eigen::VectorXd deriv1;
		Eigen::VectorXd amp1 = amp;
		for (int j = 0; j < amp.rows(); j++)
		{
			amp1(j) += eps * dir(j);
		}
		double E1 = amplitudeEnergyWithGivenOmega(amp1, w, &deriv1, NULL);

		std::cout << "\neps: " << eps << std::endl;
		std::cout << "gradient check: " << std::abs((E1 - E) / eps - dir.dot(deriv)) << std::endl;
		std::cout << "hess check: " << ((deriv1 - deriv) / eps - hess * dir).norm() << std::endl;
	}
}

void WrinkleEditingProcess::testAmpEnergyWithGivenOmegaPerface(const Eigen::VectorXd& amp, const Eigen::MatrixXd& w, int faceId)
{
	Eigen::Vector3d deriv;
	Eigen::Matrix3d hess;
	double E = amplitudeEnergyWithGivenOmegaPerface(amp, w, faceId, &deriv, &hess);
	Eigen::Vector3d dir = deriv;
	dir.setRandom();

	std::cout << "tested amp energy for face: " << faceId << ", energy: " << E << ", gradient norm: " << deriv.norm() << std::endl;

	for (int i = 3; i < 10; i++)
	{
		double eps = std::pow(0.1, i);
		Eigen::VectorXd amp1 = amp;
		for (int j = 0; j < 3; j++)
		{
			int vid = _mesh.faceVertex(faceId, j);
			amp1(vid) += eps * dir(j);
		}
		Eigen::Vector3d deriv1;
		double E1 = amplitudeEnergyWithGivenOmegaPerface(amp1, w, faceId, &deriv1, NULL);

		std::cout << "\neps: " << eps << std::endl;
		std::cout << "gradient check: " << std::abs((E1 - E) / eps - dir.dot(deriv)) << std::endl;
		std::cout << "hess check: " << ((deriv1 - deriv) / eps - hess * dir).norm() << std::endl;
	}

}

void WrinkleEditingProcess::testEnergy(Eigen::VectorXd x)
{
	Eigen::VectorXd deriv;
	Eigen::SparseMatrix<double> hess;

	double e = computeEnergy(x, &deriv, &hess, false);
	std::cout << "energy: " << e << std::endl;

	Eigen::VectorXd dir = deriv;
	dir.setRandom();

	for (int i = 3; i < 9; i++)
	{
		double eps = std::pow(0.1, i);

		Eigen::VectorXd deriv1;
		double e1 = computeEnergy(x + eps * dir, &deriv1, NULL, false);

		std::cout << "eps: " << eps << std::endl;
		std::cout << "value-gradient check: " << (e1 - e) / eps - dir.dot(deriv) << std::endl;
		std::cout << "gradient-hessian check: " << ((deriv1 - deriv) / eps - hess * dir).norm() << std::endl;
	}
}


///////////////////////////////////////////////////////////////////////////////
// statistic.hpp                                                             //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_PEARSON_CHISQ_INDEPENDENCE_STATISTIC_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_PEARSON_CHISQ_INDEPENDENCE_STATISTIC_HPP_ER_2010
#include <cmath>
#include <boost/mpl/size.hpp>
#include <boost/mpl/fold.hpp>
#include <boost/mpl/for_each.hpp>
#include <boost/mpl/push_back.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/vector/vector10.hpp>
#include <boost/mpl/detail/wrapper.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/numeric/conversion/converter.hpp>
#include <boost/type_traits/add_const.hpp>
#include <boost/type_traits/remove_reference.hpp>
#include <boost/foreach.hpp>
#include <boost/range.hpp>
#include <boost/accumulators/statistics/detail/weighted_count.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/pearson_chisq/common/chisq_summand_formula.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/cells/cells.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/cells/count_matching.hpp>

// Under independence, in dimension = 2
// p_{j1,j2} = p_{j1} * p{j2}

// Warning : this is incomplete / Wrong.

namespace boost {
namespace statistics{
namespace detail{
namespace contingency_table{
namespace pearson_chisq_statistic{
namespace independence_between_aux{

    template<typename T1,typename Keys,typename AccSet>
    struct chisq_summand
    {

        typedef std::size_t size_;
        typedef typename boost::mpl::size<Keys>::type keys_count_;

        template<typename N>
        chisq_summand(const AccSet& a, const N& total_count)
            : acc( a ) , denom(
                pow( total_count, keys_count_::value - 1)
            ){}

        template<typename C>
        T1 operator()(const C& cell)const
        {
            namespace ct = contingency_table;
            typedef boost::mpl::detail::wrapper<
                 boost::mpl::vector1< boost::mpl::_ >
            > op_;
            typedef boost::numeric::converter<T1,size_> conv_;
            size_ n = 1;
        	boost::mpl::for_each<Keys,op_>(
                ct::make_fun_count_matcher(acc, cell.first, prod_fun( n ) )
            );
            return pearson_chisq_statistic::chisq_summand_formula<T1>(
                conv_::convert( n ) / conv_::convert( denom ),
                cell.second
            );
        }

        struct prod_fun{

            prod_fun(size_& n) : n_( n ){}

            template<typename T>
            void operator()(const T& k)const{
                this->n_ *= k;
            }

            mutable size_& n_;
        };

        const AccSet& acc;
        size_ denom;

    };

}// independence_between_aux

    template<typename T1,typename Keys,typename AccSet>
    T1 value(
        const boost::mpl::detail::wrapper<
            pearson_chisq_statistic::tag::independence_between<Keys>
        >& hypothesis,
        const AccSet& acc
    )
    {
        namespace ct = contingency_table;
        namespace ns = ct::pearson_chisq_statistic::independence_between_aux;
        typedef typename ct::result_of::extract::cells<
            Keys,AccSet>::type ref_cells_;
        typedef typename boost::remove_reference<ref_cells_>::type cells_;
        typedef typename boost::range_reference<cells_>::type ref_cell_;
        typedef ns::chisq_summand<T1,Keys,AccSet> summand_;
        typedef std::size_t size_;

        ref_cells_ ref_cells = ct::extract::cells<Keys>( acc );
        size_ n_obs = boost::accumulators::extract::weighted_count( acc );

        summand_ summand(acc, n_obs);
        typedef boost::numeric::converter<T1,size_> conv_;

        T1 result = conv_::convert( 0 );
        BOOST_FOREACH(ref_cell_ ref_cell, ref_cells )
        {
            result += summand( ref_cell );
        }

        return result;
    }

}// pearson_chisq_statistic
}// contingency_table
}// detail
}// statistics
}// boost

#endif