#include <cstddef>
#include <cstdint>

#include <vector>
#include <string>
#include <iterator>
#include <iostream>
#include <unordered_map>

#include <boost/graph/compressed_sparse_row_graph.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/lookup_edge.hpp>
#include <boost/graph/edmonds_karp_max_flow.hpp>

#include <boost/range/algorithm/for_each.hpp>

#include <z3++.h>

using Graph = boost::compressed_sparse_row_graph<>;
using Vertex = typename boost::graph_traits<Graph>::vertex_descriptor;
using Edge = typename boost::graph_traits<Graph>::edge_descriptor;

Graph makeGraph() {
  const auto tag = boost::edges_are_unsorted_multi_pass;
  using VertexPair = std::pair<std::size_t, std::size_t>;

  /*
     1
    / \
   0 - 3
    \ /
     2
  */
  const std::size_t numVertices{4};
  std::vector<VertexPair> edges{{0, 1}, {1, 0}, {0, 2}, {2, 0}, {1, 3}, {3, 1}, {2, 3}, {3, 2}, {0, 3}, {3, 0}};

  return {tag, begin(edges), end(edges), numVertices};
}

void makeMaxFlow(const Graph& graph, const Vertex s, const Vertex t) {
  std::vector<std::uint8_t> capacities(num_edges(graph), 0);
  std::vector<std::uint8_t> residuals(num_edges(graph), 0);
  std::vector<Edge> reverse(num_edges(graph));

  auto capacityMap = boost::make_iterator_property_map(begin(capacities), get(boost::edge_index, graph));
  auto residualMap = boost::make_iterator_property_map(begin(residuals), get(boost::edge_index, graph));
  auto reverseMap = boost::make_iterator_property_map(begin(reverse), get(boost::edge_index, graph));

  for_each(edges(graph), [&](const auto edge) {
    const auto reverseEdge = lookup_edge(target(edge, graph), source(edge, graph), graph).first;
    put(reverseMap, edge, reverseEdge);

    put(capacityMap, edge, 1);
    put(residualMap, edge, 1);
  });

  const auto flow = edmonds_karp_max_flow(graph, s, t, capacity_map(capacityMap) //
                                                           .residual_capacity_map(residualMap)
                                                           .reverse_edge_map(reverseMap));

  std::cout << flow << std::endl;
}

void makeMaxFlowSolver(const Graph& graph, const Vertex s, const Vertex t) {
  z3::context context;
  z3::optimize solver{context};

  std::unordered_map<std::string, z3::expr> symbols;
  std::vector<z3::expr> constraints;

  const z3::expr zero{context.int_val(0)};

  const auto symbolize = [&](const auto edge) {
    const auto from = source(edge, graph);
    const auto to = target(edge, graph);
    return "x_" + std::to_string(from) + "_" + std::to_string(to);
  };

  boost::for_each(edges(graph), [&](const auto edge) {
    const auto edgeSym = symbolize(edge);
    symbols.emplace(edgeSym, context.int_const(edgeSym.c_str()));
  });

  boost::for_each(edges(graph), [&](const auto edge) {
    const auto edgeSym = symbolize(edge);
    constraints.emplace_back(symbols.at(edgeSym) >= 0);
    constraints.emplace_back(symbols.at(edgeSym) <= 1);
  });

  boost::for_each(vertices(graph), [&](const auto vertex) {
    if (vertex == s or vertex == t)
      return;

    z3::expr outgoing{zero}, incoming{zero};
    boost::for_each(out_edges(vertex, graph), [&](const auto edge) { outgoing = outgoing + symbols.at(symbolize(edge)); });

    std::vector<Edge> inEdges;
    boost::for_each(edges(graph), [&](const auto edge) {
      const auto to = target(edge, graph);
      if (to == vertex)
        inEdges.push_back(edge);
    });

    boost::for_each(inEdges, [&](const auto edge) { incoming = incoming + symbols.at(symbolize(edge)); });

    constraints.emplace_back(outgoing - incoming == 0);
  });

  z3::expr outgoing{zero}, incoming{zero};
  boost::for_each(out_edges(s, graph), [&](const auto edge) { outgoing = outgoing + symbols.at(symbolize(edge)); });

  std::vector<Edge> inEdges;
  boost::for_each(edges(graph), [&](const auto edge) {
    const auto to = target(edge, graph);
    if (to == s)
      inEdges.push_back(edge);
  });

  boost::for_each(inEdges, [&](const auto edge) { incoming = incoming + symbols.at(symbolize(edge)); });

  boost::for_each(constraints, [&](const auto& constraint) { std::cout << constraint << std::endl; });

  solver.maximize(outgoing - incoming);

  if (solver.check() not_eq z3::sat)
    return;

  const auto model = solver.get_model();
  std::cout << model << std::endl;
}

int main() {
  const auto graph = makeGraph();
  const Vertex source{0}, target{3};

  //makeMaxFlow(graph, source, target);
  makeMaxFlowSolver(graph, source, target);
}


#include <NTL/GF2E.h>

#include <NTL/new.h>

NTL_START_IMPL

GF2EInfoT::GF2EInfoT(const GF2X& NewP)
{
   ref_count = 1;

   build(p, NewP);

   if (p.size == 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         KarCross = 4;
      else
         KarCross = 8;
   }
   else if (p.size == 2)
      KarCross = 8;
   else if (p.size <= 5)
      KarCross = 4;
   else if (p.size == 6)
      KarCross = 3;
   else
      KarCross = 2;

   if (p.size <= 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         ModCross = 20;
      else
         ModCross = 40;
   }
   else if (p.size <= 2)
      ModCross = 75;
   else if (p.size <= 4)
      ModCross = 50;
   else
      ModCross = 25;

   if (p.size == 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         DivCross = 100;
      else
         DivCross = 200;
   }
   else if (p.size == 2)
      DivCross = 400;
   else if (p.size <= 4)
      DivCross = 200;
   else if (p.size == 5)
      DivCross = 150;
   else if (p.size <= 13)
      DivCross = 100;
   else
      DivCross = 75;

   _card_init = 0;
   _card_exp = p.n;
}

const ZZ& GF2E::cardinality()
{
   if (!GF2EInfo) Error("GF2E::cardinality: undefined modulus");

   if (!GF2EInfo->_card_init) {
      power(GF2EInfo->_card, 2, GF2EInfo->_card_exp);
      GF2EInfo->_card_init = 1;
   }

   return GF2EInfo->_card;
}

GF2EInfoT *GF2EInfo = 0;

typedef GF2EInfoT *GF2EInfoPtr;

static
void CopyPointer(GF2EInfoPtr& dst, GF2EInfoPtr src)
{
   if (src == dst) return;

   if (dst) {
      dst->ref_count--;

      if (dst->ref_count < 0)
         Error("internal error: negative GF2EContext ref_count");

      if (dst->ref_count == 0) delete dst;
   }

   if (src) {
      if (src->ref_count == NTL_MAX_LONG)
         Error("internal error: GF2EContext ref_count overflow");

      src->ref_count++;

   }

   dst = src;
}

void GF2E::init(const GF2X& p)
{
   GF2EContext c(p);
   c.restore();
}

GF2EContext::GF2EContext(const GF2X& p)
{
   ptr = NTL_NEW_OP GF2EInfoT(p);
}

GF2EContext::GF2EContext(const GF2EContext& a)
{
   ptr = 0;
   CopyPointer(ptr, a.ptr);
}

GF2EContext& GF2EContext::operator=(const GF2EContext& a)
{
   CopyPointer(ptr, a.ptr);
   return *this;
}

GF2EContext::~GF2EContext()
{
   CopyPointer(ptr, 0);
}

void GF2EContext::save()
{
   CopyPointer(ptr, GF2EInfo);
}

void GF2EContext::restore() const
{
   CopyPointer(GF2EInfo, ptr);
}

GF2EBak::~GF2EBak()
{
   if (MustRestore)
      CopyPointer(GF2EInfo, ptr);

   CopyPointer(ptr, 0);
}

void GF2EBak::save()
{
   MustRestore = 1;
   CopyPointer(ptr, GF2EInfo);
}

void GF2EBak::restore()
{
   MustRestore = 0;
   CopyPointer(GF2EInfo, ptr);
}

const GF2E& GF2E::zero()
{
   static GF2E z(GF2E_NoAlloc);
   return z;
}

istream& operator>>(istream& s, GF2E& x)
{
   GF2X y;

   s >> y;
   conv(x, y);

   return s;
}

void div(GF2E& x, const GF2E& a, const GF2E& b)
{
   GF2E t;

   inv(t, b);
   mul(x, a, t);
}

void div(GF2E& x, GF2 a, const GF2E& b)
{
   inv(x, b);
   mul(x, x, a);
}

void div(GF2E& x, long a, const GF2E& b)
{
   inv(x, b);
   mul(x, x, a);
}

void inv(GF2E& x, const GF2E& a)
{
   InvMod(x._GF2E__rep, a._GF2E__rep, GF2E::modulus());
}

NTL_END_IMPL


// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Benjamin Nuernberger (bnuernberger@cs.ucsb.edu)

#include "theia/sfm/estimators/estimate_dominant_plane_from_points.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <limits>
#include <memory>
#include <vector>

#include "theia/sfm/create_and_initialize_ransac_variant.h"
#include "theia/sfm/pose/util.h"
#include "theia/solvers/estimator.h"
#include "theia/solvers/sample_consensus_estimator.h"
#include "theia/util/util.h"

namespace theia {
namespace {

using Eigen::Vector3d;

// An estimator for computing a dominant plane from a set of 3D points.
class DominantPlaneEstimator : public Estimator<Vector3d, Plane> {
 public:
  DominantPlaneEstimator() {}

  // 3 non-collinear points are needed to determine a plane.
  double SampleSize() const { return 3; }

  // Estimates candidate dominant planes from three 3D points.
  bool EstimateModel(const std::vector<Vector3d>& points,
                     std::vector<Plane>* planes) const {
    // If the points are collinear, there are no possible solutions.
    static const double kTolerance = 1e-6;
    const Vector3d a = points[1] - points[0];
    const Vector3d b = points[2] - points[0];
    const Vector3d cross = a.cross(b);
    if (cross.squaredNorm() < kTolerance) {
      VLOG(3) << "The 3 world points are collinear! No solution for a plane "
                 "exists.";
      return false;
    }
    Plane plane;
    plane.point = points[0];
    plane.unit_normal = cross.normalized();

    planes->emplace_back(plane);
    return true;
  }

  // The error for a point given a plane model is the point-to-plane distance.
  double Error(const Vector3d& point, const Plane& plane) const {
    return std::abs(plane.unit_normal.dot(point - plane.point));
  }

 private:
  DISALLOW_COPY_AND_ASSIGN(DominantPlaneEstimator);
};

}  // namespace

bool EstimateDominantPlaneFromPoints(const RansacParameters& ransac_params,
                                     const RansacType& ransac_type,
                                     const std::vector<Vector3d>& points,
                                     Plane* plane,
                                     RansacSummary* ransac_summary) {
  DominantPlaneEstimator dominant_plane_estimator;
  std::unique_ptr<SampleConsensusEstimator<DominantPlaneEstimator> > ransac =
      CreateAndInitializeRansacVariant(
          ransac_type, ransac_params, dominant_plane_estimator);
  // Estimate the dominant plane.
  return ransac->Estimate(points, plane, ransac_summary);
}

}  // namespace theia


#pragma once
#ifndef CANNON_MATH_LATTICE_POINTS_H
#define CANNON_MATH_LATTICE_POINTS_H

/*!
 * \file cannon/math/lattice_points.hpp
 * \brief File containing utilities for working with points making up a lattice
 * in arbitrary real vector spaces.
 */

#include <queue>

#include <Eigen/Dense>

using namespace Eigen;

namespace cannon {
  namespace math {

      using VectorXu = Matrix<unsigned int, Dynamic, 1>;

