#include "ode45.hpp"

#include <iostream>

#include <Eigen/Dense>
#include <Eigen/Sparse>

// Comment to disable test
#define MAKE_TEST1
#define MAKE_TEST2
#define MAKE_TEST3
#define MAKE_TEST4
#define EIGEN_DONT_VECTORIZE
#define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT // I just added that to fix an error
void test1() {
#ifdef MAKE_TEST1
    std::cout << "Prey/Predator model test:" << std::endl;

    // Prey/Predator model
    const double alpha1 = 3;
    const double alpha2 = 2;
    const double beta1 = 0.1;
    const double beta2 = 0.1;
    auto f = [&alpha1, &alpha2, &beta1, &beta2] (const Eigen::VectorXd & y) {
        Eigen::VectorXd temp = y;
        temp(0) *= (alpha1 - beta1*y(1));
        temp(1) *= (beta2*y(0) - alpha2);
        return temp;
    };

//     Eigen::VectorXd y0(2);
    // or
    Eigen::Vector2d y0;
    y0 << 100, 5;

    const double T = 10;

    // Basic usage:
    ode45<Eigen::VectorXd> O(f);
//     O.options.do_statistics = true;
    auto sol = O.solve(y0, T);

    // Print info
//     O.print();

    // Print some info
    std::cout << "T = " << sol.back().second << std::endl;
    std::cout << "y(T) = " << std::endl << sol.back().first << std::endl;
#endif
}

void test2() {
#ifdef MAKE_TEST2
    std::cout << "Fundamental types test and validation:" << std::endl;

    // Test class with fundamental types
    auto f = [] (double y) { return 1 / y; };

    double y0 = 0.2;
    // Large step size
    const double T = 10000;

    // Quick syntax
    ode45<double> O(f);
//     O.options.do_statistics = true;
    auto sol = O.solve(y0, T);

    // Print info
//     O.print();

    // Print some info
    std::cout << "T = " << sol.back().second << std::endl;
    std::cout << "y(T) = " << std::endl << sol.back().first << std::endl;
    auto y_ex = [] (double t) { return std::sqrt(2*t+0.04); };
    std::cout << "y_ex(T) = " << std::endl << y_ex(T) << std::endl;
#endif
}

void test3() {
#ifdef MAKE_TEST3
    std::cout << "Multidimensional test:" << std::endl;

    // Construct data for the IVP
    double T = 1;
    // Many dimensions
    int n = 5;

    // Multidimensional rhs
    Eigen::VectorXd y0(2*n);
    for(int i = 0; i < n; ++i) {
        y0(i)=(i+1.)/n;
        y0(i+n)=-1;
    }

    // Multidimensional rhs
    auto f = [n] (Eigen::VectorXd y) {
        Eigen::VectorXd fy(2*n);

        Eigen::VectorXd g(n);
        g(0) = y(0)*(y(1)+y(0));
        g(n-1) = y(n-1)*(y(n-1)+y(n-2));
        for(int i = 1; i < n-1; ++i) {
            g(i) = y(i)*(y(i-1)+y(i+1));
        }

        Eigen::SparseMatrix<double> C(n,n);
        C.reserve(3);
        for(int i = 0; i < n; ++i) {
            C.insert(i,i) = 2;
            if(i < n-1) C.insert(i,i+1) = -1;
            if(i >= 1)  C.insert(i,i-1) = -1;
        }
        C.makeCompressed();
        fy.head(n) = y.head(n);

        Eigen::SparseLU< Eigen::SparseMatrix<double> >  solver;
        solver.analyzePattern(C);
        solver.compute(C);
        fy.tail(n) = solver.solve(g);
        return fy;
    };

    // Constructor:
    ode45<Eigen::VectorXd> O(f);

    // Setup options
    O.options.do_statistics = true;

    // Solve
    auto sol = O.solve(y0, T);

    // Print info
    O.print();

    std::cout << "T = " << sol.back().second << std::endl;
    std::cout << "y(T) = " << std::endl << sol.back().first << std::endl;
#endif
}

void test4() {
#ifdef MAKE_TEST4
    std::cout << "Stiff ode test:" << std::endl;

    // Test class with fundamental types
    auto f = [] (double y) { return y*(y - y*y); };

    // cf. http://ch.mathworks.com/company/newsletters/articles/stiff-differential-equations.html
    double y0 = 0.0001; // try 0.01
    // Large step size
    const double T = 2 / y0;

    // Quick syntax
    ode45<double> O(f);
    O.options.do_statistics = true;
    auto sol = O.solve(y0, T);

    // Print info
    O.print();

    // Print some info
    std::cout << "T = " << sol.back().second << std::endl;
    std::cout << "y(T) = " << std::endl << sol.back().first << std::endl;

//     for(auto v: sol) {
//         std::cout << v.first << std::endl;
//     }
#endif
}

int main() {

    // Basic prey/predator test
    test1();

    // Test double interface
    test2();

    // A bit more involved example, also testing options and statistics
    test3();

    // A bit more involved example, also testing options and statistics
    test4();

    return 0;
}

Nummerical Methods for CSE/PS13/templates_ps13/ode45_test.cpp


#include <algorithm>
#include <cmath>
#include <iostream>
#include <vector>
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <fstream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/index/rtree.hpp>

// #define DATASET_SIZE 1000000
#define DIMENTION 2
#define ELIPSON 30
#define MIN_POINTS 10

using namespace std;

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

typedef bg::model::point<float, 2, bg::cs::cartesian> dataPoint;
typedef bg::model::box<dataPoint> box;
typedef std::pair<box, int> value;

class DBSCAN {
 private:
  double** dataset;
  int elipson;
  int minPoints;
  int cluster;
  int* clusters;
  double getDistance(int center, int neighbor);
  vector<int> findNeighbors(int pos);
  void expandCluster(int pointId, vector<int> &neighbors);
  bgi::rtree<value, bgi::quadratic<4>> rtree;

 public:
  DBSCAN(double ** dataset);
  void run();
  void results();
};

int main(int, char **) {
  // Generate random datasets
  double **dataset =
      (double **)malloc(sizeof(double *) * DATASET_SIZE);
  for (int i = 0; i < DATASET_SIZE; i++) {
    dataset[i] = (double *)malloc(sizeof(double) * DIMENTION);
  }

  // Import Dataset from a file
  ifstream file("../dataset/dataset.txt");
  if (file.is_open()) {
    string token;
    int rowCount = 0;
    while (getline(file, token)) {
      int colCount = 0;
      char* x = (char*)token.c_str();
      char* field = strtok(x, ",");
      double tmp;
      sscanf(field, "%lf", &tmp);
      dataset[rowCount][colCount] = tmp;
      while (field) {
        colCount++;
        if(colCount == DIMENTION) break;
        field = strtok(NULL, ",");
        if (field!=NULL) {
          double tmp;
          sscanf(field,"%lf",&tmp);
          dataset[rowCount][colCount] = tmp;
        }
      }
      rowCount++;
      if(rowCount == DATASET_SIZE) break;
    }
    file.close();
  }

  // Initialize DBSCAN with dataset
  DBSCAN dbscan(dataset);

  // Run the DBSCAN algorithm
  dbscan.run();

  // Print the cluster results of DBSCAN
  dbscan.results();

  return 0;
}

DBSCAN::DBSCAN(double **loadData) {

  dataset =
      (double **)malloc(sizeof(double *) * DATASET_SIZE);
  for (int i = 0; i < DATASET_SIZE; i++) {
    dataset[i] = (double *)malloc(sizeof(double) * DIMENTION);
  }
  clusters = (int *)malloc(sizeof(int) * DATASET_SIZE);
  elipson = ELIPSON;
  minPoints = MIN_POINTS;
  cluster = 0;

  for (int i = 0; i < DATASET_SIZE; i++) {
    dataset[i][0] = loadData[i][0];
    dataset[i][1] = loadData[i][1];
    clusters[i] = 0;
  }

  // Create an Rtree of the dataset
  for (int i = 0; i < DATASET_SIZE; i++) {
    // create a box for each points
    box b(dataPoint(dataset[i][0], dataset[i][1]),
          dataPoint(dataset[i][0], dataset[i][1]));
    // insert points to the rtree
    rtree.insert(std::make_pair(b, i));

  }
}

double DBSCAN::getDistance(int center, int neighbor) {
  int dist = (dataset[center][0] - dataset[neighbor][0]) *
                 (dataset[center][0] - dataset[neighbor][0]) +
             (dataset[center][1] - dataset[neighbor][1]) *
                 (dataset[center][1] - dataset[neighbor][1]);

  return sqrt(dist);
}

void DBSCAN::run() {
  // Neighbors of the point
  vector<int> neighbors;

  for (int i = 0; i < DATASET_SIZE; i++) {

    if (clusters[i] == 0) {

      // Find neighbors of point P
      neighbors = findNeighbors(i);

      // Mark noise points
      if (neighbors.size() < minPoints) {
        clusters[i] = -1;
      } else {
        // Increment cluster and initialize it will the current point
        cluster++;

        clusters[i] = cluster;

        // Expand the neighbors of point P
        for (int j = 0; j < neighbors.size(); j++) {

          // Mark neighbour as point Q
          int dataIndex = neighbors[j];

          if(clusters[dataIndex] == -1) {
            clusters[dataIndex] = cluster;
          } else if (clusters[dataIndex] == 0) {

            clusters[dataIndex] = cluster;

            // Expand more neighbors of point Q
            vector<int> moreNeighbors;
            moreNeighbors = findNeighbors(dataIndex);

            // Continue when neighbors point is higher than minPoint threshold

            if (moreNeighbors.size() >= minPoints) {
              // Check if neighbour of Q already exists in neighbour of P
              for (int x = 0; x < moreNeighbors.size(); x++) {
                bool doesntExist = true;
                for (int y = 0; y < neighbors.size(); y++) {
                  if (moreNeighbors[x] == neighbors[y]) {
                    doesntExist = false;
                    break;
                  }
                }

                // If neighbour doesn't exist, add to neighbor list
                if (doesntExist) {
                  neighbors.push_back(moreNeighbors[x]);
                }
              }
            }
          }
      }
    }
  }
}
}

void DBSCAN::results() {
  printf("Number of clusters: %d\n", cluster);
  int noises = 0;
  for(int x = 1; x <= cluster; x++) {
    int count = 0;
    for(int i = 0; i < DATASET_SIZE; i++) {
      if(clusters[i] == x) {
        count++;
      }
      if(clusters[i] == -1) {
        noises++;
      }
    }
    printf("Cluster %d has %d data\n", x, count);
  }
  printf("Noises: %d\n", noises);

}

vector<int> DBSCAN::findNeighbors(int pos) {

  vector<int> neighbors;
  vector<value> result_n;

  // Create a search box for the given poiny
  box searchBox(dataPoint(dataset[pos][0] - elipson, dataset[pos][1] - elipson),
                dataPoint(dataset[pos][0] + elipson, dataset[pos][1] + elipson));

  // Query the intersection of search box on Rtree
  rtree.query(bgi::intersects(searchBox), std::back_inserter(result_n));

  // collect the points of box
  vector<int> pointsInBox = {};
  for (value pair : result_n) pointsInBox.push_back(pair.second);

  // Compute the distance only with points in a box
  for (int x = 0; x < pointsInBox.size(); x++) {
    // Compute neighbor points
    double distance = getDistance(pos, pointsInBox[x]);
    if (distance <= elipson && pos != pointsInBox[x]) {
      neighbors.push_back(pointsInBox[x]);
    }
  }

  return neighbors;

}


