/*
// 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 <iostream>
#include <fstream>
#include <sstream>
#include <unordered_map>
#include <boost/format.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/progress.hpp>
#include <boost/program_options.hpp>
#include <boost/log/trivial.hpp>
#include <boost/log/utility/setup/console.hpp>
#define LOG(x) BOOST_LOG_TRIVIAL(x)
#include "csvlint.h"
#define USE_OPENMP 1
#include "bigtext.h"

using namespace std;
using namespace boost;
namespace po = boost::program_options;
namespace ba = boost::accumulators;
namespace qi = boost::spirit::qi;
typedef ba::accumulator_set<double, ba::stats<ba::tag::mean, ba::tag::variance>> Acc;

size_t topk = 20;

struct Column {
    vector<csvlint::crange> strings;
    vector<float> floats;
    size_t missing;
};

struct Chunk {
    vector<string> lines;
    vector<csvlint::crange> cols;
    vector<Column> data;
    size_t total;
};

size_t total (vector<Chunk> const &chunks) {
    size_t v = 0;
    for (auto const &ch: chunks) {
        v += ch.total;
    }
    return v;
}

size_t good (vector<Chunk> const &chunks) {
    size_t v = 0;
    for (auto const &ch: chunks) {
        v += ch.data.size();
    }
    return v;
}

// percentiles must be previously sorted
void percentiles (vector<float> &all, vector<float> &ps) {
    vector<size_t> offs(ps.size());
    for (unsigned i = 0; i < offs.size(); ++i) {
        offs[i] = round(ps[i] * all.size());
        if (offs[i] >= all.size()) {
            offs[i] = all.size() - 1;
        }
        if (i) {
            BOOST_VERIFY(offs[i] > offs[i-1]);
        }
    }
    size_t last = 0;
    for (unsigned i = 0; i < offs.size(); ++i) {
        nth_element(all.begin() + last, all.begin() + offs[i], all.end());
        ps[i] = all[offs[i]];
        last = offs[i];
    }
}

void stat_number (csvlint::Field const &f,  vector<Chunk> const &chunks, string *out) {
    vector<float> all(good(chunks));
    unsigned col = f.column;
    unsigned off = 0;
    size_t missing = 0;
    Acc acc;
    for (auto const &ch: chunks) {
        for (float e: ch.data[col].floats) {
            all[off++] = e;
            acc(e);
        }
        missing += ch.data[col].missing;
    }
    all.resize(off);
    vector<float> ps{0, 0.25, 0.5, 0.75, 1.0};
    percentiles(all, ps);
    ostringstream ss;
    ss << 'N' << f.column <<':' << f.name
        << ',' << missing
        << ',' << all.size()
        << ',' << ba::mean(acc)
        << ',' << sqrt(ba::variance(acc));
    for (auto v: ps) {
        ss << ',' << v;
    }
    *out = ss.str();
}

void stat_string (csvlint::Field const &f, vector<Chunk> const &chunks, string *out, char C='S') {
    unordered_map<string, size_t> cnts;
    unsigned col = f.column;
    size_t missing = 0;
    for (auto const &ch: chunks) {
        for (auto e: ch.data[col].strings) {
            ++cnts[string(e.begin(), e.end())];
        }
        missing += ch.data[col].missing;
    }
    vector<pair<size_t,string>> sz(cnts.size());
    {
        size_t o = 0;
        for (auto const &p: cnts) {
            sz[o].first = p.second;
            sz[o].second = p.first;
            ++o;
        }
    }
    sort(sz.begin(), sz.end());
    reverse(sz.begin(), sz.end());
    ostringstream ss;
    ss << C << f.column << ':' << f.name << ",NA:" << missing << ",VALUES:" << sz.size();
    if (sz.size() > topk) sz.resize(topk);
    for (auto const &e: sz) {
        ss << ",'" << e.second << "':" << e.first;
    }
    if (cnts.size() > sz.size()) {
        ss << ",...";
    }
    *out = ss.str();
}

void stat_column (csvlint::Field const &f, vector<Chunk> const &chunks, string *out) {
    if (f.type == csvlint::TYPE_NUMERIC) {
        string o1, o2;
        stat_number(f, chunks, &o1);
        stat_string(f, chunks, &o2, 'I');
        o1.push_back('\n');
        *out = o1 + o2;
    }
    else if (f.type == csvlint::TYPE_STRING) {
        stat_string(f, chunks, out);
    }
    else BOOST_VERIFY(0);
}

int main (int argc, char *argv[]) {
    unsigned guess_size;

    string input_path;
    string output_path;
    po::options_description desc_visible("General options");
    desc_visible.add_options()
    ("help,h", "produce help message.")
    ("input,I", po::value(&input_path), "input path")
    (",M", po::value(&guess_size)->default_value(10), "")
    ("topk", po::value(&topk)->default_value(topk), "")
    ;
    po::options_description desc("Allowed options");
    desc.add(desc_visible);

    po::positional_options_description p;
    p.add("input", 1);

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
    po::notify(vm);

    if (vm.count("help") || vm.count("input") == 0) {
        cout << "Usage: csvlint-stat [OTHER OPTIONS]... <data>" << endl;
        cout << desc_visible << endl;
        return 0;
    }

    boost::log::add_console_log(cerr);

    csvlint::Format fmt;
    fmt.train(input_path, guess_size * 1024 * 1024);

    cerr << "Parsing text..." << endl;
    BigText<Chunk> text(input_path, fmt.data_offset, '\n', fmt.max_line, 10 * 1024*1024);

    for (auto &ch: text) {
        ch.total = 0;
        ch.data.resize(fmt.fields.size());
        for (auto &v: ch.data) {
            v.missing = 0;
        }
    }
    text.lines ([&fmt](char const *begin, char const *end, Chunk *ch, size_t i_in_block) {
        auto &cols = ch->cols;
        ch->lines.emplace_back(begin, end);
        bool r = fmt.parse(csvlint::crange(std::ref(ch->lines.back())), &cols);
        if (r) {
            for (unsigned i = 0; i < fmt.fields.size(); ++i) {
                Column &data = ch->data[i];
                auto const &field = fmt.fields[i];
                csvlint::crange e = cols[i];
                if (e.missing()) {
                    ++data.missing;
                }
                else {
                    if (field.type == csvlint::TYPE_NUMERIC) {
                        float v;
                        qi::parse(e.begin(), e.end(), qi::float_, v);
                        data.floats.push_back(v);
                    }
                    data.strings.push_back(e);
                }
            }
        }
        ++ch->total;
    });

    cerr << total(text) << " total lines." << endl;
    cerr << good(text) << " good lines." << endl;

    vector<string> stats(fmt.fields.size());

    cerr << "Counting numbers..." << endl;
    progress_display progress(fmt.fields.size(), cerr);
#pragma omp parallel for
    for (unsigned i = 0; i < fmt.fields.size(); ++i) {
        stat_column(fmt.fields[i], text, &stats[i]);
#pragma omp critical
        ++progress;
    }

    for (auto &st: stats) {
        if (st.empty()) continue;
        cout << st << endl;
    }

    return 0;
}


#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;
}


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/csc.hpp>
#include <boost/simd/function/restricted.hpp>
#include <scalar_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/sqrt_2.hpp>
#include <boost/simd/constant/pio_2.hpp>
#include <boost/simd/constant/pio_4.hpp>

STF_CASE_TPL (" csc",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::csc;

  using r_t = decltype(csc(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(csc(-bs::Zero<T>()), bs::Minf<r_t>(), 0.5);
  STF_ULP_EQUAL(csc(bs::Inf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(csc(bs::Minf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(csc(bs::Nan<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(csc(bs::Zero<T>()), bs::Inf<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(csc(-bs::Pio_2<T>()), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(csc(-bs::Pio_4<T>()), -bs::Sqrt_2<r_t>(), 0.5);
  STF_ULP_EQUAL(csc(bs::Pio_2<T>()), bs::One<r_t>(), 0.5);
  STF_ULP_EQUAL(csc(bs::Pio_4<T>()), bs::Sqrt_2<r_t>(), 0.5);
}

STF_CASE_TPL (" csc restricted_",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::csc;

  using r_t = decltype(csc(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(bs::restricted_(csc)(-bs::Zero<T>()), bs::Minf<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(csc)(bs::Inf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(csc)(bs::Minf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(csc)(bs::Nan<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(csc)(bs::Zero<T>()), bs::Inf<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(bs::restricted_(csc)(-bs::Pio_2<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(csc)(-bs::Pio_4<T>()), -bs::Sqrt_2<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(csc)(bs::Pio_2<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(csc)(bs::Pio_4<T>()), bs::Sqrt_2<r_t>(), 0.5);
}

STF_CASE_TPL (" csc clipped_small_",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bst = bs::tag;
  namespace bd = boost::dispatch;
  using bs::csc;

  using r_t = decltype(csc(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(csc(-bs::Zero<T>(), bst::clipped_small_), bs::Minf<r_t>(), 0.5);
  STF_ULP_EQUAL(csc(bs::Inf<T>(), bst::clipped_small_), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(csc(bs::Minf<T>(), bst::clipped_small_), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(csc(bs::Nan<T>(), bst::clipped_small_), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(csc(bs::Zero<T>(), bst::clipped_small_), bs::Inf<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(csc(-bs::Pio_2<T>(), bst::clipped_small_), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(csc(-bs::Pio_4<T>(), bst::clipped_small_), -bs::Sqrt_2<r_t>(), 0.5);
  STF_ULP_EQUAL(csc(bs::Pio_2<T>(), bst::clipped_small_), bs::One<r_t>(), 0.5);
  STF_ULP_EQUAL(csc(bs::Pio_4<T>(), bst::clipped_small_), bs::Sqrt_2<r_t>(), 0.5);
}


/*
 * 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 "kinematics.h"
#include "motor_control.h"
#include <armadillo>

using namespace arma;
using namespace std;

int main()
{
	double x,y,z;
	while(1)
	{
		cout<<"Enter x,y,z values: ";
		cin>>x>>y>>z;
		mat target;
		target<<x<<y<<z;
		target.print();
		mat theta_default;
		theta_default<<0<<1.5707<<1.5707<<0;
		theta_default.print();
		mat calc = calculate_ccd(theta_default, target);
		calc.print("CALC");
	}
	return 0;
}


#include <boost/math/special_functions/gamma.hpp>


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../util/FluidEigenMappings.hpp"
#include "../util/OutlierDetection.hpp"
#include "../util/Stats.hpp"
#include "../util/WeightedStats.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <cassert>
#include <cmath>

namespace fluid {
namespace algorithm {

class MultiStats
{
public:
  using ArrayXd = Eigen::ArrayXd;
  using ArrayXi = Eigen::ArrayXi;

  void init(index numDerivatives, double low, double mid, double high)
  {
    assert(numDerivatives <= 2);
    mNumDerivatives = numDerivatives;
    mLow = low / 100.0;
    mMiddle = mid / 100.0;
    mHigh = high / 100.0;
  }

  index numStats() { return 7; }

  ArrayXd diff(Eigen::Ref<ArrayXd> in)
  {
    return in.segment(1, in.size() - 1) - in.segment(0, in.size() - 1);
  }

  void process(const RealMatrixView in, RealMatrixView out, double cutoff = -1,
               RealVectorView w = RealVectorView(nullptr, 0, 0))
  {
    using namespace Eigen;
    using namespace _impl;
    using fluid::Slice;
    assert(out.size() == in.rows() * numStats() * (mNumDerivatives + 1));
    bool     weighted = w.size() > 0;
    ArrayXXd input = asEigen<Array>(in);
    ArrayXd  weights = asEigen<Array>(w);
    index    numChannels = input.rows();
    index    numFrames = input.cols();
    ArrayXi  mask = ArrayXi::Ones(numFrames);

    if (cutoff >= 0)
    {
      for (index i = 0; i < numChannels; i++)
      { OutlierDetection().process(input.row(i), mask, cutoff); }
    }
    index numCleanFrames = mask.sum();
    if (numCleanFrames <= 0) return;
    if (weighted && weights.sum() <= 0) return;
    ArrayXXd filtered = ArrayXXd::Zero(numChannels, numCleanFrames);
    ArrayXd  filteredWeights;
    if (weighted) filteredWeights = ArrayXd::Zero(numCleanFrames);
    index k = 0;
    for (index j = 0; j < mask.size(); j++)
    {
      if (mask(j) > 0)
      {
        filtered.col(k) = input.col(j);
        if (weighted) filteredWeights(k) = weights(j);
        k++;
      }
    }
    filteredWeights = (filteredWeights >= 0).select(filteredWeights, 0);
    if (weighted && filteredWeights.size() > 0)
    {
      double sum = filteredWeights.sum();
      if (sum > 0) filteredWeights = filteredWeights / filteredWeights.sum();
    }
    ArrayXXd result =
        ArrayXXd::Zero(numChannels, numStats() * (mNumDerivatives + 1));
    for (index i = 0; i < numChannels; i++)
    {
      ArrayXd d1, d2, d1Weights, d2Weights;
      ArrayXd channel = filtered.row(i);
      result.block(i, 0, 1, numStats()) =
          weighted
              ? WeightedStats()
                    .process(channel, filteredWeights, mLow, mMiddle, mHigh)
                    .matrix()
                    .transpose()
              : Stats()
                    .process(channel, mLow, mMiddle, mHigh)
                    .matrix()
                    .transpose();
      if (mNumDerivatives > 0 && numCleanFrames >= 2)
      {
        d1 = diff(channel);
        if (weighted)
          d1Weights = filteredWeights.segment(1, numCleanFrames - 1);
        result.block(i, numStats(), 1, numStats()) =
            weighted ? WeightedStats()
                           .process(d1, d1Weights, mLow, mMiddle, mHigh)
                           .matrix()
                           .transpose()
                     : Stats()
                           .process(d1, mLow, mMiddle, mHigh)
                           .matrix()
                           .transpose();
      }
      if (mNumDerivatives > 1 && numCleanFrames >= 3)
      {
        d2 = diff(d1);
        if (weighted)
          d2Weights = filteredWeights.segment(2, numCleanFrames - 2);
        result.block(i, 2 * numStats(), 1, numStats()) =
            weighted ? WeightedStats()
                           .process(d2, d2Weights, mLow, mMiddle, mHigh)
                           .matrix()
                           .transpose()
                     : Stats()
                           .process(d2, mLow, mMiddle, mHigh)
                           .matrix()
                           .transpose();
      }
    }
    out = asFluid(result);
  }
  index  mNumDerivatives{0};
  double mLow{0};
  double mMiddle{0.5};
  double mHigh{1};
};
} // namespace algorithm
} // namespace fluid


#pragma once
#include <algorithm>
#include <array>
#include <boost/assert.hpp>
#include <boost/intrusive/list.hpp>
#include <boost/intrusive/set.hpp>
#include <memory>

namespace sssp {

inline int ceil_log2(size_t n) {
    size_t result = 1;
    int bit = 0;
    while (result < n) {
        result *= 2;
        bit += 1;
    }
    return bit;
}

// 128 MiB chunks with 256 byte minimum allocation
static constexpr int buddy_allocator_layers = 19;
static constexpr int buddy_allocator_missing_layers = 8;
static constexpr size_t buddy_allocator_chunk_bytes = 1 << (buddy_allocator_layers + buddy_allocator_missing_layers);
static constexpr size_t buddy_allocator_min_bytes = 1 << buddy_allocator_missing_layers;

// buddy_allocator_nodes form a doubly-linked list and contain a free flag.
struct buddy_allocator_node : boost::intrusive::list_base_hook<> {
    int free_in_layer = buddy_allocator_layers;
};

// A chunk of memory.
struct buddy_allocator_chunk : boost::intrusive::set_base_hook<> {
  public:
    buddy_allocator_chunk() { m_layers.back().push_front(m_nodes[0]); }
    buddy_allocator_chunk(const buddy_allocator_chunk& other) = delete;
    buddy_allocator_chunk(buddy_allocator_chunk&& other) = delete;
    buddy_allocator_chunk& operator=(const buddy_allocator_chunk& other) = delete;
    buddy_allocator_chunk& operator=(buddy_allocator_chunk&& other) = delete;

    size_t alloc_node(int layer);
    void free_node(int layer, size_t offset);
    char* base_pointer() { return &m_memory[0]; }
    const char* base_pointer() const { return &m_memory[0]; }

  private:
    // Preallocate all possibly required nodes
    std::array<buddy_allocator_node, 1 << buddy_allocator_layers> m_nodes;
    // Free list for each layer. The extra layer is the indivisible root layer
    std::array<boost::intrusive::list<buddy_allocator_node>, buddy_allocator_layers + 1> m_layers;
    // The memory
    alignas(max_align_t) char m_memory[buddy_allocator_chunk_bytes];
};

inline bool operator<(const buddy_allocator_chunk& a, const buddy_allocator_chunk& b) {
    return a.base_pointer() < b.base_pointer();
}
inline bool operator==(const buddy_allocator_chunk& a, const buddy_allocator_chunk& b) {
    return a.base_pointer() == b.base_pointer();
}

// This holds all the resources needed by an buddy_allocator. Only
// free this once no more allocators use it!
class buddy_allocator_memory {
  public:
    buddy_allocator_memory() = default;
    buddy_allocator_memory(const buddy_allocator_memory& other) = delete;
    buddy_allocator_memory(buddy_allocator_memory&& other) = delete;
    buddy_allocator_memory& operator=(const buddy_allocator_memory& other) = delete;
    buddy_allocator_memory& operator=(buddy_allocator_memory&& other) = delete;
    ~buddy_allocator_memory() {
        m_chunks.clear_and_dispose([](buddy_allocator_chunk* chunk) { delete chunk; });
    }

    char* alloc(int layer);
    void free(int layer, char* address);

  private:
    boost::intrusive::set<buddy_allocator_chunk> m_chunks;
    buddy_allocator_chunk* m_last_success = nullptr;
};

// This is a *none* thread safe buddy allocator.
template <typename T> class buddy_allocator {
  public:
    template <typename OtherT> friend class buddy_allocator;

    using pointer = T*;
    using const_pointer = const T*;
    using reference = T&;
    using const_reference = const T&;
    using void_pointer = void*;
    using const_void_pointer = const void*;
    using value_type = T;
    using size_type = size_t;
    using difference_type = ptrdiff_t;
    template <typename OtherT> struct rebind { using other = buddy_allocator<OtherT>; };

    // Creates a buddy allocator with the given state. Once the state is
    // destroyed, the allocator becomes invalid and all memory is lost.
    // Therefore only free the state once all allocator instances are free'd as well.
    buddy_allocator(buddy_allocator_memory* state) : m_state(state) {}
    // This is the type converting constructor.
    template <typename OtherT> buddy_allocator(const buddy_allocator<OtherT>& other) : m_state(other.m_state) {}
    // Appropriate copy and move semantics.
    buddy_allocator(const buddy_allocator<T>& other) : m_state(other.m_state) {}
    buddy_allocator(const buddy_allocator<T>&& other) : m_state(other.m_state) {}
    buddy_allocator& operator=(const buddy_allocator<T>& other) {
        m_state = other.m_state;
        return *this;
    }
    buddy_allocator& operator=(const buddy_allocator<T>&& other) {
        m_state = other.m_state;
        return *this;
    }

    // Equality operators
    template <typename OtherT> bool operator==(const buddy_allocator<OtherT>& other) {
        return m_state == other.m_state;
    }
    template <typename OtherT> bool operator!=(const buddy_allocator<OtherT>& other) {
        return m_state != other.m_state;
    }

    // Allocate memory.
    T* allocate(size_t n) {
        int layer = std::max(0, ceil_log2(sizeof(T) * n) - buddy_allocator_missing_layers);
        if (layer < buddy_allocator_layers) {
            return reinterpret_cast<T*>(m_state->alloc(layer));
        } else {
            return static_cast<T*>(malloc(sizeof(T) * n));
        }
    }

    // Free memory.
    void deallocate(T* ptr, size_t n) {
        int layer = std::max(0, ceil_log2(sizeof(T) * n) - buddy_allocator_missing_layers);
        if (layer < buddy_allocator_layers) {
            m_state->free(layer, reinterpret_cast<char*>(ptr));
        } else {
            free(ptr);
        }
    }

    void destroy(T* ptr) { ptr->~T(); }

  private:
    buddy_allocator_memory* m_state;
};

} // namespace sssp


#include "rlbot/rlbot_generated.h"
#include "rlbot/bot.h"
#include "rlbot/scopedrenderer.h"

#include "../badbot.h"

#include "path.h"

#include <vector>
#include <algorithm>
#include <iostream>

#include <Eigen/Dense>

using namespace Eigen;

namespace badbot {

	namespace paths {
		Bezier::Bezier(const util::Car car, const Vector3f t, const util::BallPrediction& bp) :
			target(t),
			car(car),
			bp(bp){

			float t_approx = (car.physics.location - target).norm() / car.physics.velocity.norm() + bp.predictionSlices[0].gameSeconds;

			int intercept = 0;
			for (; intercept < bp.predictionSlices.size(); ++intercept) {

				if (bp.predictionSlices[intercept].gameSeconds > t_approx) break;

			}

			if (intercept != 0) --intercept;

			Vector3f target = bp.predictionSlices[intercept].physics.location;

			float dist = (car.physics.location - target).norm();

			Vector3f p0 = car.physics.location;
			Vector3f p1 = car.physics.location + car.physics.forward() * std::min(dist, 100.0f);
			Vector3f goalVector = car.other_goal() - target;
			goalVector.z() = 0;
			goalVector.normalize();
			Vector3f p2 = target - goalVector*dist/2;
			Vector3f p3 = target;

			bool debug = true;
			if (debug && car.team == 0) {
				rlbot::ScopedRenderer renderer("BezierDebug" + car.name);
				renderer.DrawRect3D(rlbot::Color::magenta, util::convert(p0), 20, 20, 1, 1);
				renderer.DrawRect3D(rlbot::Color::magenta, util::convert(p1), 20, 20, 1, 1);
				renderer.DrawRect3D(rlbot::Color::magenta, util::convert(p2), 20, 20, 1, 1);
				renderer.DrawRect3D(rlbot::Color::magenta, util::convert(p3), 20, 20, 1, 1);
			}

			//p1 = { 0, 0, 0 };
			//p2 = { 500, 1000, 0 };
			//p3 = {-500, 1000, 0 };

			//p1 = (p0 + p3) / 2;
			//p2 = p1;

			curve = [=](float t) -> Vector3f{	return	std::pow(1 - t, 3) * p0
														+ 3 * std::pow(1 - t, 2) * t * p1
														+ 3 * (1 - t) * t * t * p2
														+ std::pow(t, 3) * p3; };

			for (size_t i = 1; i <= util::NUM_POINTS; ++i) {
				path.push_back(curve(i * 1.0f / util::NUM_POINTS));
			}
			//rlbot::ScopedRenderer renderer("Renderer" + std::to_string(car.team));

			//std::vector <const rlbot::flat::Vector3*> points;

			//points.push_back(new rlbot::flat::Vector3{ p0[0], p0[1], p0[2] });
			//points.push_back(new rlbot::flat::Vector3{ p1[0], p1[1], p1[2] });
			//points.push_back(new rlbot::flat::Vector3{ p2[0], p2[1], p2[2] });
			//points.push_back(new rlbot::flat::Vector3{ p3[0], p3[1], p3[2] });

			//renderer.DrawPolyLine3D(rlbot::Color::yellow, points);
		}

		void Bezier::getPath(std::vector<const rlbot::flat::Vector3*>& points) const {

			for (auto point : points) delete point;
			points.clear();

			//TODO IMPLEMENT ACTUAL THINGY
			//unsigned len = std::min(path.size(), util::NUM_POINTS);
			for (unsigned i = 0; i < util::NUM_POINTS; ++i) {
				points.push_back(new rlbot::flat::Vector3{ path[i][0], path[i][1], path[i][2] });
			}

		}

		rlbot::Controller Bezier::getControl() const {
			return util::optimalGroundControl(car, path);
		}

	}//namespace paths
}//namespace badbot


#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