      /*!
       * \brief Generate all lattice points on an integer-valued grid.
       *
       * \param dim Which dimension of the lattice to generate.
       * \param sizes Number of elements in each dimension of the grid.
       *
       * \return Lattice points for dimensions 0 through dim-1
       */
      std::vector<VectorXu> make_lattice_points(unsigned int dim, const VectorXu& sizes) {
        std::vector<VectorXu> ret_vec;

        if (dim == 1) {
          for (unsigned int i = 0; i < sizes[dim-1]; i++) {
            VectorXu tmp = VectorXu::Zero(dim);
            tmp[0] = i;
            ret_vec.push_back(tmp);
          }
        } else {
          for (unsigned int i = 0; i < sizes[dim-1]; i++) {
            auto smaller_vec = make_lattice_points(dim-1, sizes);
            for (const auto& coord : smaller_vec) {
              VectorXu tmp = VectorXu::Zero(dim);
              tmp.head(dim-1) = coord;
              tmp[dim-1] = i;

              ret_vec.push_back(tmp);
            }
          }
        }

        return ret_vec;
      }

      /*!
       * \brief Generate all lattice points on an integer-valued grid.
       *
       * \param sizes Number of elements in each dimension of the grid.
       *
       * \return Lattice points for the grid.
       */
      std::vector<VectorXu> make_lattice_points(const VectorXu& sizes) {
        return make_lattice_points(sizes.size(), sizes);
      }

  } // namespace math
} // namespace cannon

#endif /* ifndef CANNON_MATH_LATTICE_POINTS_H */


#ifndef STAN_MATH_PRIM_SCAL_FUN_CHOOSE_HPP
#define STAN_MATH_PRIM_SCAL_FUN_CHOOSE_HPP

#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_less_or_equal.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <limits>
#include <cmath>

namespace stan {
namespace math {

/**
 * Return the binomial coefficient for the specified integer
 * arguments.
 *
 * The binomial coefficient, \f${n \choose k}\f$, read "n choose k", is
 * defined for \f$0 \leq k \leq n\f$ (otherwise return 0) by
 *
 * \f${n \choose k} = \frac{n!}{k! (n-k)!}\f$.
 *
 * @param n total number of objects
 * @param k number of objects chosen
 * @return n choose k or 0 iff k > n
 * @throw std::domain_error if either argument is negative or the
 * result will not fit in an int type
 */
inline int choose(int n, int k) {
  check_nonnegative("choose", "n", n);
  check_nonnegative("choose", "k", k);
  if (k > n)
    return 0;
  const double choices = boost::math::binomial_coefficient<double>(n, k);
  check_less_or_equal("choose", "n choose k", choices,
                      std::numeric_limits<int>::max());
  return static_cast<int>(std::round(choices));
}

}  // namespace math
}  // namespace stan
#endif


//---------------------------------Spheral++----------------------------------//
// GeomSymmetricTensor -- the symmetric tensor class
//----------------------------------------------------------------------------//
#include <cmath>
#include <limits>
#include <float.h>
#include <vector>

#include "GeomSymmetricTensor.hh"
#include "EigenStruct.hh"
#include "buildEigenVector.hh"
#include "findEigenValues3.hh"
#include "Utilities/SpheralFunctions.hh"
#include "Utilities/rotationMatrix.hh"

#include "Jacobi2.hh"

#include <Eigen/Dense>

#include <cmath>
using std::min;
using std::max;
using std::abs;

namespace Spheral {

using std::abs;

//------------------------------------------------------------------------------
// Return the eigen values and eigen vectors of a symmetric tensor
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// 3-D.
template<>
EigenStruct<3>
GeomSymmetricTensor<3>::eigenVectors() const {

  // Some useful typedefs.
  typedef GeomVector<3> Vector;
  typedef GeomTensor<3> Tensor;
  typedef GeomSymmetricTensor<3> SymTensor;

  // Tolerances for fuzzy math.
  const double degenerate = 1.0e-20;
  const double tolerance = 5.0e-5;

  // Prepare the result.
  EigenStruct<3> result;

  // Create a scaled version of this tensor, with all elements in the range [-1,1].
  const double fscale = max(10.0*std::numeric_limits<double>::epsilon(), this->maxAbsElement());
  CHECK(fscale > 0.0);
  const double fscalei = 1.0/fscale;
  SymTensor A = (*this)*fscalei;

  // Check for any degenerate elements, and just zero 'em out.
  A.xx(abs(A.xx()) < degenerate ? 0.0 : A.xx());
  A.xy(abs(A.xy()) < degenerate ? 0.0 : A.xy());
  A.xz(abs(A.xz()) < degenerate ? 0.0 : A.xz());
  A.yy(abs(A.yy()) < degenerate ? 0.0 : A.yy());
  A.yz(abs(A.yz()) < degenerate ? 0.0 : A.yz());
  A.zz(abs(A.zz()) < degenerate ? 0.0 : A.zz());

// #ifdef USEJACOBI

//   // Use the Jacobi iterative diagonalization method to determine
//   // the eigen values/vectors.
//   const int nrot = jacobiDiagonalize<Dim<3> >(A,
//                                               result.eigenVectors,
//                                               result.eigenValues);
//   result.eigenValues *= fscale;

// #elif USEEIGEN

  // Use the Eigen library to determine the eigen values/vectors.
  {
    Eigen::Matrix3d B;
    B <<
      A.xx(), A.xy(), A.xz(),
      A.yx(), A.yy(), A.yz(),
      A.zx(), A.zy(), A.zz();
    const Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(B);
    const Eigen::Vector3d& Bvals = eigensolver.eigenvalues();
    const Eigen::Matrix3d& Bvecs = eigensolver.eigenvectors();
    result.eigenValues = Vector(Bvals(0), Bvals(1), Bvals(2)) * fscale;
    const double x1 = 1.0/std::sqrt(Bvecs(0,0)*Bvecs(0,0) + Bvecs(1,0)*Bvecs(1,0) + Bvecs(2,0)*Bvecs(2,0));
    const double x2 = 1.0/std::sqrt(Bvecs(0,1)*Bvecs(0,1) + Bvecs(1,1)*Bvecs(1,1) + Bvecs(2,1)*Bvecs(2,1));
    const double x3 = 1.0/std::sqrt(Bvecs(0,2)*Bvecs(0,2) + Bvecs(1,2)*Bvecs(1,2) + Bvecs(2,2)*Bvecs(2,2));
    result.eigenVectors = Tensor(Bvecs(0,0)*x1, Bvecs(0,1)*x2, Bvecs(0,2)*x3,
                                 Bvecs(1,0)*x1, Bvecs(1,1)*x2, Bvecs(1,2)*x3,
                                 Bvecs(2,0)*x1, Bvecs(2,1)*x2, Bvecs(2,2)*x3);
  }

// #else

//   // Compute the scaled eigen-values, and sort them.
//   Vector lambdaVec = A.eigenValues();
//   sort(lambdaVec.begin(), lambdaVec.end());
//   CHECK(lambdaVec.x() <= lambdaVec.y() and
//         lambdaVec.y() <= lambdaVec.z());

//   // Assign the true eigen-values in the result.
//   result.eigenValues = fscale*lambdaVec;
//   result.eigenVectors = SymTensor::one;

//   // If any of the eigen-values result in a tensor that is not positive-rank
//   // (all zero elements), we assume the eigen-values are equal and punt
//   // with the identity tensor for the eigen-vectors.
//   // We simultaneously compute the row containing the maximum absolute value
//   // element for each eigen-value.
//   bool punt = false;
//   double maxEVelement = -1.0;
//   Vector maxEVrow;
//   int iFirst = -1;
//   for (int ivalue = 0; ivalue != 3; ++ivalue) {
//     const SymTensor M = A - lambdaVec(ivalue)*SymTensor::one;
//     if (M.maxAbsElement() < degenerate) punt = true;
//     for (int irow = 0; irow != 3; ++irow) {
//       const Vector Mvec = M.getRow(irow);
//       const double thpt = Mvec.maxAbsElement();
//       if (thpt > maxEVelement) {
//         maxEVelement = thpt;
//         maxEVrow = Mvec;
//         iFirst = ivalue;
//       }
//     }
//   }

//   // If we found an all zero M (= A - lambda*I) matrix, we punt and accept the identity
//   // tensor as our eigen-vectors.  Otherwise, continue the compuation.
//   if (!punt) {
//     CHECK(iFirst >= 0 and iFirst < 3);

//     // Select the ordering we'll go through the eigen-values in, starting
//     // with the row with the largest absolute value element.
//     const int iSecond = (iFirst + 1) % 3;
//     const int iThird = (iSecond + 1) % 3;
//     CHECK(iFirst + iSecond + iThird == 3);

//     // We need two orthogonal unit vectors in the plane perpendicular to
//     // the maximum row selected previously.  We can do this by finding the
//     // rotational transformation wherein x' axis is aligned with this row, and
//     // taking our two vectors as the other two rows of this transform.
//     const Vector R = maxEVrow.unitVector();
//     const Tensor Tr = rotationMatrix(R);
//     const Vector U0 = Tr.getRow(1);
//     const Vector U1 = Tr.getRow(2);

//     // Now we can compute the eigen-vector corresponding the first eigen-value
//     // selected previously.
//     const Vector V0 = buildUniqueEigenVector(A,
//                                              lambdaVec(iFirst),
//                                              U0,
//                                              U1);
//     result.eigenVectors.setColumn(iFirst, V0);

//     // Now we know the remaining eigen-vectors are in the plane perpendicular to
//     // V0.  We know R is in that plane, and so is R x V0.  With that knowledge
//     // we can basically repeat the same procedure for the next eigen-vector.
//     Vector S = R.cross(V0);
//     CHECK(fuzzyEqual(S.magnitude2(), 1.0, tolerance));
//     const Vector V1 = buildUniqueEigenVector(A,
//                                              lambdaVec(iSecond),
//                                              R,
//                                              S);
//     result.eigenVectors.setColumn(iSecond, V1);

//     // The last eigen-vector is orthogonal to the first two, so we can find it
//     // simply by taking the cross-product of the previous eigen-vectors.
//     const Vector V2 = V0.cross(V1);
//     CHECK(fuzzyEqual(V2.magnitude2(), 1.0, tolerance));
//     CHECK(fuzzyEqual(((A - lambdaVec(iThird)*SymTensor::one)*V2).maxAbsElement(), 0.0, tolerance));
//     result.eigenVectors.setColumn(iThird, V2);
//   }

// #endif

  BEGIN_CONTRACT_SCOPE
  // Check the result.
  const double lambda1 = result.eigenValues.x();
  const double lambda2 = result.eigenValues.y();
  const double lambda3 = result.eigenValues.z();
  const Vector v1 = result.eigenVectors.getColumn(0);
  const Vector v2 = result.eigenVectors.getColumn(1);
  const Vector v3 = result.eigenVectors.getColumn(2);
  ENSURE2(fuzzyEqual(v1.dot(v2), 0.0, tolerance) and
          fuzzyEqual(v1.dot(v3), 0.0, tolerance) and
          fuzzyEqual(v2.dot(v3), 0.0, tolerance),
          v1 << " " << v2 << " " << v3 << " : " << *this);
  ENSURE2(fuzzyEqual(v1.magnitude2(), 1.0, tolerance) and
          fuzzyEqual(v2.magnitude2(), 1.0, tolerance) and
          fuzzyEqual(v3.magnitude2(), 1.0, tolerance),
          v1 << " " << v2 << " " << v3);
  const double tol = tolerance*max(1.0, this->maxAbsElement());
  ENSURE2(fuzzyEqual((SymTensor(xx() - lambda1, xy(), xz(),
                                yx(), yy() - lambda1, yz(),
                                zx(), zy(), zz() - lambda1)*v1).maxAbsElement(), 0.0, tol),
          *this << " " << A << " " << lambda1 << " " << v1 << " " << tol << " "
          << SymTensor(xx() - lambda1, xy(), xz(),
                       yx(), yy() - lambda1, yz(),
                       zx(), zy(), zz() - lambda1)*v1);
  ENSURE(fuzzyEqual((SymTensor(xx() - lambda2, xy(), xz(),
                               yx(), yy() - lambda2, yz(),
                               zx(), zy(), zz() - lambda2)*v2).maxAbsElement(), 0.0, tol));
  ENSURE(fuzzyEqual((SymTensor(xx() - lambda3, xy(), xz(),
                               yx(), yy() - lambda3, yz(),
                               zx(), zy(), zz() - lambda3)*v3).maxAbsElement(), 0.0, tol));
  ENSURE(fuzzyEqual(abs(result.eigenVectors.Determinant()), 1.0, tolerance));
  END_CONTRACT_SCOPE

  return result;
}

//------------------------------------------------------------------------------
// Explicit instantiation.
//------------------------------------------------------------------------------
template class GeomSymmetricTensor<1>;
template class GeomSymmetricTensor<2>;
template class GeomSymmetricTensor<3>;

//------------------------------------------------------------------------------
// Set the static variables.
//------------------------------------------------------------------------------
template<> const unsigned GeomSymmetricTensor<1>::nDimensions = 1;
template<> const unsigned GeomSymmetricTensor<1>::numElements = 1;
template<> const GeomSymmetricTensor<1> GeomSymmetricTensor<1>::zero = GeomSymmetricTensor<1>(0.0);
template<> const GeomSymmetricTensor<1> GeomSymmetricTensor<1>::one = GeomSymmetricTensor<1>(1.0);

template<> const unsigned GeomSymmetricTensor<2>::nDimensions = 2;
template<> const unsigned GeomSymmetricTensor<2>::numElements = 3;
template<> const GeomSymmetricTensor<2> GeomSymmetricTensor<2>::zero = GeomSymmetricTensor<2>(0.0, 0.0,
                                                                                0.0, 0.0);
template<> const GeomSymmetricTensor<2> GeomSymmetricTensor<2>::one = GeomSymmetricTensor<2>(1.0, 0.0,
                                                                                0.0, 1.0);

template<> const unsigned GeomSymmetricTensor<3>::nDimensions = 3;
template<> const unsigned GeomSymmetricTensor<3>::numElements = 6;
template<> const GeomSymmetricTensor<3> GeomSymmetricTensor<3>::zero = GeomSymmetricTensor<3>(0.0, 0.0, 0.0,
                                                                                0.0, 0.0, 0.0,
                                                                                0.0, 0.0, 0.0);
template<> const GeomSymmetricTensor<3> GeomSymmetricTensor<3>::one = GeomSymmetricTensor<3>(1.0, 0.0, 0.0,
                                                                                0.0, 1.0, 0.0,
                                                                                0.0, 0.0, 1.0);

template<> const double GeomSymmetricTensor<1>::onethird = 1.0/3.0;
template<> const double GeomSymmetricTensor<2>::onethird = 1.0/3.0;
template<> const double GeomSymmetricTensor<3>::onethird = 1.0/3.0;

template<> const double GeomSymmetricTensor<1>::sqrt3 = std::sqrt(3.0);
template<> const double GeomSymmetricTensor<2>::sqrt3 = std::sqrt(3.0);
template<> const double GeomSymmetricTensor<3>::sqrt3 = std::sqrt(3.0);

}