/*
Modified BSD License

This file is a part of Statistical package and originates from:
http://sf.net/projects/enjomitchsorbit

Copyright (c) 2012, Szymon "Enjo" Ender
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    * Neither the name of the <organization> nor the
      names of its contributors may be used to endorse or promote products
      derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include "EigenEigen3.hpp"
#include "../Matrix.hpp"
#include "../../Util/VecD.hpp"
#include <Util/Except.hpp>
#include <Util/CharManipulations.hpp>

#include <STD/VectorCpp.hpp>

//#define EIGEN_USE_MKL_ALL
//#define EIGEN_USE_BLAS // Enables openblas64 - somewhat slower
//#define EIGEN_USE_LAPACKE // Don't use currently
#include <Eigen/Dense>

#include <Util/CoutBuf.hpp>

using namespace std;
using namespace EnjoLib;
//using namespace Eigen;

EigenEigen3::~EigenEigen3(){}
EigenEigen3::EigenEigen3(){}

static Eigen::MatrixXd ConvertMatrix( const Matrix & m )
{
    const int nrows = m.GetNRows();
    const int ncols = m.GetNCols();
    Eigen::MatrixXd mateig(nrows, ncols);
    for ( int i = 0; i < nrows; ++i ) for ( int j = 0; j < ncols; ++j )
            mateig(i, j) = m.at(i).at(j);

    return mateig;
}

std::vector<EigenValueVector> EigenEigen3::GetEigenValVecClient( const Matrix & m ) const
{
    std::vector<EigenValueVector> ret;
    ret.reserve(m.GetNRows());

    //cout << "M = " << m.GetNCols() << ", " << m.GetNRows() << endl;

    Eigen::EigenSolver<Eigen::MatrixXd> es;
    const Eigen::MatrixXd & matE = ConvertMatrix(m);
    //cout << "Pre\n" << m.Print() << endl;
    es.compute(matE, /* computeEigenvectors = */ true);
    //cout << "Post" << endl;
    if (es.info() != Eigen::ComputationInfo::Success)
    {
        ELO
        LOG << "Eigenvalue Calculation not successful: " << es.info() << NL3;
        //cout << m.Print() << endl;
        throw EnjoLib::ExceptRuntimeError("Eigenvalue Calculation not successful " + CharManipulations().ToStr(es.info()));
        LOG << "After throw: " << es.info() << NL3;
    }

    const auto & eval  = es.eigenvalues();
    const auto & evect = es.eigenvectors();
    for ( int i = 0; i < m.GetNRows(); ++i )
    {
        const auto & evali = eval(i);
        const double eigenValue = evali.real();
        VecD eigenVector;
        eigenVector.reserve(m.GetNCols());

        for ( int j = 0; j < m.GetNCols(); ++j )
        {
            const double val = evect(j, i).real(); // Transposition (j, i) due to internal Eigen storage!
            eigenVector.push_back(val);
        }

        ret.push_back( EigenValueVector(eigenValue, eigenVector) );
    }
    return ret;
}


#include "eigenvalues.h"
#include <Eigen/Eigenvalues>

/*!
 * Adjust a matrix so that its eigenvalues are less than a given magnitude.
 * Moved to its on CU because this doesn't change often and takes like 2 minutes to recompile on O3 :P
 */
template<typename T>
DenseMatrix<T> constrainEigenvalueMagnitude(const DenseMatrix<T> &m, T magnitude)
{
  Eigen::EigenSolver<DenseMatrix<T>> eigSolver(m);
  using MatCT = Eigen::Matrix<std::complex<T>, Eigen::Dynamic, Eigen::Dynamic>;
  MatCT D = eigSolver.eigenvalues().asDiagonal();
  MatCT V = eigSolver.eigenvectors();

  for(s64 i = 0; i < D.rows(); i++) {
    T eigMag = std::abs(D(i,i));
    if(eigMag > magnitude)
      D(i,i) = magnitude * D(i,i) / eigMag;
  }

  MatCT restored = V * D * V.inverse(); // slow, but this matrix is small.
  DenseMatrix<T> result(m.rows(), m.rows());
  for(s64 i = 0; i < m.rows(); i++) {
    for(s64 j = 0; j < m.rows(); j++) {
      result(i,j) = std::real(restored(i,j));
    }
  }

  return result;
}

template DenseMatrix<float> constrainEigenvalueMagnitude(const DenseMatrix<float> &m, float magnitude);
template DenseMatrix<double> constrainEigenvalueMagnitude(const DenseMatrix<double> &m, double magnitude);


/**
 * @date Tue Apr 2 21:08:00 2013 +0200
 * @author Laurent El Shafey <Laurent.El-Shafey@idiap.ch>
 *
 * Copyright (C) Idiap Research Institute, Martigny, Switzerland
 */

#include <boost/make_shared.hpp>
#include <bob.math/inv.h>
#include <bob.math/lu.h>
#include <bob.math/stats.h>

#include <bob.learn.linear/whitening.h>

namespace bob { namespace learn { namespace linear {

  WhiteningTrainer::WhiteningTrainer()
  {
  }

  WhiteningTrainer::WhiteningTrainer(const WhiteningTrainer& other)
  {
  }

  WhiteningTrainer::~WhiteningTrainer() {}

  WhiteningTrainer& WhiteningTrainer::operator= (const WhiteningTrainer& other)
  {
    return *this;
  }

  bool WhiteningTrainer::operator== (const WhiteningTrainer& other) const
  {
    return true;
  }

  bool WhiteningTrainer::operator!= (const WhiteningTrainer& other) const
  {
    return false;
  }

  void WhiteningTrainer::train(Machine& machine, const blitz::Array<double,2>& ar) const {
    // training data dimensions
    const size_t n_samples = ar.extent(0);
    const size_t n_features = ar.extent(1);
    // machine dimensions
    const size_t n_inputs = machine.inputSize();
    const size_t n_outputs = machine.outputSize();

    // Checks that the dimensions are matching
    if (n_inputs != n_features) {
      boost::format m("machine input size (%u) does not match the number of columns in input array (%d)");
      m % n_inputs % n_features;
      throw std::runtime_error(m.str());
    }
    if (n_outputs != n_features) {
      boost::format m("machine output size (%u) does not match the number of columns in output array (%d)");
      m % n_outputs % n_features;
      throw std::runtime_error(m.str());
    }

    // 1. Computes the mean vector and the covariance matrix of the training set
    blitz::Array<double,1> mean(n_features);
    blitz::Array<double,2> cov(n_features,n_features);
    bob::math::scatter(ar, cov, mean);
    cov /= (double)(n_samples-1);

    // 2. Computes the inverse of the covariance matrix
    blitz::Array<double,2> icov(n_features,n_features);
    bob::math::inv(cov, icov);

    // 3. Computes the Cholesky decomposition of the inverse covariance matrix
    blitz::Array<double,2> whiten(n_features,n_features);
    bob::math::chol(icov, whiten);

    // 4. Updates the linear machine
    machine.setInputSubtraction(mean);
    machine.setInputDivision(1.);
    machine.setWeights(whiten);
    machine.setBiases(0);
    machine.setActivation(boost::make_shared<bob::learn::activation::IdentityActivation>());
  }

}}}


#include <igl/sort_vectors_ccw.h>
#include <igl/sort.h>
#include <Eigen/Dense>

template <typename DerivedS, typename DerivedI>
IGL_INLINE void igl::sort_vectors_ccw(
  const Eigen::PlainObjectBase<DerivedS>& P,
  const Eigen::PlainObjectBase<DerivedS>& N,
  Eigen::PlainObjectBase<DerivedI> &order,
  const bool do_sorted,
  Eigen::PlainObjectBase<DerivedS> &sorted,
  const bool do_inv_order,
  Eigen::PlainObjectBase<DerivedI> &inv_order)
{
  int half_degree = P.cols()/3;

  //local frame
  Eigen::Matrix<typename DerivedS::Scalar,1,3> e1 = P.head(3).normalized();
  Eigen::Matrix<typename DerivedS::Scalar,1,3> e3 = N.normalized();
  Eigen::Matrix<typename DerivedS::Scalar,1,3> e2 = e3.cross(e1);

  Eigen::Matrix<typename DerivedS::Scalar,3,3> F; F<<e1.transpose(),e2.transpose(),e3.transpose();

  Eigen::Matrix<typename DerivedS::Scalar,Eigen::Dynamic,1> angles(half_degree,1);
  for (int i=0; i<half_degree; ++i)
  {
    Eigen::Matrix<typename DerivedS::Scalar,1,3> Pl = F.colPivHouseholderQr().solve(P.segment(i*3,3).transpose()).transpose();
    assert(fabs(Pl(2))/Pl.cwiseAbs().maxCoeff() <1e-5);
    angles[i] = atan2(Pl(1),Pl(0));
  }

  igl::sort( angles, 1, true, angles, order);
  //make sure that the first element is always  at the top
  while (order[0] != 0)
  {
    //do a circshift
    int temp = order[0];
    for (int i =0; i< half_degree-1; ++i)
      order[i] = order[i+1];
    order(half_degree-1) = temp;
  }
  if (do_sorted)
  {
    sorted.resize(1,half_degree*3);
    for (int i=0; i<half_degree; ++i)
      sorted.segment(i*3,3) = P.segment(order[i]*3,3);
  }
  if (do_inv_order)
  {
    inv_order.resize(half_degree,1);
    for (int i=0; i<half_degree; ++i)
    {
      for (int j=0; j<half_degree; ++j)
        if (order[j] ==i)
        {
          inv_order(i) = j;
          break;
        }
    }
    assert(inv_order[0] == 0);
  }

}

#ifdef IGL_STATIC_LIBRARY
// Explicit template specialization
template void igl::sort_vectors_ccw<Eigen::Matrix<double, 1, -1, 1, 1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, 1, -1, 1, 1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 1, -1, 1, 1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&, bool, Eigen::PlainObjectBase<Eigen::Matrix<double, 1, -1, 1, 1, -1> >&, bool, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&);
#endif


#ifndef STAN_MATH_PRIM_SCAL_FUN_LBETA_HPP
#define STAN_MATH_PRIM_SCAL_FUN_LBETA_HPP

#include <stan/math/prim/meta.hpp>
#include <boost/math/tools/promotion.hpp>
#include <stan/math/prim/scal/fun/lgamma.hpp>

namespace stan {
namespace math {

/**
 * Return the log of the beta function applied to the specified
 * arguments.
 *
 * The beta function is defined for \f$a > 0\f$ and \f$b > 0\f$ by
 *
 * \f$\mbox{B}(a, b) = \frac{\Gamma(a) \Gamma(b)}{\Gamma(a+b)}\f$.
 *
 * This function returns its log,
 *
 * \f$\log \mbox{B}(a, b) = \log \Gamma(a) + \log \Gamma(b) - \log
 \Gamma(a+b)\f$.
 *
 * See stan::math::lgamma() for the double-based and stan::math for the
 * variable-based log Gamma function.
 *
 *
   \f[
   \mbox{lbeta}(\alpha, \beta) =
   \begin{cases}
     \ln\int_0^1 u^{\alpha - 1} (1 - u)^{\beta - 1} \, du & \mbox{if } \alpha,
 \beta>0 \\[6pt] \textrm{NaN} & \mbox{if } \alpha = \textrm{NaN or } \beta =
 \textrm{NaN} \end{cases} \f]

   \f[
   \frac{\partial\, \mbox{lbeta}(\alpha, \beta)}{\partial \alpha} =
   \begin{cases}
     \Psi(\alpha)-\Psi(\alpha+\beta) & \mbox{if } \alpha, \beta>0 \\[6pt]
     \textrm{NaN} & \mbox{if } \alpha = \textrm{NaN or } \beta = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{lbeta}(\alpha, \beta)}{\partial \beta} =
   \begin{cases}
     \Psi(\beta)-\Psi(\alpha+\beta) & \mbox{if } \alpha, \beta>0 \\[6pt]
     \textrm{NaN} & \mbox{if } \alpha = \textrm{NaN or } \beta = \textrm{NaN}
   \end{cases}
   \f]
 *
 * @param a First value
 * @param b Second value
 * @return Log of the beta function applied to the two values.
 * @tparam T1 Type of first value.
 * @tparam T2 Type of second value.
 */
template <typename T1, typename T2>
inline typename boost::math::tools::promote_args<T1, T2>::type lbeta(
    const T1 a, const T2 b) {
  return lgamma(a) + lgamma(b) - lgamma(a + b);
}

}  // namespace math
}  // namespace stan

#endif


/* ScaFES
 * Copyright (c) 2017-2018, ZIH, TU Dresden, Federal Republic of Germany.
 * For details, see the files COPYING and LICENSE in the base directory
 * of the package.
 */

/**
 *  @file TumorHeatEqnFDM.hpp
 *
 *  @brief Implementation of a n-dimensional Pennes bioheat equation problem
 *         with tumor model inside the grid.
 */

#include <iostream>
#include "ScaFES.hpp"

#include <boost/property_tree/ini_parser.hpp>

/*******************************************************************************
 ******************************************************************************/
/**
 * \class TumorHeatEqnFDM
 *  @brief Class for discretized Pennes bioheat equation problem.
 *
*/
template<typename CT, std::size_t DIM>
class TumorHeatEqnFDM : public ScaFES::Problem<TumorHeatEqnFDM<CT,DIM>, CT, DIM> {
  private:
    /** Parser for ini files. */
    using PTree = boost::property_tree::ptree;
    const PTree ptree;

  public:

    /* Based on Bousselham et al. (2017). */

    /************************************************************************/
    /** constant rho. Density. */
    const CT RHO; /* kg/m^3 */

    /** constant C. Specific heat capacity. */
    const CT C; /* J/(kg K) */

    /** constant K. Thermal conductivity). */
    const CT K; /* W/(m K) */

    /** constant Q_brain. Metabolism heat generation of the brain. */
    const CT Q_BRAIN; /* W/(m^3) */

    /** constant Q_tumor. Metabolism heat generation of the tumor. */
    const CT Q_TUMOR; /* W/(m^3) */

    /** constant rho_b. Density of the blood. */
    const CT RHO_B; /* kg/m^3 */

