/***********************************************************************
This file is part of the librjmcmc project source files.

Copyright : Institut Geographique National (2008-2012)
Contributors : Mathieu Brédif, Olivier Tournaire, Didier Boldo
email : librjmcmc@ign.fr

This software is a generic C++ library for stochastic optimization.

This software is governed by the CeCILL license under French law and
abiding by the rules of distribution of free software. You can use,
modify and/or redistribute the software under the terms of the CeCILL
license as circulated by CEA, CNRS and INRIA at the following URL
"http://www.cecill.info".

As a counterpart to the access to the source code and rights to copy,
modify and redistribute granted by the license, users are provided only
with a limited warranty and the software's author, the holder of the
economic rights, and the successive licensors have only limited liability.

In this respect, the user's attention is drawn to the risks associated
with loading, using, modifying and/or developing or reproducing the
software by the user in light of its specific status of free software,
that may mean that it is complicated to manipulate, and that also
therefore means that it is reserved for developers and experienced
professionals having in-depth computer knowledge. Users are therefore
encouraged to load and test the software's suitability as regards their
requirements in conditions enabling the security of their systems and/or
data to be ensured and, more generally, to use and operate it in the
same conditions as regards security.

The fact that you are presently reading this means that you have had
knowledge of the CeCILL license and that you accept its terms.

***********************************************************************/

#ifndef RJMCMC_SIMPLEX_VARIATE_HPP
#define RJMCMC_SIMPLEX_VARIATE_HPP

#include <boost/random/uniform_real.hpp>

namespace rjmcmc {

    template<int N> struct factorial    { enum { value = factorial<N-1>::value * N };};
    template<     > struct factorial<0> { enum { value = 1 }; };

    struct simplex_variate_log_policy
    {
        template<int N, typename Engine, typename OutputIterator, typename Rand>
        void apply(Engine& e, OutputIterator it, Rand& rand) const
        {
            double sum = 0;
            double x[N+1];
            for(unsigned int i=0; i<=N; ++i)
            {
                x[i] = -log(rand(e));
                sum += x[i];
            }
            for(unsigned int i=0; i<N; ++i, ++it)
            {
                *it = x[i]/sum;
            }
        }
    };

    struct simplex_variate_sort_policy
    {
        template<int N, typename Engine, typename OutputIterator, typename Rand>
        void apply(Engine& e, OutputIterator it, Rand& rand) const
        {
            double x[N];
            for(unsigned int i=0; i<N; ++i) x[i] = rand(e);
            std::sort(x,x+N);
            double prev = 0;
            for(unsigned int i=0; i<N; ++i, ++it)
            {
                *it = x[i]-prev;
                prev = x[i];
            }
        }
    };

    struct simplex_variate_power_policy
    {
        template<int N, typename Engine, typename OutputIterator, typename Rand>
        void apply(Engine& e, OutputIterator it, Rand& rand) const
        {
            static_assert(N>1,"simplex_variate_power_policy is only valid for 2D variates or more !");
            double prev = pow(rand(e),1./N);
            for(unsigned int i=N-1; i>1; --i)
            {
                double next = prev*pow(rand(e),1./i);
                *it++ = prev-next;
                prev = next;
            }
            double next = prev*rand(e);
            *it++ = prev-next;
            *it++ = next;
        }
    };

    struct simplex_variate_rejection_policy
    {
        template<int N, typename Engine, typename OutputIterator, typename Rand>
        void apply(Engine& e, OutputIterator it, Rand& rand) const
        {
            for(;;)
            {
                double x[N], sum;
                for(unsigned int i=0; i<N; ++i)
                {
                    x[i] = rand(e);
                    sum += x[i];
                }
                if(sum<=1.)
                {
                    for(unsigned int i=0; i<N; ++i)  *it++ = x[i];
                    return;
                }
            }
        }
    };

    struct simplex_variate_reflect_policy
    {
        template<int N, typename Engine, typename OutputIterator, typename Rand>
        void apply(Engine& e, OutputIterator it, Rand& rand) const
        {
            static_assert(N==2,"simplex_variate_reflect_policy is only valid for 2D variates !");
            double x = rand(e);
            double y = rand(e);
            if(x+y>1.)
            {
                x = 1.-x;
                y = 1.-y;
            }
            *it++ = x;
            *it++ = y;
        }
    };

    struct simplex_variate_sqrt_policy
    {
        template<int N, typename Engine, typename OutputIterator, typename Rand>
        void apply(Engine& e, OutputIterator it, Rand& rand) const
        {
            static_assert(N==2,"simplex_variate_sqrt_policy is only valid for 2D variates, use the generalized simplex_variate_power_policy instead !");
            double x = sqrt(rand(e));
            double y = x*rand(e);
            *it++ = x-y;
            *it++ = y;
        }
    };

    template<int N, typename Policy = simplex_variate_power_policy >
    class simplex_variate
    {
        typedef boost::uniform_real<> rand_type;
        mutable rand_type m_rand;
        const double m_pdf;
        Policy m_policy;
    public:
        typedef double value_type;
        enum { dimension = N };
        template<typename InputIterator>
        inline double pdf(InputIterator it) const {
            double sum = 0;
            for(unsigned int i=0; i<N; ++i, ++it)
            {
                if(*it<0) return 0;
                sum += *it;
            }
            return double(sum <= 1.)*m_pdf;
        }
        template<typename Engine, typename OutputIterator>
        inline double operator()(Engine& e, OutputIterator it) const {
            m_policy.template apply<N>(e,it,m_rand);
            return m_pdf;
        }
        simplex_variate() : m_rand(0,1), m_pdf(1./factorial<N>::value) {}

    };

}; // namespace rjmcmc

#endif // RJMCMC_SIMPLEX_VARIATE_HPP


//==================================================================================================
/**
  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/ifrexp.hpp>
#include <boost/simd/function/pedantic.hpp>
#include <boost/simd/constant/nbmantissabits.hpp>
#include <boost/simd/constant/halfeps.hpp>
#include <scalar_test.hpp>

namespace bs = boost::simd;
namespace bd = boost::dispatch;

STF_CASE_TPL("Check basic behavior of pedantic_(ifrexp)", STF_IEEE_TYPES)
{
  STF_EXPR_IS ( (bs::pedantic_(bs::ifrexp)(T(0)))
              , (std::pair<T,bd::as_integer_t<T,signed>>)
              );

  auto p = bs::pedantic_(bs::ifrexp)(T(1));
  STF_EQUAL(p.first  , T(0.5));
  STF_EQUAL(p.second , T(1));
}

STF_CASE_TPL("Check behavior of pedantic_(ifrexp) on Zero", STF_IEEE_TYPES)
{
  auto r = bs::pedantic_(bs::ifrexp)(T(0));

  STF_EQUAL (r.first , T(0));
  STF_EQUAL (r.second, T(0));
  STF_EQUAL (ldexp(r.first,r.second), T(0));
}

STF_CASE_TPL("Check behavior of pedantic_(ifrexp) on Valmax", STF_IEEE_TYPES)
{
  auto r = bs::pedantic_(bs::ifrexp)(bs::Valmax<T>());

  STF_ULP_EQUAL (r.first , T(1)-bs::Halfeps<T>(), 1);
  STF_EQUAL     (r.second, bs::Limitexponent<T>());
  STF_EQUAL     (ldexp(r.first,r.second),bs::Valmax<T>());
}

#ifndef BOOST_SIMD_NO_INVALID
#include <boost/simd/constant/nan.hpp>

STF_CASE_TPL("Check behavior of pedantic_(ifrexp) on NaN", STF_IEEE_TYPES)
{
  auto r = bs::pedantic_(bs::ifrexp)(bs::Nan<T>());

  STF_IEEE_EQUAL(r.first , bs::Nan<T>());
  STF_EQUAL     (r.second, T(0));
  STF_IEEE_EQUAL(ldexp(r.first,r.second), bs::Nan<T>());
}
#endif

#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>

STF_CASE_TPL("Check behavior of pedantic_(ifrexp) on infinites", STF_IEEE_TYPES)
{
  auto r = bs::pedantic_(bs::ifrexp)(bs::Inf<T>());
  auto q = bs::pedantic_(bs::ifrexp)(bs::Minf<T>());

  STF_IEEE_EQUAL(r.first , bs::Inf<T>());
  STF_EQUAL     (r.second, T(0));
  STF_IEEE_EQUAL(ldexp(r.first,r.second), bs::Inf<T>());

  STF_IEEE_EQUAL(q.first , bs::Minf<T>());
  STF_EQUAL     (q.second, T(0));
  STF_IEEE_EQUAL(ldexp(q.first,q.second), bs::Minf<T>());
}
#endif

#ifndef BOOST_SIMD_NO_DENORMALS
#include <boost/simd/detail/constant/minexponent.hpp>
#include <boost/simd/constant/mindenormal.hpp>

STF_CASE_TPL("Check behavior of pedantic_(ifrexp) on denormals", STF_IEEE_TYPES)
{
  auto r = bs::pedantic_(bs::ifrexp)(bs::Mindenormal<T>());

  STF_ULP_EQUAL (r.first, T(0.5), 1);
  STF_EQUAL     (r.second, bs::Minexponent<T>()-bs::Nbmantissabits<T>()+1);
  STF_EQUAL     (ldexp(r.first,r.second),bs::Mindenormal<T>());
}
#endif


#ifndef LATTICE_REGULARCUBE_CONNECTED_TAGGEDPARTICLE_HPP
#define LATTICE_REGULARCUBE_CONNECTED_TAGGEDPARTICLE_HPP

#include <string>
#include <vector>
#include <boost/lexical_cast.hpp>

namespace lattice {

class RegularCubeConnectedTaggedParticle{
public:
  static std::string name() { return "Regular Cube lasso (total must be even)";}
  static int set_num_particles(int Ns){return Ns*Ns*Ns+1;}
  RegularCubeConnectedTaggedParticle(unsigned int LatticeSize) : dim_(LatticeSize) {}

  void create_table(std::vector<std::vector < int > >& table){
  //for bulk
    for (int i = 0; i < dim_; ++i){
      for (int j = 0; j < dim_; ++j){
        for (int k = 0; k < dim_; ++k){
          int l = numberize(i,j,k);
          int temp = 0;
          table[l][6] = l;
          for(int d = -1; d < 2; d =d+2){
            table[l][temp] = numberize(i+d,j,k);
            temp += 1;
            table[l][temp] = numberize(i,j+d,k);
            temp += 1;
            table[l][temp] = numberize(i,j,k+d);
            temp += 1;
          }
        }
      }
    }
   int tagg = dim_ * dim_ * dim_ ;
   int l = numberize((dim_-1)/2,(dim_-1)/2,(dim_-1)/2);
   table[l][6] = tagg;
   table[tagg][0] = l;
   for(int temp = 1; temp < 7 ; ++temp) table[tagg][temp] = tagg;

  }

  int numberize(const int i, const int j,const int k){
    int i_t = (i + dim_);
    int j_t = (j + dim_);
    int k_t = (k + dim_);
    i_t %= dim_ ;
    j_t %= dim_ ;
    k_t %= dim_ ;
    return i_t + j_t * dim_ + k_t * dim_ * dim_;
  }

  int latticize(int d, const int l){ //d is direction(0 ~ ) ex. 0 denotes x, 1 denotes y,~
    int k = l / (dim_*dim_) ;
    int j = (l - dim_ * dim_* k)/dim_ ;
    int i = (l - dim_ * dim_ * k - dim_ *j);
    if(d==0) return i;
    else if (d==1) return j;
    else return k;
  }

  int number_adjacent() {return 7;}

private:
  unsigned int dim_;
};

} //end namespace

#endif //LATTICE_REGULARCUBE_CONNECTED_TAGGEDPARTICLE_HPP


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file LevMarqGaussNewtonSolver.hpp
///
/// \author Sean Anderson, ASRL
//////////////////////////////////////////////////////////////////////////////////////////////

#ifndef STEAM_LEVMARQ_GAUSS_NEWTON_SOLVER_HPP
#define STEAM_LEVMARQ_GAUSS_NEWTON_SOLVER_HPP

#include <Eigen/Core>

#include <steam/solver/GaussNewtonSolverBase.hpp>

namespace steam {

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Solver using Levenberg–Marquardt for the trust region
//////////////////////////////////////////////////////////////////////////////////////////////
class LevMarqGaussNewtonSolver : public GaussNewtonSolverBase
{
 public:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Dogleg parameters
  //////////////////////////////////////////////////////////////////////////////////////////////
  struct Params : public SolverBase::Params {

    //////////////////////////////////////////////////////////////////////////////////////////////
    /// \brief Default constructor
    //////////////////////////////////////////////////////////////////////////////////////////////
    Params() : SolverBase::Params(), ratioThreshold(0.25),
      shrinkCoeff(0.1), growCoeff(10.0), maxShrinkSteps(50) {
    }

    /// Minimum ratio of actual to predicted reduction, shrink trust region if lower, else grow (range: 0.0-1.0)
    double ratioThreshold;

    /// Amount to shrink by (range: <1.0)
    double shrinkCoeff;

    /// Amount to grow by (range: >1.0)
    double growCoeff;

    /// Maximum number of times to shrink trust region before giving up
    unsigned int maxShrinkSteps;
  };

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor
  //////////////////////////////////////////////////////////////////////////////////////////////
  LevMarqGaussNewtonSolver(OptimizationProblem* problem, const Params& params = Params());

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Solve the Levenberg–Marquardt system of equations:
  ///        A*x = b, A = (J^T*J + diagonalCoeff*diag(J^T*J))
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::VectorXd solveLevMarq(const Eigen::VectorXd& gradientVector, double diagonalCoeff);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Perform a plain LLT decomposition on the approx. Hessian matrix in
  ///        order to solve for the proper covariances (unmodified by the LM diagonal)
  //////////////////////////////////////////////////////////////////////////////////////////////
  void solveCovariances();

 private:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Build the system, solve for a step size and direction, and update the state
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual bool linearizeSolveAndUpdate(double* newCost, double* gradNorm);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Casts parameters to base type (for SolverBase class)
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual const SolverBase::Params& getSolverBaseParams() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Parameters
  //////////////////////////////////////////////////////////////////////////////////////////////
  Params params_;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Diagonal multiplier (lambda in most papers - related to trust region size)
  //////////////////////////////////////////////////////////////////////////////////////////////
  double diagCoeff;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief The 'left-hand-side' of the Gauss-Newton problem, generally known as the
  ///        approximate Hessian matrix (note we only store the upper-triangular elements).
  ///        Note that LM stores this to later solve the plain system for covariances.
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::SparseMatrix<double> approximateHessian_;

};

} // steam

#endif // STEAM_LEVMARQ_GAUSS_NEWTON_SOLVER_HPP


/***********************************************************************
Copyright 2018 Gregory Bryant

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.
***********************************************************************/