/**
 * \file dcs/math/random/linear_congruential.hpp
 *
 * \brief Linear Congruential Random Number Engine.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2009 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef DCS_MATH_RANDOM_LINEAR_CONGRUENTIAL_HPP
#define DCS_MATH_RANDOM_LINEAR_CONGRUENTIAL_HPP

#include <dcs/detail/config/boost.hpp>

#if !DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION(101500) // 1.15
#	error "Required Boost libraries version >= 1.15."
#endif

#include <boost/random/linear_congruential.hpp>
#include <cstddef>
#include <dcs/math/random/base_generator.hpp>
#include <stdint.h>

namespace dcs { namespace math { namespace random {

//using ::std::size_t;

/*
template <typename IntT, IntT a, IntT c, IntT m>
class linear_congruential
{
	public: typedef IntT result_type;
	public: static const result_type min_value = (c == 0 ? 1 : 0);
	public: static const result_type max_value = m-1;
	public: static const result_type multiplier = a;
	public: static const result_type increment = c;
	public: static const result_type modulus = m;

	public: explicit linear_congruential(IntT x0=1)
	{
		seed(x0);
	}

	public: void seed(IntT x0=1)
	{
		x_ = ((modulus != 0) ? (x0 % modulus) : x0);

		// handle negative seeds
		if (x_ < 0)
		{
			x_ += modulus;
		}
		// adjust to the correct range
		if (increment == 0 && x_ == 0)
		{
			x_ = 1;
		}

		// post-conditions
		DCS_ASSERT(
			x_ >= min_value,
			throw std::domain_error("Seed less than min value")
		);
		DCS_ASSERT(
			x_ <= max_value,
			throw std::domain_error("Seed greater than max value")
		);
	}

	public: result_type operator()()
	{
		return next();
	}

	public: friend bool operator==(linear_congruential const& l1, linear_congruential const& l2)
	{
		return l1.x_ == l2.x_;
	}

	public: friend bool operator!=(linear_congruential const& l1, linear_congruential const& l2)
	{
		return !(l1 == l2);
	}

	// Perform: (a*x+c) mod m
	private: IntT next()
	{
		IntT max_d_a = std::numeric_limits<IntT>::max()/multiplier;

		if(modulus <= (max_d_a - increment/multiplier))   // i.e. a*m+c <= max
		{
			x_= (multiplier * x_ + increment) % modulus;
		}
		else
		{
			// increment and multiplier are big

			// Perform the product
			if (multiplier != 1)
			{
				if (modulus <= max_d_a) // i.e. a*m <= max
				{
					// small product
					x_ = (multiplier * x_) % modulus;
				}
				else if (std::numeric_limits<IntT>::is_signed && (modulus % multiplier) < (modulus / multiplier))
				{
					// Use the Schrage's Algorithm:
					//   an algorithm for multiplying two 32-bit integers modulo
					//   a 32-bit constant without using any intermediates
					//   larger than 32 bits.

					const IntT q = modulus / multiplier;
					const IntT r = modulus % multiplier;

					assert(r < q);        // check that overflow cannot happen

					x_ = multiplier*(x_%q) - r*(x_/q);
					while (x_ <= 0)
					{
						x_ += modulus;
					}
				}
				else
				{
//					DCS_ASSERT("multiplier is too large");
					x_ = 0;
				}
			}

			// Perform the sum
			if (increment != 0)
			{
				if (increment <= (std::numeric_limits<IntT>::max()-modulus)) // i.e. m+x < max
				{
					// small sum
					x_ += increment;
					if (x_ >= modulus)
					{
						x_ -= modulus;
					}
				}
				else if (std::numeric_limits<IntT>::is_signed)
				{
					IntT m_sub_c(modulus-increment);

					if (x_ < m_sub_c)
					{
						x_ += increment;
					}
					else
					{
						x_ -= m_sub_c;
					}
				}
				else
				{
//					DCS_ASSERT("increment is too large°);
					x_ = 0;
				}
			}
		}

		return x_;
	}

	private: IntT x_;
};
*/