    /** constant C_Pb. Specific heat of the blood. */
    const CT C_PB; /* J/(kg K) */

    /** constant omega_b. Blood perfusion rate (normal brain tissue). */
    const CT OMEGA_B_BRAIN; /* 1/s */

    /** constant omega_b. Blood perfusion rate (Astrocytoma brain tumor). */
    const CT OMEGA_B_TUMOR; /* 1/s */

    /** constant T_i. Initial condition for T. */
    const CT T_I; /* K */

    /** constant h. Ambient convetion. */
    const CT H; /* W/(m^2 K) */

    /** constant T_inf. Air temperature. */
    const CT T_INF; /* K */

    /** constant q_bc. Heat flux at surface. */
    const CT Q_BC; /* W/(m^2) */

    /** constant diameter. diameter of the tumor. */
    const CT DIAMETER; /* m */

    /** constant depth. depth of the tumor. */
    const CT DEPTH; /* m */

    /* constant T_a. Temperature of artery. */
    const CT T_A; /* K */

    /************************************************************************/
    /* Auxiliary values. */
    /* Radius of tumor. */
    CT RADIUS;

    /* Center of tumor. */
    ScaFES::Ntuple<CT,DIM> tumorCenter;

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

    /** All fields which are related to the underlying problem
     * are added in terms of an entry of the parameters of
     * type \c std::vector.
     * @param params Set of ScaFES parameters.
     * @param gg Global grid.
     * @param useLeapfrog Should the leap frog scheme be used?
     * @param nameDatafield Name of the fields.
     * @param stencilWidth Stencil width of the fields.
     * @param isKnownDf Is the data field are known or unknown one?
     * @param ptree_ Config file parser.
     * @param nLayers Number of layers at the global boundary.
     * @param defaultValue Default value of fields.
     * @param writeToFile How often should the data field be written to file.
     * @param computeError Should the Linf error between the numerical
     *                     and exact solution be computed?
     * @param geomparamsInit Initial guess of geometrical parameters.
     * @param checkConvergence Should convergence be checked?
     */
    TumorHeatEqnFDM(ScaFES::Parameters const& params,
                    ScaFES::GridGlobal<DIM> const& gg,
                    bool useLeapfrog,
                    std::vector<std::string> const& nameDatafield,
                    std::vector<int> const& stencilWidth,
                    std::vector<bool> const& isKnownDf,
                    PTree const& ptree_,
                    std::vector<int> const& nLayers = std::vector<int>(),
                    std::vector<CT> const& defaultValue = std::vector<CT>(),
                    std::vector<ScaFES::WriteHowOften> const& writeToFile
                      = std::vector<ScaFES::WriteHowOften>(),
                    std::vector<bool> const& computeError = std::vector<bool>(),
                    std::vector<CT> const& geomparamsInit = std::vector<CT>(),
                    std::vector<bool> const& checkConvergence = std::vector<bool>() )
        : ScaFES::Problem<TumorHeatEqnFDM<CT, DIM>, CT, DIM>(params, gg, useLeapfrog,
                                                             nameDatafield, stencilWidth,
                                                             isKnownDf, nLayers,
                                                             defaultValue, writeToFile,
                                                             computeError, geomparamsInit,
                                                             checkConvergence),
        ptree(ptree_),
        RHO(ptree.get<CT>("Parameters.RHO")),
        C(ptree.get<CT>("Parameters.C")),
        K(ptree.get<CT>("Parameters.K")),
        Q_BRAIN(ptree.get<CT>("Parameters.Q_BRAIN")),
        Q_TUMOR(ptree.get<CT>("Parameters.Q_TUMOR")),
        RHO_B(ptree.get<CT>("Parameters.RHO_B")),
        C_PB(ptree.get<CT>("Parameters.C_PB")),
        OMEGA_B_BRAIN(ptree.get<CT>("Parameters.OMEGA_B_BRAIN")),
        OMEGA_B_TUMOR(ptree.get<CT>("Parameters.OMEGA_B_TUMOR")),
        T_I(ptree.get<CT>("Parameters.T_I")),
        H(ptree.get<CT>("Parameters.H")),
        T_INF(ptree.get<CT>("Parameters.T_INF")),
        Q_BC(ptree.get<CT>("Parameters.Q_BC")),
        DIAMETER(ptree.get<CT>("Parameters.DIAMETER")),
        DEPTH(ptree.get<CT>("Parameters.DEPTH")),
        T_A(ptree.get<CT>("Parameters.T_A"))
        {
            RADIUS = DIAMETER/2.0;

            /* Calculate center of tumor. */
            for (std::size_t pp = 0; pp < DIM; ++pp) {
                if (pp == (DIM-1)) {
                    /* In the highest dimension parameter 'depth' will be used. */
                    if (this->params().coordNodeLast()[pp] <= DIAMETER) {
                        std::cerr << "WARNING: Diameter of tumor is bigger than grid in dimension: "
                                  << pp << "." << std::endl;
                    }
                    if (this->params().coordNodeLast()[pp] < std::fabs(DEPTH) ||
                        DEPTH < 0.0) {
                        std::cerr << "WARNING: Center of tumor is outside of grid in dimension: "
                                  << pp << "." << std::endl;
                    }
                    if ((this->params().coordNodeLast()[pp] + RADIUS) < std::fabs(DEPTH)) {
                        std::cerr << "WARNING: Tumor is completely outside of grid." << std::endl;
                    }
                    if (std::fabs(this->params().coordNodeLast()[pp] - DEPTH) < RADIUS ||
                        std::fabs(DEPTH) < RADIUS) {
                        std::cerr << "WARNING: Part of tumor is outside of grid in dimension: "
                                  << pp << "." << std::endl;
                    }
                    tumorCenter[pp] = this->params().coordNodeLast()[pp] - DEPTH;
                } else {
                    /* In every other dimension the tumor will be located in the center. */
                    if (this->params().coordNodeLast()[pp] <= DIAMETER) {
                        std::cerr << "WARNING: Diameter of tumor is bigger than grid in dimension: "
                                  << pp << "." << std::endl;
                    }
                    tumorCenter[pp] = this->params().coordNodeLast()[pp]/2.0;
                }
            }
        }

    /** Evaluates all fields at one given global inner grid node.
     */
    void evalInner(std::vector< ScaFES::DataField<CT, DIM> >& /*vNew*/,
                   ScaFES::Ntuple<int,DIM> const& /*idxNode*/,
                   int const& /*timestep*/) {
    }

    /** Evaluates all fields at one given global border grid node.
     */
    void evalBorder(std::vector< ScaFES::DataField<CT, DIM> >& /*vNew*/,
                    ScaFES::Ntuple<int,DIM> const& /*idxNode*/,
                    int const& /*timestep*/) {
    }

    /** Initializes all unknown fields at one given global inner grid node.
     *  @param vNew Set of all unknown fields (return value).
     *  @param idxNode Index of given grid node.
     */
    template<typename TT>
    void initInner(std::vector< ScaFES::DataField<TT, DIM> >& vNew,
                   std::vector<TT> const& /*vOld*/,
                   ScaFES::Ntuple<int,DIM> const& idxNode,
                   int const& /*timestep*/) {
        /* Init all nodes with the same temperature. */
        vNew[0](idxNode) = T_I;
    }

    /** Initializes all unknown fields at one given global border grid node.
     *  @param vNew Set of all unknown fields (return value).
     *  @param vOld Set of all unknown fields at old time step.
     *  @param idxNode Index of given grid node.
     *  @param timestep Given time step.
     */
    template<typename TT>
    void initBorder(std::vector< ScaFES::DataField<TT, DIM> >& vNew,
                   std::vector<TT> const& vOld,
                   ScaFES::Ntuple<int,DIM> const& idxNode,
                   int const& timestep) {
        this->template initInner<TT>(vNew, vOld, idxNode, timestep);
    }

    /** Updates all unknown fields at one given global inner grid node.
     *  @param vNew Set of all unknown fields at new time step (return value).
     *  @param vOld Set of all unknown fields at old time step.
     *  @param idxNode Index of given grid node.
     */
    template<typename TT>
    void updateInner(std::vector<ScaFES::DataField<TT,DIM>>& vNew,
                     std::vector<ScaFES::DataField<TT,DIM>> const& vOld,
                     ScaFES::Ntuple<int,DIM> const& idxNode,
                     int const& /*timestep*/) {
        CT omega = 0.0;
        CT Q = 0.0;
        /* Get coordinates of current node. */
        ScaFES::Ntuple<double,DIM> x = this->coordinates(idxNode);
        /* Calculate distance to tumor center. */
        CT distance = 0.0;
        for (std::size_t pp = 0; pp < DIM; ++pp) {
            distance += (x[pp] - tumorCenter[pp]) * (x[pp] - tumorCenter[pp]);
        }
        /* Check if current point is inside tumor. */
        if (distance <= (this->RADIUS*this->RADIUS)) {
            /* Inside tumor. */
            omega = OMEGA_B_TUMOR;
            Q = Q_TUMOR;
        } else {
            /* Outside tumor, i.e. normal healthy brain tissue. */
            omega = OMEGA_B_BRAIN;
            Q = Q_BRAIN;
        }
        /* Discrete Pennes Bioheat Equation for updating inner nodes. */
        vNew[0](idxNode) = vOld[0](idxNode);
        for (std::size_t pp = 0; pp < DIM; ++pp) {
            vNew[0](idxNode) += this->tau() * (K/(RHO*C))
                                * (vOld[0](this->connect(idxNode, 2*pp))
                                   + vOld[0](this->connect(idxNode, 2*pp+1))
                                   - 2.0 * vOld[0](idxNode))
                                / (this->gridsize(pp) * this->gridsize(pp));
        }
        vNew[0](idxNode) += this->tau() * ((RHO_B*C_PB)/(RHO*C)) * omega
                            * (T_A - vOld[0](idxNode));
        vNew[0](idxNode) += this->tau() * (1.0/(RHO*C)) * Q;
    }

    /** Updates all unknown fields at one given global border grid node.
     *  @param vNew Set of all unknown fields at new time step (return value).
     *  @param vOld Set of all unknown fields at old time step.
     *  @param idxNode Index of given grid node.
     */
    template<typename TT>
    void updateBorder(std::vector<ScaFES::DataField<TT,DIM>>& vNew,
                      std::vector<ScaFES::DataField<TT,DIM>>const& vOld,
                      ScaFES::Ntuple<int,DIM> const& idxNode,
                      int const& /*timestep*/) {
        CT omega = 0.0;
        CT Q = 0.0;
        /* Get coordinates of current node. */
        ScaFES::Ntuple<double,DIM> x = this->coordinates(idxNode);
        /* Calculate distance to tumor center. */
        CT distance = 0.0;
        for (std::size_t pp = 0; pp < DIM; ++pp) {
            distance += (x[pp] - tumorCenter[pp]) * (x[pp] - tumorCenter[pp]);
        }
        /* Check if current point is inside tumor. */
        if (distance <= (this->RADIUS*this->RADIUS)) {
            /* Inside tumor. */
            omega = OMEGA_B_TUMOR;
            Q = Q_TUMOR;
        } else {
            /* Outside tumor, i.e. normal healthy brain tissue. */
            omega = OMEGA_B_BRAIN;
            Q = Q_BRAIN;
        }
        /* Discrete Pennes Bioheat Equation modified for updating border nodes. */
        vNew[0](idxNode) = vOld[0](idxNode);
        for (std::size_t pp = 0; pp < DIM; ++pp) {
            /* Last node/edge/surface in highest dimension will be convection
             * boundary condition. */
            if (idxNode.elem(pp) == (this->nNodes(pp)-1)) {
            /* vOld[0](this->connect(idxNode, 2*pp+1) needs to be replaced. */
                if (pp == (DIM-1)) {
                /* Cauchy boundary condition. */
                    vNew[0](idxNode) += this->tau() * (K/(RHO*C))
                                        * (vOld[0](this->connect(idxNode, 2*pp))
                                        /* + vOld[0](this->connect(idxNode, 2*pp+1) */
                                           + vOld[0](this->connect(idxNode, 2*pp))
                                           - ((2.0*this->gridsize(pp)/K)
                                              * H * (vOld[0](idxNode) - T_INF))
                                        /********************************************/
                                           - 2.0 * vOld[0](idxNode))
                                        / (this->gridsize(pp) * this->gridsize(pp));
                } else {
                /* Neumann boundary condition. */
                    vNew[0](idxNode) += this->tau() * (K/(RHO*C))
                                        * (vOld[0](this->connect(idxNode, 2*pp))
                                        /* + vOld[0](this->connect(idxNode, 2*pp+1)) */
                                           + vOld[0](this->connect(idxNode, 2*pp))
                                           - ((2.0*this->gridsize(pp)/K) *
                                              (-1.0 * Q_BC))
                                        /*********************************************/
                                           - 2.0 * vOld[0](idxNode))
                                        / (this->gridsize(pp) * this->gridsize(pp));
                }
            } else if (idxNode.elem(pp) == 0){
            /* vOld[0](this->connect(idxNode, 2*pp) needs to be replaced.
             * Neumann boundary condition. */
                vNew[0](idxNode) += this->tau() * (K/(RHO*C))
                                    /* + vOld[0](this->connect(idxNode, 2*pp)) */
                                    * (vOld[0](this->connect(idxNode, 2*pp+1))
                                       + ((2.0*this->gridsize(pp)/K) * Q_BC)
                                    /*******************************************/
                                       + vOld[0](this->connect(idxNode, 2*pp+1))
                                       - 2.0 * vOld[0](idxNode))
                                    / (this->gridsize(pp) * this->gridsize(pp));
            } else {
            /* No value needs to be replaced.
             * Use central differencing scheme. */
                vNew[0](idxNode) += this->tau() * (K/(RHO*C))
                                    * (vOld[0](this->connect(idxNode, 2*pp))
                                       + vOld[0](this->connect(idxNode, 2*pp+1))
                                       - 2.0 * vOld[0](idxNode))
                                    / (this->gridsize(pp) * this->gridsize(pp));
            }
        }

        /* These terms are independet of the boundary condition. */
        vNew[0](idxNode) += this->tau() * ((RHO_B*C_PB)/(RHO*C)) * omega
                            * (T_A - vOld[0](idxNode));
        vNew[0](idxNode) += this->tau() * (1.0/(RHO*C)) * Q;
    }