#include "factormap.h"
#include "neconsolewindow.h"
#include "neview.h"
#include <boost/multiprecision/gmp.hpp>
#include <iostream>

using boost::multiprecision::abs;
using boost::multiprecision::mpz_int;

mpz_int FactorMap::fixYOffsetAndSign(mpz_int x, mpz_int y)
{
    if(y>=0)
    {
        if(y>abs(x))
        {
            if(y%abs(x)!=0)
            {return abs(x)-(y%abs(x))+y;}
            else{return y;}
        }
        else{return abs(x);}
    }
    else
    {
        if(abs(y)>=abs(x))
        {
            temp=abs(x)-(abs(y)%abs(x))+abs(y);
            temp=temp-abs(x);
            temp*=-1;
            return temp;
        }
        else{return 0;}
    }
}

FactorMap::FactorMap()
{
    name.append("FactorMap");
}

void FactorMap::render(NEView *view)
{

    view->getScreenBounds(&top,&bottom,&left,&right);

    //for(int i=left;i<right;i++)
    i = left;
    while(i<right)
    {
        if(i==0){i++;continue;}

        j = fixYOffsetAndSign(i,bottom);

        while(j<=top)
        {
            //mpz_int yVal = view->height()-1-j+view->yOffset;
            //mpz_int xVal = i-view->xOffset;
            //view->setPoint(static_cast<int>(xVal),static_cast<int>(yVal));
            view->setPoint(i,j);
            j+=abs(i);
        }
    i++;
    }
}


/*
 * Mesh.cpp
 *
 * This class contains the mesh and mesh properties to be used
 * in the finite element method (FiniteElem.cpp).
 *
 * It constructs the mesh, then constructs the stiffness matrix and
 * calculates the boundary.
 *
 *  Created on: Aug 13, 2016
 *      Author: Ted Kwan
 */

#include "Mesh.h"
#include <cmath>
#include <iostream>
#include <armadillo>

using namespace std;
using namespace arma;

Mesh::Mesh() {

}

/**
 * Constructor for Mesh. It takes one input, a vector containing the
 * properties of the mesh to be created.
 *
 * @param meshprops - double vector with all of the necessary properties.
 */
Mesh::Mesh(vector<double> meshprops) {
	// Extract the mesh size value.
	h = meshprops[4];
	n = round(1 / h);
	// Create vectors to be used for mesh creation.
	vec xr = linspace<vec>(meshprops[0], meshprops[1], n + 1);
	vec yr = linspace<vec>(meshprops[2], meshprops[3], n + 1);
	// Create the mesh.
	makeMesh(xr, yr);
	// Assemble stiffness matrix

	vector<sp_mat> stiffmass = assembleMatrix();
	stiffness=stiffmass[0];
	mass=stiffmass[1];
	// Calculate boundary nodes.
	findBoundary();
}

/**
 * makeMesh creates the mesh for the 2D Cartesian grid.
 *
 * @param xr - input vector containing the x values.
 * @param yr - input vector containing the y values.
 */
void Mesh::makeMesh(vec xr, vec yr) {
	// Findthe lengths to use.
	uword xrl = xr.n_rows;
	uword yrl = xr.n_rows;
	mat x = zeros<mat>(xrl, yrl);
	mat y = zeros<mat>(xrl, yrl);
	// Construct the meshes. This is just the same
	// as calling meshgrid() in MATLAB.
	for (uword i = 0; i < yrl; i++) {
		x.row(i) = xr.t();
	}
	for (uword i = 0; i < xrl; i++) {
		y.col(i) = yr;
	}
	// Vectorise the values to use for the nodes matrix.
	vec xv = vectorise(x);
	N = xv.n_rows;
	uword ni = x.n_rows;
	// Initialize node matrix.
	node = zeros<mat>(N, 2);
	// put all the values in the node matrix
	node.col(0) = xv;
	node.col(1) = vectorise(y);
	// Map the indices for the nodes to create elem matrix.
	uvec t2nidxMapnz = regspace<uvec>(0, N - ni - 1);
	uvec topNode = regspace<uvec>(ni - 1, ni, N - ni - 1);
	// Set all of the doubled nodes to not be included.
	t2nidxMapnz(topNode) = zeros<uvec>(topNode.n_rows);
	// Find nonzeros in the original indices map.
	uvec nnz = nonzeros(t2nidxMapnz);
	uvec k = zeros<uvec>(nnz.n_rows + 1);
	// Calculate the different values used to create nodes.
	k(span(1, nnz.n_rows)) = nnz;
	uword NE = k.n_rows;
	// Create the elements which will differ by odd and
	// even elements.
	umat elemup = zeros<umat>(NE, 3);
	umat elemdown = zeros<umat>(NE, 3);
	uvec niv = ones<uvec>(k.n_rows);
	niv = niv * ni;
	uvec onek = ones<uvec>(k.n_rows);
	// Map elements to nodes in order.
	elemup.col(0) = k + niv;
	elemup.col(1) = k + niv + onek;
	elemup.col(2) = k;
	elemdown.col(0) = k + onek;
	elemdown.col(1) = k;
	elemdown.col(2) = k + niv + onek;
	// join all columns together and stack them properly.
	elem = join_cols(elemup, elemdown);
	NT = elem.n_rows;
}

/**
 * assembleMatrix assembles the stiffness matrix using the quick
 * construction method from armadillo for sparse matrices.
 *
 * @return - Stiffness matrix as a sparse matrix.
 */
vector<sp_mat> Mesh::assembleMatrix() {
	// Initialize index maps and value vector.
	uvec ii = zeros<uvec>(9 * NT);
	uvec jj = zeros<uvec>(9 * NT);
	vec sA = zeros<vec>(9 * NT);
	// Calculate the area of each node.
	cube ve(NT, 2, 3);
	ve.slice(0) = node.rows(elem.col(2)) - node.rows(elem.col(1));
	ve.slice(1) = node.rows(elem.col(0)) - node.rows(elem.col(2));
	ve.slice(2) = node.rows(elem.col(1)) - node.rows(elem.col(0));
	// Find the area using the dot product on the second dimension.
	area = 0.5*abs((ve.slice(2).col(0) % ve.slice(1).col(1))
					-(ve.slice(2).col(1) % ve.slice(1).col(0)));
	uword index = 0;
	// Loop to map values and indices.
	for (uword i = 0; i < 3; i++) {
		for (uword j = 0; j < 3; j++) {
			// Setup indices to map in this iteration.
			uvec inds = regspace<uvec>(index, index + NT - 1);
			// Setup element maps for indices.
			ii(inds) = elem.col(i);
			jj(inds) = elem.col(j);
			// Calculate values of stiffness matrix at
			// these points.
			mat prod = ve.slice(i) % ve.slice(j);
			// Store calculated value.
			sA(inds) = sum(prod, 1) / (4 * area);
			index = index + NT;
		}
	}
	// Setup index map to be a 2x9NT matrix.
	umat inds = join_horiz(ii, jj);
	// Create the sparse matrix using the same
	// method as sparse(row indices, col indices, values, size)
	sp_mat A(true, inds.t(), sA, N, N, true, true);
	vector<sp_mat> matvec(2);
	matvec[0]=A;
	vec Mv=accumArrayM(join_vert(elem.col(0), join_vert(elem.col(1),elem.col(2)))
			,join_vert(area, join_vert(area,area))/(3.0), N);
	uvec inds2=regspace<uvec>(0,Mv.n_rows-1);
	umat subs2=join_horiz(inds2,inds2);
	subs2.print("indices: ");
	sp_mat M(true,subs2.t(),Mv,N,N,true,true);
	matvec[1]=M;
	return matvec;
}

/**
 * Find the boundary nodes and elements.
 *
 * This method calculates the boundary for the given mesh to be used
 * to set the boundary condition for the finite element method.
 *
 * Values are stored so that they can be accesssed later.
 *
 */
void Mesh::findBoundary() {

	// Setup as two column vectors to find the edges.
	umat e1 = join_vert(join_horiz(elem.col(2), elem.col(1)),
			join_horiz(elem.col(0), elem.col(2)));
	// Calculate all of the values of the edges.
	umat totalEdge = join_vert(e1, join_horiz(elem.col(1), elem.col(0)));
	totalEdge = sort(totalEdge, "ascend", 1);
	vec onev = ones<vec>(totalEdge.n_rows);
	// Create sparse matrix containing the edges which are not being double counted
	// and are thus exterior edges.
	sp_mat fndmat(true, totalEdge.t(), onev, totalEdge.n_rows, totalEdge.n_rows,
			true, true);
	// Initialize edge matrix.
	umat bdEdge = zeros<umat>(totalEdge.n_rows, 2);
	vec s = nonzeros(fndmat);
	uvec ii = zeros<uvec>(s.n_elem);
	uvec jj = zeros<uvec>(s.n_elem);
	uword k = 0;
	// Get the indices for the edges which are exterior edges.
	// The indices map back to nodes.
	for (uword i = 0; i < fndmat.n_rows; i++) {
		for (uword j = 0; j < fndmat.n_cols; j++) {
			double spot = fndmat(i, j);
			// Only find edges where there are two boundary nodes.
			if (spot == 1.0 && k < s.n_elem) {
				ii(k) = i;
				jj(k) = j;
				k++;
			}
		}
	}
	uvec i1 = nonzeros(ii);
	uvec j1 = nonzeros(jj);
	// Setup boolean vector which has whether or not a node is a boundary
	// node.
	isbdNode = zeros<uvec>(N);
	isbdNode.rows(i1) = ones<uvec>(i1.n_rows);
	isbdNode.rows(j1) = ones<uvec>(j1.n_rows);
	isbdNode(0) = 1.0;
	// Find indices of boundary nodes.
	bdNode = find(isbdNode);
	uvec onebd = ones<uvec>(isbdNode.n_rows);
	// Find indices of interior nodes.
	freeNode = find(onebd - isbdNode);
}

vec Mesh::accumArrayM(uvec subs,vec ar,uword N){

	vec S=zeros<vec>(N);

	for(uword i=0;i<N;i++){
		// Get the subscripts.
		uvec q1=find(subs ==(i));
		vec spot;
		double thesum=0.0;
		if(!q1.is_empty()){
			// Find elements at indices q1
			spot=ar.elem(q1);
			// Sum elements.
			thesum=sum(spot);
			// Set at position i.
			S(i)=thesum;
		}
		// If it is empty, add 0.
		else{
			S(i)=0.0;
		}
	}

	return S;
}

Mesh::~Mesh() {
	// TODO Auto-generated destructor stub
}


// ExprFormulaTester.cpp : This file contains the 'main' function. Program execution begins and ends there.
//

#include <iostream>

#include "AltNum\MediumDec.hpp"
//#include <boost/timer/timer.hpp>
using MediumDec = BlazesRusCode::MediumDec;

#ifdef BlazesRus_PerformMediumFormulaTests
#include "AltNum\AltNumDebug.hpp"
#include "Formulas\MediumDecFormula.hpp"
using MediumDecFormula = BlazesRusCode::MediumDecFormula;
#endif
#include "AltNum\FloatingOperations.hpp"
//#include "Databases\IntFormula.hpp"
//using IntFormula = BlazesRusCode::IntFormula;
//#include "Databases\DoubleFormula.hpp"
//using DoubleFormula = BlazesRusCode::DoubleFormula;