/**
 * \brief Linear Congruential Random Number Engine.
 *
 * \tparam UIntT The type of randomly generated numbers.
 * \tparam a The multiplier.
 * \tparam c The increment.
 * \tparam m The modulus.
 *
 * The Linear Congruential random number generator has the following form:
 * \f[
 *   X_{i+1} = a X_{i} + c \quad (\operatorname{mod} m)
 * \f]
 * where:
 * - \f$a\f$ is the \e multiplier.
 * - \f$c\f$ is the \e increment.
 * - \f$m\f$ is the \e modulus.
 * .
 * This class implements the \c RandomNumberEngine concept.
 *
 * \see "Numerical Recipes in C: The Art of Scientific Computing" (William H. Press, Brian P. Flannery, Saul A. Teukolsky, William T. Vetterling).
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template <typename UIntT, UIntT a, UIntT c, UIntT m>
class linear_congruential: public base_generator<UIntT>
{
	private: typedef base_generator<UIntT> base_type;
	public: typedef UIntT result_type;
	private: typedef ::boost::random::linear_congruential<UIntT,a,c,m,0> impl_type;
	public: typedef typename base_type::ulonglong_type ulonglong_type;
//	public: typedef long long ulonglong_type;

	public: static const result_type multiplier = a;
	public: static const result_type increment = c;
	public: static const result_type modulus = m;
	public: static const result_type default_seed = impl_type::default_seed;

	public: linear_congruential()
		: impl_()
	{
		// empty
	}

	public: explicit linear_congruential(result_type s)
		: impl_(s)
	{
		// empty
	}

//	public: template <typename ItT>
//		linear_congruential(ItT& first, ItT& last)
//		: impl_(first,last)
//	{
//		// empty
//	}

//	public: template <typename ItT>
//		void seed(ItT& first, ItT& last)
//	{
//		impl_.seed(first, last);
//	}

	public: static result_type min()
	{
		return (increment == 0u ? 1u : 0u);
	}

	public: static result_type max()
	{
		return (modulus - 1);
	}

	private: result_type do_min() const
	{
		return impl_.min();
	}

	private: result_type do_max() const
	{
		return impl_.max();
	}

	private: void do_seed()
	{
		impl_.seed();
	}

	private: void do_seed(result_type s)
	{
		impl_.seed(s);
	}

	private: result_type do_generate()
	{
		return impl_();
	}

	//FIXME: actually cannot use impl_.discard since it is defined only when
	//       BOOST_NO_LONG_LONG is undefined.
	private: void do_discard(ulonglong_type z)
	{
		for ( ; z != 0; --z)
		{
			this->operator()();
		}
//		impl_.discard(z);
	}

	public: friend bool operator==(linear_congruential const& l1, linear_congruential const& l2)
	{
		return l1.impl_ == l2.impl_;
	}

	public: friend bool operator!=(linear_congruential const& l1, linear_congruential const& l2)
	{
		return !(l1 == l2);
	}

	private: impl_type impl_;
};

// Some standard LCG (see http://random.mat.sbg.ac.at/~charly/server/node3.html)

typedef linear_congruential<int32_t, 16807, 0, 2147483647> minstd_rand0;
typedef linear_congruential<int32_t, 48271, 0, 2147483647> minstd_rand1;
typedef linear_congruential<int32_t, 69621, 0, 2147483647> minstd_rand2;
//FIXME: the declaration below requires a true 64 bit integer in order to
// store the two integral constants.
// For the moment use the boost::rand48 class (which emulates drand48
// without requiring true 64bit support) until we find a way the underlying
// architecture type.
#if true
typedef linear_congruential<uint64_t, 25214903917, 11, 281474976710656> rand48;
#else
class rand48
{
	public: typedef ::boost::rand48::result_type result_type;
	public: static const result_type default_seed = 1u;

	public: rand48()
		: impl_(default_seed)
	{
		// empty
	}

	public: explicit rand48(result_type s)
		: impl_(s)
	{
		// empty
	}

	public: template <typename ItT>
		rand48(ItT& first, ItT& last)
		: impl_(first,last)
	{
		// empty
	}

	public: void seed(result_type s=default_seed)
	{
		impl_.seed(s);
	}

	public: template <typename ItT>
		void seed(ItT& first, ItT& last)
	{
		impl_.seed(first, last);
	}

	public: static result_type min()
	{
		return 0;
	}

	public: static result_type max()
	{
		return ::std::numeric_limits<int32_t>::max();
	}

	public: result_type operator()()
	{
		return impl_();
	}

	public: void discard(size_t z)
	{
		for ( ; z != 0; --z)
		{
			operator()();
		}
	}

	public: friend bool operator==(rand48 const& lhs, rand48 const& rhs)
	{
		return lhs.impl_ == rhs.impl_;
	}

	public: friend bool operator!=(rand48 const& lhs, rand48 const& rhs)
	{
		return !(lhs == rhs);
	}

	private: ::boost::rand48 impl_;
};
#endif // false

}}} // Namespace dcs::math::random

#endif // DCS_MATH_RANDOM_LINEAR_CONGRUENTIAL_HPP


#pragma once

#include <stdlib.h>
#include <math.h>

#include "vec.h"
#include "gnilk/engine/core/Core.hpp"
#include "gnilk/engine/core/Edge.hpp"
#include "gnilk/engine/core/Triangle.hpp"
#include "gnilk/engine/core/FaceGroup.hpp"

#include <boost/unordered/unordered_map.hpp>

#include <vector>

namespace gnilk
{
	namespace engine
	{
		using namespace gnilk::engine::core;

		class FaceHelper
		{
		public:
			//
			// calculates vertex normals per group basis
			// vertex normals are stored directly in to the triangles
			// I did not want unsynchronized buffer sizes nor buffer dependencies on group
			// This is not optimal for rendering - that has to be solved when computing render buffers
			//
			static void CalculateVertexNormals(int vertices, std::vector<int> &faces, std::vector<Triangle *> &triangles) {
				std::vector<Normal> normals;
				normals.reserve(vertices);	// need intermediate storage
				// need to initialize array
				for(int i=0;i<vertices;i++) {
					vIni(normals[i].data,0,0,0);
				}
				// add up normals per vertex - note: normals should already be normalized!
				for(int i=0;i<faces.size();i++) {
					int idxTri = faces[i];
					Triangle *tri = triangles[idxTri];
					for(int j=0;j<3;j++) {
						vAdd(normals[tri->v[j]].data, normals[tri->v[j]].data, tri->normal.data);
					}
				}
				// normalize and assign
				for(int i=0;i<faces.size();i++) {
					Triangle *tri = triangles[faces[i]];
					for(int j=0;j<3;j++) {
						vNorm(tri->vn[j].data,normals[tri->v[j]].data);
					}
				}
			}

			static void CalculateVertexNormals(int vertices, std::vector<FaceGroup*> &groups, std::vector<Triangle *> &triangles)
			{
				for(int i=0;i<groups.size();i++)
				{
					printf("CVN, G: %d, T: %d\n",i, groups[i]->triangles.size());
					CalculateVertexNormals(vertices, groups[i]->triangles, triangles);
				}
			}

			static void CalculateFaceNormal(Normal *faceNormal, std::vector<Vertex3D> &coords, int v1, int v2, int v3) {
				float *pV1 = coords[v1].data;
				float *pV2 = coords[v2].data;
				float *pV3 = coords[v3].data;

				float vE1[3];
				float vE2[3];
				vSub(vE1, pV3, pV1);
				vSub(vE2, pV2, pV1);
				vCross(faceNormal->data, vE1, vE2);
				vNorm(faceNormal->data,faceNormal->data);
			}

			// Calculate normals for a list of faces and assign to face
			static void CalculateFaceNormals(std::vector<Triangle *> &triangles, std::vector<Vertex3D> &coords) {
				for(int i=0;i<triangles.size();i++) {
					Triangle *tri = triangles[i];
					if (!tri->HasNormal()) {
						CalculateFaceNormal(tri->normal.data, coords, tri->v[0], tri->v[1], tri->v[2]);
					}
				}
			}

			// Calculates a single face normal
			static void CalculateFaceNormal(float *normal, std::vector<Vertex3D> &coords, int v1, int v2, int v3) {
				float *pV1 = coords[v1].data;
				float *pV2 = coords[v2].data;
				float *pV3 = coords[v3].data;

				float vE1[3];
				float vE2[3];
				vSub(vE1, pV3, pV1);
				vSub(vE2, pV2, pV1);
				vCross(normal, vE2, vE1);
				vNorm(normal, normal);
			}

			// calculate list of normals for each face
			static void CalculateFaceNormals(std::vector<Normal> &normals, std::vector<Triangle *> &triangles, std::vector<Vertex3D> &coords) {
				normals.reserve(triangles.size());
				for(int i=0;i<triangles.size();i++) {
					CalculateFaceNormal(normals[i].data, coords, triangles[i]->v[0], triangles[i]->v[1], triangles[i]->v[2]);
				}
			}

			// Gets or adds (a new) group for face depending on the deviation between face normal and group normal
			// Not used..
			static FaceGroup *GetOrAddGroup(std::vector<FaceGroup *> &groups, Normal &normal, float threshold) {
				FaceGroup *group = NULL;
				float lastFac = threshold;

				// Find group (if any) which fulfills the the normal threshold
				for(size_t i=0;i<groups.size();i++) {
					float fac = 1.0 - vDot(normal.data, groups[i]->baseNormal.data);
					if ((fac > 0) && (fac < threshold)) {
						group = groups[i];
						lastFac = fac;
					}
				}
				if (group == NULL)
				{
					group = new FaceGroup();
					vDup(group->baseNormal.data, normal.data);
					groups.push_back(group);
				}
				return group;
			}

			// Recursive, assigns faces to groups - based on edges and if two faces have a similar normal they are allowed in the same group
			static void DoCalculateFaceGroups(FaceGroup *pGroup,
					int triangle,
					float threshold,
					std::vector<bool> &visited,
					std::vector<Edge *> &edges,
					std::vector<Triangle *> &triangles,
					std::vector<Normal> &faceNormals,
					int depth)
			{
				if (visited[triangle])
				{
					//printf("%d:%d -> .\n",depth,triangle);
					return;
				}

				pGroup->triangles.push_back(triangle);
				visited[triangle] = true;

				Triangle *pTri = triangles[triangle];
				for(int i=0;i<3;i++)
				{
					Edge *edge = triangles[triangle]->edges[i];
					if (edge == NULL)
					{
						printf("NULL edge %d for triangle %d\n",i, triangle);
						continue;
					}
					int tNext = -1;
					if ((edge->f1 != triangle) && (edge->f1 != -1)) tNext = edge->f1;
					else if ((edge->f2 != triangle) && (edge->f2 != -1)) tNext = edge->f2;

					if (tNext >= 0)
					{
						Triangle *pTriNext = triangles[tNext];
						float faceDiff = 1.0 - vDot(pTri->normal.data, pTriNext->normal.data);
						//float faceDiff = 1.0 - vDot(faceNormals[triangle].data, faceNormals[tNext].data);
						faceDiff = fabs(faceDiff);
						if ((faceDiff >= 0) && (faceDiff <= threshold))
						{
							//printf("%d:%d -> %d - %f\n",depth,triangle, tNext, faceDiff);
							DoCalculateFaceGroups(pGroup, tNext, threshold, visited, edges, triangles, faceNormals,depth+1);
						} else
						{
							// rejected because of threshold
							//printf("%d, r: %f\n",faceDiff);
						}
					}
				} // Foreach edge
				//printf("%d:%d -> x\n",depth,triangle);
			}

			// Automatic group of faces depending on
			static void CalculateFaceGroups(std::vector<FaceGroup *> &groups, std::vector<Edge *> &edges, std::vector<Triangle *> &triangles, std::vector<Normal> &faceNormals)
			{
				std::vector<bool> visited;
				visited.reserve(triangles.size());
				for(size_t i=0;i<triangles.size();i++)
				{
					visited.push_back(false);
				}
				for(size_t i=0;i<triangles.size();i++)
				{
					if (!visited[i])
					{
						FaceGroup *pGroup = new FaceGroup();
						DoCalculateFaceGroups(pGroup, i, 0.5, visited, edges, triangles, faceNormals,0);
						printf("Group %d, triangles: %d\n", groups.size(), pGroup->triangles.size());
						groups.push_back(pGroup);
					}
				}

				for(size_t i=0;i<groups.size();i++)
				{
					FaceGroup *group = groups[i];
					vMul(group->normal.data, group->normal.data, 1.0 / (double)group->triangles.size());
				}
				printf("Groups: %d\n",groups.size());
			}

			// Calculate edges
			static void CalculateEdges(std::vector<Edge *> &edges, std::vector<Triangle *> &triangles, const std::vector<Vertex3D> &vertices)
			{
				boost::unordered::unordered_map<int, Edge *> edgemap;
				for(size_t i=0;i<triangles.size();i++)
				{
					triangles[i]->Reset();
					GetOrAddEdgeMap(edgemap, triangles, vertices, i, 0, 1);
					GetOrAddEdgeMap(edgemap, triangles, vertices, i, 1, 2);
					GetOrAddEdgeMap(edgemap, triangles, vertices, i, 2, 0);
				}
				// I guess there is a better way to generate a vector instead of this...
				int count = 0;
				for (boost::unordered::unordered_map<int, Edge *>::iterator it = edgemap.begin(); it != edgemap.end(); ++it )
				{
					Edge *edge = it->second;
					if (edge->f1 >= 0) triangles[edge->f1]->AddEdge(edge);
					if (edge->f2 >= 0) triangles[edge->f2]->AddEdge(edge);
					edges.push_back(it->second);
					count++;
				}
			} // CalculateEdges

			// Convert faces to triangles - quads are split
			static void FacesToTriangles(
					std::vector<Triangle *> &triangles,
					std::vector<Face *> &faces) {

				for(size_t i=0;i<faces.size();i++) {
					Triangle *tri = new Triangle();
					tri->faceData = faces[i]->faceData;
					for(int j=0;j<3;j++) {
						tri->v[j] = faces[i]->v[j];
						//tri->vn[j] = faces[i]->vn[j];
						tri->uv[j] = faces[i]->uv[j];
					}
					triangles.push_back(tri);
					if (faces[i]->faceType == kFaceType_Quad) {
						tri = new Triangle();
						tri->faceData = faces[i]->faceData;
						for(int j=0;j<3;j++) {
							int idx = ((j+2)>3) ? 0 : j+2;
							tri->v[j] = faces[i]->v[idx];
							//tri->vn[j] = faces[i]->vn[idx];
							tri->uv[j] = faces[i]->uv[idx];
						}
						triangles.push_back(tri);
					}
				}
			} // FacesToTriangles

			static void CollapseEdge(int edge, std::vector<Triangle *> &triangles, std::vector<Edge *> &edges, std::vector<Vertex3D> &coords)
			{
				Edge *pEdge = edges[edge];
				int f1 = pEdge->f1;
				int f2 = pEdge->f2;
				int v1 = pEdge->v1;
				int v2 = pEdge->v2;
				// will this work?
				triangles.erase(triangles.begin()+f1);
				triangles.erase(triangles.begin()+f2);

				edges.erase((edges.begin()+edge));
			}

			static int CountDuplicateVertices(std::vector<Vertex3D> &coords) {
				int duplicates = 0;
				for(int i=0;i<coords.size();i++) {
					float temp[3];
					vNorm(temp,coords[i].data);
					for(int j=0;j<coords.size();j++) {
						if (i==j) continue;
						float temp2[3];
						vNorm(temp2, coords[j].data);
						float fac = vDot(temp,temp2);
						if (fac>0 && fac<0.01) {
							duplicates++;
						}
					}
				}
				return duplicates;
			}

			// Calculates bound sphere radius and mid point
			static double CalculateBoundingSphere(Vertex3D &mid, std::vector<Vertex3D> &coords) {
				Vertex3D cmax,cmin;
				float tmp[3];
				cmax = coords[0];
				cmin = coords[1];
				// Find max/min vertex
				for(int i=0;i<coords.size();i++) {
					cmax.data[0] = fmax(coords[i].data[0], cmax.data[0]);
					cmax.data[1] = fmax(coords[i].data[1], cmax.data[1]);
					cmax.data[2] = fmax(coords[i].data[2], cmax.data[2]);

					cmin.data[0] = fmin(coords[i].data[0], cmin.data[0]);
					cmin.data[1] = fmin(coords[i].data[1], cmin.data[1]);
					cmin.data[2] = fmin(coords[i].data[2], cmin.data[2]);
				}

				// Calculate radius and mid-point
				// Note: Why 2.5???
				double r = vAbs(vSub(tmp,cmax.data, cmin.data)) / 2.5;
				vAdd(tmp, cmax.data, cmin.data);
				vMul(mid.data, tmp, 0.5);
				return r;
			}

			static int SplitEdge(std::vector<Vertex3D> &coordOut, std::vector<Vertex3D> &coords, int v1, int v2) {
				Vertex3D mid;
				vMul(mid.data, vAdd(mid.data,coords[v1].data,coords[v2].data),0.5f);
				coordOut.push_back(mid);
				return (coordOut.size()-1);
			}

			/*      2
			 *     4 5
			 *    1 6 3
			 *
			 * Tris:
			 * 	1,4,6
			 * 	2,5,4
			 * 	3,6,5
			 * 	4,5,6
			 *
			*/
			static void SplitTriangle(std::vector<Triangle *> &triOut, std::vector<Vertex3D> &coordOut, Triangle *triangle, std::vector<Vertex3D> &coords) {
				int v1 = triangle->v[0];
				int v2 = triangle->v[1];
				int v3 = triangle->v[2];

				int idxStart = coordOut.size();
				coordOut.push_back(coords[v1]);
				coordOut.push_back(coords[v2]);
				coordOut.push_back(coords[v3]);
				SplitEdge(coordOut, coords, v1, v2);
				SplitEdge(coordOut, coords, v2, v3);
				SplitEdge(coordOut, coords, v3, v1);
				// Create 4 new triangles

				triOut.push_back(new Triangle(idxStart+0, idxStart+3, idxStart+5));
				triOut.push_back(new Triangle(idxStart+1, idxStart+4, idxStart+3));
				triOut.push_back(new Triangle(idxStart+2, idxStart+5, idxStart+4));
				triOut.push_back(new Triangle(idxStart+3, idxStart+4, idxStart+5));
			}

		private:
			static void GetOrAddEdgeMap(boost::unordered::unordered_map<int, Edge *> &edges,
				std::vector<Triangle *> &triangles,
				const std::vector<Vertex3D> &vertices,
				int face, int v1, int v2) {
				Edge *pEdge = NULL;

				int hashCode = Edge::HashCode(triangles[face]->v[v1], triangles[face]->v[v2]);
				//printf("hc: %d (%d, %d)\n",hashCode,faces[face]->v[v1], faces[face]->v[v2]);
				if (edges.find(hashCode) == edges.end()) {
					pEdge = new Edge(triangles[face]->v[v1], triangles[face]->v[v2], vertices);
					pEdge->AddFace(face);
					edges.insert(std::pair<int, Edge *>(hashCode, pEdge));
				} else {
					if (edges[hashCode]->AddFace(face) != true) {
						//printf("!!!! MULTIPLE FACES SHARING SAME EDGE!!!\n");
						// TODO: Create new face!
					}
				}
			} // GetOrAddEdgeMap
		}; // class FaceHelper
	}
}