    /** Updates (2nd cycle) all unknown fields at one given global inner grid node.
     *  \remarks Only important if leap frog scheme is used.
     */
    template<typename TT>
    void updateInner2(std::vector<ScaFES::DataField<TT,DIM>>&,
                      std::vector<ScaFES::DataField<TT,DIM>> const&,
                      ScaFES::Ntuple<int,DIM> const&,
                      int const&) { }

    /** Updates (2nd cycle) all unknown fields at one given global border
     *  grid node.
     *  \remarks Only important if leap frog scheme is used.
     */
    template<typename TT>
    void updateBorder2(std::vector<ScaFES::DataField<TT,DIM>>&,
                       std::vector<ScaFES::DataField<TT,DIM>>const&,
                       ScaFES::Ntuple<int,DIM> const&,
                       int const&) { }
};


#include <iostream>
#include <boost/concept_check.hpp>
#include <ceres/ceres.h>
#include <cmath>

using namespace std;
using namespace ceres;
using ceres::AutoDiffCostFunction;
using ceres::Solver;
using ceres::Problem;
using ceres::CostFunction;
using ceres::Solve;

struct CostFuntorF1 {
  template <typename T>
  bool operator()(const T* const x1, const T* const x2, T* residuals) const {
    residuals[0] = x1[0] + T(10)*x2[0];
    return true;
  }
};

struct CostFuntorF2 {
  template <typename T>
  bool operator()(const T* const x3, const T* const x4, T* residuals) const {
    residuals[0] = sqrt(5)*(x3[0] - x4[0]);
    return true;
  }
};

struct CostFuntorF3 {
  template <typename T>
  bool operator()(const T* const x2, const T* const x3, T* residuals) const {
    residuals[0] = (x2[0] - T(2)*x3[0])*(x2[0] - T(2)*x3[0]);
    return true;
  }
};

struct CostFuntorF4 {
  template <typename T>
  bool operator()(const T* const x1, const T* const x4, T* residuals) const {
    residuals[0] = sqrt(10)*(x1[0] - x4[0])*(x1[0] - x4[0]);
    return true;
  }
};

struct CostFunctor {
  template <typename T>
  bool operator()(const T* const x, T* residuals)  const{
    residuals[0] = x[0] - T(10) * x[1];
    residuals[1] = T(sqrt(5)) * (x[2] - x[4]);
    residuals[2] = (x[1] - T(2) * x[2]) * (x[1] - T(2) * x[2]);
    residuals[3] = T(sqrt(10)) * (x[0] - x[3]) * (x[0] - x[3]);
    return true;
  }
};

int main(int argc, char** argv)
{
  const double initial_x1 = 10, initial_x2 = 5,
	       initial_x3 = 2, initial_x4 = 1;
  double x1 = initial_x1, x2 = initial_x2,
	 x3 = initial_x3, x4 = initial_x4;

  Problem problem;
  Solver::Options options;
  //control whether the log is output to STDOUT
  options.minimizer_progress_to_stdout = true;
  Solver::Summary summary;

  CostFunction* costfunction1 =
    new AutoDiffCostFunction<CostFuntorF1,1,1,1>(new CostFuntorF1);
  CostFunction* costfunction2 =
    new AutoDiffCostFunction<CostFuntorF2,1,1,1>(new CostFuntorF2);
  CostFunction* costfunction3 =
    new AutoDiffCostFunction<CostFuntorF3,1,1,1>(new CostFuntorF3);
  CostFunction* costfunction4 =
    new AutoDiffCostFunction<CostFuntorF4,1,1,1>(new CostFuntorF4);
  problem.AddResidualBlock(costfunction1,NULL,&x1,&x2);
  problem.AddResidualBlock(costfunction2,NULL,&x3,&x4);
  problem.AddResidualBlock(costfunction3,NULL,&x2,&x3);
  problem.AddResidualBlock(costfunction4,NULL,&x1,&x4);

  Solve(options,&problem,&summary);

  cout << "x1 : " << initial_x1 << "->" << x1 << endl;
  cout << "x2 : " << initial_x2 << "->" << x2 << endl;
  cout << "x3 : " << initial_x3 << "->" << x3 << endl;
  cout << "x4 : " << initial_x4 << "->" << x4 << endl;

  cout << "---------------------line----------------------------" << endl;
  double x[4] = {initial_x1, initial_x2, initial_x3, initial_x4};
  CostFunction* costfunction = new AutoDiffCostFunction<CostFunctor, 4, 4>(new CostFunctor);
  Problem pb;
  pb.AddResidualBlock(costfunction,NULL,x);
  Solve(options,&pb,&summary);
  cout << "x[0] : " << initial_x1 << "->" << x[0] << endl;
  cout << "x[1] : " << initial_x2 << "->" << x[1] << endl;
  cout << "x[2] : " << initial_x3 << "->" << x[2] << endl;
  cout << "x[3] : " << initial_x4 << "->" << x[3] << endl;

  return 0;

}


#include <armadillo>

int main() {

    const size_t dim = 6;
    arma::mat A(dim, dim, arma::fill::ones);
    A(arma::span(0, (dim / 2) - 1), arma::span(0, (dim / 2) - 1)).fill(-1.032e-310);
    A.print("A");
    arma::vec eigval = arma::eig_sym(A);
    eigval.print("eigval");
    arma::vec eigvalsqrt = arma::sqrt(eigval);
    // This has junk tiny (negative) values that need to be removed.
    eigvalsqrt.print("eigvalsqrt");

    const double thresh = 1.0e-15;
    // arma::uvec mask = arma::abs(eigval) < thresh;
    // mask.print("mask");
    // eigval(mask).zeros();
    arma::uvec mask = arma::find(arma::abs(eigval) < thresh);
    mask.print("mask");
    eigval(mask).zeros();
    eigval.print("eigval");
    arma::sqrt(eigval).print("eigvalsqrt");

    return 0;
}


//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//% Code implementing the paper "Accelerated Quadratic Proxy for Geometric Optimization", SIGGRAPH 2016.
//% Disclaimer: The code is provided as-is for academic use only and without any guarantees.
//%             Please contact the author to report any bugs.
//% Written by Shahar Kovalsky (http://www.wisdom.weizmann.ac.il/~shaharko/)
//%            Meirav Galun (http://www.wisdom.weizmann.ac.il/~/meirav/)
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

#include "mex.h"
#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include "mexHelpers.cpp"

using namespace Eigen;

void projBlockRotation(VectorXd &pA, int dim)
{
	int block_size = dim*dim;
	int num_blocks = pA.size() / block_size;
	JacobiSVD<MatrixXd> svdA(dim, dim, (ComputeFullU | ComputeFullV));
	bool flipped;
	MatrixXd U, V;
	Map<MatrixXd> currA(pA.data(), dim, dim);

	// project
	for (int ii = 0; ii < num_blocks; ii++)
	{
		// get current block
		new (&currA) Map<MatrixXd>(pA.data() + ii*block_size, dim, dim);
		// sign of determinant
		flipped = (currA.determinant() < 0);
		// svd
		svdA.compute(currA);
		// compute frames
		U = svdA.matrixU();
		V = svdA.matrixV();
		// ssvd
		if (flipped)
		{
			U.col(dim - 1) = -U.col(dim - 1);
		}
		// project block
		currA = U*V.transpose();
	}
}

void mexFunction(int nlhs, mxArray *plhs[],
	int nrhs, const mxArray*prhs[])

{
	// assign input
	int A_rows = mxGetM(prhs[0]); // # rows of A
	int A_cols = mxGetN(prhs[0]); // # cols of A
	double *dim;
	const Map<VectorXd> A(mxGetPr(prhs[0]), A_rows, A_cols);
	dim = mxGetPr(prhs[1]);

	// init output
	VectorXd pA(A_rows);
	pA = A;

	// project
	projBlockRotation(pA, *dim);

	// assign outputs
	mapDenseMatrixToMex(pA, &(plhs[0]));

}


/*! \file solveeom.cpp
    \brief 単振り子に対して運動方程式を解くクラスの実装

    Copyright © 2016-2018 @dc1394 All Rights Reserved.
    This software is released under the BSD 2-Clause License.
*/
#include "solveeom.h"
#include <cmath>                                // for std::sin, std::cos
#include <fstream>                              // for std::ofstream
#include <boost/assert.hpp>                     // for BOOST_ASSERT
#include <boost/format.hpp>                     // for boost::format
#include <boost/math/constants/constants.hpp>   // for boost::math::constants::pi
#include "solveeommain.h"

namespace solveeom {
    // #region コンストラクタ・デストラクタ

    SolveEoM::SolveEoM(float l, float r, float theta0) :
		Isconsider_Inertial_Resistance(nullptr, [this](auto isconsider_inertial_resistance) { return isconsider_inertial_resistance_ = isconsider_inertial_resistance; }),
        Theta([this] { return static_cast<float>(x_[0]); }, [this](auto theta) { return x_[0] = theta; }),
		Theta0(nullptr, [this](auto theta0) { return theta0_ = theta0; }),
		Time([this] { return static_cast<float>(t_); }, [this](auto t) { return t_ = t; }),
		V([this] { return static_cast<float>(l_ * x_[1]); }, [this](auto v) { return x_[1] = v / l_; }),
        l_(l),
		omega0_2_(g / l_),
        r_(r),
		m_(4.0 / 3.0 * boost::math::constants::pi<double>() * r * r * r * SolveEoM::ALUMINIUMRHO),
		gamma_(3.0 * boost::math::constants::pi<double>() * r_ * AIRMYU / m_),
		stepper_(SolveEoM::EPS, SolveEoM::EPS),
		t_(0.0),
		theta0_(theta0),
		x_({ theta0, 0.0 })
    {
    }

    // #endregion コンストラクタ・デストラクタ

    // #region publicメンバ関数

	float SolveEoM::gettheta_fumofumobun_approx() const
	{
		return static_cast<float>(theta0_ * std::exp(-gamma_ * t_) *
								  std::cos(std::sqrt((omega0_2_ - gamma_ * gamma_) * (3.0 + std::cos(theta0_ * std::exp(-gamma_ * t_)))) / 2.0 * t_));
	}

	float SolveEoM::getv_fumofumobun_approx() const
    {
		auto const term1 = -gamma_ * gettheta_fumofumobun_approx();

		auto const alpha = 0.5 * std::sqrt((omega0_2_ - gamma_ * gamma_) * (3.0 + std::cos(theta0_ * std::exp(-gamma_ * t_))));

		auto const term2 = -alpha * theta0_* std::exp(-gamma_ * t_) * std::sin(alpha * t_) *
						   (0.5 * theta0_ * gamma_ * std::exp(-gamma_ * t_) * std::sin(theta0_ * std::exp(-gamma_ * t_)) /
						   (3.0 + std::cos(theta0_ * std::exp(-gamma_ * t_))) * t_ + 1.0);

		return l_ * (term1 + term2);
    }

	float SolveEoM::kinetic_energy(double v) const
	{
		return static_cast<float>(0.5 * m_ * sqr(v));
	}

    float SolveEoM::operator()(float dt)
    {
        boost::numeric::odeint::integrate_adaptive(
            stepper_,
            getEoM(),
            x_,
            0.0,
            static_cast<double>(dt),
            SolveEoM::DX);

        return static_cast<float>(x_[0]);
    }