#include "AltNum\AltDec.hpp"
#include "AltNum\MixedDec.hpp"
using AltDec = BlazesRusCode::AltDec;
using MixedDec = BlazesRusCode::MixedDec;
#include <boost/math/constants/constants.hpp>
#include <sstream>
#include <iomanip>

#include <Windows.h>

int main()
{
#ifdef BlazesRus_PerformMediumFormulaTests
    MediumDec targetVal;// = "5.5";
    MediumDec rightVal;// = "1.25";
    MediumDec altResult; //= targetVal - rightVal;
    //std::cout << targetVal.ToString() << " - " << rightVal.ToString() << " = " << altResult.ToString() << std::endl;
    //altResult = targetVal + rightVal;
    //std::cout << targetVal.ToString() << " + " << rightVal.ToString() << " = " << altResult.ToString() << std::endl;

    //altResult = targetVal*rightVal;
    //double floatingVal = (double)targetVal * (double)rightVal;
    //std::cout << targetVal.ToString() << " * " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:"<<floatingVal<< std::endl;
    //altResult = targetVal/rightVal;
    //floatingVal = (double)targetVal / (double)rightVal;
    //std::cout << targetVal.ToString() << " / " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;
    //altResult = targetVal - 3;
    //std::cout << targetVal.ToString() << " - 3 = " << altResult.ToString() << std::endl;
    //altResult = targetVal + 3;
    //std::cout << targetVal.ToString() << " + 3 = " << altResult.ToString() << std::endl;
    //altResult = targetVal * 3;
    //std::cout << targetVal.ToString() << " * 3 = " << altResult.ToString() << std::endl;
    //altResult = targetVal / 3;
    //std::cout << targetVal.ToString() << " / 3 = " << altResult.ToString() << std::endl;
    //altResult = targetVal; altResult -= rightVal;
    //std::cout << targetVal.ToString() << " -= :" << rightVal.ToString() << " = " << altResult.ToString() << std::endl;
    //altResult = targetVal; altResult += rightVal;
    //std::cout << targetVal.ToString() << " += :" << rightVal.ToString() << " = " << altResult.ToString() << std::endl;
    //altResult = targetVal; altResult *= rightVal;
    //std::cout << targetVal.ToString() << " *= :" << rightVal.ToString() << " = " << altResult.ToString() << std::endl;
    //altResult = targetVal; altResult /= rightVal;
    //std::cout << targetVal.ToString() << " /= :" << rightVal.ToString() << " = " << altResult.ToString() << std::endl;
    //altResult = targetVal; altResult -= 3;
    //std::cout << targetVal.ToString() << " -= 3 :" << altResult.ToString() << std::endl;
    //altResult = targetVal; altResult += 3;
    //std::cout << targetVal.ToString() << " += 3 :" << altResult.ToString() << std::endl;
    //altResult = targetVal; altResult *= 3;
    //std::cout << targetVal.ToString() << " *= 3 :" << altResult.ToString() << std::endl;
    //altResult = targetVal; altResult /= 3;
    //std::cout << targetVal.ToString() << " /= 3 : " << altResult.ToString() << std::endl;
    //bool boolCheck = targetVal < rightVal;
    //std::cout << targetVal.ToString() << " < " << rightVal.ToString() << " = " << boolCheck << std::endl;
    //boolCheck = targetVal <= rightVal;
    //std::cout << targetVal.ToString() << " <= " << rightVal.ToString() << " = " << boolCheck << std::endl;
    //boolCheck = targetVal > rightVal;
    //std::cout << targetVal.ToString() << " > " << rightVal.ToString() << " = " << boolCheck << std::endl;
    //boolCheck = targetVal >= rightVal;
    //std::cout << targetVal.ToString() << " >= " << rightVal.ToString() << " = " << boolCheck << std::endl;
    ////-----------------------------------------------------------------------------------------------------------
    ////All multiplication & Division tests now successful
    //targetVal = "0.5";
    //rightVal = "0.25";
    //altResult = targetVal * rightVal;
    //floatingVal = (double)targetVal * (double)rightVal;
    //std::cout << targetVal.ToString() << " * " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;
    //altResult = targetVal / rightVal;
    //floatingVal = (double)targetVal / (double)rightVal;
    //std::cout << targetVal.ToString() << " / " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;

    //altResult = targetVal + rightVal;
    //floatingVal = (double)targetVal + (double)rightVal;
    //std::cout << targetVal.ToString() << " + " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;
    //altResult = targetVal - rightVal;
    //floatingVal = (double)targetVal - (double)rightVal;
    //std::cout << targetVal.ToString() << " - " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;

    //targetVal = "1.5";
    //altResult = targetVal * rightVal;
    //floatingVal = (double)targetVal * (double)rightVal;
    //std::cout << targetVal.ToString() << " * " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;
    //altResult = targetVal / rightVal;
    //floatingVal = (double)targetVal / (double)rightVal;
    //std::cout << targetVal.ToString() << " / " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;

    //targetVal = "0.5";
    //rightVal = "2.5";
    //altResult = targetVal * rightVal;
    //floatingVal = (double)targetVal * (double)rightVal;
    //std::cout << targetVal.ToString() << " * " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;
    //altResult = targetVal / rightVal;
    //floatingVal = (double)targetVal / (double)rightVal;
    //std::cout << targetVal.ToString() << " / " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;

    //altResult = targetVal + rightVal;
    //floatingVal = (double)targetVal + (double)rightVal;
    //std::cout << targetVal.ToString() << " + " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;
    //altResult = targetVal - rightVal;
    //floatingVal = (double)targetVal - (double)rightVal;
    //std::cout << targetVal.ToString() << " - " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;

    //rightVal = "2.0";
    //altResult = targetVal * rightVal;
    //floatingVal = (double)targetVal * (double)rightVal;
    //std::cout << targetVal.ToString() << " * " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;
    //altResult = targetVal / rightVal;
    //floatingVal = (double)targetVal / (double)rightVal;
    //std::cout << targetVal.ToString() << " / " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;

    //altResult = targetVal + rightVal;
    //floatingVal = (double)targetVal + (double)rightVal;
    //std::cout << targetVal.ToString() << " + " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;
    //altResult = targetVal - rightVal;
    //floatingVal = (double)targetVal - (double)rightVal;
    //std::cout << targetVal.ToString() << " - " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;

    //targetVal = "4.0";
    //rightVal = "2.0";
    //altResult = targetVal * rightVal;
    //floatingVal = (double)targetVal * (double)rightVal;
    //std::cout << targetVal.ToString() << " * " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;
    //altResult = targetVal / rightVal;
    //floatingVal = (double)targetVal / (double)rightVal;
    //std::cout << targetVal.ToString() << " / " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;

    //altResult = targetVal + rightVal;
    //floatingVal = (double)targetVal + (double)rightVal;
    //std::cout << targetVal.ToString() << " + " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;
    //altResult = targetVal - rightVal;
    //floatingVal = (double)targetVal - (double)rightVal;
    //std::cout << targetVal.ToString() << " - " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;

    //rightVal = "0.5";
    //altResult = targetVal * rightVal;
    //floatingVal = (double)targetVal * (double)rightVal;
    //std::cout << targetVal.ToString() << " * " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;
    //altResult = targetVal / rightVal;
    //floatingVal = (double)targetVal / (double)rightVal;
    //std::cout << targetVal.ToString() << " / " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;

    //std::cout << "------Additional Addition/Subtraction Tests(For Negative numbers etc)--------------------" << std::endl;

    //targetVal = "-0.5";
    //rightVal = "-0.25";
    //altResult = targetVal + rightVal;
    //floatingVal = (double)targetVal + (double)rightVal;
    //std::cout << targetVal.ToString() << " + " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;
    //altResult = targetVal - rightVal;
    //floatingVal = (double)targetVal - (double)rightVal;
    //std::cout << targetVal.ToString() << " - " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;
    //rightVal = "0.25";
    //altResult = targetVal + rightVal;
    //floatingVal = (double)targetVal + (double)rightVal;
    //std::cout << targetVal.ToString() << " + " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;
    //altResult = targetVal - rightVal;
    //floatingVal = (double)targetVal - (double)rightVal;
    //std::cout << targetVal.ToString() << " - " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;

    //targetVal = "-1.5";
    //altResult = targetVal + rightVal;
    //floatingVal = (double)targetVal + (double)rightVal;
    //std::cout << targetVal.ToString() << " + " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;
    //altResult = targetVal - rightVal;
    //floatingVal = (double)targetVal - (double)rightVal;
    //std::cout << targetVal.ToString() << " - " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;

    //targetVal = "-0.5";
    //rightVal = "-2.5";
    //altResult = targetVal + rightVal;
    //floatingVal = (double)targetVal + (double)rightVal;
    //std::cout << targetVal.ToString() << " + " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;
    //altResult = targetVal - rightVal;
    //floatingVal = (double)targetVal - (double)rightVal;
    //std::cout << targetVal.ToString() << " - " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;

    //rightVal = "2.5";
    //altResult = targetVal + rightVal;
    //floatingVal = (double)targetVal + (double)rightVal;
    //std::cout << targetVal.ToString() << " + " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;
    //altResult = targetVal - rightVal;
    //floatingVal = (double)targetVal - (double)rightVal;
    //std::cout << targetVal.ToString() << " - " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;

    //rightVal = "-2.0";
    //altResult = targetVal + rightVal;
    //floatingVal = (double)targetVal + (double)rightVal;
    //std::cout << targetVal.ToString() << " + " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;
    //altResult = targetVal - rightVal;
    //floatingVal = (double)targetVal - (double)rightVal;
    //std::cout << targetVal.ToString() << " - " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;

    //targetVal = "-4.0";
    //rightVal = "-2.0";
    //altResult = targetVal + rightVal;
    //floatingVal = (double)targetVal + (double)rightVal;
    //std::cout << targetVal.ToString() << " + " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;
    //altResult = targetVal - rightVal;
    //floatingVal = (double)targetVal - (double)rightVal;
    //std::cout << targetVal.ToString() << " - " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;

    //rightVal = "0.5";
    //altResult = targetVal + rightVal;
    //floatingVal = (double)targetVal + (double)rightVal;
    //std::cout << targetVal.ToString() << " + " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;
    //altResult = targetVal - rightVal;
    //floatingVal = (double)targetVal - (double)rightVal;
    //std::cout << targetVal.ToString() << " - " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;

    //rightVal = "-0.5";
    //altResult = targetVal + rightVal;
    //floatingVal = (double)targetVal + (double)rightVal;
    //std::cout << targetVal.ToString() << " + " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;
    //altResult = targetVal - rightVal;
    //floatingVal = (double)targetVal - (double)rightVal;
    //std::cout << targetVal.ToString() << " - " << rightVal.ToString() << " = " << altResult.ToString() << " FloatResult:" << floatingVal << std::endl;

    //std::cout << "---------------Log Tests------------------------------" << std::endl;
    //targetVal = MediumDec::FiveThousandth;
    //floatingVal = 0.005;
    //double floatingRes;
    //do
    //{
    //    floatingRes = log(floatingVal);
    //    std::cout << "Builtin-Ln(" << floatingVal << ") = " << floatingRes;
    //    floatingRes = BlazesFloatingCode::LnRefV2(floatingVal);
    //    std::cout << " Ln(value) =" << floatingRes << std::endl;

    //    altResult = MediumDec::LnRef(targetVal);
    //    std::cout << "(MediumDec)Ln(" << targetVal.ToString() << ")= " << altResult.ToString() << std::endl;
    //    altResult = MediumDec::LnRefV2(targetVal);
    //    std::cout << "(MediumDec)LnV2(" << targetVal.ToString() << ")= " << altResult.ToString() << std::endl;
    //    targetVal += MediumDec::FiveThousandth; floatingVal += 0.005;
    //} while (targetVal < MediumDec::One);
    std::cout << "---------------Testing Formula Code-------------------" << std::endl;
    //std::cout << "-------------------------Formula Code Tests---------------------------------" << std::endl;
    //IntFormula IntFormTest = "(5+5)^2";
    //tsl::ordered_map<std::string, int> IntValueDefinitions;
    //std::cout << IntFormTest.ToString() << " = " << IntFormTest.EvalValues(IntValueDefinitions) << std::endl;
    //IntValueDefinitions.insert_or_assign("x", 2);
    //IntFormTest = "5+10x";
    //std::cout << IntFormTest.ToString() << " = " << IntFormTest.EvalValues(IntValueDefinitions) << std::endl;

    tsl::ordered_map<std::string, MediumDec> ValueDefinitions;
    ValueDefinitions.insert_or_assign("x", MediumDec::Two);
    MediumDecFormula AltFormTest = "x+x";//"(x+1)^(5+4)";

    try
    {
        std::cout << "(MediumDecFormula) " << AltFormTest.ToString() << std::endl;
        AltFormTest.ReplaceVariablesWithValues(ValueDefinitions);
        std::cout << " = " << AltFormTest.ToString() << std::endl;
        AltFormTest.EvaluateOperations();
        std::cout << " = " << AltFormTest.ToString() << std::endl;

        //AltFormTest = "9.0 thBaseLog 4";
        //std::cout << "(MediumDecFormula) " << AltFormTest.ToString() << std::endl;
        //AltFormTest.ReplaceVariablesWithValues(ValueDefinitions);
        //std::cout << " = " << AltFormTest.ToString() << std::endl;
        //AltFormTest.EvaluateOperations();
        //std::cout << " = " << AltFormTest.ToString() << std::endl;

        AltFormTest = "(x+1)^(5+4/0.5-2)";
        std::cout << "(MediumDecFormula) " << AltFormTest.ToString() << std::endl;
        AltFormTest.ReplaceVariablesWithValues(ValueDefinitions);
        std::cout << " = " << AltFormTest.ToString() << std::endl;
        AltFormTest.EvaluateOperations();
        std::cout << " = " << AltFormTest.ToString() << std::endl;

        AltFormTest = "(x+1)^(5+4*0.5-2)";
        std::cout << "(MediumDecFormula) " << AltFormTest.ToString() << std::endl;
        AltFormTest.ReplaceVariablesWithValues(ValueDefinitions);
        std::cout << " = " << AltFormTest.ToString() << std::endl;
        AltFormTest.EvaluateOperations();
        std::cout << " = " << AltFormTest.ToString() << std::endl;
    }
    catch (const std::runtime_error& re)
    {
        std::cerr << "Runtime error: " << re.what() << std::endl;
    }
    catch (const std::exception& ex)
    {
        // specific handling for all exceptions extending std::exception, except
        // std::runtime_error which is handled explicitly
        std::cerr << "Error occurred: " << ex.what() << std::endl;
    }
    catch(...)
    {
        std::cout << "Unknown exception" << std::endl;
    }
#else
    std::ostringstream streamObj;
    streamObj << std::fixed << std::setprecision(99);
#ifdef BlazesRus_PITests
    streamObj << "HiperCalc result:18.84955592153875943077586029967701730518301639625063492584966755384689843771725399176820895205270241" << std::endl;

    double LAsDouble = boost::math::constants::pi<double>()*2.0;
    LAsDouble *= 3.0;
    float LAsFloat = boost::math::constants::pi<float>()*2.0f;
    LAsFloat *= 3.0f;
    /*streamObj << "-------Boost PI based Tests------" << std::endl;*/
    streamObj << "Double Result:" << LAsDouble << std::endl;
    streamObj << "Float Result:" << LAsFloat << std::endl;
    //streamObj << "---------------HiperCalc display based FloatingPoint Tests------------------" << std::endl;
    //LAsDouble = 3.141592653589793238462643383279502884197169399375105820974944592307816406286208998628034825342117068 * 2.0;
    //LAsFloat = 3.141592653589793238462643383279502884197169399375105820974944592307816406286208998628034825342117068f * 2.0f;
    //LAsDouble *= 3.0;
    //LAsFloat *= 3.0f;
    //streamObj << "Double Result:" << LAsDouble << std::endl;
    //streamObj << "Float Result:" << LAsFloat << std::endl;
    streamObj << "-------AltNum Tests------" << std::endl;
    AltDec LAlt;
    LAlt.SetPiVal(2);
    LAlt *= 3;
    streamObj << "AltDec Result:" << LAlt.ToString() << std::endl;
    MixedDec LMixed;
    LMixed.SetPiVal(2);
    LMixed *= 3;
    streamObj << "MixedDec Result:" << LMixed.ToString() << std::endl;
#elif defined(BlazesRus_PerformDualAltFormulaTests)

#else
    AltDec LAlt;
    LAlt.SetAsApproachingAwayFromValue(1);
    AltDec RAlt;
    LAlt.SetAsApproachingValueFromRight(-5);
    AltDec AltResult = LAlt+RAlt;
    streamObj << LAlt.ToString() <<" + "<< RAlt.ToString() << " = " << AltResult.ToString()<< std::endl;
#endif
    ::OutputDebugStringA(streamObj.str().c_str());//Outputing to debug output based on https://www.codeproject.com/Articles/1053/Using-an-Output-Stream-for-Debugging
#endif
}