//
// Created by Vlad Argunov on 24/10/2021.
//

#include "Option.h"
#include <iostream>
#include <Eigen/Dense>
#include <string>
#include <algorithm>
#include <iomanip>

Option::Option(std::string type_option_, bool european_){
    type_option = type_option_;
    european = european_;
}

void Option::set_tn(double tn_) {
    tn = tn_;

}

void Option::set_s_max(double s_max_) {
    s_max = s_max_;
}

void Option::set_numdiff_t(int numdiff_t_) {
    numdiff_t = numdiff_t_;
}

void Option::set_numdiff_s(int numdiff_s_) {
    numdiff_s = numdiff_s_;
    }

void Option::set_volatility(double vol_, bool stochastic_vol_) {
    stochastic_vol = stochastic_vol_;
    if (stochastic_vol == 0){
        deterministic_vol = vol_;
    }
    // Code for implementation of stochastic volatility
}

void Option::set_interest_rate(double rate_, bool stochastic_rate_) {
    stochastic_rate = stochastic_rate_;
    if (stochastic_rate == 0){
        deterministic_rate = rate_;
    }
    // Code for implementation of stochastic interest rate
}

void Option::set_strike(double strike_) {
    strike = strike_;
}

void Option::set_stock_boundary_condition(std::string stock_boundary_condition_) {
    stock_boundary_condition = stock_boundary_condition_;
}

double Option::parameter_a(int n) {
    if (stochastic_vol == 0 && stochastic_rate == 0 && fixed_step_time == 1){
        return 0.5 * (deterministic_rate * n - deterministic_vol * deterministic_vol * n * n) * fixed_dt;
    }

}

double Option::parameter_b(int n) {
    if (stochastic_vol == 0 && stochastic_rate == 0 && fixed_step_time == 1) {
        return (deterministic_vol * deterministic_vol * n * n + deterministic_rate) * fixed_dt;
    }
}

double Option::parameter_c(int n) {
    if (stochastic_vol == 0 && stochastic_rate == 0 && fixed_step_time == 1) {
        return -0.5 * (deterministic_vol * deterministic_vol * n * n + deterministic_rate * n) * fixed_dt;
    }
}

Eigen::MatrixXd Option::implicit_matrix_vanilla() {
    double a;
    double b;
    double c;
    Eigen::MatrixXd computation_matrix;
    computation_matrix.resize(numdiff_s - 1, numdiff_s - 1);
    for (int row = 0; row < computation_matrix.rows(); ++row) {
        if (row == 0){
            b = parameter_b(row + 1);
            c = parameter_c(row + 1);
            computation_matrix(row,row) = 1 + b;
            computation_matrix(row,row + 1) = c;
        } else if (row == computation_matrix.rows() - 1) {
            b = parameter_b(row + 1);
            a = parameter_a(row + 1);
            computation_matrix(row, row) = 1 + b;
            computation_matrix(row, row - 1) = a;
        } else {
            a = parameter_a(row + 1);
            b = parameter_b(row + 1);
            c = parameter_c(row + 1);
            computation_matrix(row, row) = 1 + b;
            computation_matrix(row, row - 1) = a;
            computation_matrix(row, row + 1) = c;
        }
    }
    return computation_matrix;
}

void Option::fixed_difference_step(bool fixed_step_stock_, bool fixed_step_time_) {
    fixed_step_stock = fixed_step_stock_;
    fixed_step_time = fixed_step_time_;
    if (fixed_step_stock == 1 && fixed_step_time == 1){
        fixed_dt = tn / numdiff_t;
        fixed_ds = s_max / numdiff_s;
    }

    // Code of the step size is not fixed

}

void Option::compute_solution_grid(std::string method) {
    if (type_option == "Call"  || type_option == "Put" && european == 1){
        solution_grid = solution_grid_vanilla(method);
    }

}

void Option::create_boundary_condition_time(Eigen::VectorXd& boundary_vector) {

    if (fixed_step_time == 1 && fixed_step_stock == 1){
        for (int row=0; row < boundary_vector.rows(); ++row) {
            if (type_option == "Call")
                boundary_vector(row, 0) = std::max((row + 1) * fixed_ds - strike, 0.0);
            else if (type_option == "Put")
                boundary_vector(row, 0) = std::max(strike - (row + 1) * fixed_ds, 0.0);
        }
    }

}