    void SolveEoM::operator()(double dt, std::string const & filename, double t)
    {
        std::ofstream result(filename);

        boost::numeric::odeint::integrate_const(
            stepper_,
            getEoM(),
            x_,
            0.0,
            t,
            dt,
            [&result, this](auto const & x, auto const t)
            {
				result << boost::format("%.3f, %.15f, %.15f\n") % t % x[0] % gettheta_fumofumobun_approx();
            });
    }

	float SolveEoM::potential_energy(double theta) const
	{
		return static_cast<float>(m_ * SolveEoM::g * l_ * (1.0 - std::cos(theta)));
	}

	void SolveEoM::timereset()
    {
		t_ = 0.0;
    }

    // #endregion publicメンバ関数

    // #region privateメンバ関数

	std::function<void(SolveEoM::state_type const &, SolveEoM::state_type &, double const)> SolveEoM::getEoM() const
    {
        auto const eom = [this](state_type const & x, state_type & dxdt, double const)
        {
            // dθ/dt = v / l
            dxdt[0] = x[1];

            // 振り子に働く力
            auto const f1 = -SolveEoM::g * std::sin(x[0]) / l_;

			// レイノルズ数
			auto const Re = 2.0 * r_ * std::fabs(l_ * x[1]) / AIRNYU;

            // 粘性抵抗
            auto const F = 6.0 * boost::math::constants::pi<double>() * AIRMYU * r_ * l_ * x[1];

			// レイノルズ数が閾値より小さいか、「慣性抵抗も考慮」チェックボックスが外れていたら
			if (Re < SolveEoM::REYNOLDS_THRESHOLD || !isconsider_inertial_resistance_) {
				// 粘性抵抗のみを考慮する
				dxdt[1] = f1 - F / (m_ * l_);

				return;
			}

			auto const FD = 0.5 * AIRRHO * boost::math::constants::pi<double>() * sqr(r_ * (l_ * x[1]));

			// Drag coefficient
			double CD;

			// N.-S. Cheng, Comparison of formulas for drag coefficient and settling velocity of
			// spherical particles, Powder Technology 189 (2009) 395–398.
			if (Re <= 3000) {
				CD = 24.0 / Re * std::pow(1.0 + 0.27 * Re, 0.43) + 0.47 * (1.0 - std::exp(-0.04 * std::pow(Re, 0.38)));
			}
			else {
				// Re > 3000
				// Almedeij J. Drag coefficient of flow around a sphere: Matching asymptotically the wide
				// trend. std::powder Technology. (2008);doi:10.1016/j.std::powtec.2007.12.006.
				auto const phi1 = std::pow(24.0 / Re, 10) + std::pow(21.0 * std::pow(Re, -0.67), 10) +
					std::pow(4.0 * std::pow(Re, -0.33), 10) + std::pow(0.4, 10);
				auto const phi2 = 1.0 / (1.0 / std::pow(0.148 * std::pow(Re, 0.11), 10) + 1.0 / std::pow(0.5, 10));
				auto const phi3 = std::pow((1.57E+8) * std::pow(Re, -1.625), 10);
				auto const phi4 = 1.0 / (1.0 / std::pow((6.0E-17) * std::pow(Re, 2.63), 10) + 1.0 / std::pow(0.2, 10));

				CD = std::pow((1.0 / (1.0 / (phi1 + phi2) + 1.0 / phi3) + phi4), 0.1);
			}

			// 慣性抵抗÷(m×l)
			auto const f2 = (x[1] >= 0.0) ? -FD * CD / (m_ * l_) : FD * CD / (m_ * l_);
			dxdt[1] = f1 + f2 - F / (m_ * l_);
        };

        return eom;
    }

    // #endregion privateメンバ関数
}


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

#ifndef DCS_MATH_STATS_DISTRIBUTION_CHI_SQUARED_HPP
#define DCS_MATH_STATS_DISTRIBUTION_CHI_SQUARED_HPP

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

#if !DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION(103500) // 1.35
# 	error "Required Boost library version >= 1.35"
#endif

#include <boost/math/distributions/chi_squared.hpp>
#include <cstddef>
#include <dcs/math/policies/policy.hpp>
#include <dcs/math/stats/distribution/gamma.hpp>
#include <dcs/math/stats/function/rand.hpp>
#include <iostream>
#include <vector>

namespace dcs { namespace math { namespace stats {

/**
 * \brief The Chi Squared distribution with degrees of freedom parameter
 * \f$\nu\f$.
 *
 *
 * \tparam RealT The type used for real numbers.
 * \tparam PolicyT The policy type.
 *
 * The probability density function (pdf):
 * \f[
 *   \Pr(x|\nu) = \frac{1}{2^{k/2}\Gamma(k/2)}\; x^{k/2-1} e^{-x/2}
 * \f]
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template < typename RealT=double, typename PolicyT=::dcs::math::policies::policy<> >
class chi_squared_distribution
{
	public: typedef RealT support_type;
	public: typedef RealT value_type;
	public: typedef PolicyT policy_type;

	public: explicit chi_squared_distribution(support_type df)
		: dist_(df)
	{
		// empty
	}

	// compiler-generated copy ctor and assignment operator are fine

	/**
	 * \brief Generate a random number distributed according to this
	 * chi_squared distribution.
	 *
	 * \param rng A uniform random number generator.
	 * \return A random number distributed according to this chi_squared
	 * distribution.
	 *
	 * A \c chi_squared random number distribution produces random numbers
	 * \f$x > 0\f$ distributed according to the probability density function:
	 * \f[
	 *   \Pr(x|\nu) = \frac{1}{2^{k/2}\Gamma(k/2)}\; x^{k/2-1} e^{-x/2}
	 * \f]
	 */
	public: template <typename UniformRandomGeneratorT>
		support_type rand(UniformRandomGeneratorT& rng) const
	{
		return ::dcs::math::stats::rand(
			gamma_distribution<value_type>(
				dist_.degrees_of_freedom()/value_type(2),
				value_type(2)
			),
			rng
		);
	}

	/**
	 * \brief Generate a vector of random numbers distributed according to this
	 * chi_squared distribution.
	 *
	 * \param rng A uniform random number generator.
	 * \param n The number of random numbers to generate.
	 * \return A vector of random numbers distributed according to this
	 * chi_squared distribution.
	 *
	 * A \c chi_squared random number distribution produces random numbers
	 * \f$x > 0\f$ distributed according to the probability density function:
	 * \f[
	 *   \Pr(x|\nu) = \frac{1}{2^{k/2}\Gamma(k/2)}\; x^{k/2-1} e^{-x/2}
	 * \f]
	 */
	public: template <typename UniformRandomGeneratorT>
		::std::vector<support_type> rand(UniformRandomGeneratorT& rng, ::std::size_t n) const
	{
		::std::vector<support_type> rnds(n);

        for ( ; n > 0; --n)
		{
			rnds.push_back(
				::dcs::math::stats::rand(
					gamma_distribution<value_type>(
						dist_.degrees_of_freedom()/value_type(2),
						value_type(2)
					),
					rng
				)
			);
		}

		return rnds;
	}

	public: support_type degrees_of_freedom() const
	{
		return dist_.degrees_of_freedom();
	}

	public: support_type quantile(value_type p) const
	{
		return ::boost::math::quantile(dist_, p);
	}

	private: ::boost::math::chi_squared_distribution<value_type,policy_type> dist_;
};

template <
	typename CharT,
	typename CharTraitsT,
	typename RealT,
	typename PolicyT
>
::std::basic_ostream<CharT,CharTraitsT>& operator<<(::std::basic_ostream<CharT,CharTraitsT>& os, chi_squared_distribution<RealT,PolicyT> const& dist)
{
	return os << "ChiSquared("
			  << "df=" <<  dist.degrees_of_freedom()
			  << ")";
}

}}} // Namespace dcs::math::stats

#endif // DCS_MATH_STATS_DISTRIBUTION_CHI_SQUARED_HPP


// Copyright (c) 2005-2009  INRIA Sophia-Antipolis (France).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org)
//
// $URL$
// $Id$
// SPDX-License-Identifier: LGPL-3.0-or-later OR LicenseRef-Commercial
//
//
// Author(s)     : Frédérik Paradis

#include <iostream>
#include <CGAL/Exact_predicates_exact_constructions_kernel_with_root_of.h>
#include <CGAL/CGAL_Ipelet_base.h>
#include <CGAL/Construct_theta_graph_2.h>
#include <CGAL/Construct_yao_graph_2.h>
#include <CGAL/Compute_cone_boundaries_2.h>
#include <CGAL/Cone_spanners_enum_2.h>
#include <CGAL/property_map.h>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>

namespace CGAL_cone_spanners {

typedef CGAL::Exact_predicates_exact_constructions_kernel_with_root_of Kernel;
typedef Kernel::Point_2                                                Point_2;
typedef Kernel::Direction_2                                            Direction_2;

/* Note: due to a bug in the boost library, using a directed graph
 * will cause a compilation error with g++ and clang++ when using c++11 standard.
 * See https://lists.boost.org/Archives/boost/2016/05/229458.php.
 */
typedef boost::adjacency_list<boost::listS,
                              boost::vecS,
                              boost::undirectedS,
                              Point_2
                             > Graph;

const std::string labels[] = {  "Theta-k-graph", "Yao-k-graph", "Half-theta-k-graph with even cones", "Half-Yao-k-graph with even cones", "Half-theta-k-graph with odd cones", "Half-Yao-k-graph with odd cones", "k cones", "Help" };
const std::string hmsg[] = {
  "Draws a theta-graph with k cones.",
  "Draws a Yao-graph with k cones.",
  "Draws an half-theta-graph with the even of k cones.",
  "Draws an half-Yao-graph with the even of k cones.",
  "Draws an half-theta-graph with the odd of k cones.",
  "Draws an half-Yao-graph with the odd of k cones.",
  "Draws k cones around the points.",
};

class Cone_spanners_ipelet
  : public CGAL::Ipelet_base<Kernel,7> {
public:
  Cone_spanners_ipelet()
    :CGAL::Ipelet_base<Kernel,7>("Cone Spanners",labels,hmsg){}
  void protected_run(int);
private:
};

void Cone_spanners_ipelet::protected_run(int fn)
{
  std::vector<Point_2> lst;
  int number_of_cones;
  switch (fn){
    case 0:
    case 1:
    case 2:
    case 3:
    case 4:
    case 5:
    case 6:
    {
      std::vector<Point_2> points_read;
      read_active_objects(
        CGAL::dispatch_or_drop_output<Point_2>(std::back_inserter(points_read))
      );

      if (points_read.empty()) {
        print_error_message("No mark selected");
        return;
      }
      for(std::vector<Point_2>::iterator it = points_read.begin(); it != points_read.end(); it++) {
        if(std::find(points_read.begin(), it, *it) == it) {
          lst.push_back(*it);
        }
      }

      int ret_val;
      boost::tie(ret_val,number_of_cones)=request_value_from_user<int>("Enter the number of cones");
      if (ret_val < 0) {
        print_error_message("Incorrect value");
        return;
      }
      if(number_of_cones < 2) {
        print_error_message("The number of cones must be larger than 1!");
        return;
      }
      break;
    }
    case 7:
      show_help();
      return;
  }

  if(fn >= 0 && fn <= 5) {
    CGAL::Cones_selected cones_selected = CGAL::ALL_CONES;
    if(fn == 2 || fn == 3)
      cones_selected = CGAL::EVEN_CONES;
    else if(fn == 4 || fn == 5)
      cones_selected = CGAL::ODD_CONES;

    Graph g;
    switch (fn){
      case 0:
      case 2:
      case 4:
      {
        CGAL::Construct_theta_graph_2<Kernel, Graph> theta(number_of_cones, Direction_2(1,0), cones_selected);
        theta(lst.begin(), lst.end(), g);
        break;
      }
      case 1:
      case 3:
      case 5:
      {
        CGAL::Construct_yao_graph_2<Kernel, Graph> yao(number_of_cones, Direction_2(1,0), cones_selected);
        yao(lst.begin(), lst.end(), g);
        break;
      }
    }
    boost::graph_traits<Graph>::edge_iterator ei, ei_end;
    for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei) {
      boost::graph_traits<Graph>::edge_descriptor e = *ei;
      boost::graph_traits<Graph>::vertex_descriptor  u = source(e, g);
      boost::graph_traits<Graph>::vertex_descriptor  v = target(e, g);
      draw_in_ipe(Segment_2(g[u], g[v]));
    }
    group_selected_objects_();
  }
  else if(fn == 6) {
    CGAL::Compute_cone_boundaries_2<Kernel> cones;
    std::vector<Direction_2> directions(number_of_cones);
    cones(number_of_cones, Direction_2(1,0), directions.begin());
    for(std::vector<Point_2>::iterator it = lst.begin(); it != lst.end(); it++) {
      for(std::vector<Direction_2>::iterator dir = directions.begin(); dir != directions.end(); dir++) {
        draw_in_ipe(Segment_2(*it,*it + 100*dir->to_vector()));
      }
      group_selected_objects_();
      get_IpePage()->deselectAll();
    }
  }
}

}