//
//  EigenLibSolver.hpp
//  IPC
//
//  Created by Minchen Li on 6/30/18.
//
#pragma once

#include "LinSysSolver.hpp"

#include <Eigen/Eigen>

#include <vector>
#include <set>

namespace IPC {

template <typename vectorTypeI, typename vectorTypeS>
class EigenLibSolver : public LinSysSolver<vectorTypeI, vectorTypeS> {
    typedef LinSysSolver<vectorTypeI, vectorTypeS> Base;

protected:
    Eigen::SparseMatrix<double> coefMtr;
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> simplicialLDLT;

public:
    LinSysSolverType type() const override { return LinSysSolverType::EIGEN; }

    void set_pattern(const std::vector<std::set<int>>& vNeighbor, const std::set<int>& fixedVert) override;
    void set_pattern(const Eigen::SparseMatrix<double>& mtr) override; //NOTE: mtr must be SPD

    void analyze_pattern(void) override;

    bool factorize(void) override;

    void solve(Eigen::VectorXd& rhs, Eigen::VectorXd& result) override;

    double coeffMtr(int rowI, int colI) const override;

    void setZero(void) override;

    virtual void setCoeff(int rowI, int colI, double val) override;

    virtual void addCoeff(int rowI, int colI, double val) override;

    virtual void setUnit_row(int rowI) override;

    virtual void setUnit_col(int colI, const std::set<int>& rowVIs) override;
};

} // namespace IPC


#include <stan/math/mix/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/digamma.hpp>
#include <test/unit/math/rev/scal/fun/util.hpp>
#include <test/unit/math/mix/scal/fun/nan_util.hpp>