Eigen::MatrixXd Option::solution_grid_vanilla(std::string method) {
    Eigen::MatrixXd solution_matrix;

    if (method == "Implicit"){
        // Implementation of the solution grid computation with Dirichlet boundary conditions
        if (stock_boundary_condition == "Dirichlet"){

            Eigen::MatrixXd implicit_matrix = implicit_matrix_vanilla().inverse();
            Eigen::VectorXd current_vt;
            Eigen::VectorXd previous_vt;

            Eigen::Index implicit_matrix_rows = implicit_matrix.rows();

            current_vt.resize(implicit_matrix_rows);
            create_boundary_condition_time(current_vt);
            previous_vt.resize(implicit_matrix_rows);

            Eigen::Index solution_matrix_cols = numdiff_t + 1;

            solution_matrix.resize(implicit_matrix_rows, solution_matrix_cols);

            Eigen::VectorXd adjustment_dirichlet = Eigen::VectorXd::Zero(implicit_matrix_rows);

            double a = parameter_a(1);
            double c = parameter_c(numdiff_s - 1);

            for (int t = 0; t < solution_matrix_cols; ++t) {
                // Add adjustment for Dirichlet
                if (type_option == "Call"){
                    adjustment_dirichlet(implicit_matrix_rows - 1,0) = c * (s_max - strike * exp( - deterministic_rate * (t - 1) * fixed_dt));
                    adjustment_dirichlet(0,0) = 0;
                } else if (type_option == "Put"){
                    adjustment_dirichlet(implicit_matrix_rows - 1,0) = 0;
                    adjustment_dirichlet(0,0) = a * strike * exp( - deterministic_rate * (t - 1) * fixed_dt);
                }

                // Perform the iteration
                if (t == 0) {
                    solution_matrix.col(solution_matrix_cols - 1) = current_vt;
                } else {
                    previous_vt = implicit_matrix * (current_vt - adjustment_dirichlet);
                    solution_matrix.col(solution_matrix_cols - t - 1) = previous_vt;
                    current_vt = previous_vt;
                }
            }

            Eigen::MatrixXd solution_matrix_adjusted;
            solution_matrix_adjusted.resize(implicit_matrix_rows + 2, solution_matrix_cols);
            Eigen::MatrixXd & ref_solution_matrix = solution_matrix;
            solution_matrix_adjusted.middleRows(1,implicit_matrix_rows) = ref_solution_matrix;

            for (int col = 0; col < solution_matrix_cols; ++col) {
                if (type_option == "Call"){
                    solution_matrix_adjusted(0,solution_matrix_cols - col - 1) = 0;
                    solution_matrix_adjusted(implicit_matrix_rows + 1, solution_matrix_cols - col - 1) = s_max - strike * exp( - deterministic_rate * col * fixed_dt);
                } else if (type_option == "Put") {
                    solution_matrix_adjusted(0,solution_matrix_cols - col - 1) = strike * exp( - deterministic_rate * col * fixed_dt);
                    solution_matrix_adjusted(implicit_matrix_rows + 1, solution_matrix_cols - col - 1) = 0;
                }
            }
            return solution_matrix_adjusted;
        }

        if (stock_boundary_condition == "Neumann"){

            double a = parameter_a(1);
            double c = parameter_c(numdiff_s - 1);

            Eigen::MatrixXd implicit_matrix = implicit_matrix_vanilla();
            Eigen::Index implicit_matrix_rows = implicit_matrix.rows();
            implicit_matrix(0,0) += 2 * a;
            implicit_matrix(0,1) -= a;

            implicit_matrix(implicit_matrix_rows - 1, implicit_matrix_rows - 1) += 2 * c;
            implicit_matrix(implicit_matrix_rows - 1, implicit_matrix_rows - 2) -= c;

            implicit_matrix = implicit_matrix.inverse();

            Eigen::VectorXd current_vt;
            current_vt.resize(implicit_matrix_rows);
            create_boundary_condition_time(current_vt);

            Eigen::VectorXd previous_vt;
            previous_vt.resize(implicit_matrix_rows);

            Eigen::Index solution_matrix_cols = numdiff_t + 1;

            solution_matrix.resize(implicit_matrix_rows, solution_matrix_cols);

            for (int t = 0; t < solution_matrix_cols; ++t) {
                // Perform the iteration
                if (t == 0) {
                    solution_matrix.col(solution_matrix_cols - 1) = current_vt;
                } else {
                    previous_vt = implicit_matrix * current_vt;
                    solution_matrix.col(solution_matrix_cols - t - 1) = previous_vt;
                    current_vt = previous_vt;
                }
            }

            Eigen::MatrixXd solution_matrix_adjusted;
            solution_matrix_adjusted.resize(implicit_matrix_rows + 2, solution_matrix_cols);
            Eigen::MatrixXd & ref_solution_matrix = solution_matrix;
            solution_matrix_adjusted.middleRows(1,implicit_matrix_rows) = ref_solution_matrix;

            for (int col = 0; col < solution_matrix_cols; ++col) {
                Eigen::Index column_number = solution_matrix_cols - col - 1;
                Eigen::Index last_row = implicit_matrix_rows + 1;
                solution_matrix_adjusted(0,column_number) = 2 * solution_matrix_adjusted(1,column_number)
                            - solution_matrix_adjusted(2,column_number);
                solution_matrix_adjusted(last_row, column_number) = 2 * solution_matrix_adjusted(last_row - 1, column_number)
                            - solution_matrix_adjusted(last_row - 2, column_number);

            }
            return solution_matrix_adjusted;
        }}

    if (method == "Crank-Nicholson"){
        if (stock_boundary_condition == "Dirichlet"){

            Eigen::MatrixXd implicit_matrix = implicit_matrix_vanilla();
            Eigen::MatrixXd implicit_matrix_cn = - implicit_matrix;
            Eigen::Index implicit_matrix_rows = implicit_matrix_cn.cols();
            Eigen::MatrixXd identity = Eigen::MatrixXd::Identity(implicit_matrix_rows, implicit_matrix_rows);
            const Eigen::MatrixXd & ref_identity = identity;
            implicit_matrix_cn += 3 * ref_identity;
            implicit_matrix += ref_identity;
            implicit_matrix = implicit_matrix.inverse();

            Eigen::VectorXd current_vt;
            Eigen::VectorXd previous_vt;

            current_vt.resize(implicit_matrix_rows);
            create_boundary_condition_time(current_vt);
            previous_vt.resize(implicit_matrix_rows);

            Eigen::Index solution_matrix_cols = numdiff_t + 1;
            solution_matrix.resize(implicit_matrix_rows, solution_matrix_cols);

            Eigen::VectorXd adjustment_dirichlet = Eigen::VectorXd::Zero(implicit_matrix_rows);

            double a = parameter_a(1);
            double c = parameter_c(numdiff_s - 1);

            for (int t = 0; t < solution_matrix_cols; ++t) {
                // Add adjustment for Dirichlet
                if (type_option == "Call"){
                    adjustment_dirichlet(adjustment_dirichlet.rows() - 1,0) = c * (2 * s_max - strike * exp( - deterministic_rate * (t - 1) * fixed_dt)
                            - strike * exp( - deterministic_rate * std::max(t - 2,0) * fixed_dt));
                    adjustment_dirichlet(0,0) = 0;
                } else if (type_option == "Put"){
                    adjustment_dirichlet(adjustment_dirichlet.rows() - 1,0) = 0;
                    adjustment_dirichlet(0,0) = a * strike * ( exp( - deterministic_rate * (t - 1) * fixed_dt)
                            + exp( - deterministic_rate * std::max(t - 2,0) * fixed_dt));
                }

                // Perform the iteration
                if (t == 0) {
                    solution_matrix.col(solution_matrix_cols - 1) = current_vt;
                } else {
                    previous_vt = implicit_matrix * (implicit_matrix_cn * current_vt - adjustment_dirichlet);
                    solution_matrix.col(solution_matrix_cols - t - 1) = previous_vt;
                    current_vt = previous_vt;
                }
            }

            Eigen::MatrixXd solution_matrix_adjusted;
            solution_matrix_adjusted.resize(implicit_matrix_rows + 2, solution_matrix_cols);
            Eigen::MatrixXd & ref_solution_matrix = solution_matrix;
            solution_matrix_adjusted.middleRows(1,implicit_matrix_rows) = ref_solution_matrix;

            for (int col = 0; col < solution_matrix_cols; ++col) {
                if (type_option == "Call"){
                    solution_matrix_adjusted(0,solution_matrix_cols - col - 1) = 0;
                    solution_matrix_adjusted(implicit_matrix_rows + 1, solution_matrix_cols - col - 1) = s_max - strike * exp( - deterministic_rate * col * fixed_dt);
                } else if (type_option == "Put") {
                    solution_matrix_adjusted(0,solution_matrix_cols - col - 1) = strike * exp( - deterministic_rate * col * fixed_dt);
                    solution_matrix_adjusted(implicit_matrix_rows + 1, solution_matrix_cols - col - 1) = 0;
                }
            }
            return solution_matrix_adjusted;
        }
        if (stock_boundary_condition == "Neumann"){
            Eigen::MatrixXd implicit_matrix = implicit_matrix_vanilla();
            Eigen::MatrixXd implicit_matrix_cn = - implicit_matrix;
            Eigen::Index implicit_matrix_rows = implicit_matrix_cn.rows();
            Eigen::MatrixXd identity = Eigen::MatrixXd::Identity(implicit_matrix_rows, implicit_matrix_rows);
            const Eigen::MatrixXd & ref_identity = identity;
            implicit_matrix_cn += 3 * ref_identity;
            implicit_matrix += ref_identity;

            double a = parameter_a(1);
            double c = parameter_c(numdiff_s - 1);

            implicit_matrix(0,0) += 2 * a;
            implicit_matrix(0,1) -= a;

            implicit_matrix(implicit_matrix.rows() - 1, implicit_matrix.cols() - 1) += 2 * c;
            implicit_matrix(implicit_matrix.rows() - 1, implicit_matrix.cols() - 2) -= c;

            implicit_matrix_cn(0,0) -= 2 * a;
            implicit_matrix_cn(0,1) += a;

            implicit_matrix_cn(implicit_matrix.rows() - 1, implicit_matrix.cols() - 1) -= 2 * c;
            implicit_matrix_cn(implicit_matrix.rows() - 1, implicit_matrix.cols() - 2) += c;

            implicit_matrix = implicit_matrix.inverse();

            Eigen::VectorXd current_vt;
            current_vt.resize(implicit_matrix_rows);
            create_boundary_condition_time(current_vt);

            Eigen::VectorXd previous_vt;
            previous_vt.resize(implicit_matrix_rows);

            Eigen::Index solution_matrix_cols = numdiff_t + 1;
            solution_matrix.resize(implicit_matrix_rows, solution_matrix_cols);

            for (int t = 0; t < solution_matrix_cols; ++t) {
                // Perform the iteration
                if (t == 0) {
                    solution_matrix.col(solution_matrix_cols - 1) = current_vt;
                } else {
                    previous_vt = implicit_matrix * (implicit_matrix_cn * current_vt);
                    solution_matrix.col(solution_matrix_cols - t - 1) = previous_vt;
                    current_vt = previous_vt;
                }
            }

            Eigen::MatrixXd solution_matrix_adjusted;

            solution_matrix_adjusted.resize(implicit_matrix_rows + 2, solution_matrix_cols);
            Eigen::MatrixXd & ref_solution_matrix = solution_matrix;
            solution_matrix_adjusted.middleRows(1,implicit_matrix_rows) = ref_solution_matrix;

            for (int col = 0; col < solution_matrix_cols; ++col) {
                Eigen::Index column_number = solution_matrix_cols - col - 1;
                Eigen::Index last_row = implicit_matrix_rows + 1;
                solution_matrix_adjusted(0,column_number) = 2 * solution_matrix_adjusted(1,column_number)
                                                            - solution_matrix_adjusted(2,column_number);
                solution_matrix_adjusted(last_row, column_number) = 2 * solution_matrix_adjusted(last_row - 1, column_number)
                                                                    - solution_matrix_adjusted(last_row - 2, column_number);
            }
            return solution_matrix_adjusted;
        }

    }

}

void Option::print_solution_grid(int precision) {
    std::cout << "Type of Option: " + type_option + "\n";
    std::cout << "Stock boundary condition: " + stock_boundary_condition + "\n\n";
    std::cout << "Legend of the matrix: s/t 1 2 . . T\n";
    std::cout << "                      0   x x x x x\n";
    std::cout << "                      1   x x x x x\n";
    std::cout << "                      .   x x x x x\n";
    std::cout << "                      .   x x x x x\n";
    std::cout << "                  S_max   x x x x x\n\n";
    std::cout << "Printing the solution grid:\n\n";

    Eigen::MatrixXd printing_matrix;
    printing_matrix.resize(solution_grid.rows() + 1, solution_grid.cols() + 1);
    printing_matrix.bottomRightCorner(solution_grid.rows(),solution_grid.cols()) = solution_grid;

    if (fixed_step_time == 1){
        for (int col = 1; col < printing_matrix.cols(); ++col) {
            printing_matrix(0, col) = fixed_dt * (col - 1);
        }

    }

    if (fixed_step_stock == 1){
        for (int row = 1; row < printing_matrix.rows(); ++row) {
            printing_matrix(row, 0) = fixed_ds * (row - 1);
        }
    }

    std::streamsize prec = std::cout.precision();
    std::cout << std::setprecision(precision) << printing_matrix << std::setprecision(prec) << "\n";
}


#include<stdio.h>
#include<cstring>
#include<iostream>
#include "Eigen/Core"
#include "Eigen/Dense"
#include "Eigen/Sparse"
#include "Eigen/SVD"
#include <Eigen/Eigenvalues>
#include <unsupported/Eigen/SparseExtra>
#include <unsupported/Eigen/KroneckerProduct>

using namespace Eigen;
std :: string filename = "D:/sydney/first/data/tester_ (";
MatrixXf A,result;
const int nShape = 47;
const int nVerts = 11510;
const int nFaces = 11540;
const int test_num = 77;
const int iden_num = 10;
std::vector<int> mouse_edge[1000];
using namespace std;
//int cnt_vtx[nVerts];
void smooth_mesh(MatrixX3f &mesh, int iteration, std::vector<int> *mouse_edge) {
	while (iteration--)
	{
		MatrixX3f temp = mesh;

		for (int i_e = 0; mouse_edge[i_e].size() >0; i_e++) {
			if (mouse_edge[i_e].size() < 6) continue;
			int v = mouse_edge[i_e][0];
			mesh.block(v, 0, 1, 3).setZero();
			for (int j = 1; j < mouse_edge[i_e].size(); j++)
				mesh.row(v).array() += temp.row(mouse_edge[i_e][j]).array() / (mouse_edge[i_e].size() - 1);
		}
	}
}
void load_smooth_edge(std::vector<int> *mouse_edge) {
	puts("loading mouse edge...");
	FILE *fp;
	fopen_s(&fp, "D:\\sydney\\first\\code\\2017\\cal_coeffience_Q_M_u_e_3\\cal_coeffience_Q_M_u_e_3/mouse_point.txt", "r");
	int n;
	fscanf_s(fp, "%d", &n);
	for (int i = 0; i < n; i++) {
		mouse_edge[i].clear();
		int t, num;
		fscanf_s(fp, "%d%d", &t, &num);
		mouse_edge[i].push_back(t);
		for (int j = 0; j < num; j++) {
			fscanf_s(fp, "%d", &t);
			mouse_edge[i].push_back(t);
		}
	}
	fclose(fp);
}