CGAL_IPELET(CGAL_cone_spanners::Cone_spanners_ipelet)


//////////////////////////////////////////////////////////////////////////////
// random::chi_squared.hpp                                                  //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  Use, modification and distribution are 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_RANDOM_CHI_SQUARED_HPP_ER_2009
#define BOOST_RANDOM_CHI_SQUARED_HPP_ER_2009
#include <vector>
#include <algorithm>
#include <iterator>
#include <boost/range.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/chi_squared.hpp>
#include <boost/math/distributions/chi_squared.hpp>

namespace boost{
namespace random{

    template<typename T>
    class chi_squared_distribution{
            typedef boost::normal_distribution<T> nd_t;
        public:
            typedef typename nd_t::input_type input_type;
            typedef typename nd_t::result_type result_type;

        chi_squared_distribution(): df_(2) {}
        chi_squared_distribution(unsigned df): df_(df) {}
        chi_squared_distribution(const chi_squared_distribution& that)
        :df_(that.df_){}
        chi_squared_distribution&
        operator=(const chi_squared_distribution& that){
            if(&that!=this){
                df_ = that.df_;
            }
            return *this;
        }

        template<typename U>
        result_type
        operator()(U& urng){
            typedef boost::variate_generator<U&,nd_t> vg_t;
            static nd_t nd(0,1);
            result_type z;
            result_type res = static_cast<T>(0);
            for(unsigned i = 0; i<this->df(); i++){
                z = nd(urng);
                res += (z * z);
            }
            return res;
        }

        result_type min () const { return static_cast<result_type>(0); }
        result_type max () const { return (nd_t().max)(); }
        unsigned df()const { return df_; }

        typedef math::chi_squared_distribution<T> math_dist_;

        operator math_dist_ (){ return math_dist(this->df()); }

        private:
            unsigned df_;
    };

}
}

#endif // CHI_SQUARE_HPP_INCLUDED


#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
using namespace std;

int main( int argc, char** argv )
{
    Eigen::Quaterniond q1(0.35, 0.2, 0.3, 0.1);
    Eigen::Quaterniond q2(-0.5, 0.4, -0.1, 0.2);
    Eigen::Vector3d t1(0.3, 0.1, 0.1);
    Eigen::Vector3d t2(-0.1, 0.5, 0.3);
    Eigen::Vector3d p1(0.5, 0, 0.2);

    Eigen::Quaterniond q1_one = q1.normalized();
    Eigen::Quaterniond q2_one = q2.normalized();

    //way1

    Eigen::Vector3d v = q1_one.inverse() * (p1 - t1);
    /** first translation
     * first translation, qi_one is world to camera
     * (p1 - t1) is in camera coordinate, after that , it is in world coordinator
     * so , we can use the same way to compute another point
     */

    return 0;
}


#include "pch_bcl.h"
#include "InverseKinematics.h"
#include <Eigen\Core>
#include <unsupported\Eigen\NonLinearOptimization>
#include "Tests.h"

#define _DEBUG_JACCOBI 1

using namespace Causality;

enum RotationEncodeMethodsEnum
{
	EulerAngles = 0,
	LnQuaterternion = 1,
};

static constexpr RotationEncodeMethodsEnum RotationEncodeMethod = LnQuaterternion;

namespace DirectX
{
	// Matrix Representation of Cross Production
	// =============Usage===============
	// This matrix is for row major vectors
	// Vector3Transform(V2,MatrixCrossProduct(V1)) == Vector3Cross(V1,V2)
	// Vector3Transform(V2,MatrixTranspose(MatrixCrossProduct(V1))) == Vector3Cross(V2,V1)
	// =============Internal============
	// \mathbf{a} \times \mathbf{b} = [\mathbf{a}]_{\times} \mathbf{b} = \begin{bmatrix}\,0&\!-a_3&\,\,a_2\\ \,\,a_3&0&\!-a_1\\-a_2&\,\,a_1&\,0\end{bmatrix}\begin{bmatrix}b_1\\b_2\\b_3\end{bmatrix}
	// \mathbf{ a } \times \mathbf{ b } = [\mathbf{ b }]_{ \times }^\mathrm T \mathbf{ a } = \begin{ bmatrix }\, 0 & \, \, b_3&\!- b_2\\ - b_3 & 0 & \, \, b_1\\\, \, b_2&\!- b_1&\, 0\end{ bmatrix }\begin{ bmatrix }a_1\\a_2\\a_3\end{ bmatrix }
	// [\mathbf{a}]_{\times} \stackrel{\rm def}{=} \begin{bmatrix}\,\,0&\!-a_3&\,\,\,a_2\\\,\,\,a_3&0&\!-a_1\\\!-a_2&\,\,a_1&\,\,0\end{bmatrix}.
	inline XMMATRIX XM_CALLCONV XMMatrixCrossProduct(FXMVECTOR V)
	{
		// negate v
		XMVECTOR v = XMVectorSelect(g_XMSelect1110.v, V, g_XMSelect1110.v);
		XMVECTOR nv = XMVectorNegate(v);
		XMMATRIX m;
		m.r[0] = _DXMEXT XMVectorPermute<3, 2, 1 + 4, 3 + 4>(v, nv); // [0, a3, -a2]
		m.r[1] = _DXMEXT XMVectorPermute<2 + 4, 3 + 4, 0, 3>(v, nv);	 // [-a3, 0, a1]
		m.r[2] = _DXMEXT XMVectorPermute<1, 0 + 4, 3, 3>(v, nv);		 // [a2,-a1, 0 ]
		m.r[3] = XMVectorZero();
		return m;
	}

	bool XMMatrixCrossProductTest()
	{
		XMVECTOR v1 = XMVectorSet(0, 1, 0, 0);
		v1 = XMVector3Rotate(v1, XMQuaternionRotationRollPitchYaw(0.01, 0.01, 0.5));

		XMVECTOR v2 = XMVectorSet(0, 0.01, 0, 0);
		XMVECTOR right = XMVector3Cross(v1, v2);

		XMMATRIX m1 = XMMatrixCrossProduct(v1);
		XMVECTOR result = XMVector3TransformNormal(v2, m1);

		return XMVector3NearEqual(result, right, XMVectorReplicate(0.001f));
	}
}

namespace Test
{
	using namespace DirectX;
	using namespace std;

	ostream& operator<< (ostream& os, const Matrix4x4& m)
	{
		os << m.m[0][0] << ',' << m.m[0][1] << ',' << m.m[0][2] << ',' << m.m[0][3] << endl;
		os << m.m[1][0] << ',' << m.m[1][1] << ',' << m.m[1][2] << ',' << m.m[1][3] << endl;
		os << m.m[2][0] << ',' << m.m[2][1] << ',' << m.m[2][2] << ',' << m.m[2][3] << endl;
		os << m.m[3][0] << ',' << m.m[3][1] << ',' << m.m[3][2] << ',' << m.m[3][3] << endl;
		return os;
	};

	void QuaternionMultiplyTest(const Quaternion &q, const Vector3 &v, const Quaternion &ldq);

	float randf()
	{
		float r = (float)rand();
		r /= (float)(RAND_MAX);
		return r;
	}

	float rand2pi()
	{
		return randf() * XM_2PI - XM_PI;
	}

	float randpi()
	{
		return randf() * XM_PI - XM_PIDIV2;
	}

	bool QuaternionEulerTest()
	{
		XMMatrixCrossProductTest();
		// Patch Yaw Roll = (1.4,0.8,0.4)
		Vector3 v(0, 1, 0);
		Quaternion ldq;
		Quaternion q = XMQuaternionRotationRollPitchYaw(0.01, 0.01, 0.5);
		//Quaternion ldq(0.001, 0.0, 0.0, 0);
		//QuaternionMultiplyTest(q, v, ldq);
		//cout << "=====" << endl;

		ldq = Quaternion(0.0, 0.001, 0.0, 0);
		QuaternionMultiplyTest(q, v, ldq);
		cout << "=====" << endl;

		v = XMVector3Rotate(v, q);
		q = XMQuaternionIdentity();

		XMVECTOR v0 = XMVector3Rotate(v, XMVectorSet(-0.000244786148,0.000958836172,6.69062138e-06,0.999999464));
		XMVECTOR v1 = XMVector3Rotate(v, XMVectorSet(0.000000000,0.000999999815,0.000000000,0.999999523));
		v0 -= v;
		v1 -= v;

		ldq = Quaternion(0.001, 0.0, 0.0, 0);
		QuaternionMultiplyTest(q, v, ldq);
		cout << "=====" << endl;
		ldq = Quaternion(0.0, 0.001, 0.0, 0);
		QuaternionMultiplyTest(q, v, ldq);
		cout << "=====" << endl;
		return true;
	}

	void QuaternionMultiplyTest(const Quaternion &q, const Vector3 &v, const Quaternion &ldq)
	{
		XMVECTOR axis;
		float ang;
		XMQuaternionToAxisAngle(&axis, &ang, q);
		axis = XMVector3Normalize(axis);
		ang *= 0.5;

		cout << "q = " << q << endl;

		Quaternion lq = XMQuaternionLn(q);
		cout << "ln(q) = " << lq << endl;

		cout << "dlnq = d(ln(q)) = " << ldq << endl;
		Quaternion dq = XMQuaternionExp(ldq);
		cout << "dq = exp(d(ln(q))) = " << dq << endl;

		dq = XMQuaternionExp((lq + ldq));
		cout << "exp(ln(q)+d(ln(q))) = " << dq << endl;
		dq = XMQuaternionMultiply(XMQuaternionConjugate(q), dq);
		cout << "dq = q^-1 * exp(ln(q)+d(ln(q))) = " << dq << endl;

		cout << "||dq|| =" << XMVectorGetX(XMVector3Length(dq)) << endl;

		float sinadiva = 1.0f;
		if (fabs(ang) > std::numeric_limits<float>::epsilon())
			sinadiva = sin(ang) / ang;

		Quaternion estimate = sinadiva * (cos(ang) * (XMVECTOR)ldq + sin(ang)*XMVector3Cross(axis, ldq));
		Quaternion another = XMVector3Dot(ldq, axis);
		//another *= axis;
		//estimate += another;

		cout << "estimate = " << estimate << endl;

		XMVECTOR cosA = XMVectorSplatW(q);
		XMVECTOR sinA = XMVectorSqrt(g_XMOne.v - cosA * cosA);

		//Matrix4x4 jac = XMVectorGetX(sinA) * XMMatrixCrossProduct(axis) + XMMatrixScalingFromVector(cosA);

		Matrix4x4 jac = sinadiva* (sin(ang)* XMMatrixCrossProduct(axis) + cos(ang) * XMMatrixIdentity());
		cout << "jaccobi == " << endl << jac << endl;
		Quaternion estimate2 = XMVector3TransformNormal(ldq, jac);
		cout << "estimate2 = " << estimate2 << endl;

		Vector3 qv = XMVector3Rotate(v, q);
		cout << "qv = " << qv << endl;

		Matrix4x4 derv = -XMMatrixCrossProduct(qv);
		cout << endl << "analatic derv d(qv)/d(q) == " << endl << derv;

		derv = jac * derv;
		cout << endl << "overall derv == " << endl << derv ;

		Vector3 derest = Vector3::TransformNormal(Vector3(ldq), derv);
		derest *= XMVector3ReciprocalLength(ldq) * 2;
		cout << "new analatic derv = " << derest << endl;

		Vector3 dotvdq = XMVector3Dot(qv, dq);
		Vector3 crossvdq = XMVector3Cross(qv, dq);
		cout << "dot(qv,dq) = " << dotvdq << endl;
		cout << "cross(qv) = " << crossvdq << endl;

		Vector3 dif = XMVector3Rotate(qv, dq) - (XMVECTOR)qv;
		dif *= XMVector3ReciprocalLength(dq);

		cout << "d(rv)/dq = " << dif << endl;

		lq = XMQuaternionMultiply(q, dq);
		cout << "q * dq= " << lq << endl;

		lq = XMQuaternionMultiply(dq, q);
		cout << "dq * q= " << lq << endl;
	}