TEST(AgradFwdLbeta, FvarVar_FvarVar_1stDeriv) {
  using boost::math::digamma;
  using stan::math::fvar;
  using stan::math::lbeta;
  using stan::math::var;

  fvar<var> x(3.0, 1.3);
  fvar<var> z(6.0, 1.0);
  fvar<var> a = lbeta(x, z);

  EXPECT_FLOAT_EQ(lbeta(3.0, 6.0), a.val_.val());
  EXPECT_FLOAT_EQ(
      1.3 * digamma(3.0) + digamma(6.0) - (1.0 + 1.3) * digamma(3.0 + 6.0),
      a.d_.val());

  AVEC y = createAVEC(x.val_, z.val_);
  VEC g;
  a.val_.grad(y, g);
  EXPECT_FLOAT_EQ(digamma(3.0) - digamma(9.0), g[0]);
  EXPECT_FLOAT_EQ(digamma(6.0) - digamma(9.0), g[1]);
}
TEST(AgradFwdLbeta, FvarVar_Double_1stDeriv) {
  using boost::math::digamma;
  using stan::math::fvar;
  using stan::math::lbeta;
  using stan::math::var;

  fvar<var> x(3.0, 1.3);
  double z(6.0);
  fvar<var> a = lbeta(x, z);

  EXPECT_FLOAT_EQ(lbeta(3.0, 6.0), a.val_.val());
  EXPECT_FLOAT_EQ(1.3 * digamma(3.0) - (1.3) * digamma(3.0 + 6.0), a.d_.val());

  AVEC y = createAVEC(x.val_);
  VEC g;
  a.val_.grad(y, g);
  EXPECT_FLOAT_EQ(digamma(3.0) - digamma(9.0), g[0]);
}
TEST(AgradFwdLbeta, Double_FvarVar_1stDeriv) {
  using boost::math::digamma;
  using stan::math::fvar;
  using stan::math::lbeta;
  using stan::math::var;

  double x(3.0);
  fvar<var> z(6.0, 1.0);
  fvar<var> a = lbeta(x, z);

  EXPECT_FLOAT_EQ(lbeta(3.0, 6.0), a.val_.val());
  EXPECT_FLOAT_EQ(digamma(6.0) - digamma(3.0 + 6.0), a.d_.val());

  AVEC y = createAVEC(z.val_);
  VEC g;
  a.val_.grad(y, g);
  EXPECT_FLOAT_EQ(digamma(6.0) - digamma(9.0), g[0]);
}
TEST(AgradFwdLbeta, FvarVar_FvarVar_2ndDeriv) {
  using boost::math::digamma;
  using stan::math::fvar;
  using stan::math::lbeta;
  using stan::math::var;

  fvar<var> x(3.0, 1.3);
  fvar<var> z(6.0, 1.0);
  fvar<var> a = lbeta(x, z);

  EXPECT_FLOAT_EQ(lbeta(3.0, 6.0), a.val_.val());
  EXPECT_FLOAT_EQ(
      1.3 * digamma(3.0) + digamma(6.0) - (1.0 + 1.3) * digamma(3.0 + 6.0),
      a.d_.val());

  AVEC y = createAVEC(x.val_, z.val_);
  VEC g;
  a.d_.grad(y, g);
  EXPECT_FLOAT_EQ(1.3 * 0.39493407 - 2.3 * 0.11751201, g[0]);
  EXPECT_FLOAT_EQ(0.18132296 - 2.3 * 0.11751201, g[1]);
}
TEST(AgradFwdLbeta, FvarVar_Double_2ndDeriv) {
  using boost::math::digamma;
  using stan::math::fvar;
  using stan::math::lbeta;
  using stan::math::var;

  fvar<var> x(3.0, 1.3);
  double z(6.0);
  fvar<var> a = lbeta(x, z);

  EXPECT_FLOAT_EQ(lbeta(3.0, 6.0), a.val_.val());
  EXPECT_FLOAT_EQ(1.3 * digamma(3.0) - (1.3) * digamma(3.0 + 6.0), a.d_.val());

  AVEC y = createAVEC(x.val_);
  VEC g;
  a.d_.grad(y, g);
  EXPECT_FLOAT_EQ(1.3 * 0.39493407 - 1.3 * 0.11751201, g[0]);
}
TEST(AgradFwdLbeta, Double_FvarVar_2ndDeriv) {
  using boost::math::digamma;
  using stan::math::fvar;
  using stan::math::lbeta;
  using stan::math::var;

  double x(3.0);
  fvar<var> z(6.0, 1.0);
  fvar<var> a = lbeta(x, z);

  EXPECT_FLOAT_EQ(lbeta(3.0, 6.0), a.val_.val());
  EXPECT_FLOAT_EQ(digamma(6.0) - digamma(3.0 + 6.0), a.d_.val());

  AVEC y = createAVEC(z.val_);
  VEC g;
  a.d_.grad(y, g);
  EXPECT_FLOAT_EQ(0.18132296 - 0.11751201, g[0]);
}
TEST(AgradFwdLbeta, FvarFvarVar_FvarFvarVar_1stDeriv) {
  using boost::math::digamma;
  using stan::math::fvar;
  using stan::math::lbeta;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = lbeta(x, y);

  EXPECT_FLOAT_EQ(lbeta(3.0, 6.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(digamma(3.0) - digamma(9.0), a.val_.d_.val());
  EXPECT_FLOAT_EQ(digamma(6.0) - digamma(9.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(-0.11751202, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_, y.val_.val_);
  VEC g;
  a.val_.val_.grad(p, g);
  EXPECT_FLOAT_EQ(digamma(3.0) - digamma(9.0), g[0]);
  EXPECT_FLOAT_EQ(digamma(6.0) - digamma(9.0), g[1]);
}
TEST(AgradFwdLbeta, FvarFvarVar_Double_1stDeriv) {
  using boost::math::digamma;
  using stan::math::fvar;
  using stan::math::lbeta;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;

  double y(6.0);

  fvar<fvar<var> > a = lbeta(x, y);

  EXPECT_FLOAT_EQ(lbeta(3.0, 6.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(digamma(3.0) - digamma(9.0), a.val_.d_.val());
  EXPECT_FLOAT_EQ(0, a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.val_.val_.grad(p, g);
  EXPECT_FLOAT_EQ(digamma(3.0) - digamma(9.0), g[0]);
}
TEST(AgradFwdLbeta, Double_FvarFvarVar_1stDeriv) {
  using boost::math::digamma;
  using stan::math::fvar;
  using stan::math::lbeta;
  using stan::math::var;

  double x(3.0);

  fvar<fvar<var> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = lbeta(x, y);

  EXPECT_FLOAT_EQ(lbeta(3.0, 6.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0, a.val_.d_.val());
  EXPECT_FLOAT_EQ(digamma(6.0) - digamma(9.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.val_.val_.grad(p, g);
  EXPECT_FLOAT_EQ(digamma(6.0) - digamma(9.0), g[0]);
}
TEST(AgradFwdLbeta, FvarFvarVar_FvarFvarVar_2ndDeriv_x) {
  using boost::math::digamma;
  using stan::math::fvar;
  using stan::math::lbeta;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = lbeta(x, y);

  EXPECT_FLOAT_EQ(lbeta(3.0, 6.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(digamma(3.0) - digamma(9.0), a.val_.d_.val());
  EXPECT_FLOAT_EQ(digamma(6.0) - digamma(9.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(-0.11751202, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_, y.val_.val_);
  VEC g;
  a.val_.d_.grad(p, g);
  EXPECT_FLOAT_EQ(0.39493407 - 0.11751201, g[0]);
  EXPECT_FLOAT_EQ(-0.11751202, g[1]);
}
TEST(AgradFwdLbeta, FvarFvarVar_FvarFvarVar_2ndDeriv_y) {
  using boost::math::digamma;
  using stan::math::fvar;
  using stan::math::lbeta;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = lbeta(x, y);

  EXPECT_FLOAT_EQ(lbeta(3.0, 6.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(digamma(3.0) - digamma(9.0), a.val_.d_.val());
  EXPECT_FLOAT_EQ(digamma(6.0) - digamma(9.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(-0.11751202, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_, y.val_.val_);
  VEC g;
  a.d_.val_.grad(p, g);
  EXPECT_FLOAT_EQ(-0.11751202, g[0]);
  EXPECT_FLOAT_EQ(0.18132296 - 0.11751201, g[1]);
}
TEST(AgradFwdLbeta, FvarFvarVar_Double_2ndDeriv) {
  using boost::math::digamma;
  using stan::math::fvar;
  using stan::math::lbeta;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;

  double y(6.0);

  fvar<fvar<var> > a = lbeta(x, y);

  EXPECT_FLOAT_EQ(lbeta(3.0, 6.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(digamma(3.0) - digamma(9.0), a.val_.d_.val());
  EXPECT_FLOAT_EQ(0, a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.val_.d_.grad(p, g);
  EXPECT_FLOAT_EQ(0.39493407 - 0.11751201, g[0]);
}
TEST(AgradFwdLbeta, Double_FvarFvarVar_2ndDeriv) {
  using boost::math::digamma;
  using stan::math::fvar;
  using stan::math::lbeta;
  using stan::math::var;

  double x(3.0);

  fvar<fvar<var> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = lbeta(x, y);

  EXPECT_FLOAT_EQ(lbeta(3.0, 6.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0, a.val_.d_.val());
  EXPECT_FLOAT_EQ(digamma(6.0) - digamma(9.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.d_.val_.grad(p, g);
  EXPECT_FLOAT_EQ(0.18132296 - 0.11751201, g[0]);
}
TEST(AgradFwdLbeta, FvarFvarVar_FvarFvarVar_3rdDeriv) {
  using boost::math::digamma;
  using stan::math::fvar;
  using stan::math::lbeta;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = lbeta(x, y);

  AVEC p = createAVEC(x.val_.val_, y.val_.val_);
  VEC g;
  a.d_.d_.grad(p, g);
  EXPECT_FLOAT_EQ(0.013793319, g[0]);
  EXPECT_FLOAT_EQ(0.013793319, g[1]);
}
TEST(AgradFwdLbeta, FvarFvarVar_Double_3rdDeriv) {
  using boost::math::digamma;
  using stan::math::fvar;
  using stan::math::lbeta;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;
  x.d_.val_ = 1.0;

  double y(6.0);

  fvar<fvar<var> > a = lbeta(x, y);

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.d_.d_.grad(p, g);
  EXPECT_FLOAT_EQ(-0.140320487123420796890184645287, g[0]);
}
TEST(AgradFwdLbeta, Double_FvarFvarVar_3rdDeriv) {
  using boost::math::digamma;
  using stan::math::fvar;
  using stan::math::lbeta;
  using stan::math::var;

  double x(3.0);

  fvar<fvar<var> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;
  y.val_.d_ = 1.0;

  fvar<fvar<var> > a = lbeta(x, y);

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.d_.d_.grad(p, g);
  EXPECT_FLOAT_EQ(-0.0189964130493467228161105712126, g[0]);
}

struct lbeta_fun {
  template <typename T0, typename T1>
  inline typename boost::math::tools::promote_args<T0, T1>::type operator()(
      const T0 arg1, const T1 arg2) const {
    return lbeta(arg1, arg2);
  }
};

TEST(AgradFwdLbeta, nan) {
  lbeta_fun lbeta_;
  test_nan_mix(lbeta_, 3.0, 5.0, false);
}


/**
 * for body lean and angular velocity,
 * using kalman filter
 */
#pragma once
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <iostream>
#include "types/SensorValues.hpp"
#include "perception/kinematics/Kinematics.hpp"
#include "motion/touch/FeetState.hpp"

using namespace boost::numeric::ublas;
using namespace std;

class TorsoStateFilter {
   public:
      explicit TorsoStateFilter();
      matrix<float> update(matrix<float> obs, FeetState feetState);
      void init(float dt, float obsAngleSD, float obsVelSD, bool frontal);
      ~TorsoStateFilter();

   private:
      matrix<float> est;
      matrix<float> estBar;
      matrix<float> covEst;
      matrix<float> covEstBar;
      matrix<float> covR;
      matrix<float> covQ;

      float dt;
      bool frontal;

      float getFulcrum(matrix<float> obs, FeetState feetState);
};


#include "Damped_Rational.hxx"

#include <El.hpp>

#include <boost/math/constants/constants.hpp>

#include <vector>

// Rescaled Laguerre
std::vector<Boost_Float>
sample_scalings(const std::vector<Boost_Float> &points,
                const Damped_Rational &damped_rational)
{
  std::vector<Boost_Float> result;
  result.reserve(points.size());
  for(auto &point : points)
    {
      Boost_Float numerator(damped_rational.constant
                            * pow(damped_rational.base, point));
      Boost_Float denominator(1);
      for(auto &pole : damped_rational.poles)
        {
          denominator *= (point - pole);
        }
      result.emplace_back(to_string(numerator / denominator));
    }
  return result;
}


/**
 * @author Alessandro Bianco
 */

/**
 * @addtogroup DFNs
 * @{
 */

#ifndef BUNDLEADJUSTMENT_SVDDECOMPOSITION_HPP
#define BUNDLEADJUSTMENT_SVDDECOMPOSITION_HPP

#include "BundleAdjustmentInterface.hpp"
#include <Types/CPP/FramesSequence.hpp>
#include <Types/CPP/PosesSequence.hpp>
#include <Helpers/ParametersListHelper.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <yaml-cpp/yaml.h>
#include <Eigen/Dense>
#include <Converters/CorrespondenceMaps2DSequenceToMatConverter.hpp>

namespace CDFF
{
namespace DFN
{
namespace BundleAdjustment
{
	/**
	 * Implementation of the factorization algorithm by Tomasi and Kanade
	 *
	 * @param leftCameraMatrix: the camera matrix of the left camera
	 * @param rightCameraMatrix: the camera maxtrix of the right camera
	 */
	class SvdDecomposition : public BundleAdjustmentInterface
	{
		public:

			SvdDecomposition();
			virtual ~SvdDecomposition();

			virtual void configure() override;
			virtual void process() override;

		private:
			typedef Eigen::Transform<float, 3, Eigen::Affine, Eigen::DontAlign> AffineTransform;

			//DFN Parameters
			struct CameraMatrix
			{
				float focalLengthX;
				float focalLengthY;
				float principalPointX;
				float principalPointY;
			};

			struct SvdDecompositionOptionsSet
			{
				CameraMatrix leftCameraMatrix;
				CameraMatrix rightCameraMatrix;
				float baseline;
			};

			Helpers::ParametersListHelper parametersHelper;
			SvdDecompositionOptionsSet parameters;
			static const SvdDecompositionOptionsSet DEFAULT_PARAMETERS;

			//External conversion helpers
			Converters::CorrespondenceMaps2DSequenceToMatConverter correspondencesSequenceConverter;

			//Configuration Parameters conversion
			cv::Mat leftCameraMatrix, rightCameraMatrix;
			cv::Mat leftCameraMatrixInverse, rightCameraMatrixInverse;
			cv::Mat leftAbsoluteConicImage,	rightAbsoluteConicImage;
			cv::Mat CameraMatrixToCvMatrix(const CameraMatrix& cameraMatrix);

			//Internal Type Conversion Methods
			void ConvertRotationTranslationMatricesToPosesSequence(cv::Mat translationMatrix, cv::Mat rotationMatrix, PoseWrapper::Poses3DSequence& posesSequence);

			//Core Computation Methods
			void DecomposeMeasurementMatrix(cv::Mat measurementMatrix, cv::Mat& compatibleRotationMatrix, cv::Mat& compatiblePositionMatrix);
			cv::Mat ComputeTranslationMatrix(cv::Mat centroidMatrix);
			cv::Mat ComputeMeasuresCentroid(cv::Mat measurementMatrix);
			void CentreMeasurementMatrix(cv::Mat centroidMatrix, cv::Mat& measurementMatrix);
			cv::Mat ComputeMetricRotationMatrix(cv::Mat rotationMatrix, int poseIndex);

			//Validation Methods
			void ValidateParameters();
			void ValidateInputs();
	};
}
}
}

#endif // BUNDLEADJUSTMENT_SVDDECOMPOSITION_HPP

/** @} */


#include "sfge/graphics/light.hpp"
#include "sfge/math/convex_hull.hpp"
#include "sfge/math/intersections.hpp"
#include "sfge/math/point.hpp"
#include "sfge/math/vector_utilities.hpp"

#include <vector>
#include <iterator>

#include <boost/array.hpp>

#include <SFML/Graphics/ConvexShape.hpp>

#include <sfge/math/circle.hpp>

using namespace std;
using namespace boost;
using namespace sf;

namespace sfge
{

typedef vector<Edge2f> EdgeList;

typedef Point<float, float> Pointf;
typedef vector<Pointf> PointfList;

typedef Point<float, int> PointInt;
typedef vector<PointInt> PointIntList;

typedef ConvexHull<PointInt> ConvexHullT;

Light::Light()
	: mShadowOutline(0, 0, 0, 0), mShadowFill(0, 0, 0, 255)
{
}

Light::Light(const sf::Vector2f &pos, float rad)
	: mLightDesc(pos, rad), mShadowOutline(0, 0, 0, 0), mShadowFill(0, 0, 0, 255)
{
}

void Light::reset()
{
	swap(mShadows, Shadows());
	mIsInside = false;
}

void Light::addOccluders(const Shapes &occluders)
{
	Shapes _occluders(occluders);
	sort(_occluders.begin(), _occluders.end(),
		[&] (const ShapePtr &o1, const ShapePtr &o2) -> bool
		{
			const float	o1d = sqrDistance(o1->getPosition(), mLightDesc.mPos),
						o2d = sqrDistance(o2->getPosition(), mLightDesc.mPos);
			return o1d < o2d;
		} );

	Shapes::const_iterator it = _occluders.begin();
	while (it != _occluders.end() && addOccluder(*(*it)))
		++it;
}

bool Light::addOccluder(const Shape &occluder)
{
	// Setup
	if (mIsInside)
		return false;

	const unsigned int ptsCount = occluder.getPointCount();
	if (ptsCount < 2)
		return true;

	const sf::Vector2f	occLocalLPos	= occluder.getInverseTransform().transformPoint(mLightDesc.mPos);
	const float		scaledRadius		= mLightDesc.mRadius;
	const float		sqrRadius			= scaledRadius * scaledRadius;

	EdgeList edges;
	edges.reserve(ptsCount);
	PointfList points;
	points.reserve(ptsCount);

	// Generate occluder's edges & store points
	for (unsigned int ptIdx = 0; ptIdx != ptsCount - 1; ptIdx++)
	{
		const Vector2f &pt1 = occluder.getPoint(ptIdx);
		const Vector2f &pt2 = occluder.getPoint(ptIdx + 1);
		edges.push_back(Edge2f(pt1, pt2));

		sf::Vector2f diff(occLocalLPos - pt1);
		points.push_back(Pointf(pt1, dot(diff, diff)));
	}
	const Vector2f &lastPoint = occluder.getPoint(ptsCount - 1);
	edges.push_back(Edge2f(lastPoint, points[0].mPos, true));

	sf::Vector2f diff(occLocalLPos - lastPoint);
	points.push_back(Pointf(lastPoint, dot(diff, diff)));

	// Check that occluder is at least partially within light radius
	int pointsWithinLightRad = 0;
	for (PointfList::const_iterator it = points.begin(); it != points.end(); ++it)
	{
		pointsWithinLightRad += static_cast<int>(it->mUserVal < sqrRadius);

		if (pointsWithinLightRad > 1)
			break;
	}

	if (pointsWithinLightRad < 2)
		return false;

	// Special case: simple line, no need to think long
	if (edges.size() == 1)
	{
		Edge2f globalEdge(occluder.getTransform().transformPoint(edges[0].v1), occluder.getTransform().transformPoint(edges[0].v2));
		generateShadowFromLine(edges[0]);
		return true;
	}

	// Check if the light lies inside the occluder
	typedef vector<Edge2f::EdgeSide> EdgeSideList;
	EdgeSideList facing;
	facing.reserve(edges.size());
	transform(edges.begin(), edges.end(), back_inserter(facing),
		[&] (const Edge2f &e) -> Edge2f::EdgeSide { return e.checkSide(occLocalLPos); } );

	// And remove all edges which are facing the light
	EdgeSideList::const_iterator facingIt = facing.begin();
	const Edge2f::EdgeSide validSide = Edge2f::SideLeft;
	const size_t originalEdgesCount = edges.size();
	edges.erase(remove_if(edges.begin(), edges.end(),
							[&] (const Edge2f &) -> bool
							{
								return (*facingIt++) != validSide;
							} ),
				edges.end());

	mIsInside |= edges.size() == originalEdgesCount;
	if (mIsInside)
		return false;

	// Generate occluding edge
	static const int NullPointTag	= 0;
	static const int HullPointTag	= 1;
	static const int LightPointTag	= 2;

	ConvexHullT hull;

	PointIntList occludingPoints;
	for_each(edges.begin(), edges.end(),
		[&] (Edge2f &e)
		{
			if (e.mClosePolygon)
				swap(e.v1, e.v2);

			hull.addPoint(PointInt(e.v1, HullPointTag));
			hull.addPoint(PointInt(e.v2, HullPointTag));
		} );

	// Add the light point to the points, in order to get the points that'll generate the occluding edge
	hull.addPoint(PointInt(occLocalLPos, LightPointTag));
	hull.build(PointInt(Vector2f(), NullPointTag));
	hull.clean( [] (const PointInt &p) { return p.mUserVal == NullPointTag; } );
	const ConvexHullT::HullPoints &hullPoints = hull.get();

	// And now that we have the convex hull, simply extract the points enclosing the light point.
	PointIntList::const_iterator lightPosIt = find_if(hullPoints.begin(), hullPoints.end(),
													[] (const PointInt &p) { return p.mUserVal == LightPointTag; } );
	mIsInside = lightPosIt == hullPoints.end();
	if (mIsInside)
		return false;

	PointIntList::const_iterator firstIt	= hullPoints.begin();
	PointIntList::const_iterator lastIt		= hullPoints.end() - 1;

	const Vector2f &v1 = lightPosIt == firstIt	? (*lastIt).mPos	: (*(lightPosIt - 1)).mPos;
	const Vector2f &v2 = lightPosIt == lastIt	? (*firstIt).mPos	: (*(lightPosIt + 1)).mPos;
	const Edge2f occludingEdge(v1, v2);

	Edge2f globalEdge(occluder.getTransform().transformPoint(occludingEdge.v1), occluder.getTransform().transformPoint(occludingEdge.v2));
	generateShadowFromLine(globalEdge);

	return true;
}

void Light::DrawShadows(RenderTarget &target) const
{
	if (mIsInside)
		return;

	for_each(mShadows.begin(), mShadows.end(), [&] (const ShapePtr &shadow) { target.draw(*shadow); } );
}

void Light::DebugDraw(RenderTarget &target) const
{
	if (mIsInside)
		return;

    const sfge::Circle<float> influence(mLightDesc.mPos - sf::Vector2f(mLightDesc.mRadius, mLightDesc.mRadius), mLightDesc.mRadius);
    const sfge::Circle<float> point(mLightDesc.mPos - sf::Vector2f(3, 3), 3);
	target.draw(*shapeFromCircle(influence, Color(150, 150, 30, 64)));
    target.draw(*shapeFromCircle(point, Color::Yellow));
}

void Light::generateShadowFromLine(Edge2f &e)
{
	const Vector2f	scaledPos		= mLightDesc.mPos;
	const float		scaledRadius	= mLightDesc.mRadius;
	const float		sqrScaledRadius	= scaledRadius * scaledRadius;

	// Clamp edge's endpoints to light radius
	Vector2f	lightToV1		= e.v1 - scaledPos,
				lightToV2		= e.v2 - scaledPos;
	const float	lightToV1Dist	= sqrLength(lightToV1),
				lightToV2Dist	= sqrLength(lightToV2);

	if (lightToV1Dist > sqrScaledRadius || lightToV2Dist > sqrScaledRadius)
	{
		EdgeCircleIntersectionCont<float> intersectInfo;
		IntersectionResult res = intersect(e, mLightDesc, intersectInfo);
		if (res != IR_TwoIntersections)
			return;

		if (lightToV1Dist > sqrScaledRadius)
			e.v1 = intersectInfo.mStartPoint + intersectInfo.mTs[0] * intersectInfo.mDir;

		if (lightToV2Dist > sqrScaledRadius)
			e.v2 = intersectInfo.mStartPoint + intersectInfo.mTs[1] * intersectInfo.mDir;
	}

	// Generate 6 additionnal points to be the shadow's endpoints
	// FIXME adaptive tessellation w/r <angle,dist> needed!
	const int endPointsCount = 6;
	const float invSplit = 1.f / (endPointsCount - 1);
	array<Vector2f, endPointsCount> endPoints;
	const Vector2f delta(e.v2 - e.v1);

	endPoints[0]					= e.v1;
	endPoints[endPointsCount - 1]	= e.v2;

	for (size_t i = 1; i != endPoints.size() - 1; i++)
	{
		const float splitFactor	= invSplit * i;
		endPoints[i]			= delta * splitFactor  + e.v1;
	}

	// Compute extrusion distance (from midpoint to be sure to stay within light's radius
	for (size_t i = 0; i != endPoints.size(); i++)
	{
		Vector2f diff			= endPoints[i] - scaledPos;
		const float diffLength	= length(diff);
		const float extrudeLen	= scaledRadius - diffLength + 1.f;

		diff /= diffLength;
		endPoints[i] += diff * extrudeLen;
	}

    // Generate shadow SFML shape
	std::shared_ptr<sf::ConvexShape> shadow(new sf::ConvexShape(2 + endPoints.size()));
    shadow->setFillColor(mShadowFill);
    shadow->setOutlineColor(mShadowOutline);
	shadow->setOutlineThickness(1.f);

    unsigned int ptIndex = 0;
	shadow->setPoint(ptIndex++, e.v1);
	for_each(endPoints.begin(), endPoints.end(),
		[&] (const Vector2f &v)
		{
			sf::Color endShadowFill(mShadowFill);
			endShadowFill.a = static_cast<Uint8>(endShadowFill.a * 0.25f);
			shadow->setPoint(ptIndex++, v);
		} );
	shadow->setPoint(ptIndex++, e.v2);

	mShadows.push_back(shadow);
}

}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2019 Tinko Bartels, Berlin, Germany.

// Use, modification and distribution is subject to 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_GEOMETRY_EXTENSIONS_RANDOM_STRATEGIES_AGNOSTIC_UNIFORM_CONVEX_FAN_HPP
#define BOOST_GEOMETRY_EXTENSIONS_RANDOM_STRATEGIES_AGNOSTIC_UNIFORM_CONVEX_FAN_HPP

#include <random>
#include <vector>
#include <cstdlib>
#include <cmath>

#include <boost/geometry/algorithms/equals.hpp>
#include <boost/geometry/algorithms/transform.hpp>
#include <boost/geometry/algorithms/within.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace uniform_point_distribution {

// The following strategy is suitable for convex rings and polygons with
// non-empty interior.
template
<
    typename Point,
    typename DomainGeometry,
    typename TriangleStrategy,
    typename SideStrategy   //Actually, we need a triangle area strategy here.
>
struct uniform_convex_fan
{
private:
    std::vector<double> accumulated_areas;
    // It is hard to see a reason not to use double here. If a triangles
    // relative size is smaller than doubles epsilon, it is too unlikely to
    // realistically occur in a random sample anyway.
public:
    uniform_convex_fan(DomainGeometry const& g)
	{
        accumulated_areas.push_back(0);
        for (int i = 2 ; i < g.size() ; ++i) {
            accumulated_areas.push_back(
                accumulated_areas.back() +
                std::abs(SideStrategy::template side_value<double, double>(
                         *g.begin(),
                         *(g.begin() + i - 1),
                         *(g.begin() + i))));
        }
    }
    bool equals(DomainGeometry const& l_domain,
                DomainGeometry const& r_domain,
                uniform_convex_fan const& r_strategy) const
    {
        if( l_domain.size() != r_domain.size() ) return false;
        for (int i = 0; i < l_domain.size(); ++i) {
            if( !boost::geometry::equals(*(l_domain.begin() + i),
                                         *(r_domain.begin() + i)))
                return false;
        }
        return true;
    }
    template<typename Gen>
    Point apply(Gen& g, DomainGeometry const& d)
    {
        std::uniform_real_distribution<double> dist(0, 1);
        double r = dist(g) * accumulated_areas.back(),
               s = dist(g);
        std::size_t i = std::distance(
            accumulated_areas.begin(),
            std::lower_bound(accumulated_areas.begin(),
                             accumulated_areas.end(),
                             r));
        return TriangleStrategy::template map
            <
                double
            >(* d.begin(),
              *(d.begin() + i),
              *(d.begin() + i + 1),
              ( r - accumulated_areas[ i - 1 ]) /
                ( accumulated_areas[ i ] - accumulated_areas[ i - 1 ] ),
              s);
    }
    void reset(DomainGeometry const&) {};
};

}} // namespace strategy::uniform_point_distribution

}} // namespace boost::geometry

#endif //  BOOST_GEOMETRY_EXTENSIONS_RANDOM_STRATEGIES_AGNOSTIC_UNIFORM_CONVEX_FAN_HPP


#include "setup_skinning_handles.h"
#include "kmeans_clustering.h"
#include <Eigen/Sparse>
#include <Eigen/Dense>
#include <iostream>
#include <igl/boundary_conditions.h>
#include <igl/lbs_matrix.h>
#include <igl/bbw.h>
#include <igl/harmonic.h>
#include <igl/biharmonic_coordinates.h>
#include <unsupported/Eigen/KroneckerProduct>
#include <igl/slice_into.h>
#include <igl/remove_unreferenced.h>
#include <igl/normalize_row_sums.h>
#include <igl/writeOBJ.h>
#include <igl/list_to_matrix.h>

using namespace Eigen;
using namespace std;
MatrixXd bbw_strain_skinning_matrix(VectorXi& handles, const MatrixXd& mV, MatrixXi& mT){
    std::set<int> unique_vertex_handles;
    std::set<int>::iterator it;
    //from the tet handle indexes, get the unique verts that can act as handles
    for(int i=0; i<handles.size(); i++){
        unique_vertex_handles.insert(mT(handles[i], 0));
        unique_vertex_handles.insert(mT(handles[i], 1));
        unique_vertex_handles.insert(mT(handles[i], 2));
        unique_vertex_handles.insert(mT(handles[i], 3));
    }

    int i=0;
    it = unique_vertex_handles.end();
    VectorXi map_verts_to_unique_verts = VectorXi::Zero(*(--it)+1).array() -1;
    for (it=unique_vertex_handles.begin(); it!=unique_vertex_handles.end(); ++it){
        map_verts_to_unique_verts[*it] = i;
        i++;
    }

    MatrixXi vert_to_tet = MatrixXi::Zero(handles.size(), 4);
    i=0;
    for(i=0; i<handles.size(); i++){
        vert_to_tet.row(i)[0] = map_verts_to_unique_verts[mT.row(handles[i])[0]];
        vert_to_tet.row(i)[1] = map_verts_to_unique_verts[mT.row(handles[i])[1]];
        vert_to_tet.row(i)[2] = map_verts_to_unique_verts[mT.row(handles[i])[2]];
        vert_to_tet.row(i)[3] = map_verts_to_unique_verts[mT.row(handles[i])[3]];
    }

    MatrixXd C = MatrixXd::Zero(unique_vertex_handles.size(), 3);
    VectorXi P = VectorXi::Zero(unique_vertex_handles.size());
    VectorXi HandleIndexes = VectorXi::Zero(unique_vertex_handles.size());
    i=0;
    for (it=unique_vertex_handles.begin(); it!=unique_vertex_handles.end(); ++it){
        HandleIndexes[i] =  *it;
        C.row(i) = mV.row(*it);
        P(i) = i;
        i++;
    }

    // List of boundary indices (aka fixed value indices into VV)
    VectorXi b;
    // List of boundary conditions of each weight function
    MatrixXd bc;
    cout<<"---------0--------"<<endl;
    igl::boundary_conditions(mV, mT, C, P, MatrixXi(), MatrixXi(), b, bc);
    // compute BBW weights matrix
    igl::BBWData bbw_data;
    // only a few iterations for sake of demo
    bbw_data.active_set_params.max_iter = 100;
    bbw_data.verbosity = 2;

    MatrixXd W, M;
    cout<<"---------1--------"<<endl;
    if(handles.size()==1){
        return MatrixXd::Ones(mT.rows(), handles.size());
    }

    // if(!igl::bbw(mV, mT, b, bc, bbw_data, W))
    // {
    //     std::cout<<"EXIT: Error here"<<std::endl;
    //     exit(0);
    //     return MatrixXd();
    // }
    // if(!igl::harmonic(mV, mT, b, bc, 1, W))
    // {
    //     std::cout<<"EXIT: Error here"<<std::endl;
    //     exit(0);
    //     return MatrixXd();
    // }

    std::vector<std::vector<int> > S;
    igl::matrix_to_list(HandleIndexes,S);

    cout<<"Computing weights for "<<HandleIndexes.size()<<" handles at "<<mV.rows()<<" vertices..."<<endl;
    // Technically k should equal 3 for smooth interpolation in 3d, but 2 is
    // faster and looks OK
    const int k = 3;
    igl::biharmonic_coordinates(mV,mT,S,k,W);

    cout<<"---------2--------"<<endl;

    // Normalize weights to sum to one
    igl::normalize_row_sums(W,W);
    cout<<"---------3--------"<<endl;

    // precompute linear blend skinning matrix
    igl::lbs_matrix(mV,W,M);
    cout<<"---------4--------"<<endl;

    MatrixXd tW = MatrixXd::Zero(mT.rows(), handles.size());
    for(int t =0; t<mT.rows(); t++){
        VectorXi e = mT.row(t);
        for(int h=0; h<handles.size(); h++){
            if(t==handles[h]){
                tW.row(t) *= 0;
                tW(t,h) = 1;
                break;
            }
            double p0 = 0;
            double p1 = 0;
            double p2 = 0;
            double p3 = 0;
            for(int j=0; j<vert_to_tet.cols(); ++j){
                p0 += W(e[0], vert_to_tet(h, j));
                p1 += W(e[1], vert_to_tet(h, j));
                p2 += W(e[2], vert_to_tet(h, j));
                p3 += W(e[3], vert_to_tet(h, j));
            }
            tW(t, h) = (p0+p1+p2+p3)/4;
        }
    }
    cout<<"---------5--------"<<endl;
    igl::normalize_row_sums(tW, tW);

    cout<<"---------6--------"<<endl;
    return tW;
}

Vector3d tet_center(MatrixXi& T, MatrixXd& V, int ind){
    Vector3d v1 = V.row(T.row(ind)[0]);
    Vector3d v2 = V.row(T.row(ind)[1]);
    Vector3d v3 = V.row(T.row(ind)[2]);
    Vector3d v4 = V.row(T.row(ind)[3]);

    return (v1 + v2 + v3 + v4)/4;
}

MatrixXd setup_skinning_helper(int indx,
    int nsh_on_component,
    MatrixXi& mT,
    MatrixXd& mV,
    SparseMatrix<double>& mC,
    SparseMatrix<double>& mA,
    VectorXd& mx0,
    std::map<int, std::vector<int>>& ms_handle_elem_map){

    //---------BBW Skinning Handles ---------
    VectorXi handles_ind = VectorXi::Zero(nsh_on_component);
    VectorXd CAx0 = mC*mA*mx0;

    for(int k=0; k<nsh_on_component; k++){
        std::vector<int> els = ms_handle_elem_map[indx-k];
        double centx = 0;//= VectorXd::Zero(els.size());
        double centy = 0;//= VectorXd::Zero(els.size());
        double centz = 0;//= VectorXd::Zero(els.size());
        Vector3d avg_cent;
        for(int i=0; i<els.size(); i++){
            centx += CAx0[12*els[i]];
            centy += CAx0[12*els[i]+1];
            centz += CAx0[12*els[i]+2];
        }

        avg_cent<<centx/els.size(), centy/els.size(), centz/els.size();
        int minind = 0;
        double mindist = (avg_cent - tet_center(mT, mV, 0)).norm();
        for(int i=1; i<mT.rows(); i++){
            double dist = (avg_cent - tet_center(mT, mV, i)).norm();
            if(dist<mindist){
                mindist = dist;
                minind = i;
            }
        }

        handles_ind[k] = minind;
    }
    return bbw_strain_skinning_matrix(handles_ind, mV, mT);

}

void setup_skinning_handles(int nsh, bool reduced, const MatrixXi& mT, const MatrixXd& mV, std::vector<VectorXi>& ibones, std::vector<VectorXi>& imuscle,
    SparseMatrix<double>& mC, SparseMatrix<double>& mA, MatrixXd& mG, VectorXd& mx0, VectorXd& mred_s, MatrixXd& msW, std::map<int, std::vector<int>>& ms_handle_elem_map, VectorXd& relStiff){

    std::cout<<"+ Skinning Handles"<<std::endl;
    int handles_per_tendon=2;
    VectorXi skinning_elem_cluster_map;
    std::map<int, std::vector<int>> skinning_cluster_elem_map;

    std::cout<<nsh<<std::endl;
    if(nsh==0){
        nsh = mT.rows();
    }

    if(nsh<(ibones.size()+imuscle.size())){
        std::cout<<"Too few skinning handles, too many components"<<std::endl;
        exit(0);
    }

    //-----------------------------------------------------
    if(nsh==mT.rows() && reduced==false){
        //unreduced
        std::cout<<"here"<<std::endl;
        skinning_elem_cluster_map.resize(mT.rows());
        skinning_elem_cluster_map.setZero();
        for(int i=0; i<mT.rows(); i++){
            skinning_elem_cluster_map[i] = i;
        }
    }else{
        kmeans_clustering(skinning_elem_cluster_map, nsh, handles_per_tendon, ibones, imuscle, mG, mC, mA, mx0, relStiff);
    }

    for(int i=0; i<mT.rows(); i++){
        ms_handle_elem_map[skinning_elem_cluster_map[i]].push_back(i);
    }
    //------------------------------------------------------

    if(reduced==false){
        mred_s.resize(6*mT.rows());
        for(int i=0; i<mT.rows(); i++){
            mred_s[6*i+0] = 1;
            mred_s[6*i+1] = 1;
            mred_s[6*i+2] = 1;
            mred_s[6*i+3] = 0;
            mred_s[6*i+4] = 0;
            mred_s[6*i+5] = 0;
        }
        std::cout<<"- Skinning Handles"<<std::endl;
        return;

    }
    if(nsh==mT.rows()){
        std::cout<<"Too many skinning handles. "<<std::endl;
        exit(0);
    }

    mred_s.resize(6*nsh);
    for(int i=0; i<nsh; i++){
        mred_s[6*i+0] = 1;
        mred_s[6*i+1] = 1;
        mred_s[6*i+2] = 1;
        mred_s[6*i+3] = 0;
        mred_s[6*i+4] = 0;
        mred_s[6*i+5] = 0;
    }

    MatrixXd Id6 = MatrixXd::Identity(6, 6);
    MatrixXd sW = MatrixXd::Zero(mT.rows(), nsh);
    sW.setZero();
    std::cout<<"nsh: "<<nsh<<std::endl;
    std::cout<<ms_handle_elem_map.size()<<std::endl;
    // //Constant weights of 1 for each handle region
    // for(int h=0; h<ms_handle_elem_map.size(); h++){
    //   std::vector<int> mshem =  ms_handle_elem_map[ms_handle_elem_map.size() - h - 1];
    //   MatrixXd ones_vec = VectorXd::Ones(mshem.size());
    //   MatrixXd sWslice = MatrixXd::Zero(mT.rows(), 1);
    //   VectorXi handles_elem_vec;
    //   igl::list_to_matrix(mshem, handles_elem_vec);
    //   igl::slice_into(ones_vec , handles_elem_vec, 1, sWslice);
    //   sW.block(0, h, mT.rows(), 1) = sWslice;
    // }
    // msW =  Eigen::kroneckerProduct(sW, Id6);

    // return;

    //blocked construction of full skinning weights matrix
    int maxnsh = nsh;
    cout<<"----------Bone HANDLES------------"<<maxnsh<<"--"<<ms_handle_elem_map.size()<<endl;
    int insert_index = 0;
    for(int b=0; b<ibones.size(); b++){
        MatrixXi subT(ms_handle_elem_map[maxnsh - 1 - insert_index].size(), 4);
        MatrixXi componentT;
        MatrixXd componentV;
        VectorXi J;

        for(int i=0; i<ms_handle_elem_map[maxnsh -1 - insert_index].size() ; i++){
            subT.row(i) = mT.row(ms_handle_elem_map[maxnsh -1 - insert_index][i]);
        }

        igl::remove_unreferenced(mV, subT, componentV, componentT, J);

        std::string nam = "bone"+to_string(b)+".obj";

        igl::writeOBJ(nam, componentV, componentT);

        MatrixXd sWi = setup_skinning_helper(maxnsh-1- insert_index , 1, componentT, componentV, mC, mA, mx0, ms_handle_elem_map);

        MatrixXd sWslice = MatrixXd::Zero(mT.rows(), sWi.cols());
        igl::slice_into(sWi , ibones[b], 1, sWslice);
        sW.block(0,insert_index, mT.rows(), sWi.cols()) = sWslice;
        nsh = nsh - 1; //bone skinning handle has been made, so decrease nsh by 1
        insert_index += 1;
    }

    cout<<"----------MUSCLE HANDLES----"<<nsh<<"---"<<insert_index<<"-----"<<endl;
    int number_handles_per_muscle = nsh/imuscle.size();
    for(int m=0; m<imuscle.size(); m++){ //through muscle vector
        // std::vector<int> tendon_elements_list;
        // std::vector<int> muscle_elements_list;
        // for(int i=0; i<imuscle[m].size(); i++){
        //     if(relStiff[imuscle[m][i]]>10){
        //         tendon_elements_list.push_back(imuscle[m][i]);
        //     }else{
        //         muscle_elements_list.push_back(imuscle[m][i]);
        //     }
        // }

        VectorXi muscle_els, tendon_els;
        muscle_els = imuscle[m];
        // igl::list_to_matrix(tendon_elements_list, tendon_els);
        // igl::list_to_matrix(muscle_elements_list, muscle_els);

        // //Tendon skinning handles---------------------------
        // if(tendon_els.size()>0){

        //     MatrixXi TcomponentT;
        //     MatrixXd TcomponentV;
        //     MatrixXi TsubT(tendon_els.size(), 4);
        //     VectorXi TJ;
        //     for(int i=0; i<tendon_els.size() ; i++){
        //         TsubT.row(i) = mT.row(tendon_els[i]);
        //     }
        //     igl::remove_unreferenced(mV, TsubT, TcomponentV, TcomponentT, TJ);
        //     std::string Tnam = "tendon"+to_string(m)+".obj";
        //     igl::writeOBJ(Tnam, TcomponentV, TcomponentT);
        //     MatrixXd TsWi;
        //     if(m==imuscle.size() -1){
        //         TsWi = setup_skinning_helper(maxnsh -1 - insert_index - nsh + handles_per_tendon, handles_per_tendon, TcomponentT, TcomponentV, mC, mA, mx0, ms_handle_elem_map);
        //     }else{
        //         TsWi = setup_skinning_helper(maxnsh -1 - insert_index - number_handles_per_muscle + handles_per_tendon, handles_per_tendon, TcomponentT, TcomponentV, mC, mA, mx0, ms_handle_elem_map);
        //     }
        //     MatrixXd TsWslice = MatrixXd::Zero(mT.rows(), TsWi.cols());

        //     igl::slice_into(TsWi , tendon_els, 1, TsWslice);
        //     sW.block(0,insert_index, mT.rows(), TsWi.cols()) = TsWslice;
        // }
        handles_per_tendon=0;

        //Muscle skinning handles---------------------------
        MatrixXi componentT;
        MatrixXd componentV;
        MatrixXi subT(muscle_els.size(), 4);
        VectorXi J;
        for(int i=0; i<muscle_els.size() ; i++){
            subT.row(i) = mT.row(muscle_els[i]);
        }
        igl::remove_unreferenced(mV, subT, componentV, componentT, J);
        std::string nam = "muscle"+to_string(m)+".obj";
        igl::writeOBJ(nam, componentV, componentT);
        MatrixXd sWi;
        if(m==imuscle.size()-1){
            //Deal with remainder handles
            sWi = setup_skinning_helper(maxnsh - 1 - insert_index, nsh - handles_per_tendon, componentT, componentV, mC, mA, mx0, ms_handle_elem_map);
        }else{
            sWi = setup_skinning_helper(maxnsh - 1 - insert_index, number_handles_per_muscle - handles_per_tendon, componentT, componentV, mC, mA, mx0, ms_handle_elem_map );
        }

        MatrixXd sWslice = MatrixXd::Zero(mT.rows(), sWi.cols());

        igl::slice_into(sWi , muscle_els, 1, sWslice);
        sW.block(0,insert_index+handles_per_tendon, mT.rows(), sWi.cols()) = sWslice;
        insert_index += number_handles_per_muscle;
        nsh =  nsh - number_handles_per_muscle;
    }
    assert(nsh==0);

    // MatrixXd Id6 = MatrixXd::Identity(6, 6);
    msW =  Eigen::kroneckerProduct(sW, Id6);

    std::cout<<"- Skinning Handles"<<std::endl;
}


/**********************************************************************
*  Copyright (c) 2008-2015, Alliance for Sustainable Energy.
*  All rights reserved.
*
*  This library is free software; you can redistribute it and/or
*  modify it under the terms of the GNU Lesser General Public
*  License as published by the Free Software Foundation; either
*  version 2.1 of the License, or (at your option) any later version.
*
*  This library is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
*  Lesser General Public License for more details.
*
*  You should have received a copy of the GNU Lesser General Public
*  License along with this library; if not, write to the Free Software
*  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**********************************************************************/

#include "Transformation.hpp"
#include "Point3d.hpp"
#include "Vector3d.hpp"
#include "Plane.hpp"
#include "BoundingBox.hpp"
#include "EulerAngles.hpp"
#include "Geometry.hpp"
#include "../core/Assert.hpp"

#include <boost/math/constants/constants.hpp>

#include <algorithm>

#include <math.h>

using boost::numeric::ublas::identity_matrix;
using std::min;

namespace openstudio{

  /// default constructor creates identity transformation
  Transformation::Transformation()
    : m_storage(identity_matrix<double>(4))
  {}

  /// copy constructor
  Transformation::Transformation(const Transformation& other)
    : m_storage(other.m_storage)
  {}

  /// constructor from storage, asserts matrix is 4x4
  Transformation::Transformation(const Matrix& matrix)
    : m_storage(matrix)
  {
    OS_ASSERT(matrix.size1() == 4);
    OS_ASSERT(matrix.size2() == 4);
  }

  /// constructor from storage, asserts vector is size 16
  Transformation::Transformation(const Vector& vector)
    : m_storage(Matrix(4,4))
  {
    OS_ASSERT(vector.size() == 16);

    m_storage(0,0) = vector[0];
    m_storage(1,0) = vector[1];
    m_storage(2,0) = vector[2];
    m_storage(3,0) = vector[3];
    m_storage(0,1) = vector[4];
    m_storage(1,1) = vector[5];
    m_storage(2,1) = vector[6];
    m_storage(2,1) = vector[7];
    m_storage(0,2) = vector[8];
    m_storage(1,2) = vector[9];
    m_storage(2,2) = vector[10];
    m_storage(3,2) = vector[11];
    m_storage(0,3) = vector[12];
    m_storage(1,3) = vector[13];
    m_storage(2,3) = vector[14];
    m_storage(3,3) = vector[15];
  }

  /// rotation about origin defined by axis and angle (radians)
  Transformation Transformation::rotation(const Vector3d& axis, double radians)
  {
    Matrix storage = identity_matrix<double>(4);

    Vector3d temp = axis;
    if (!temp.normalize()){
      LOG(Error, "Could not normalize axis");
    }
    Vector normalVector = temp.vector();

    // Rodrigues' rotation formula / Rotation matrix from Euler axis/angle
    // I*cos(radians) + I*(1-cos(radians))*axis*axis^T + Q*sin(radians)
    // Q = [0, -axis[2], axis[1]; axis[2], 0, -axis[0]; -axis[1], axis[0], 0]
    Matrix P = outer_prod(normalVector, normalVector);
    Matrix I = identity_matrix<double>(3);
    Matrix Q(3, 3, 0.0);
    Q(0,1) = -normalVector(2);
    Q(0,2) =  normalVector(1);
    Q(1,0) =  normalVector(2);
    Q(1,2) = -normalVector(0);
    Q(2,0) = -normalVector(1);
    Q(2,1) =  normalVector(0);

    // rotation matrix
    Matrix R = I*cos(radians) + (1-cos(radians))*P + Q*sin(radians);

    for (unsigned i = 0; i < 3; ++i){
      for (unsigned j = 0; j < 3; ++j){
        storage(i,j) = R(i,j);
      }
    }

    return Transformation(storage);
  }

  /// rotation about point defined by axis and angle (radians)
  Transformation Transformation::rotation(const Point3d& origin, const Vector3d& axis, double radians)
  {
    Vector3d temp(origin.x(), origin.y(), origin.z());

    // translate point to origin, rotate, and then translate back
    return translation(temp)*rotation(axis, radians)*translation(-temp);
  }

  /// rotation specified by Euler angles
  Transformation Transformation::rotation(const EulerAngles& angles)
  {
    Transformation result = Transformation::rotation(Vector3d(0,0,1), angles.phi()) *
                            Transformation::rotation(Vector3d(0,1,0), angles.theta()) *
                            Transformation::rotation(Vector3d(1,0,0), angles.psi());
    return result;
  }

  /// translation along vector
  Transformation Transformation::translation(const Vector3d& translation)
  {
    Matrix storage = identity_matrix<double>(4);

    storage(0,3) = translation.x();
    storage(1,3) = translation.y();
    storage(2,3) = translation.z();

    return Transformation(storage);
  }

  /// transforms system with z' to regular system
  /// will try to align y' with z, but if that fails will align y' with y
  Transformation Transformation::alignZPrime(const Vector3d& zPrime)
  {
    Vector3d xp;
    Vector3d yp;
    Vector3d zp = zPrime;
    if (!zp.normalize()){
      LOG(Error, "Could not normalize zPrime");
    }

    Vector3d xAxis(1,0,0);
    Vector3d yAxis(0,1,0);
    Vector3d zAxis(0,0,1);
    Vector3d negXAxis(-1,0,0);

    // check if face normal is up or down
    if (fabs(zp.dot(zAxis)) < 0.99){
      // not facing up or down, set yPrime along zAxis
      yp = zAxis - (zp.dot(zAxis)*zp);
      if (!yp.normalize()){
        LOG(Error, "Could not normalize axis");
      }
      xp = yp.cross(zp);
    }else{
      // facing up or down, set xPrime along -xAxis
      xp = negXAxis - (zp.dot(negXAxis)*zp);
      if (!xp.normalize()){
        LOG(Error, "Could not normalize axis");
      }
      yp = zp.cross(xp);
    }

    Matrix storage = identity_matrix<double>(4);
    storage(0,0) = xp.x();
    storage(1,0) = xp.y();
    storage(2,0) = xp.z();
    storage(0,1) = yp.x();
    storage(1,1) = yp.y();
    storage(2,1) = yp.z();
    storage(0,2) = zp.x();
    storage(1,2) = zp.y();
    storage(2,2) = zp.z();

    return Transformation(storage);
  }

  /// transforms face coordinates to regular system, face normal will be z'
  /// will try to align y' with z, but if that fails will align y' with y
  /// face origin will be minimum point in x', y' and z'=0
  /// will return identity transformation if cannot compute plane for vertices
  Transformation Transformation::alignFace(const std::vector<Point3d>& vertices)
  {
    OptionalVector3d zPrime = getOutwardNormal(vertices);
    if (!zPrime){
      LOG(Error, "Cannot compute outward normal for vertices");
      return Transformation();
    }

    // align z' with outward normal
    Transformation align = alignZPrime(*zPrime);
    Point3dVector alignedVertices = align.inverse()*vertices;

    // compute translation to minimum in aligned system
    double minX = alignedVertices[0].x();
    double minY = alignedVertices[0].y();
    double minZ = alignedVertices[0].z();

    for (const Point3d& vertex : alignedVertices){
      minX = min(minX, vertex.x());
      minY = min(minY, vertex.y());
      minZ = min(minZ, vertex.z());
    }
    Transformation translate = translation(Vector3d(minX, minY, minZ));

    return align*translate;
  }

  /// returns a transformation which is the inverse of this
  Transformation Transformation::inverse() const
  {
    Matrix matrix(4,4);
    bool test = invert(m_storage, matrix);
    if (!test){
      // this should never happen
      LOG_AND_THROW("Matrix inversion failed");
    }
    return Transformation(matrix);
  }

  /// get the matrix representation directly
  Matrix Transformation::matrix() const
  {
    return m_storage;
  }

  /// get the vector representation directly
  Vector Transformation::vector() const
  {
    openstudio::Vector result(16);
    result[0] = m_storage(0,0);
    result[1] = m_storage(1,0);
    result[2] = m_storage(2,0);
    result[3] = m_storage(3,0);
    result[4] = m_storage(0,1);
    result[5] = m_storage(1,1);
    result[6] = m_storage(2,1);
    result[7] = m_storage(3,1);
    result[8] = m_storage(0,2);
    result[9] = m_storage(1,2);
    result[10] = m_storage(2,2);
    result[11] = m_storage(3,2);
    result[12] = m_storage(0,3);
    result[13] = m_storage(1,3);
    result[14] = m_storage(2,3);
    result[15] = m_storage(3,3);
    return result;
  }

  /// get the Euler angles for the transformation, does not include translation
  EulerAngles Transformation::eulerAngles() const
  {
    double psi;
    double theta;
    double phi;
    if (m_storage(2,0) == 1.0){
      phi = 0;
      theta = -boost::math::constants::pi<double>()/2.0;
      psi = atan2(-m_storage(0,1), -m_storage(0,2));
    }else if(m_storage(2,0) == -1.0){
      phi = 0;
      theta = boost::math::constants::pi<double>()/2.0;
      psi = atan2(m_storage(0,1), m_storage(0,2));
    }else{
      theta = -asin(m_storage(2,0));
      // theta = pi + asin(m_storage(2,0)); // alternate solution
      psi = atan2(m_storage(2,1)/cos(theta), m_storage(2,2)/cos(theta));
      phi = atan2(m_storage(1,0)/cos(theta), m_storage(0,0)/cos(theta));

    }
    EulerAngles result(psi, theta, phi);
    return result;
  }

  /// get the rotation matrix for the transformation, does not include translation
  Matrix Transformation::rotationMatrix() const
  {
    Matrix result(3,3);
    for(unsigned i = 0 ; i < 3; ++i){
      for(unsigned j = 0; j < 3; ++j){
        result(i,j) = m_storage(i,j);
      }
    }
    return result;
  }

  /// get the translation for the transformation, does not include rotation
  Vector3d Transformation::translation() const
  {
    Vector3d result(m_storage(0, 3), m_storage(1, 3), m_storage(2, 3));
    return result;
  }

  /// apply the transformation to the point
  Point3d Transformation::operator*(const Point3d& point) const
  {
    Vector temp(4);
    temp(0) = point.x();
    temp(1) = point.y();
    temp(2) = point.z();
    temp(3) = 1.0;
    temp = prod(m_storage, temp);
    return Point3d(temp[0], temp[1], temp[2]);
  }

  /// apply the transformation to the vector
  Vector3d Transformation::operator*(const Vector3d& vector) const
  {
    Vector temp(4);
    temp(0) = vector.x();
    temp(1) = vector.y();
    temp(2) = vector.z();
    temp(3) = 1.0;
    temp = prod(m_storage, temp);
    return Vector3d(temp[0], temp[1], temp[2]);
  }

  /// apply the transformation to the BoundingBox
  BoundingBox Transformation::operator*(const BoundingBox& boundingBox) const
  {
    BoundingBox result;
    std::vector<Point3d> transformedPoints = (*this)*boundingBox.corners();
    result.addPoints(transformedPoints);
    return result;
  }

  /// apply the transformation to the plane
  Plane Transformation::operator*(const Plane& plane) const
  {
    // translate a point on the plane, just project (0,0,0)
    Point3d point = plane.project(Point3d(0,0,0));

    // get a point at outward normal
    Vector3d outwardNormal = plane.outwardNormal();
    Point3d refPoint = point + outwardNormal;

    // translate the two points and recompute the normal
    Point3d newPoint = (*this) * point;
    Point3d newRefPoint = (*this) * refPoint;
    Vector3d newNormal = newRefPoint - newPoint;

    return Plane(newPoint, newNormal);
  }

  /// apply the transformation to a vector of points
  std::vector<Point3d> Transformation::operator*(const std::vector<Point3d>& points) const
  {
    std::vector<Point3d> result(points.size());
    for(unsigned i = 0; i < points.size(); ++i){
      result[i] = (*this)*points[i];
    }
    return result;
  }

  /// apply the transformation to a vector of vector
  std::vector<Vector3d> Transformation::operator*(const std::vector<Vector3d>& vectors) const
  {
    std::vector<Vector3d> result(vectors.size());
    for(unsigned i = 0; i < vectors.size(); ++i){
      result[i] = (*this)*vectors[i];
    }
    return result;
  }

  /// apply the transformation to the other transformation
  Transformation Transformation::operator*(const Transformation& other) const
  {
    return Transformation(prod(m_storage, other.m_storage));
  }

  /// ostream operator
  std::ostream& operator<<(std::ostream& os, const Transformation& t)
  {
    os << t.matrix();
    return os;
  }

  Transformation createRotation(const Vector3d& axis, double radians)
  {
    return Transformation::rotation(axis, radians);
  }

  Transformation createRotation(const Point3d& origin, const Vector3d& axis, double radians)
  {
    return Transformation::rotation(origin, axis, radians);
  }

  Transformation createRotation(const EulerAngles& angles)
  {
    return Transformation::rotation(angles);
  }

  Transformation createTranslation(const Vector3d& translation)
  {
    return Transformation::translation(translation);
  }

} // openstudio