int main() {
	A.resize(test_num, nShape*nVerts * 3);
	for (int i = 0; i < test_num; i++) {
		std::string name = filename + std::to_string(i + 1) + ")/Blendshape/shape.bs";
		std :: cout << name << std :: endl;
		FILE *fp;
		fopen_s(&fp,name.c_str(), "rb");
		int nShapes = 0, nVerts = 0, nFaces = 0;
		fread(&nShapes, sizeof(int), 1, fp);			// nShape = 46
		fread(&nVerts, sizeof(int), 1, fp);			// nVerts = 11510
		fread(&nFaces, sizeof(int), 1, fp);			// nFaces = 11540
		printf("%d %d %d\n", nShapes, nVerts, nFaces);

													// Load neutral expression B_0
		float temp;
		for (int j = 0; j < nVerts * 3; j++) {
			fread(&temp, sizeof(float), 1, fp);
			A(i, j) = temp;
		}
		//for (int j = 0; j < 10; j++)
		//	printf("%.10f ", A(i, j));
		//puts("");

		// Load other expressions B_i ( 1 <= i <= 46 )
		for (int exprId = 0; exprId < nShapes; exprId++)
			for (int j = 0; j < nVerts * 3; j++) {
				fread(&temp, sizeof(float), 1, fp);
				A(i, 3 * nVerts*(exprId + 1) + j) = temp;
			}

		fclose(fp);
	}
	puts("loading initial bldshps complete!...");
	load_smooth_edge(mouse_edge);
	for (int i_id=0;i_id< iden_num;i_id++)
		for (int i_exp = 0; i_exp < nShape; i_exp++) {
			printf("smoothing id:%d exp:%d\n",i_id,i_exp);
			Eigen::MatrixX3f temp(nVerts, 3);
			for (int i_v = 0; i_v < nVerts; i_v++)
				for (int axis = 0; axis < 3; axis++)
					temp(i_v, axis) = A(i_id, i_exp*nVerts*3 + i_v * 3 + axis);
			smooth_mesh(temp, 25, mouse_edge);
			for (int i_v = 0; i_v < nVerts; i_v++)
				for (int axis = 0; axis < 3; axis++)
					A(i_id, i_exp*nVerts * 3 + i_v * 3 + axis) = temp(i_v, axis);
		}

	puts("saving...");
	FILE *fp;
	fopen_s(&fp, "blendshape_ide_svd_77_ite25_bound.lv", "wb");
	for (int i = 0; i < iden_num; i++)
		for (int j = 0; j < 3 * nVerts*nShape; j++)
			fwrite(&A(i, j), sizeof(float), 1, fp);
	fclose(fp);

	//MatrixXf mean = A.rowwise().mean();
	//cout << mean.rows() << ' ' << mean.cols() << '\n';
	//cout << mean;
	//cout << A(32, 22301) << "\n";
	//cout << A(24, 11144) << "\n";
	//cout << A(53, 32333) << "\n";
	//cout << A(60, 523441) << "\n";
	//cout << A(8, 765201) << "\n";

	//system("pause");
	//VectorXf vmean = mean;
	//A.colwise() -= vmean;
	//BDCSVD<Eigen::MatrixXf> svd((A * A.transpose()).array()/ A.cols(), ComputeThinU);//*A.transpose()
	//puts("asd");
	////printf("%d %d %d %d\n", U.rows(), U.cols(), V.rows(), V.cols());
	////cout <<"U:\n"<< U << endl;
	////cout << "V:\n" << V << endl;
	//MatrixXf  S = svd.singularValues();
	////printf("%d %d\n",S.cols(),S.rows());
	//float tot = 0,temp=0;
	//for (int i = 0; i < test_num; i++) tot += sqrt(S(i));
	//for (int i = 0; i < test_num; i++, puts("")) {
	//	temp += sqrt(S(i));
	//	printf("%d %.10f %.2f", i, sqrt(S(i)), temp / tot * 100);
	//}
	//A.colwise() += vmean;
	//result = svd.matrixU().block(0, 0, test_num, iden_num).transpose()*A;
	//FILE *fp;
	//fopen_s(&fp, "blendshape_ide_svd_50.lv", "wb");
	//for (int i = 0; i < iden_num; i++)
	//	for (int j = 0; j < 3 * nVerts*nShape; j++)
	//		fwrite(&result(i,j),sizeof(float),1,fp);
	//fclose(fp);
	//fopen_s(&fp, "blendshape_ide_svd_value_sqrt_50.txt", "w");
	//for (int i = 0; i < iden_num; i++)
	//	fprintf(fp, "%.10f\n", sqrt(S(i)));
	//fclose(fp);

	system("pause");
	return 0;
}

/*
test svd

MatrixXf m = MatrixXf::Random(3, 2);
m.resize(3, 2);
cout << "Here is the matrix m:" << endl << m << endl;
JacobiSVD<MatrixXf> svd(m, ComputeThinU | ComputeThinV);
cout << "Its singular values are:" << endl << svd.singularValues() << endl;
cout << "Its left singular vectors are the columns of the thin U matrix:" << endl << svd.matrixU() << endl;
cout << "Its right singular vectors are the columns of the thin V matrix:" << endl << svd.matrixV() << endl;*/


// Author: Dai Wei (wdai@cs.cmu.edu), Pengtao Xie (pxie@cs.cmu.edu)
// Date: 2014.10.21

#include <ml/util/math_util.hpp>
#include <ml/util/fastapprox/fastapprox.hpp>
#include <glog/logging.h>
#include <cmath>
#include <sstream>
#include <Eigen/Dense>

namespace petuum {
namespace ml {

namespace {

const float kCutoff = 1e-15;

}  // anonymous namespace

float SafeLog(float x) {
  if (std::abs(x) < kCutoff) {
    x = kCutoff;
  }
  return fastlog(x);
}

float Sigmoid(float x) {
  return 1. / (1. + exp(-x));
}

float LogSum(float log_a, float log_b) {
  return (log_a < log_b) ? log_b + fastlog(1 + fastexp(log_a - log_b)) :
    log_a + fastlog(1 + fastexp(log_b-log_a));
}

float LogSumVec(const std::vector<float>& logvec) {
	float sum = 0.;
	sum = logvec[0];
	for (int i = 1; i < logvec.size(); ++i) {
		sum = LogSum(sum, logvec[i]);
	}
	return sum;
}

void Softmax(std::vector<float>* vec) {
  CHECK_NOTNULL(vec);
  // TODO(wdai): Figure out why this is necessary. Doubt it is.
	for (int i = 0; i < vec->size(); ++i) {
		if (std::abs((*vec)[i]) < kCutoff) {
			(*vec)[i] = kCutoff;
    }
	}
	double lsum = LogSumVec(*vec);
	for (int i = 0; i < vec->size(); ++i) {
		(*vec)[i] = fastexp((*vec)[i] - lsum);
		//(*vec)[i] = exp((*vec)[i] - lsum);
    (*vec)[i] = (*vec)[i] > 1 ? 1. : (*vec)[i];
  }
}

float DenseDenseFeatureDotProduct(const AbstractFeature<float>& f1,
    const AbstractFeature<float>& f2) {
  CHECK_EQ(f1.GetFeatureDim(), f2.GetFeatureDim());
  auto f1_dense_ptr = static_cast<const DenseFeature<float>*>(&f1);
  auto f2_dense_ptr = static_cast<const DenseFeature<float>*>(&f2);
  const std::vector<float>& v1 = f1_dense_ptr->GetVector();
  const std::vector<float>& v2 = f2_dense_ptr->GetVector();
  Eigen::Map<const Eigen::VectorXf> e1(v1.data(), v1.size());
  Eigen::Map<const Eigen::VectorXf> e2(v2.data(), v2.size());
  return e1.dot(e2);
}

float SparseDenseFeatureDotProduct(const AbstractFeature<float>& f1,
    const AbstractFeature<float>& f2) {
  CHECK_EQ(f1.GetFeatureDim(), f2.GetFeatureDim());
  float sum = 0.;
  for (int i = 0; i < f1.GetNumEntries(); ++i) {
    int32_t f1_fid = f1.GetFeatureId(i);
    sum += f1.GetFeatureVal(i) * f2[f1_fid];
  }
  return sum;
}

float DenseSparseFeatureDotProduct(const AbstractFeature<float>& f1,
    const AbstractFeature<float>& f2) {
  return SparseDenseFeatureDotProduct(f2, f1);
}

float SparseSparseFeatureDotProduct(const AbstractFeature<float>& f1,
    const AbstractFeature<float>& f2) {
  CHECK_EQ(f1.GetFeatureDim(), f2.GetFeatureDim());
  int j = 0;
  float sum = 0.;
  int f2_num_entries = f2.GetNumEntries();
  for (int i = 0; i < f1.GetNumEntries() && j < f2_num_entries; ++i) {
    int32_t f1_fid = f1.GetFeatureId(i);
    while (f2.GetFeatureId(j) < f1_fid && j < f2_num_entries) {
      ++j;
    }
    if (f1_fid == f2.GetFeatureId(j)) {
      sum += f1.GetFeatureVal(i) * f2.GetFeatureVal(j);
    }
  }
  return sum;
}

void FeatureScaleAndAdd(float alpha, const DenseFeature<float>& f1,
    DenseFeature<float>* f2) {
  CHECK_EQ(f1.GetFeatureDim(), f2->GetFeatureDim());
  const std::vector<float>& f1_vec = f1.GetVector();
  std::vector<float>& f2_vec = f2->GetVector();
  for (int i = 0; i < f1_vec.size(); ++i) {
    f2_vec[i] += alpha * f1_vec[i];
  }
}

// f1 sparse, f2 dense.
void FeatureScaleAndAdd(float alpha, const AbstractFeature<float>& f1,
    AbstractFeature<float>* f2) {
  CHECK_EQ(f1.GetFeatureDim(), f2->GetFeatureDim());
  for (int i = 0; i < f1.GetNumEntries(); ++i) {
    int32_t f1_fid = f1.GetFeatureId(i);
    f2->SetFeatureVal(f1_fid, alpha * f1.GetFeatureVal(i) + (*f2)[f1_fid]);
  }
}

}  // namespace ml
}  // namespace petuum


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2012-2015 Rokko Developers https://github.com/t-sakashita/rokko
*
* 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 ROKKO_UTILITY_HELMERT_MATRIX_HPP
#define ROKKO_UTILITY_HELMERT_MATRIX_HPP

#include <cmath>
#include <stdexcept>
#include <boost/throw_exception.hpp>
#include <rokko/config.h>
#include <rokko/localized_matrix.hpp>
#if defined(ROKKO_HAVE_PARALLEL_DENSE_SOLVER)
# include <rokko/distributed_matrix.hpp>
#endif

namespace rokko {

class helmert_matrix {
public:
  template<typename T, typename MATRIX_MAJOR>
  static void generate(rokko::localized_matrix<T, MATRIX_MAJOR>& mat) {
    if (mat.rows() != mat.cols())
      BOOST_THROW_EXCEPTION(std::invalid_argument("helmert_matrix::generate() : non-square matrix"));
    int n = mat.rows();
    mat.row(0).fill( 1 / sqrt(n) );
    for (int i=1; i < mat.rows(); ++i) {
      mat.row(i).head(i).fill( 1 / sqrt(static_cast<T>(i*(i+1))) );
      mat(i,i) = - sqrt(static_cast<T>(i)/(i+1));
    }
  }