	bool InverseKinematicsTest()
	{
		QuaternionEulerTest();
		using namespace DirectX;

		ChainInverseKinematics cik(3);
		cik.bone(0) = Vector4(0, 1.0f, 0, 1.0f);
		cik.bone(1) = Vector4(0, 1.0f, 0, 1.0f);
		cik.bone(2) = Vector4(0, 1.0f, 0, 1.0f);
		//cik.m_boneMinLimits[0].x = 0;
		//cik.m_boneMaxLimits[0].x = 0;
		cik.minRotation(1).y = 0;
		cik.maxRotation(1).y = 0;
		cik.minRotation(2).y = 0;
		cik.maxRotation(2).y = 0;
		//cik.computeJointWeights();
		vector<Quaternion> rotations(3);
		for (size_t i = 0; i < 1; i++)
		{
			rotations[0] = XMQuaternionRotationRollPitchYaw(0.01, 0, 0.01);
			rotations[1] = XMQuaternionRotationRollPitchYaw(0.01, 0, 0.5);
			rotations[2] = XMQuaternionRotationRollPitchYaw(0.5, 0.01, 0.5);
			Vector3 goal = XMVectorSet(1.0f + randf(), randf(), randf(), 0);
			auto code = cik.solve(goal, rotations);
			Vector3 achieved = cik.endPosition(rotations);
			cout << "ik test : goal = " << goal << " ; achieved position = " << achieved << endl;
		}
		cout << "rotations = {" << std::endl;
		for (size_t i = 0; i < 3; i++)
		{
			cout << "  " << rotations[i] << std::endl;
		}
		cout << '}' << endl;

		return true;
	}

	REGISTER_TEST_METHOD(InverseKinematicsTest,InverseKinematicsTest);
}

namespace Internal
{
	// Generic functor
	template<typename _Scalar, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>
	struct Functor
	{
		typedef _Scalar Scalar;
		enum {
			InputsAtCompileTime = NX,
			ValuesAtCompileTime = NY
		};
		typedef Eigen::Matrix<Scalar, InputsAtCompileTime, 1> InputType;
		typedef Eigen::Matrix<Scalar, ValuesAtCompileTime, 1> ValueType;
		typedef Eigen::Matrix<Scalar, ValuesAtCompileTime, InputsAtCompileTime> JacobianType;

		int m_inputs, m_values;

		Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
		Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

		int inputs() const { return m_inputs; }
		int values() const { return m_values; }

	};
}
namespace Causality
{
	static void DecodeRotationsEuler(const Eigen::VectorXf &x, Causality::array_view<DirectX::SimpleMath::Quaternion> rotations)
	{
		int n = rotations.size();
		for (int i = 0; i < n; i++)
		{
			auto euler = x.segment<3>(i * 3);
			rotations[i] = DirectX::XMQuaternionRotationRollPitchYaw(euler[0], euler[1], euler[2]);
		}
	}

	static void EncodeRotationsEuler(Eigen::VectorXf &x, Causality::array_view<DirectX::SimpleMath::Quaternion> rotations)
	{
		int n = rotations.size();
		for (int i = 0; i < n; i++)
		{
			auto euler = x.segment<3>(i * 3);
			auto dxeuler = DirectX::XMQuaternionEulerAngleYawPitchRoll(rotations[i]);
			DirectX::XMStoreFloat3(euler.data(), dxeuler);
		}
	}

	static void DecodeRotationsLnQ(const Eigen::VectorXf &x, Causality::array_view<DirectX::SimpleMath::Quaternion> rotations)
	{
		int n = rotations.size();
		for (int i = 0; i < n; i++)
		{
			auto lnq = x.segment<3>(i * 3);
			rotations[i] = DirectX::XMQuaternionExp(DirectX::XMLoadFloat3(lnq.data()));
		}
	}

	static void EncodeRotationsLnQ(Eigen::VectorXf &x, Causality::array_view<const DirectX::SimpleMath::Quaternion> rotations)
	{
		int n = rotations.size();
		for (int i = 0; i < n; i++)
		{
			auto lnq = x.segment<3>(i * 3);
			auto dxlnq = DirectX::XMQuaternionLn(rotations[i]);
			DirectX::XMStoreFloat3(lnq.data(), dxlnq);
		}
	}

	static inline void DecodeRotations(const Eigen::VectorXf &x, Causality::array_view<DirectX::SimpleMath::Quaternion> rotations)
	{
		if (RotationEncodeMethod == EulerAngles)
			DecodeRotationsEuler(x, rotations);
		else
			DecodeRotationsLnQ(x, rotations);
	}

	static inline void EncodeRotations(Eigen::VectorXf &x, Causality::array_view<DirectX::SimpleMath::Quaternion> rotations)
	{
		if (RotationEncodeMethod == EulerAngles)
			EncodeRotationsEuler(x, rotations);
		else
			EncodeRotationsLnQ(x, rotations);
	}

}

struct ChainInverseKinematics::OptimizeFunctor : public Internal::Functor<float>
{
	const ChainInverseKinematics&	ik;
	size_t							n;
	Vector3						                                    m_goal;
	float						                                    m_limitPanalty;
	Eigen::Map<const Eigen::VectorXf>                               m_min;
	Eigen::Map<const Eigen::VectorXf>                               m_max;

	Eigen::VectorXf					                                m_ref;
	float							                                m_refWeights;
	bool							                                m_useRef;

	mutable std::vector<DirectX::Quaternion, DirectX::XMAllocator>	m_rots;
	mutable std::vector<DirectX::Vector3, DirectX::XMAllocator>	m_jac;

	void fillRotations(const InputType &x) const
	{
		assert(x.size() == 3 * n);
		DecodeRotations(x, m_rots);
	}

	OptimizeFunctor(const ChainInverseKinematics& _ik, const Vector3 & goal)
		: ik(_ik), n(_ik.size()),
		Internal::Functor<float>(3 * _ik.size(), 3 + 3 * _ik.size()),
		m_rots(_ik.size()),
		m_jac(_ik.size() * 3),
		m_goal(goal),
		m_ref(_ik.size() * 3),
		m_min(&_ik.m_boneMinLimits[0].x, 3 * _ik.size()),
		m_max(&_ik.m_boneMaxLimits[0].x, 3 * _ik.size()),
		m_limitPanalty(1000.0f),
		m_refWeights(.0f),
		m_useRef(false)
	{
	}

	void setGoal(const Vector3 & goal)
	{
		m_goal = goal;
	}

	template <class Derived>
	void setReference(const Eigen::DenseBase<Derived>& refernece, float referenceWeight)
	{
		m_ref = refernece;
		m_refWeights = referenceWeight;
		m_useRef = true;
	}

	void disableRef()
	{
		m_useRef = false;
		m_refWeights = .0f;
	}

	int operator()(const InputType &x, ValueType& fvec) const {
		fillRotations(x);
		Vector3 v = ik.endPosition(m_rots);
		v -= m_goal;

		assert(fvec.size() == 3 + x.size());

		fvec.setZero();
		fvec.head<3>() = Eigen::Vector3f::Map(&v.x);

		// limit-exceed panelaty
		auto limpanl = fvec.tail(x.size());
		for (int i = 0; i < 3 * n; i++)
		{
			if (x[i] < m_min[i])
				limpanl[i] = m_limitPanalty*(x[i] - m_min[i])*(x[i] - m_min[i]);
			else if (x[i] > m_max[i])
				limpanl[i] = m_limitPanalty*(x[i] - m_max[i])*(x[i] - m_max[i]);
		}

		if (m_useRef)
		{
			limpanl += m_refWeights *(x - m_ref);
		}

		return 0;
	}

	int df(const InputType &x, JacobianType& fjac) {
		fillRotations(x);
		fjac.setZero();

		if (RotationEncodeMethod == EulerAngles)
			ik.endPositionJaccobiRespectEuler(m_rots, m_jac);
		else
		{
			ik.endPositionJaccobiRespectLnQuaternion(m_rots, m_jac);
		}

		auto jacb = fjac.topRows<3>();
		jacb = Eigen::Matrix3Xf::Map(&m_jac[0].x, 3, 3 * n);

		// limit-exceed panelaty
		for (int i = 0; i < 3 * n; i++)
		{
			if (x[i] < m_min[i])
				fjac(3 + i, i) = m_limitPanalty * (x[i] - m_min[i]);
			else if (x[i] > m_max[i])
				fjac(3 + i, i) = m_limitPanalty * (x[i] - m_max[i]);

			if (m_useRef)
			{
				fjac(3 + i, i) += m_refWeights;
			}
		}

		//fjac.topRows<3>() = m_jac;
		return 0;
	}
};

ChainInverseKinematics::ChainInverseKinematics(size_t n)
	: ChainInverseKinematics()
{
	resize(n);
}

ChainInverseKinematics::ChainInverseKinematics()
{
	m_tol = 5e-4;
	m_maxItrs = 200;
}

void ChainInverseKinematics::resize(size_t n)
{
	m_bones.resize(n);
	m_boneMinLimits.resize(n);
	m_boneMaxLimits.resize(n);
	m_jointWeights.resize(n);

	using namespace DirectX;
	Vector3 dlim(XM_PIDIV2, XM_PI, XM_PI);
	for (int i = 0; i < n; i++)
	{
		m_boneMinLimits[i] = -dlim;
		m_boneMaxLimits[i] = dlim;
		m_jointWeights[i] = Vector3(1.0f);
	}
}

void ChainInverseKinematics::computeJointWeights()
{
	using namespace Math;
	XMVECTOR V;
	XMVECTOR LV = XMVectorZero();
	for (int i = m_bones.size() - 1; i >= 0; --i)
	{
		V = XMLoadA(m_bones[i]);
		V = XMVector3Length(V);
		LV += V;
		m_jointWeights[i] = LV;
	}
}

// Jaccobbi from a rotation radius vector (r) respect to a small rotation dr = (drx,dry,drz) in global reference frame
// [\mathbf{a}]_{\times} \stackrel{\rm def}{=} \begin{bmatrix}\,\,0&\!-a_3&\,\,\,a_2\\\,\,\,a_3&0&\!-a_1\\\!-a_2&\,\,a_1&\,\,0\end{bmatrix}.
void ChainInverseKinematics::jacobbiRespectAxisAngle(Matrix4x4 & j, const float * r)
{
	j._11 = 0, j._12 = -r[2], j._13 = r[1];
	j._21 = r[2], j._22 = 0, j._23 = -r[0];
	j._31 = -r[1], j._32 = r[0], j._33 = 0;
}

// Roll-Patch-Yaw
XMMATRIX XM_CALLCONV ChainInverseKinematics::jacobbiTransposeRespectEuler(const Vector3 & rv, const Vector3& euler, FXMVECTOR globalRot)
{
	using namespace DirectX;

	// Jaccobi to Euler
	// J = Ry*Jy + Ry*Rx*Jx + Ry*Rx*Rz*Jz

	XMVECTOR V;
	XMMATRIX MJ;
	XM_ALIGNATTR Vector4 v, r = rv;

	XMVECTOR Q = globalRot;
	XMVECTOR lQ;

	lQ = XMQuaternionRotationRoll(euler.z);
	Q = XMQuaternionMultiply(lQ, Q); // Q = base * roll
	V = XMVectorSet(-r.y, r.x ,0 ,0);
	V = XMVector3Rotate(V, Q);
	MJ.r[2] = V;
	r = XMVector3Rotate(r, lQ);

	lQ = XMQuaternionRotationPatch(euler.x);
	Q = XMQuaternionMultiply(lQ, Q); // Q = base * roll * patch
	V = XMVectorSet(0,-r.z,r.y,0);
	V = XMVector3Rotate(V, Q);
	MJ.r[0] = V;
	r = XMVector3Rotate(r, lQ);

	lQ = XMQuaternionRotationYaw(euler.y);
	Q = XMQuaternionMultiply(lQ, Q); // Q = base * roll * patch * yaw
	V = XMVectorSet(r.z,0,-r.x,0);
	V = XMVector3Rotate(V, Q);
	MJ.r[1] = V;

	//MJ = XMMatrixTranspose(MJ);
	return MJ;
}

XMMATRIX XM_CALLCONV Causality::ChainInverseKinematics::jacobbiTransposeRespectAxisAngle(const Vector3 & rv, FXMVECTOR qrot, FXMVECTOR globalRot)
{
	using namespace DirectX;

	XMVECTOR V;
	XMMATRIX MJ;
	XM_ALIGNATTR Matrix4x4 jac;
	XM_ALIGNATTR Vector4 r;
	V = XMVector3Rotate(rv, qrot);
	XMStoreA(r, V);
	XMVECTOR Q = globalRot; //XMQuaternionMultiply(qrot, globalRot);

	//r.v = rv;
	//XMVECTOR Q = XMQuaternionMultiply(qrot, globalRot);

	jacobbiRespectAxisAngle(jac, &r.x);

	// Rotate each row of the matrix
	V = XMLoadFloat4A(jac.m[0]);
	V = XMVector3Rotate(V, Q);
	MJ.r[0] = V;
	V = XMLoadFloat4A(jac.m[1]);
	V = XMVector3Rotate(V, Q);
	MJ.r[1] = V;
	V = XMLoadFloat4A(jac.m[2]);
	V = XMVector3Rotate(V, Q);
	MJ.r[2] = V;
	MJ.r[3] = XMVectorZero();

	XMVECTOR axis = XMVector3Normalize(qrot);
	XMVECTOR cosA = XMVectorSplatW(qrot);
	XMVECTOR sinA = XMVectorSqrt(g_XMOne.v - cosA * cosA);
	XMVECTOR sinc_a = g_XMOne.v - (g_XMOne.v - cosA) / 3.0; // sinc(x) ~= 1 - x^2/6 == 1 - (1-cos(x))/3

	cosA *= sinc_a;
	sinA *= sinc_a;
	XMMATRIX jacq_lnq = XMVectorGetX(sinA) * XMMatrixCrossProduct(axis) + XMMatrixScalingFromVector(cosA);
	//cout << "jaccobi == " << endl << jac << endl;
	MJ = jacq_lnq * MJ;

	return MJ;
}

XMVECTOR Causality::ChainInverseKinematics::endPosition(array_view<const Quaternion> rotations) const
{
	using namespace DirectX;

	const auto n = m_bones.size();

	XMVECTOR q, t, gt, gq;
	Eigen::Vector4f qs;
	qs.setZero();

	gt = XMVectorZero();

	for (int i = n - 1; i >= 0; i--)
	{
		q = XMLoadA(rotations[i]);
		t = XMLoadA(m_bones[i]);
		gt += t;
		gt = XMVector3Rotate(gt, q);
	}

	return gt;
}

// rotations must be aligned

void ChainInverseKinematics::endPositionJaccobiRespectEuler(array_view<const Quaternion> rotations, array_view<Vector3> jacb) const
{
	using namespace Eigen;
	using namespace DirectX;
	const auto n = m_bones.size();

	// Chain Position Vectors
	auto rad = getRadiusVectors(rotations);

	XMVECTOR gq = XMQuaternionIdentity();
	for (int i = 0; i < n; i++)
	{
		XMVECTOR q = XMLoadA(rotations[i]);
		Vector3 eular = XMQuaternionEulerAngleYawPitchRoll(q);

		auto& r = reinterpret_cast<Vector3&>(rad[i]);
		XMMATRIX jac = jacobbiTransposeRespectEuler(r, eular, gq);

		XMStoreFloat3x3(reinterpret_cast<XMFLOAT3X3*>(&jacb[i * 3].x), jac);

		gq = XMQuaternionMultiply(q, gq);
	}

	//return jacb;
}

void ChainInverseKinematics::endPositionJaccobiRespectAxisAngle(array_view<const Quaternion> rotations, array_view<Vector3> jacb) const
{
	using namespace Eigen;
	using namespace DirectX;
	const auto n = m_bones.size();

	// Chain Position Vectors
	auto rad = getRadiusVectors(rotations);

	XMVECTOR gq = XMQuaternionIdentity();
	for (int i = 0; i < n; i++)
	{
		XMVECTOR q = XMLoadA(rotations[i]);

		auto& r = reinterpret_cast<Vector3&>(rad[i]);
		XMMATRIX jac = jacobbiTransposeRespectAxisAngle(r, q, gq);

		XMStoreFloat3x3(reinterpret_cast<XMFLOAT3X3*>(&jacb[i * 3].x), jac);

		gq = XMQuaternionMultiply(q, gq);
	}
}

void ChainInverseKinematics::endPositionJaccobiRespectLnQuaternion(array_view<const Quaternion> rotations, array_view<Vector3> jacb) const
{
	endPositionJaccobiRespectAxisAngle(rotations, jacb);
	// d(lnQ) == 0.5 * d(axis*angle), thus we need to apply this factor back
	for (auto& v : jacb)
		v *= 2.0f;
}

std::vector<Vector4, DirectX::XMAllocator> ChainInverseKinematics::getRadiusVectors(array_view<const Quaternion> &rotations) const
{
	using namespace DirectX;

	const auto n = m_bones.size();
	std::vector<DirectX::Vector4, DirectX::XMAllocator>	rad(n);

	XMVECTOR q, t, gt;

	gt = XMVectorZero();
	t = XMVectorZero();

	for (int i = n - 1; i >= 0; i--)
	{
		q = XMLoadA(rotations[i]);
		t = XMLoadA(m_bones[i]);
		gt += t;
		rad[i] = gt;
		gt = XMVector3Rotate(gt, q);
	}

	return rad;
}

bool XM_CALLCONV ChainInverseKinematics::solve(FXMVECTOR goal, array_view<Quaternion> rotations) const
{
	auto n = m_bones.size();
	Eigen::VectorXf x(n * 3);
	EncodeRotations(x, rotations);
	//m_boneMinLimits += x;
	//m_boneMinLimits += x;

	//std::cout << "init x = " << x.transpose() << std::endl;
	Vector3 vgoal = goal;
	OptimizeFunctor functor(*this, vgoal);

	typedef OptimizeFunctor DfFunctor;
	//typedef Eigen::NumericalDiff<OptimizeFunctor> DfFunctor;

	Eigen::MatrixXf aJac(functor.values(), functor.inputs());
	Eigen::MatrixXf nJac(functor.values(), functor.inputs());
	Eigen::VectorXf ep(functor.values());

#if defined(_DEBUG_JACCOBI) && _DEBUG_JACCOBI
	Eigen::NumericalDiff<OptimizeFunctor> ndffunctor(functor);
	functor(x, ep);
	functor.df(x, aJac);
	ndffunctor.df(x, nJac);

	std::cout << "Numberic Jaccobi : " << std::endl << nJac.topRows(3) << std::endl;
	std::cout << "Analatic Jaccobi : " << std::endl << aJac.topRows(3) << std::endl;
#endif

	Eigen::LevenbergMarquardt<DfFunctor, float> lm(functor);
	lm.parameters.maxfev = m_maxItrs;
	lm.parameters.xtol = m_tol;
	lm.parameters.ftol = m_tol;
	lm.parameters.gtol = m_tol;

	auto code = lm.minimize(x);
	std::cout << "iteration = " << lm.iter << std::endl;
	std::cout << "ret = " << code << std::endl;
	std::cout << "x = " << x.transpose() << std::endl;

	//lm.minimizeInit(x);

	//if (code != LevenbergMarquardtSpace::Status::CosinusTooSmall)

	DecodeRotations(x, rotations);

	return true;
}

bool XM_CALLCONV ChainInverseKinematics::solveWithStyle(FXMVECTOR goal, array_view<Quaternion> rotations, array_view<Quaternion> styleReference, float styleReferenceWeight) const
{
	auto n = m_bones.size();
	Eigen::VectorXf x(n * 3);
	Eigen::VectorXf ref(n * 3);
	EncodeRotations(x, rotations);
	EncodeRotations(ref, styleReference);

	OptimizeFunctor functor(*this, goal);
	functor.setReference(ref, styleReferenceWeight);

	Eigen::LevenbergMarquardt<OptimizeFunctor, float> lm(functor);
	lm.parameters.maxfev = m_maxItrs;
	lm.parameters.xtol = m_tol;
	lm.parameters.ftol = m_tol;
	lm.parameters.gtol = m_tol;

	auto code = lm.minimize(x);

	DecodeRotations(x, rotations);

	return true;
}


// convenience.cpp

// Helper functions
#include "convenience.h"
#include "parameters.h"
#include <boost/math/special_functions/gamma.hpp>
#include <cmath>
#include <iostream>
using namespace std;
using boost::math::lgamma;
using boost::math::gamma_p;

vector<double> seq(double x0, double x1, double by)
{
    int steps = int((x1 - x0) / by + by * 1e-3);
    if (steps >= 0)
    {
        vector<double> ret(steps);
        for (int i = 0; i < steps; ++i)
            ret[i] = x0 + by * i;
        return ret;
    }
    return vector<double>(0);
}

double gamma_P(double x, double shape, double scale)
{
    return boost::math::gamma_p(x / scale, shape);
}

// binomial log density
double binom(double k, double n, double p)
{
    return lgamma(n + 1.) - lgamma(k + 1.) - lgamma(n - k + 1.) + k * log(p) + (n - k) * log(1. - p);
}

// negative binomial log density
double nbinom(unsigned int x, double mean, double size)
{
    double k = x;
    double p = size / (size + mean);
    double n = mean * p / (1 - p);

    return lgamma(n + k) - lgamma(k + 1) - lgamma(n) + n * log(p) + k * log(1 - p);
}

// negative binomial log density with retrospective confirmation
double nbinom_gammaconf(unsigned int x, double mean, double size, double days_ago, double conf_delay_mean, double conf_delay_shape)
{
    double conf_delay_scale = conf_delay_mean / conf_delay_shape;
    double prop_confirmed = gamma_P(days_ago, conf_delay_shape, conf_delay_scale);
    return nbinom(x, mean * prop_confirmed, size);
}

// construct a delay distribution following a gamma distribution with mean mu and shape parameter shape.
vector<double> delay_gamma(double mu, double shape, double t_max, double t_step, double mult)
{
    double scale = mu / shape;
    vector<double> height;

    for (double t = 0.0; t < t_max + 0.5 * t_step; t += t_step)
        height.push_back(mult * (gamma_P(t + t_step/2, shape, scale) -
            gamma_P(max(0.0, t - t_step/2), shape, scale)));
    return height;
}

// estimate the basic reproduction number
double estimate_R0(Parameters& P, double t, unsigned int p, unsigned int iter)
{
    vector<double> inf(P.pop[p].size.size(), 1.0);
    vector<double> inf2 = inf;

    double dIp = P.pop[p].dIp.Mean() * P.time_step;
    double dIs = P.pop[p].dIs.Mean() * P.time_step;
    double dIa = P.pop[p].dIa.Mean() * P.time_step;

    double n_inf = P.pop[p].size.size();
    double n_inf2 = 0;
    double R0 = 1;

    double seas = 1.0 + P.pop[p].season_A * cos(2. * M_PI * (t - P.pop[p].season_phi) / P.pop[p].season_T);

    for (unsigned int i = 0; i < iter; ++i)
    {
        n_inf2 = 0;
        for (unsigned int a = 0; a < inf.size(); ++a)
        {
            inf2[a] = 0;
            for (unsigned int b = 0; b < inf.size(); ++b)
            {
                inf2[a] += inf[b] * P.pop[p].cm(a, b) * P.pop[p].u[a] * seas * (
                    P.pop[p].y[b] * (P.pop[p].fIp[b] * dIp + P.pop[p].fIs[b] * dIs) +
                    (1 - P.pop[p].y[b]) * P.pop[p].fIa[b] * dIa
                );
            }
            n_inf2 += inf2[a];
        }

        R0 = n_inf2 / n_inf;

        swap(n_inf2, n_inf);
        swap(inf2, inf);
    }

    return R0;
}

// estimate the effective reproduction number
double estimate_Rt(Parameters& P, Reporter& dyn, double t, unsigned int p, unsigned int iter)
{
    vector<double> inf(P.pop[p].size.size(), 1.0);
    vector<double> inf2 = inf;
    vector<double> S(P.pop[p].size.size(), 0.0);

    for (unsigned int a = 0; a < S.size(); ++a)
        S[a] = dyn(t, p, a, 0) / P.pop[p].size[a];

    double dIp = P.pop[p].dIp.Mean() * P.time_step;
    double dIs = P.pop[p].dIs.Mean() * P.time_step;
    double dIa = P.pop[p].dIa.Mean() * P.time_step;

    double n_inf = P.pop[p].size.size();
    double n_inf2 = 0;
    double Rt = 1;

    double seas = 1.0 + P.pop[p].season_A * cos(2. * M_PI * (t - P.pop[p].season_phi) / P.pop[p].season_T);

    for (unsigned int i = 0; i < iter; ++i)
    {
        n_inf2 = 0;
        for (unsigned int a = 0; a < inf.size(); ++a)
        {
            inf2[a] = 0;
            for (unsigned int b = 0; b < inf.size(); ++b)
            {
                inf2[a] += S[a] * inf[b] * P.pop[p].cm(a, b) * P.pop[p].u[a] * seas * (
                    P.pop[p].y[b] * (P.pop[p].fIp[b] * dIp + P.pop[p].fIs[b] * dIs) +
                    (1 - P.pop[p].y[b]) * P.pop[p].fIa[b] * dIa
                );
            }
            n_inf2 += inf2[a];
        }

        Rt = n_inf2 / n_inf;

        swap(n_inf2, n_inf);
        swap(inf2, inf);
    }

    return Rt;
}

// clamp a number between two limits
double clamp(double x, double x0, double x1)
{
    return min(max(x, x0), x1);
}

// smootherstep function
double smootherstep(double x0, double x1, double y0, double y1, double x)
{
    x = clamp((x - x0) / (x1 - x0));
    return y0 + x * x * x * (x * (x * 6. - 15.) + 10.) * (y1 - y0);
}