  template<typename T, typename MATRIX_MAJOR>
  static void generate_for_given_eigenvalues(rokko::localized_matrix<T, MATRIX_MAJOR>& mat, rokko::localized_vector<T> const& diag) {
    if (mat.rows() != mat.cols())
      BOOST_THROW_EXCEPTION(std::invalid_argument("helmert_matrix::generate() : non-square matrix"));
    int n = mat.rows();
    for (int i=0; i<n; ++i) {
      double common_elem = diag(0) / n;
      for (int k=i+1; k<n; ++k)
	common_elem += diag(k) / (k*(k+1));  // Remark: i=max(i,j)
      mat(i, i) = common_elem + i * diag(i) / (i+1);
      double val = common_elem - diag(i) / (i+1);
      mat.row(i).head(i).setConstant(val);
      mat.col(i).head(i).setConstant(val);
    }
  }

#if defined(ROKKO_HAVE_PARALLEL_DENSE_SOLVER)
  template<typename T, typename MATRIX_MAJOR>
  static void generate(rokko::distributed_matrix<T, MATRIX_MAJOR>& mat) {
    if (mat.get_m_global() != mat.get_n_global())
      BOOST_THROW_EXCEPTION(std::invalid_argument("helmert_matrix::generate() : non-square matrix"));
    const int n = mat.get_m_global();
    int start_i;
    if (mat.is_gindex_myrow(0)) {
      T val = 1 / sqrt(n);
      for(int local_j = 0; local_j < mat.get_n_local(); ++local_j)
        mat.set_local(0, local_j, val);
      start_i = 1;
    }
    else start_i = 0;

    for(int local_i = start_i; local_i < mat.get_m_local(); ++local_i) {
      int global_i = mat.translate_l2g_row(local_i);
      T val = 1 / sqrt(static_cast<T>(global_i*(global_i+1)));
      for(int local_j = 0; local_j < mat.get_n_local(); ++local_j) {
        int global_j = mat.translate_l2g_col(local_j);
	if (global_j < global_i) mat.set_local(local_i, local_j, val);
	else if (global_j == global_i) mat.set_local(local_i, local_j, - sqrt(static_cast<T>(global_i)/(global_i+1)));
      }
    }
  }

  /*
  // another (slower) implementation using set_global function
  template<typename T, typename MATRIX_MAJOR>
  static void generate_global(rokko::distributed_matrix<MATRIX_MAJOR>& mat) {
    if (mat.m_global != mat.n_global)
      BOOST_THROW_EXCEPTION(std::invalid_argument("helmert_matrix::generate() : non-square matrix"));
    for(int global_i=0; global_i<mat.m_global; ++global_i) {
      for(int global_j=0; global_j<mat.n_global; ++global_j) {
        mat.set_global(global_i, global_j, mat.m_global - std::max(global_i, global_j) );
      }
    }
  }
  */

  template<typename T, typename MATRIX_MAJOR>
  static void generate_for_given_eigenvalues(rokko::distributed_matrix<T, MATRIX_MAJOR>& mat, rokko::localized_vector<T> const& diag) {
    if (mat.get_m_global() != mat.get_n_global())
      BOOST_THROW_EXCEPTION(std::invalid_argument("helmert_matrix::generate() : non-square matrix"));
    const int n = mat.get_m_global();

    for(int local_i = 0; local_i < mat.get_m_local(); ++local_i) {
      int global_i = mat.translate_l2g_row(local_i);
      double common_elem = diag(0) / n;
      for (int k=global_i+1; k<n; ++k)
	common_elem += diag(k) / (k*(k+1));  // Remark: i=max(i,j)
      T val = common_elem - diag(global_i) / (global_i+1);
      for(int local_j = 0; local_j < mat.get_n_local(); ++local_j) {
        int global_j = mat.translate_l2g_col(local_j);
	if (global_j < global_i) mat.set_local(local_i, local_j, val);
	else if (global_j == global_i) mat.set_local(local_i, local_j, common_elem + global_i * diag(global_i) / (global_i+1));
      }
    }

    for(int local_j = 0; local_j < mat.get_n_local(); ++local_j) {
      int global_j = mat.translate_l2g_col(local_j);
      double common_elem = diag(0) / n;
      for (int k=global_j+1; k<n; ++k)
	common_elem += diag(k) / (k*(k+1));  // Remark: i=max(i,j)
      T val = common_elem - diag(global_j) / (global_j+1);
      for(int local_i = 0; local_i < mat.get_m_local(); ++local_i) {
        int global_i = mat.translate_l2g_row(local_i);
	if (global_i < global_j) mat.set_local(local_i, local_j, val);
      }
    }
  }
#endif
};

} // namespace rokko

#endif // ROKKO_UTILITY_HELMERT_MATRIX_HPP


/*
 [auto_generated]
 boost/numeric/odeint/stepper/runge_kutta4.hpp

 [begin_description]
 Implementation of the classical Runge-Kutta stepper with the generic stepper.
 [end_description]

 Copyright 2011-2013 Mario Mulansky
 Copyright 2011-2013 Karsten Ahnert

 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_NUMERIC_ODEINT_STEPPER_RUNGE_KUTTA4_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_RUNGE_KUTTA4_HPP_INCLUDED

#include <boost/fusion/container/vector.hpp>
#include <boost/fusion/container/generation/make_vector.hpp>

#include <boost/numeric/odeint/stepper/explicit_generic_rk.hpp>
#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>
#include <boost/numeric/odeint/algebra/operations_dispatcher.hpp>

#include <boost/array.hpp>

#include <boost/numeric/odeint/util/resizer.hpp>

namespace boost {
namespace numeric {
namespace odeint {

#ifndef DOXYGEN_SKIP
template< class Value = double >
struct rk4_coefficients_a1 : boost::array< Value , 1 >
{
    rk4_coefficients_a1( void )
    {
        (*this)[0] = static_cast< Value >( 1 ) / static_cast< Value >( 2 );
    }
};

template< class Value = double >
struct rk4_coefficients_a2 : boost::array< Value , 2 >
{
    rk4_coefficients_a2( void )
    {
        (*this)[0] = static_cast<Value>(0);
        (*this)[1] = static_cast< Value >( 1 ) / static_cast< Value >( 2 );
    }
};

template< class Value = double >
struct rk4_coefficients_a3 : boost::array< Value , 3 >
{
    rk4_coefficients_a3( void )
            {
        (*this)[0] = static_cast<Value>(0);
        (*this)[1] = static_cast<Value>(0);
        (*this)[2] = static_cast<Value>(1);
            }
};

template< class Value = double >
struct rk4_coefficients_b : boost::array< Value , 4 >
{
    rk4_coefficients_b( void )
    {
        (*this)[0] = static_cast<Value>(1)/static_cast<Value>(6);
        (*this)[1] = static_cast<Value>(1)/static_cast<Value>(3);
        (*this)[2] = static_cast<Value>(1)/static_cast<Value>(3);
        (*this)[3] = static_cast<Value>(1)/static_cast<Value>(6);
    }
};

template< class Value = double >
struct rk4_coefficients_c : boost::array< Value , 4 >
{
    rk4_coefficients_c( void )
    {
        (*this)[0] = static_cast<Value>(0);
        (*this)[1] = static_cast< Value >( 1 ) / static_cast< Value >( 2 );
        (*this)[2] = static_cast< Value >( 1 ) / static_cast< Value >( 2 );
        (*this)[3] = static_cast<Value>(1);
    }
};
#endif

template<
class State ,
class Value = double ,
class Deriv = State ,
class Time = Value ,
class Algebra = typename algebra_dispatcher< State >::algebra_type ,
class Operations = typename operations_dispatcher< State >::operations_type ,
class Resizer = initially_resizer
>
#ifndef DOXYGEN_SKIP
class runge_kutta4 : public explicit_generic_rk< 4 , 4 , State , Value , Deriv , Time ,
Algebra , Operations , Resizer >
#else
class runge_kutta4 : public explicit_generic_rk
#endif
{

public:

#ifndef DOXYGEN_SKIP
    typedef explicit_generic_rk< 4 , 4 , State , Value , Deriv , Time ,
            Algebra , Operations , Resizer > stepper_base_type;
#endif
    typedef typename stepper_base_type::state_type state_type;
    typedef typename stepper_base_type::value_type value_type;
    typedef typename stepper_base_type::deriv_type deriv_type;
    typedef typename stepper_base_type::time_type time_type;
    typedef typename stepper_base_type::algebra_type algebra_type;
    typedef typename stepper_base_type::operations_type operations_type;
    typedef typename stepper_base_type::resizer_type resizer_type;

    #ifndef DOXYGEN_SKIP
    typedef typename stepper_base_type::wrapped_state_type wrapped_state_type;
    typedef typename stepper_base_type::wrapped_deriv_type wrapped_deriv_type;
    typedef typename stepper_base_type::stepper_type stepper_type;
    #endif

    runge_kutta4( const algebra_type &algebra = algebra_type() ) : stepper_base_type(
            boost::fusion::make_vector( rk4_coefficients_a1<Value>() , rk4_coefficients_a2<Value>() , rk4_coefficients_a3<Value>() ) ,
            rk4_coefficients_b<Value>() , rk4_coefficients_c<Value>() , algebra )
    { }

};

/**
 * \class runge_kutta4
 * \brief The classical Runge-Kutta stepper of fourth order.
 *
 * The Runge-Kutta method of fourth order is one standard method for
 * solving ordinary differential equations and is widely used, see also
 * en.wikipedia.org/wiki/Runge-Kutta_methods
 * The method is  explicit and fulfills the Stepper concept. Step size control
 * or continuous output are not provided.
 *
 * This class derives from explicit_stepper_base and inherits its interface via CRTP (current recurring template pattern).
 * Furthermore, it derivs from explicit_generic_rk which is a generic Runge-Kutta algorithm. For more details see
 * explicit_stepper_base and explicit_generic_rk.
 *
 * \tparam State The state type.
 * \tparam Value The value type.
 * \tparam Deriv The type representing the time derivative of the state.
 * \tparam Time The time representing the independent variable - the time.
 * \tparam Algebra The algebra type.
 * \tparam Operations The operations type.
 * \tparam Resizer The resizer policy type.
 */

/**
 * \fn runge_kutta4::runge_kutta4( const algebra_type &algebra = algebra_type() )
 * \brief Constructs the runge_kutta4 class. This constructor can be used as a default
 * constructor if the algebra has a default constructor.
 * \param algebra A copy of algebra is made and stored inside explicit_stepper_base.
 */

}
}
}

#endif // BOOST_NUMERIC_ODEINT_STEPPER_RUNGE_KUTTA4_HPP_INCLUDED


#pragma once

#include "matrix/assembly/velocity_var_form.hpp"

#include <Eigen/Sparse>

namespace boltzmann {

// ----------------------------------------------------------------------
template <typename TEST_BASIS, typename TRIAL_BASIS>
void
make_mass_matrix(Eigen::SparseMatrix<double>& dst,
                 const TEST_BASIS& test_basis,
                 const TRIAL_BASIS& trial_basis,
                 const double beta = 2)
{
  VelocityVarForm<2> velocity_var_form;
  velocity_var_form.init(test_basis, trial_basis, beta);
  const auto& s0 = velocity_var_form.get_s0();

  for (auto it = s0.begin(); it != s0.end(); ++it) {
    int i = it->row;
    int j = it->col;
    double val = it->val;
    dst.insert(i, j) = val;
  }
}

template <typename TEST_BASIS, typename TRIAL_BASIS>
Eigen::SparseMatrix<double>
make_mass_matrix(const TEST_BASIS& test_basis, const TRIAL_BASIS& trial_basis)
{
  assert(test_basis.n_dofs() == trial_basis.n_dofs());
  int N = test_basis.n_dofs();

  Eigen::SparseMatrix<double> M(N, N);
  VelocityVarForm<2> velocity_var_form;
  velocity_var_form.init(test_basis, trial_basis, 2.0);
  const auto& s0 = velocity_var_form.get_s0();

  for (auto it = s0.begin(); it != s0.end(); ++it) {
    int i = it->row;
    int j = it->col;
    double val = it->val;
    M.insert(i, j) = val;
  }

  return M;
}

template <typename TRIAL_BASIS>
Eigen::VectorXd
make_mass_vdiag(const TRIAL_BASIS& basis)
{
  int N = basis.n_dofs();

  Eigen::VectorXd out(N);
  VelocityVarForm<2> velocity_var_form;
  velocity_var_form.init(basis, basis, 2.0);
  const auto& s0 = velocity_var_form.get_s0();

  for (auto it = s0.begin(); it != s0.end(); ++it) {
    int i = it->row;
    int j = it->col;

    if (i != j) throw std::runtime_error("make_mass_vdiag: not diagonal!");
    double val = it->val;
    out[i] = val;
  }

  return out;
}

}  // end namespace boltzmann