//!
//! Contains the interface for the Gaussian distribution.
//!
//! \file distrib/gaussian.hpp
//! \author Lachlan McCalman
//! \date February 2014
//! \license Affero General Public License version 3 or later
//! \copyright (c) 2014, NICTA
//!

#pragma once

#include <Eigen/Dense>
#include <cmath>

namespace obsidian
{
  namespace distrib
  {
    //! Compute the log Gaussian distribution PDF with a diagonal covariance matrix.
    //!
    //! \param x The vector to evaluate the distribution at.
    //! \param mu The mean vector containing the means of each dimension.
    //! \param sigma The diagonal of the covariance matrix.
    //!
    double logGaussian(const Eigen::VectorXd& x, const Eigen::VectorXd& mu, const Eigen::VectorXd& sigma)
    {
      double coeff = -std::log(std::pow(2.0 * M_PI, x.size() * 0.5) * sigma.prod());
      return coeff - 0.5 * (x - mu).cwiseQuotient(sigma).squaredNorm();
    }
  }
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2007 François du Vignaud

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program 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 license for more details.
*/

#ifndef quantlib_tap_correlations_hpp
#define quantlib_tap_correlations_hpp

#include <ql/types.hpp>
#include <ql/utilities/disposable.hpp>
#include <ql/math/matrix.hpp>
#include <ql/math/optimization/costfunction.hpp>
#include <boost/function.hpp>
#include <vector>

namespace QuantLib {

    //! Returns the Triangular Angles Parametrized correlation matrix
    /*! The matrix \f$ m \f$ is filled with values corresponding to angles
        given in the \f$ angles \f$ vector. See equation (24) in
        "Parameterizing correlations: a geometric interpretation"
        by Francesco Rapisarda, Damiano Brigo, Fabio Mercurio

        \test
        - the correctness of the results is tested by reproducing
          known good data.
        - the correctness of the results is tested by checking
          returned values against numerical calculations.
    */
    Disposable<Matrix>
    triangularAnglesParametrization(const Array& angles,
                                    Size matrixSize,
                                    Size rank);

    Disposable<Matrix>
    lmmTriangularAnglesParametrization(const Array& angles,
                                       Size matrixSize,
                                       Size rank);

    // the same function using the angles parameterized by the following
    // transformation \f[ \teta_i = \frac{\Pi}{2} - arctan(x_i)\f]
    Disposable<Matrix>
    triangularAnglesParametrizationUnconstrained(const Array& x,
                                                 Size matrixSize,
                                                 Size rank);

    Disposable<Matrix>
    lmmTriangularAnglesParametrizationUnconstrained(const Array& x,
                                                    Size matrixSize,
                                                    Size rank);

    //! Returns the rank reduced Triangular Angles Parametrized correlation matrix
    /*! The matrix \f$ m \f$ is filled with values corresponding to angles
        corresponding  to the 3D spherical spiral paramterized by
        \f$ alpha \f$, \f$ t0 \f$, \f$ epsilon \f$ values. See equation (32) in
        "Parameterizing correlations: a geometric interpretation"
        by Francesco Rapisarda, Damiano Brigo, Fabio Mercurio

        \test
        - the correctness of the results is tested by reproducing
          known good data.
        - the correctness of the results is tested by checking
          returned values against numerical calculations.
    */
    Disposable<Matrix>
    triangularAnglesParametrizationRankThree(Real alpha,
                                             Real t0,
                                             Real epsilon,
                                             Size nbRows);

    // the same function with parameters packed in an Array
    Disposable<Matrix>
    triangularAnglesParametrizationRankThreeVectorial(const Array& paramters,
                                                      Size nbRows);

    // Cost function associated with Frobenius norm.
    // <http://en.wikipedia.org/wiki/Matrix_norm>
    class FrobeniusCostFunction : public CostFunction{
      public:
        FrobeniusCostFunction(
            const Matrix& target,
            const boost::function<Disposable<Matrix>(const Array&,
                                                     Size,
                                                     Size)>& f,
                                                     Size matrixSize,
                                                     Size rank)
        : target_(target), f_(f), matrixSize_(matrixSize), rank_(rank) {}
        Real value (const Array &x) const;
        Disposable<Array> values (const Array &x) const;
      private:
        Matrix target_;
        boost::function<Disposable<Matrix>(const Array&, Size, Size)> f_;
        Size matrixSize_;
        Size rank_;
    };
}

/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2007 François du Vignaud

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program 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 license for more details.
*/

#include <cmath>

namespace QuantLib {

    inline Disposable<Matrix> triangularAnglesParametrization(const Array& angles,
                                                       Size matrixSize,
                                                       Size rank) {

        // what if rank == 1?
        QL_REQUIRE((rank-1) * (2*matrixSize - rank) == 2*angles.size(),
                    "rank-1) * (matrixSize - rank/2) == angles.size()");
        Matrix m(matrixSize, matrixSize);

        // first row filling
        m[0][0] = 1.0;
        for (Size j=1; j<matrixSize; ++j)
            m[0][j] = 0.0;

        // next ones...
        Size k = 0; //angles index
        for (Size i=1; i<m.rows(); ++i) {
            Real sinProduct = 1.0;
            Size bound = std::min(i,rank-1);
            for (Size j=0; j<bound; ++j) {
                m[i][j] = std::cos(angles[k]);
                m[i][j] *= sinProduct;
                sinProduct *= std::sin(angles[k]);
                ++k;
            }
            m[i][bound] = sinProduct;
            for (Size j=bound+1; j<m.rows(); ++j)
                m[i][j] = 0;
        }
        return m;
    }

    inline Disposable<Matrix> lmmTriangularAnglesParametrization(const Array& angles,
                                                          Size matrixSize,
                                                          Size) {
        Matrix m(matrixSize, matrixSize);
        for (Size i=0; i<m.rows(); ++i) {
            Real cosPhi, sinPhi;
            if (i>0) {
                cosPhi = std::cos(angles[i-1]);
                sinPhi = std::sin(angles[i-1]);
            } else {
                cosPhi = 1.0;
                sinPhi = 0.0;
            }

            for (Size j=0; j<i; ++j)
                m[i][j] = sinPhi * m[i-1][j];

            m[i][i] = cosPhi;

            for (Size j=i+1; j<m.rows(); ++j)
                m[i][j] = 0.0;
        }
        return m;
    }

    inline Disposable<Matrix> triangularAnglesParametrizationUnconstrained(
                                                            const Array& x,
                                                            Size matrixSize,
                                                            Size rank) {
        Array angles(x.size());
        //we convert the unconstrained parameters in angles
        for (Size i = 0; i < x.size(); ++i)
            angles[i] = M_PI*.5 - std::atan(x[i]);
        return triangularAnglesParametrization(angles, matrixSize, rank);
    }

    inline Disposable<Matrix> lmmTriangularAnglesParametrizationUnconstrained(
                                                            const Array& x,
                                                            Size matrixSize,
                                                            Size rank) {
        Array angles(x.size());
        //we convert the unconstrained parameters in angles
        for (Size i = 0; i < x.size(); ++i)
            angles[i] = M_PI*.5 - std::atan(x[i]);
        return lmmTriangularAnglesParametrization(angles, matrixSize, rank);
    }

    inline Disposable<Matrix> triangularAnglesParametrizationRankThree(
                                            Real alpha, Real t0,
                                            Real epsilon, Size matrixSize) {
        Matrix m(matrixSize, 3);
        for (Size i=0; i<m.rows(); ++i) {
            Real t = t0 * (1 - std::exp(epsilon*Real(i)));
            Real phi = std::atan(alpha * t);
            m[i][0] = std::cos(t)*std::cos(phi);
            m[i][1] = std::sin(t)*std::cos(phi);
            m[i][2] = -std::sin(phi);
        }
        return m;
    }

    inline Disposable<Matrix> triangularAnglesParametrizationRankThreeVectorial(
                                                    const Array& parameters,
                                                    Size nbRows) {
        QL_REQUIRE(parameters.size() == 3,
                   "the parameter array must contain exactly 3 values" );
        return triangularAnglesParametrizationRankThree(parameters[0],
                                                        parameters[1],
                                                        parameters[2],
                                                        nbRows);

    }

  inline Real FrobeniusCostFunction::value(const Array& x) const {
        Array temp = values(x);
        return DotProduct(temp, temp);
    }

  inline Disposable<Array> FrobeniusCostFunction::values(const Array& x) const {
        Array result((target_.rows()*(target_.columns()-1))/2);
        Matrix pseudoRoot = f_(x, matrixSize_, rank_);
        Matrix differences = pseudoRoot * transpose(pseudoRoot) - target_;
        Size k = 0;
        // then we store the elementwise differences in a vector.
        for (Size i=0; i<target_.rows(); ++i) {
            for (Size j=0; j<i; ++j){
                result[k] = differences[i][j];
                ++k;
            }
        }
        return result;
    }
}

#endif


/* Main routine to run an implicit 3-node Lobatto finite-difference method
   for solution of a second-order, scalar-valued BVP:

      u'' = p(t)*u' + q(t)*u + r(t),  a<t<b,
      u(a) = ua,  u(b) = ub

   where the problem has stiffness that may be adjusted using
   the real-valued parameter lambda<0 [read from the command line]

   D.R. Reynolds
   Math 6321 @ SMU
   Fall 2020  */

#include <iostream>
#include <iomanip>
#include <vector>
#include <armadillo>
#include "bvp.hpp"

using namespace std;
using namespace arma;

// utility routine to map from physical/component space to linear algebra index space
//    interval:  physical interval index [1 <= interval <= N]
//    location:  location in interval [0=left, 1=midpoint, 2=right]
//    component: solution component at this location [0=u, 1=u']
int idx(int interval, int location, int component) {
  return ( 4*(interval-1) + 2*location + component );
}

// main routine
int main(int argc, char **argv) {

  // get lambda from the command line, otherwise set to -10
  double lambda;
  if (argc > 1)
    lambda = atof(argv[1]);
  if (lambda >= 0.0)
    lambda = -10.0;

  // define BVP object
  BVP bvp(lambda);

  // test 'idx' function by outputting mapping for small N
  cout << "Test output from 'idx' function for N = 5, M = 22:";
  for (int interval=1; interval<=5; interval++) {
    cout << "\n  interval " << interval << ", (loc,comp,idx):";
    for (int location=0; location<=2; location++) {
      for (int component=0; component<=1; component++) {
        cout << "  (" << location << ", " << component << ", "
             << idx(interval,location,component) << ")";
      }
    }
  }

  // loop over spatial resolutions for tests
  vector<int> N = {100, 1000, 10000};
  for (int i=0; i<N.size(); i++) {

    // output problem information
    cout << "\nImplicit Lobatto-3 FD method for BVP with lambda = "
         << lambda << ":" << ",  N = " << N[i] << endl;

    // compute/store analytical solution
    vec t = zeros(N[i]+1);
    t(0) = 0.0;
    t(N[i]) = 1.0;
    for (int j=1; j<=N[i]-1; j++)
      t(j) = 0.5*(1.0-cos((2*j-1)*M_PI/(2*(N[i]-1))));
    vec utrue(N[i]+1);
    for (int j=0; j<=N[i]; j++)
      utrue(j) = bvp.utrue(t(j));

    // set integer for overall linear algebra problem size
    int M = 4*N[i]+2;

    // create matrix and right-hand side vectors
#ifdef USE_SPARSE
    sp_mat A(M,M);
#else
    mat A(M,M);
    A.fill(0.0);
#endif
    vec b(M);
    b.fill(0.0);

    // set up linear system:
    //    recall 'idx' usage: idx(interval,location,component)
    //      interval:  physical interval index [1 <= interval <= N]
    //      location:  location in interval [0=left, 1=midpoint, 2=right]
    //      component: solution component at this location [0=u, 1=u']
    A(0,idx(1,0,0)) = 1.0;
    b(0) = bvp.ua;
    A(1,idx(N[i],2,0)) = 1.0;
    b(1) = bvp.ub;
    int irow = 2;
    for (int j=1; j<=N[i]; j++) {

      // setup interval-specific information
      double tl = t(j-1);
      double tr = t(j);
      double th = 0.5*(tl+tr);
      double h = tr-tl;

      // setup first equation for this interval:
      //    -24*y_{j-1,0} - 5*h*y_{j-1,1} + 24*y_{j-1/2,0} - 8*h*y_{j-1/2,1} + h*y_{j,1} = 0
      A(irow,idx(j,0,0)) = -24.0;
      A(irow,idx(j,0,1)) = -5.0*h;
      A(irow,idx(j,1,0)) = 24.0;
      A(irow,idx(j,1,1)) = -8.0*h;
      A(irow,idx(j,2,1)) = h;
      b(irow) = 0.0;
      irow++;

      // setup second equation for this interval:
      //    -5*h*q_{j-1}*y_{j-1,0} - (24+5*h*p_{j-1})*y_{j-1,1} - 8*h*q_{j-1/2}*y_{j-1/2,0}
      //      + (24-8*h*p_{j-1/2})*y_{j-1/2,1} + h*q_{j}*y_{j,0} + h*p_{j}*y_{j,1} = h*(5*r_{j-1}+8*r_{j-1/2}-r_{j})
      A(irow,idx(j,0,0)) = -5.0*h*bvp.q(tl);
      A(irow,idx(j,0,1)) = -(24.0 + 5.0*h*bvp.p(tl));
      A(irow,idx(j,1,0)) = -8.0*h*bvp.q(th);
      A(irow,idx(j,1,1)) = (24.0-8.0*h*bvp.p(th));
      A(irow,idx(j,2,0)) = h*bvp.q(tr);
      A(irow,idx(j,2,1)) = h*bvp.p(tr);
      b(irow) = h*(5.0*bvp.r(tl) + 8.0*bvp.r(th) - bvp.r(tr));
      irow++;

      // setup third equation for this interval:
      //    -6*y_{j-1,0} - h*y_{j-1,1} - 4*h*y_{j-1/2,1} + 6*y_{j,0} - h*y_{j,1} = 0
      A(irow,idx(j,0,0)) = -6.0;
      A(irow,idx(j,0,1)) = -h;
      A(irow,idx(j,1,1)) = -4.0*h;
      A(irow,idx(j,2,0)) = 6.0;
      A(irow,idx(j,2,1)) = -h;
      b(irow) = 0.0;
      irow++;

      // setup fourth equation for this interval:
      //    -h*q_{j-1}*y_{j-1,0} - (6+h*p_{j-1})*y_{j-1,1} - 4*h*q_{j-1/2}*y_{j-1/2,0} - 4*h*p_{j-1/2}*y_{j-1/2,1}
      //       - h*q_j*y_{j,0} + (6-h*p_j)*y_{j,1} = h*(r_{j-1} + 4*r_{j-1/2} + r_{j})
      A(irow,idx(j,0,0)) = -h*bvp.q(tl);
      A(irow,idx(j,0,1)) = -(6.0 + h*bvp.p(tl));
      A(irow,idx(j,1,0)) = -4.0*h*bvp.q(th);
      A(irow,idx(j,1,1)) = -4.0*h*bvp.p(th);
      A(irow,idx(j,2,0)) = -h*bvp.q(tr);
      A(irow,idx(j,2,1)) = (6.0-h*bvp.p(tr));
      b(irow) = h*(bvp.r(tl) + 4.0*bvp.r(th) + bvp.r(tr));
      irow++;

    }

    // solve linear system for BVP solution
#ifdef USE_SPARSE
    vec y = arma::spsolve(A,b);
#else
    vec y = arma::solve(A,b);
#endif

    // output maximum error
    vec u(N[i]+1);
    for (int j=0; j<=N[i]; j++)
      u(j) = y(idx(j+1,0,0));
    vec uerr = abs(u-utrue);
    cout << "  Maximum BVP solution error = " << std::setprecision(4)
       << uerr.max() << "\n";
  }

  return 0;
}


/*
// Copyright 2018 University of Liege
//
// 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.
//
// Authors:
// - Adrien Crovato
*/

//// Bilinear interpolation
// Interpolate linearly a quantity defined at 4 points to another point
//
// Inputs:
// - x0, x1, x2, x3: x-coordinates of interpolation points
// - y0, y1, y2, y3: y-coordinates of interpolation points
// - z0, z1, z2, z3: z-coordinates of interpolation points
// - s0, s1, s2, s3: quantity to interpolate at interpolation point
// - x, y, z: coordinates of interpolated point
//
// Output:
// - i: interpolated quantity

#include <iostream>
#include <Eigen/Dense>
#include "interp.h"

#define TOL 1e-2 // geometric tolerance on z (measure how far target point is w.r.t. interpolation plane)

using namespace std;
using namespace Eigen;

double interp(double x0, double y0, double z0, double x1, double y1, double z1,
              double x2, double y2, double z2, double x3, double y3, double z3,
              double s0, double s1, double s2, double s3,
              double x, double y, double z) {

    //// Initialization
    Vector3d l, p, n; // unit vector
    Vector3d cg; // CG of planed formed by interpolation points
    Vector2d A, B, C, D, X; // (rotated) interpolation and interpolated points
    Vector2d E, F, G, H; // temporary vector
    double zd, avg; // distance between interpolated point and interpolation plane & plane diagonal
    double k2, k1, k0, Delta; // coefficients of k2*x^2 + k1*x + k0 and discriminant
    double u, v ;// inverse bilinear interpolation parameters
    double i0, i1, i ;// interpolated quantities

    //// Move change of coordinates out of file to improve CPU runtime
    //// Change of axes
    // Plane center
    cg(0) = 0.25*(x0 + x1 + x2 + x3);
    cg(1) = 0.25*(y0 + y1 + y2 + y3);
    cg(2) = 0.25*(z0 + z1 + z2 + z3);
    // Unit vectors
    l(0) = 0.5*(0.5*(x0+x3)-0.5*(x1+x2));
    l(1) = 0.5*(0.5*(y0+y3)-0.5*(y1+y2));
    l(2) = 0.5*(0.5*(z0+z3)-0.5*(z1+z2));
    n(0) = (y2-y0)*(z1-z3) - (z2-z0)*(y1-y3);
    n(1) = (z2-z0)*(x1-x3) - (x2-x0)*(z1-z3);
    n(2) = (x2-x0)*(y1-y3) - (y2-y0)*(x1-x3);
    p = n.cross(l);
    l = l / l.norm(); // normalize
    p = p / p.norm(); // normalize
    n = n / n.norm(); // normalize
    // Translation & Rotation
    A(0) = l(0)*(cg(0)-x0) + l(1)*(cg(1)-y0) + l(2)*(cg(2)-z0);
    A(1) = p(0)*(cg(0)-x0) + p(1)*(cg(1)-y0) + p(2)*(cg(2)-z0);
    B(0) = l(0)*(cg(0)-x1) + l(1)*(cg(1)-y1) + l(2)*(cg(2)-z1);
    B(1) = p(0)*(cg(0)-x1) + p(1)*(cg(1)-y1) + p(2)*(cg(2)-z1);
    C(0) = l(0)*(cg(0)-x2) + l(1)*(cg(1)-y2) + l(2)*(cg(2)-z2);
    C(1) = p(0)*(cg(0)-x2) + p(1)*(cg(1)-y2) + p(2)*(cg(2)-z2);
    D(0) = l(0)*(cg(0)-x3) + l(1)*(cg(1)-y3) + l(2)*(cg(2)-z3);
    D(1) = p(0)*(cg(0)-x3) + p(1)*(cg(1)-y3) + p(2)*(cg(2)-z3);
    X(0) = l(0)*(cg(0)-x) + l(1)*(cg(1)-y) + l(2)*(cg(2)-z);
    X(1) = p(0)*(cg(0)-x) + p(1)*(cg(1)-y) + p(2)*(cg(2)-z);
    // Error measure
    avg = sqrt((x2-x0)*(x2-x0) + (y2-y0)*(y2-y0) + (z2-z0)*(z2-z0));
    zd = n(0)*(x-cg(0)) + n(1)*(y-cg(1)) + n(2)*(z-cg(2));
    #ifdef VERBOSE
    if (abs(zd/avg) > TOL) {
        cout << endl << "Interpolated point is far from interpolation plane; bilinear interpolation might be inaccurate!" << endl;
        cout << "Distance versus interpolation plane diagonal: " << zd << " / " << avg << " > " << TOL << endl;
        cout << "This might be caused by a coarse panelling of the body surface." << endl;
    }
    #endif

    //// Interpolation
    // Intermediate vectors
    E = B-A;
    F = D-A;
    G = (A-B) + (C-D);
    H = X-A;
    // Coefficients
    k2 = G(0)*F(1) - G(1)*F(0); //
    k1 = E(0)*F(1) - E(1)*F(0) + H(0)*G(1) - H(1)*G(0);
    k0 = H(0)*E(1) - H(1)*E(0);
    // Solve quadratic equation
    if (k2 == 0) {
        v = -k0/k1;
    }
    else {
        Delta = k1*k1 - 4*k2*k0;
        if (Delta < 0) {
            cout << endl << "Discriminant of inverse bilinear interpolation is negative. Cannot interpolate!" << endl;
            exit(EXIT_FAILURE);
        }
        else if (Delta == 0) {
            v = (-k1) / (2.0*k2);
        }
        else {
            Delta = sqrt(Delta);
            v = (-k1 - Delta) / (2.0*k2);
        }
    }
    u = (H(0) - F(0)*v) / (E(0) + G(0)*v);
    // Interpolation along l
    i0 = (1-u)*s0 + u*s1;
    i1 = (1-u)*s3 + u*s2;
    // Interpolation along p
    i = (1-v)*i0 + v*i1;
    return i;
}


#pragma once

#include <type_traits>

template <int N, int K>
constexpr auto Stirling2nd()
{
    if constexpr (K >= N || K <= 1)
    {
        return std::integral_constant<int, 1> {};
    }
    else
    {
        return std::integral_constant<int,
            Stirling2nd<N - 1, K - 1>() + K * Stirling2nd<N - 1, K>()> {};
    }
}

// Set Partition
//
// A set partition of the set [n] = {1,2,3,...,n} is a collection B0,
// B1, ... Bj of disjoint subsets of [n] whose union is [n]. Each Bj
// is called a block. Below we show the partitions of [4]. The periods
// separtate the individual sets so that, for example, 1.23.4 is the
// partition {{1},{2,3},{4}}.
//   1 block:  1234
//   2 blocks: 123.4   124.3   134.2   1.234   12.34   13.24   14.23
//   3 blocks: 1.2.34  1.24.3  14.2.3  13.2.4  12.3.4
//   4 blocks: 1.2.3.4
//
// Each partition above has its blocks listed in increasing order of
// smallest element; thus block 0 contains element 1, block1 contains
// the smallest element not in block 0, and so on. A Restricted Growth
// string (or RG string) is a sring a[1..n] where a[i] is the block in
// whcih element i occurs. Restricted Growth strings are often called
// restricted growth functions. Here are the RG strings corresponding
// to the partitions shown above.
//
//   1 block:  0000
//   2 blocks: 0001, 0010, 0100, 0111, 0011, 0101, 0110
//   3 blocks: 0122, 0121, 0112, 0120, 0102,
//
// ...more
//
// Reference:
// Frank Ruskey. Simple combinatorial Gray codes constructed by
// reversing sublists. Lecture Notes in Computer Science, #762,
// 201-208. Also downloadable from
// http://webhome.cs.uvic.ca/~ruskey/Publications/SimpleGray/SimpleGray.html
//
#include <boost/coroutine2/all.hpp>
#include <cassert>
#include <cctype>
#include <cstdio>
#include <functional>
#include <utility>

class set_partition_
{
    using ret_t = std::tuple<int, int>;
    using Fun = std::function<void(ret_t)>;

  private:
    // int b[100 + 1]; /* maximum value of n */
    // int cnt{};
    Fun yield;

  public:
    /**
     * @brief Construct object
     *
     * @param[in] yield_
     */
    explicit set_partition_(Fun yield_)
        : yield(std::move(yield_))
    {
    }

    /**
     * @brief
     *
     * @param[in] n
     * @param[in] k
     */
    void run(int n, int k)
    {
        if (k % 2 == 0)
        {
            this->_GEN0_even(n, k);
        }
        else
        {
            this->_GEN0_odd(n, k);
        }
    }

  private:
    /**
     * @brief Move
     *
     * @param[in] x
     * @param[in] y
     */
    void _Move(int x, int y)
    {
        yield(std::tuple {x, y});
    }

    /**
     * @brief S(n,k,0) even k
     *
     * @param[in] n
     * @param[in] k
     */
    void _GEN0_even(int n, int k);

    /**
     * @brief S'(n,k,0) even k
     *
     * @param[in] n
     * @param[in] k
     */
    void _NEG0_even(int n, int k);

    /**
     * @brief S(n,k,1) even k
     *
     * @param[in] n
     * @param[in] k
     */
    void _GEN1_even(int n, int k);

    /**
     * @brief S'(n,k,1) even k
     *
     * @param[in] n
     * @param[in] k
     */
    void _NEG1_even(int n, int k);

    /**
     * @brief S(n,k,0) odd k
     *
     * @param[in] n
     * @param[in] k
     */
    void _GEN0_odd(int n, int k);

    /**
     * @brief S'(n,k,0) odd k
     *
     * @param[in] n
     * @param[in] k
     */
    void _NEG0_odd(int n, int k);

    /**
     * @brief S(n,k,1) odd k
     *
     * @param[in] n
     * @param[in] k
     */
    void _GEN1_odd(int n, int k);

    /**
     * @brief S'(n,k,1) odd k
     *
     * @param[in] n
     * @param[in] k
     */
    void _NEG1_odd(int n, int k);
};

using ret_t = std::tuple<int, int>;
using coro_t = boost::coroutines2::coroutine<ret_t>;

extern coro_t::pull_type set_partition(int n, int k);


/*
 * stuart_landau.cpp
 *
 * This example demonstrates how one can use odeint can be used with state types consisting of complex variables.
 *
 * Copyright 2009-2012 Karsten Ahnert and Mario Mulansky.
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 */

#include <iostream>
#include <complex>
#include <boost/array.hpp>

#include <boost/numeric/odeint.hpp>

using namespace std;
using namespace boost::numeric::odeint;

//[ stuart_landau_system_function
typedef complex< double > state_type;

struct stuart_landau
{
    double m_eta;
    double m_alpha;

    stuart_landau( double eta = 1.0 , double alpha = 1.0 )
    : m_eta( eta ) , m_alpha( alpha ) { }

    void operator()( const state_type &x , state_type &dxdt , double t ) const
    {
        const complex< double > I( 0.0 , 1.0 );
        dxdt = ( 1.0 + m_eta * I ) * x - ( 1.0 + m_alpha * I ) * norm( x ) * x;
    }
};
//]

/*
//[ stuart_landau_system_function_alternative
double eta = 1.0;
double alpha = 1.0;

void stuart_landau( const state_type &x , state_type &dxdt , double t )
{
    const complex< double > I( 0.0 , 1.0 );
    dxdt[0] = ( 1.0 + m_eta * I ) * x[0] - ( 1.0 + m_alpha * I ) * norm( x[0] ) * x[0];
}
//]
*/

struct streaming_observer
{
    std::ostream& m_out;

    streaming_observer( std::ostream &out ) : m_out( out ) { }

    template< class State >
    void operator()( const State &x , double t ) const
    {
        m_out << t;
        m_out << "\t" << x.real() << "\t" << x.imag() ;
        m_out << "\n";
    }
};

int main( int argc , char **argv )
{
    //[ stuart_landau_integration
    state_type x = complex< double >( 1.0 , 0.0 );

    const double dt = 0.1;

    typedef runge_kutta4< state_type , double , state_type , double ,
                          vector_space_algebra > stepper_type;

    integrate_const( stepper_type() , stuart_landau( 2.0 , 1.0 ) , x , 0.0 , 10.0 , dt , streaming_observer( cout ) );
    //]

    return 0;
}


#include <boost/python.hpp>
#include <numpy/arrayobject.h>
#include <complex>

PyObject* IFT_mat2tau(PyObject *f_mat_in, const int &Nmax, const double &beta, const double &asymp_coefs1, const double &asymp_coefs2)
{
    PyArrayObject *f_mat = (PyArrayObject *) f_mat_in;
    import_array1(NULL);
    uint nfreq = f_mat->dimensions[0];
    npy_intp Nm = (npy_intp)Nmax;
    PyArrayObject *f_tau = (PyArrayObject *)PyArray_SimpleNew(1, &Nm, NPY_DOUBLE);
    Py_INCREF(f_tau);
    double *buf = (double *)PyArray_DATA(f_tau);
    std::complex<double> *f_matsubara = (std::complex<double> *)PyArray_DATA(f_mat);
    double C[2] = {asymp_coefs1, asymp_coefs2};

    for (int i = 0; i < Nmax; i++) {
        double tau=double(i)*beta/double(Nmax - 1);
        buf[i] = -C[0]/2. + C[1]/4.*(-beta+2*tau);
        for (uint k = 0; k < nfreq; ++k) {
            double wt((2*k+1)*double(i)*M_PI/double(Nmax - 1));
            std::complex<double> iomegan(0,(2*k+1)*M_PI/beta);
            std::complex<double> f_matsubara_nomodel = f_matsubara[k] - C[0]/iomegan - C[1]/(iomegan*iomegan);
            buf[i] += 2./beta*(cos(wt)*f_matsubara_nomodel.real() + sin(wt)*f_matsubara_nomodel.imag());
        }
    }
    return PyArray_Return(f_tau);
}

PyObject* FT_tau2mat(PyObject *f_tau_py_in, const double &beta, const int &Nmax)
{
    PyArrayObject *f_tau_py = (PyArrayObject *) f_tau_py_in;
    import_array1(NULL);
    double *f_tau = (double *)PyArray_DATA(f_tau_py);
    int nfreq = f_tau_py->dimensions[0] - 1;
    PyArrayObject *f_mat;
    npy_intp Nm = (npy_intp)Nmax;
    f_mat = (PyArrayObject *)PyArray_SimpleNew(1, &Nm, NPY_CDOUBLE);
    Py_INCREF(f_mat);
    std::complex<double> *f_matsubara = (std::complex<double> *)PyArray_DATA(f_mat);

    // trapezoidal rule for integration
    for (int n = 0; n < Nmax; n++) {
        double wn = (2*n + 1)*M_PI/beta;
        f_matsubara[n] = 0;
        for (int p = 1; p < nfreq; p++) {
            double tau = beta/double(nfreq)*p;
            f_matsubara[n] += f_tau[p]*(cos(wn*tau) + std::complex<double>(0,1)*sin(wn*tau));
        }
        f_matsubara[n] = beta/2./double(nfreq)*(f_tau[0] - f_tau[nfreq]) + beta/double(nfreq)*f_matsubara[n];
    }
    return PyArray_Return(f_mat);
}


#include <complex>
#include <iostream>
#include <stdexcept>
#include <Eigen/Dense>
#include <mkl.h>
#include <mkl_dfti.h>
#include <mkl_vsl.h>
#include "numeric_utils.h"

namespace numeric_utils {
Eigen::MatrixXd corr_to_cov(const Eigen::MatrixXd& corr,
                            const Eigen::VectorXd& std_dev) {
  Eigen::MatrixXd cov_matrix = Eigen::MatrixXd::Zero(corr.rows(), corr.cols());

  for (unsigned int i = 0; i < cov_matrix.rows(); ++i) {
    for (unsigned int j = 0; j < cov_matrix.cols(); ++j) {
      cov_matrix(i, j) = corr(i, j) * std_dev(i) * std_dev(j);
    }
  }

  return cov_matrix;
}

bool convolve_1d(const std::vector<double>& input_x,
                 const std::vector<double>& input_y,
                 std::vector<double>& response) {
  bool status = true;
  response.resize(input_x.size() + input_y.size() - 1);

  // Create convolution status and task pointer
  int conv_status;
  VSLConvTaskPtr conv_task;
  // Construct convolution task, with solution mode set to direct
  conv_status =
      vsldConvNewTask1D(&conv_task, VSL_CONV_MODE_DIRECT, input_x.size(),
                        input_y.size(), response.size());

  // Check if convolution construction was successful
  if (conv_status != VSL_STATUS_OK) {
    throw std::runtime_error(
        "\nERROR: in numeric_utils::convolve_1d: Error in convolution "
        "construction\n");
    status = false;
  }

  // Set convolution to start at first element in input_y
  vslConvSetStart(conv_task, 0);

  // Execute convolution
  conv_status = vsldConvExec1D(conv_task, input_x.data(), 1, input_y.data(), 1,
                               response.data(), 1);

  // Check if convolution exectution was successful
  if (conv_status != VSL_STATUS_OK) {
    throw std::runtime_error(
        "\nERROR: in numeric_utils::convolve_1d: Error in convolution "
        "execution\n");
    status = false;
  }

  // Delete convolution task
  vslConvDeleteTask(&conv_task);

  return status;
}

bool inverse_fft(std::vector<std::complex<double>> input_vector,
                 std::vector<double>& output_vector) {
  output_vector.resize(input_vector.size());

  // Create task descriptor and MKL status
  DFTI_DESCRIPTOR_HANDLE fft_descriptor;
  MKL_LONG fft_status;

  // Allocate the descriptor data structure and initializes it with default
  // configuration values
  fft_status = DftiCreateDescriptor(&fft_descriptor, DFTI_DOUBLE, DFTI_REAL, 1,
                                input_vector.size());
  if (fft_status != DFTI_NO_ERROR) {
    throw std::runtime_error(
        "\nERROR: in numeric_utils::inverse_fft: Error in descriptor creation\n");
    return false;
  }

  // Set configuration value to not do inplace transformation
  fft_status = DftiSetValue(fft_descriptor, DFTI_PLACEMENT, DFTI_NOT_INPLACE);
  if (fft_status != DFTI_NO_ERROR) {
    throw std::runtime_error(
        "\nERROR: in numeric_utils::inverse_fft: Error in setting configuration\n");
    return false;
  }

  // Set the backward scale factor to be 1 divided by the size of the input vector
  // to make the backward tranform the inverse of the forward transform
  fft_status = DftiSetValue(fft_descriptor, DFTI_BACKWARD_SCALE,
                            static_cast<double>(1.0 / input_vector.size()));
  if (fft_status != DFTI_NO_ERROR) {
    throw std::runtime_error(
        "\nERROR: in numeric_utils::inverse_fft: Error in setting backward "
        "scale factor\n");
    return false;
  }

  // Perform all initialization for the actual FFT computation
  fft_status = DftiCommitDescriptor(fft_descriptor);
  if (fft_status != DFTI_NO_ERROR) {
    throw std::runtime_error(
        "\nERROR: in numeric_utils::inverse_fft: Error in committing descriptor\n");
    return false;
  }

  // Compute the backward FFT
  fft_status = DftiComputeBackward(fft_descriptor, input_vector.data(),
                                   output_vector.data());
  if (fft_status != DFTI_NO_ERROR) {
    throw std::runtime_error(
        "\nERROR: in numeric_utils::inverse_fft: Error in computing backward FFT\n");
    return false;
  }

  // Free the memory allocated for descriptor
  fft_status = DftiFreeDescriptor(&fft_descriptor);
  if (fft_status != DFTI_NO_ERROR) {
    throw std::runtime_error(
        "\nERROR: in numeric_utils::inverse_fft: Error in freeing FFT descriptor\n");
    return false;
  }

  return true;
}

bool inverse_fft(const Eigen::VectorXcd& input_vector,
                 Eigen::VectorXd& output_vector) {
  // Convert input Eigen vector to std vector
  std::vector<std::complex<double>> input_vals(input_vector.size());
  std::vector<double> outputs(input_vals.size());
  Eigen::VectorXcd::Map(&input_vals[0], input_vector.size()) = input_vector;

  try {
    inverse_fft(input_vals, outputs);
  } catch (const std::exception& e) {
    std::cerr << "\nERROR: In numeric_utils::inverse_fft (With Eigen Vectors):"
              << e.what() << std::endl;
  }

  // Convert output from std vector to Eigen vector
  output_vector = Eigen::Map<Eigen::VectorXd>(outputs.data(), outputs.size());

  return true;
}

bool inverse_fft(const Eigen::VectorXcd& input_vector,
                 std::vector<double>& output_vector) {
  // Convert input Eigen vector to std vector
  std::vector<std::complex<double>> input_vals(input_vector.size());
  Eigen::VectorXcd::Map(&input_vals[0], input_vector.size()) = input_vector;
  output_vector.resize(input_vector.size());

  try {
    inverse_fft(input_vals, output_vector);
  } catch (const std::exception& e) {
    std::cerr << "\nERROR: In numeric_utils::inverse_fft (With Eigen Vectors):"
              << e.what() << std::endl;
  }

  return true;
}

bool fft(std::vector<double> input_vector,
         std::vector<std::complex<double>>& output_vector) {
  // Convert input vector to complex values
  std::vector<std::complex<double>> input_complex(input_vector.size());
  std::copy(input_vector.begin(), input_vector.end(), input_complex.begin());

  output_vector.resize(input_vector.size());

  // Create task descriptor and MKL status
  DFTI_DESCRIPTOR_HANDLE fft_descriptor;
  MKL_LONG fft_status;

  // Allocate the descriptor data structure and initializes it with default
  // configuration values
  fft_status = DftiCreateDescriptor(&fft_descriptor, DFTI_DOUBLE, DFTI_COMPLEX,
                                    1, input_complex.size());
  if (fft_status != DFTI_NO_ERROR) {
    throw std::runtime_error(
        "\nERROR: in numeric_utils::fft: Error in descriptor creation\n");
    return false;
  }

  // Set configuration value to not do inplace transformation
  fft_status = DftiSetValue(fft_descriptor, DFTI_PLACEMENT, DFTI_NOT_INPLACE);
  if (fft_status != DFTI_NO_ERROR) {
    throw std::runtime_error(
        "\nERROR: in numeric_utils::fft: Error in setting configuration\n");
    return false;
  }

  // Perform all initialization for the actual FFT computation
  fft_status = DftiCommitDescriptor(fft_descriptor);
  if (fft_status != DFTI_NO_ERROR) {
    throw std::runtime_error(
        "\nERROR: in numeric_utils::fft: Error in committing descriptor\n");
    return false;
  }

  // Compute the backward FFT
  fft_status = DftiComputeForward(fft_descriptor, input_complex.data(),
                                  output_vector.data());
  if (fft_status != DFTI_NO_ERROR) {
    throw std::runtime_error(
        "\nERROR: in numeric_utils::fft: Error in computing FFT\n");
    return false;
  }

  // Free the memory allocated for descriptor
  fft_status = DftiFreeDescriptor(&fft_descriptor);
  if (fft_status != DFTI_NO_ERROR) {
    throw std::runtime_error(
        "\nERROR: in numeric_utils::fft: Error in freeing FFT descriptor\n");
    return false;
  }

  return true;
}

bool fft(const Eigen::VectorXd& input_vector, Eigen::VectorXcd& output_vector) {
  // Convert input Eigen vector to std vector
  std::vector<double> input_vals(input_vector.size());
  std::vector<std::complex<double>> outputs(input_vals.size());
  Eigen::VectorXd::Map(&input_vals[0], input_vector.size()) = input_vector;

  try {
    fft(input_vals, outputs);
  } catch (const std::exception& e) {
    std::cerr << "\nERROR: In numeric_utils::fft (With Eigen Vectors):"
              << e.what() << std::endl;
  }

  // Convert output from std vector to Eigen vector
  output_vector = Eigen::Map<Eigen::VectorXcd>(outputs.data(), outputs.size());

  return true;
}

bool fft(const Eigen::VectorXd& input_vector,
                 std::vector<std::complex<double>>& output_vector) {
  // Convert input Eigen vector to std vector
  std::vector<double> input_vals(input_vector.size());
  Eigen::VectorXd::Map(&input_vals[0], input_vector.size()) = input_vector;
  output_vector.resize(input_vector.size());

  try {
    fft(input_vals, output_vector);
  } catch (const std::exception& e) {
    std::cerr << "\nERROR: In numeric_utils::fft (With Eigen Vector and STL vector):"
              << e.what() << std::endl;
  }

  return true;
}

double trapazoid_rule(const std::vector<double>& input_vector, double spacing) {
  double result = (input_vector[0] + input_vector[input_vector.size() - 1]) / 2.0;

  for (unsigned int i = 1; i < input_vector.size() - 1; ++i) {
    result = result + input_vector[i];
  }

  return result * spacing;
}

double trapazoid_rule(const Eigen::VectorXd& input_vector, double spacing) {
  double result = (input_vector[0] + input_vector[input_vector.size() - 1]) / 2.0;

  for (unsigned int i = 1; i < input_vector.size() - 1; ++i) {
    result = result + input_vector[i];
  }

  return result * spacing;
}

Eigen::VectorXd polyfit_intercept(const Eigen::VectorXd& points,
                                       const Eigen::VectorXd& data,
				       double intercept,
                                       unsigned int degree) {

  Eigen::MatrixXd coefficients =
      Eigen::MatrixXd::Zero(points.size(), degree - 1);

  for (unsigned int i = 0; i < degree - 1; ++i) {
    coefficients.col(i) = points.array().pow(degree - i);
  }

  // Solve system
  Eigen::VectorXd solution = coefficients.fullPivHouseholderQr().solve(
      (data.array() - intercept).matrix());

  // Set y-intercept to zero
  Eigen::VectorXd poly_fit(solution.size() + 2);

  for (unsigned int i = 0; i < solution.size(); ++i) {
    poly_fit(i) = solution(i);
  }
  poly_fit(poly_fit.size() - 1) = intercept;
  poly_fit(poly_fit.size() - 2) = 0.0;

  return poly_fit;
}

Eigen::VectorXd polynomial_derivative(const Eigen::VectorXd& coefficients) {
  Eigen::VectorXd derivative(coefficients.size() - 1);

  for (unsigned int i = 0; i < derivative.size(); ++i) {
    derivative(i) = coefficients(i) * (coefficients.size() - 1 - i);
  }

  return derivative;
}

std::vector<double> derivative(const std::vector<double>& coefficients,
                               double constant_factor, bool add_zero) {

  if (add_zero) {
    std::vector<double> derivative(coefficients.size());
    derivative[0] = coefficients[0] * constant_factor;

    for (unsigned int i = 1; i < derivative.size(); ++i) {
      derivative[i] = (coefficients[i] - coefficients[i - 1]) * constant_factor;
    }

    return derivative;
  } else {
    std::vector<double> derivative(coefficients.size() - 1);

    for (unsigned int i = 0; i < derivative.size(); ++i) {
      derivative[i] = (coefficients[i + 1] - coefficients[i]) * constant_factor;
    }

    return derivative;
  }
}

Eigen::VectorXd evaluate_polynomial(const Eigen::VectorXd& coefficients,
                                    const Eigen::VectorXd& points) {
  Eigen::VectorXd evaluations = Eigen::VectorXd::Zero(points.size());

  for (unsigned int i = 0; i < evaluations.size(); ++i) {
    for (unsigned int j = 0; j < coefficients.size(); ++j) {
      evaluations(i) +=
          coefficients(j) * std::pow(points(i), coefficients.size() - 1 - j);
    }
  }

  return evaluations;
}

Eigen::VectorXd evaluate_polynomial(const Eigen::VectorXd& coefficients,
                                    const std::vector<double>& points) {
  Eigen::VectorXd evaluations = Eigen::VectorXd::Zero(points.size());

  for (unsigned int i = 0; i < evaluations.size(); ++i) {
    for (unsigned int j = 0; j < coefficients.size(); ++j) {
      evaluations(i) +=
          coefficients(j) * std::pow(points[i], coefficients.size() - 1 - j);
    }
  }

  return evaluations;
}

std::vector<double> evaluate_polynomial(const std::vector<double>& coefficients,
                                        const std::vector<double>& points) {
  std::vector<double> evaluations(points.size(), 0.0);

  for (unsigned int i = 0; i < evaluations.size(); ++i) {
    for (unsigned int j = 0; j < coefficients.size(); ++j) {
      evaluations[i] +=
          coefficients[j] * std::pow(points[i], coefficients.size() - 1 - j);
    }
  }

  return evaluations;
}
}  // namespace numeric_utils


#include <iostream>
#include <NTL/ZZ_pX.h>
#include <NTL/matrix.h>
#include <NTL/ZZ_pXFactoring.h>
#include <NTL/ZZ_pE.h>
#include <NTL/ZZ_pEX.h>
#include <NTL/ZZX.h>

#include "BaseChange.h"
#include "MultipointEval.h"
#include "Util.h"
#include "HasseLift.h"
#include "HasseLiftExt.h"
#include "MultiComposeMod.h"
#include "FrobComp.h"
#include "SSFactoring.h"

using namespace std;

//void testBaseChange(long degree) {
//    cout << "degree: " << degree << "\n";
//
//    ZZ_pX f, g, h;
//
//    random(f, degree - 1);
//    random(h, degree - 1);
//
//    // build a square-free modulus
//    random(g, degree);
//    SetCoeff(g, degree, 1);
//    diff(h, g);
//    GCD(h, g, h);
//    div(g, g, h);
//    ZZ_pXModulus modulus;
//    build(modulus, g);
//
//    CompMod(g, h, f, modulus);
//    BaseChange baseChane;
//    baseChane.changeBasis(f, f, g, modulus);
//
//    if (f == h)
//        cout << "ok\n";
//    else
//        cout << "oops\n";
//
//    f.kill();
//    g.kill();
//    h.kill();
//    modulus.f.kill();
//}
//

void testCompose() {
    long k = 1;
    long n = 2000;
    ZZ_pX f;
    random(f, n);
    SetCoeff(f, n, 1);
    ZZ_pXModulus F;
    build(F, f);

    Vec<ZZ_pX> result;
    Vec<ZZ_pX> result1;
    Vec<ZZ_pX> gVec;
    result.SetLength(k);
    result1.SetLength(k);
    gVec.SetLength(k);

    for (long i = 0; i < k; i++)
        random(gVec[i], n);

    ZZ_pX h;
    random(h, n);

    MultiComposeMod multiComposeMod;

    Util util;
    long start = util.getTimeMillis();
    multiComposeMod.compose(result, gVec, h, F);
    cout << util.getTimeMillis() - start << endl;

    start = util.getTimeMillis();
    for (long i = 0; i < k; i++) {
        CompMod(result1[i], gVec[i], h, F);
    }
    cout << util.getTimeMillis() - start << endl;

    for (long i = 0; i < k; i++) {
        if (result[i] != result1[i]) {
            cout << "Failed" << endl;
            break;
        }
    }
}
//
//
//void testMultipointEval() {
//    long numPoints = 100;
//
//    long degree = 20;
//    ZZ_pX f;
//    random(f, degree);
//    SetCoeff(f, degree, 1);
//
//    ZZ_pE::init(f);
//
//    Vec<ZZ_pE> points;
//    Vec<ZZ_pE> results;
//    points.SetLength(numPoints);
//    results.SetLength(numPoints);
//
//    for (long i = 0; i < numPoints; i++)
//        random(points[i]);
//
//    ZZ_pEX g;
//    random(g, numPoints);
//    SetCoeff(g, numPoints + 1, 1);
//
//    Util util;
//    long start = util.getTimeMillis();
//    MultipointEval multiPointEval;
//    multiPointEval.eval(results, g, points);
//    cout << (util.getTimeMillis() - start) << endl;
//
//    start = util.getTimeMillis();
//    ZZ_pE temp;
//    for (long i = 0; i < numPoints; i++) {
//        eval(temp, g, points[i]);
//        if (temp != results[i]) {
//            cout << "not equal" << endl;
//            break;
//        }
//    }
//    cout << (util.getTimeMillis() - start) << endl;
//}
//

void testFrob() {
    ZZ_pX f;
    long d = 100;
    random(f, d);
    SetCoeff(f, d, 1);
    ZZ_pXModulus F;
    build(F, f);

    long m = 20;

    Vec<ZZ_pX> frobs;
    frobs.SetLength(m);
    ZZ_pX xq;
    SetX(xq);
    PowerMod(xq, xq, ZZ_p::modulus(), F);

    FrobComp frobComp;
    frobComp.computeFrobePowers(frobs, m, xq, F);

    Vec<ZZ_pX> frobs1;
    frobs1.SetLength(m);
    frobs1[0] = xq;
    for (long i = 1; i < m; i++) {
        PowerMod(frobs1[i], frobs1[i - 1], ZZ_p::modulus(), F);
    }

    if (frobs == frobs1)
        cout << "OK" << endl;
    else
        cout << "Failed" << endl;
}

void testHasseLift() {
    ZZ_pX f, g, delta, lift1, lift2;

    long degree = 10000;

    // build a square-free modulus
    Util util;
    util.randomMonic(f, degree);
    diff(g, f);
    GCD(g, g, f);
    div(f, f, g);
    ZZ_pXModulus F;
    build(F, f);
    degree = deg(F);

    random(g, degree);
    random(delta, degree);

    HasseLiftExt hasseLiftExt(g, delta, F);
    long start = util.getTimeMillis();
//    hasseLiftExt.computeNaive(lift1, degree / 2);
//    cout << util.getTimeMillis() - start << endl;
//
//    start = util.getTimeMillis();
    hasseLiftExt.compute(lift2, degree / 2, 1);
    cout << util.getTimeMillis() - start << endl;

    if (lift1 == lift2)
        cout << "OK" << endl;
    else
        cout << "Failed" << endl;
}

void testFactoring() {
    Util util;

    long n = 10;
    ZZ_pX f;
    util.randomMonic(f, n);
    cout << "p: " << ZZ_p::modulus() << endl;
    cout << "f: " << f << endl;

    Vec<pair_ZZ_pX_long> factors1;
    Vec<pair_ZZ_pX_long> factors2;

    SSFactoring sSfactoring;
//    long start = util.getTimeMillis();
    sSfactoring.factor(factors1, f);
    cout << factors1 << endl;
//    cout << util.getTimeMillis() - start << endl;
//
//    start = util.getTimeMillis();
//    CanZass(factors2, f, 1);
//    cout << util.getTimeMillis() - start << endl;
//
//    ZZ_pX g;
//    mul(g, factors1);
//    if (f == g)
//        cout << "OK" << endl;
//    else
//        cout << "Failed" << endl;
}

int main(int argc, char** argv) {

    ZZ p;
    NextPrime(p, to_ZZ(7));
    ZZ_p::init(p);

    testFactoring();

    return 0;
}


/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2015, University of Toronto
*  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 University of Toronto nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Jonathan Gammell*/

// This file's header
#include "ompl/util/GeometricEquations.h"

// For pre C++ 11 gamma function
#include <boost/math/special_functions/gamma.hpp>

// OMPL exceptions
#include "ompl/util/Exception.h"

double ompl::nBallMeasure(unsigned int N, double r)
{
    return std::pow(std::sqrt(boost::math::constants::pi<double>()) * r, static_cast<double>(N)) / boost::math::tgamma(static_cast<double>(N)/2.0 + 1.0);
}

double ompl::unitNBallMeasure(unsigned int N)
{
    // This is the radius version with r removed (as it is 1) for efficiency
    return std::pow(std::sqrt(boost::math::constants::pi<double>()), static_cast<double>(N)) / boost::math::tgamma(static_cast<double>(N)/2.0 + 1.0);
}

double ompl::prolateHyperspheroidMeasure(unsigned int N, double dFoci, double dTransverse)
{
    // Sanity check input
    if (dTransverse < dFoci)
    {
        throw Exception("Transverse diameter cannot be less than the minimum transverse diameter.");
    }

    // Variable
    // The conjugate diameter:
    double conjugateDiameter;
    // The Lebesgue measure return value
    double lmeas;

    // Calculate the conjugate diameter:
    conjugateDiameter = std::sqrt(dTransverse * dTransverse - dFoci * dFoci);

    // Calculate the volume
    // First multiply together the radii, noting that the first radius is the transverse diameter/2.0, and the other N-1 are the conjugate diameter/2.0
    lmeas = dTransverse/2.0;
    for (unsigned int i = 1u; i < N; ++i)
    {
        lmeas = lmeas * conjugateDiameter/2.0;
    }

    // Then multiply by the volume of the unit n-ball.
    lmeas = lmeas * unitNBallMeasure(N);

    // Finally return:
    return lmeas;
}


#define EIGEN_USE_MKL_ALL

#include <iostream>

#include <Eigen/Core>

#include "../misc.hpp"

#include "models/transformers/principal_components_analysis.hpp"
#include "models/transformers/zero_components_analysis.hpp"

void pca_examples() {
	Eigen::MatrixXd X(2, 6);

	X.row(0) << -1, -2, -3, 1, 2, 3;
	X.row(1) << -1, -1, -2, 1, 1, 2;

	mlt::models::transformers::PrincipalComponentsAnalysis pca;
	pca.fit(X);

	std::cout << "PCA: " << std::endl;
	std::cout << pca.explained_variance_ratio() << std::endl << std::endl;
	std::cout << "X2" << std::endl << X << std::endl;
	std::cout << "PCA(X2)" << std::endl << pca.transform(X) << std::endl;
	std::cout << "PCA-1(PCA(X2))" << std::endl << pca.inverse_transform(pca.transform(X)) << std::endl;
	std::cout << "PCA(X2[:,1])" << std::endl << pca.transform(X.leftCols(1)) << std::endl;
	std::cout << "PCA-1(PCA(X2[:,1]))" << std::endl << pca.inverse_transform(pca.transform(X.leftCols(1))) << std::endl << std::endl;

	mlt::models::transformers::ZeroComponentsAnalysis zca;
	zca.fit(X);

	std::cout << "ZCA: " << std::endl;
	std::cout << zca.explained_variance_ratio() << std::endl << std::endl;
	std::cout << "X2" << std::endl << X << std::endl;
	std::cout << "ZCA(X2)" << std::endl << zca.transform(X) << std::endl;
	std::cout << "ZCA-1(ZCA(X2))" << std::endl << zca.inverse_transform(zca.transform(X)) << std::endl;
	std::cout << "ZCA(X2[:,1])" << std::endl << zca.transform(X.leftCols(1)) << std::endl;
	std::cout << "ZCA-1(ZCA(X2[:,1]))" << std::endl << zca.inverse_transform(zca.transform(X.leftCols(1))) << std::endl << std::endl;
}


#pragma once
#include <math.h>
#include <stdlib.h>
#include <Eigen/Dense>
#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <chrono>

/*!
   \brief Enforce orthogonality conditions on the given rotation matrix such that det(R) == 1 and R.tranpose() * R = I
   \param R The input rotation matrix either 2x2 or 3x3, will be overwritten with a slightly modified matrix to
   satisfy orthogonality conditions.
*/
void enforce_orthogonality(Eigen::MatrixXd &R);

/*!
   \brief Retrieve the rigid transformation that transforms points in p1 into points in p2.
   The output transform type (float or double) and size SE(2) vs. SE(3) depends on the size of the input points p1, p2.
   \param p1 A dim x N vector of points in either 2D (dim = 2) or 3D (dim = 3)
   \param p2 A dim x N vector of points in either 2D (dim = 2) or 3D (dim = 3)
   \param Tf [out] This matrix will be overwritten as the output transform
   \pre p1 and p2 are the same size. p1 and p2 are the matched feature point locations between two point clouds
   \post orthogonality is enforced on the rotation matrix.
*/
void get_rigid_transform(Eigen::MatrixXd p1, Eigen::MatrixXd p2, Eigen::MatrixXd &Tf);

/*!
   \brief Returns a random subset of indices, where 0 <= indices[i] <= max_index. indices are non-repeating.
*/
std::vector<int> random_subset(int max_index, int subset_size);

/*!
   \brief Returns the output of the cross operator.
   For 3 x 1 input, cross(x) * y is equivalent to cross_product(x, y)
   For 6 x 1 input, x = [rho, phi]^T. out = [cross(phi), rho; 0 0 0 1]
   \param x Input vector which can be 3 x 1 or 6 x 1.
   \return If the input if 3 x 1, the output is 3 x 3, if the input is 6 x 1, the output is 4 x 4.
*/
Eigen::MatrixXd cross(Eigen::VectorXd x);

/*!
   \brief Returns the output of the circledot operator. cross(epsilon) * p == circledot(p) * epsilon,
   where epsilon is 6x1 and p is 4 x 1 homogeneous.
   p = [rhobar, eta]^T  circledot(p) = [eta * identity(3), -cross(rhobar); 0 0 0 0 0 0]
   \param x Input is a 4 x 1 homogeneous 3D vector.
   \return returns the 4 x 6 output of circledot(x)
*/
Eigen::MatrixXd circledot(Eigen::VectorXd x);

/*!
   \brief This function converts from a lie vector to a 4 x 4 SE(3) transform.
   // Lie Vector xi = [rho, phi]^T (6 x 1) --> SE(3) T = [C, R; 0 0 0 1] (4 x 4)
   \param x Input vector is 6 x 1
   \return Output is 4 x SE(3) transform
*/
Eigen::Matrix4d se3ToSE3(Eigen::MatrixXd xi);

/*!
   \brief This function converts from an SE(3) transform into a lie vector
   // SE(3) T = [C, R; 0 0 0 1] (4 x 4) --> Lie Vector xi = [rho, phi]^T (6 x 1)
   \param T Input is a 4x4 SE(3) transform
   \return Output is 6x1 lie vector
*/
Eigen::VectorXd SE3tose3(Eigen::MatrixXd T);

/*!
   \brief Converts a vector of x-y-z euler parameters to a rotation matrix.
   \param eul 3x1 vector of euler rotation parameters (x,y,z) == (roll, pitch, yaw)
   \return Output is a 3x3 rotation matrix, equivalent to the given euler parameters.
*/
Eigen::MatrixXd eulerToRot(Eigen::VectorXd eul);

/*!
   \brief Ensures that theta is within [0, 2 * pi)
*/
double wrapto2pi(double theta);

//* Ransac
/**
* \brief This class estimates a single rigid transform between two point clouds using RANSAC and singular value decomp
*/
class Ransac {
public:
    // p1, p2 need to be either (x, y) x N or (x, y, z) x N (must be in homogeneous coordinates)
    Ransac(Eigen::MatrixXd p1_, Eigen::MatrixXd p2_, double tolerance_, double inlier_ratio_,
        int iterations_) : p1(p1_), p2(p2_), tolerance(tolerance_),
        inlier_ratio(inlier_ratio_), iterations(iterations_) {
        int dim = p1.rows();
        assert(p1.cols() == p2.cols() && p1.rows() == p2.rows() && (dim == 2 || dim == 3));
        T_best = Eigen::MatrixXd::Identity(dim + 1, dim + 1);
    }
    void setTolerance(double tolerance_) {tolerance = tolerance_;}
    void setInlierRatio(double inlier_ratio_) {inlier_ratio = inlier_ratio_;}
    void setMaxIterations(int iterations_) {iterations = iterations_;}
    void getTransform(Eigen::MatrixXd &Tf) {Tf = T_best;}

    /*!
       \brief Computes the transform that best aligns the two pointclouds such at T * p1 = p2
    */
    double computeModel();

    /*!
       \brief Retrieves the set of point pairs which are inliers given the current transform Tf.
    */
    void getInliers(Eigen::MatrixXd Tf, std::vector<int> &inliers);

private:
    Eigen::MatrixXd p1, p2;
    double tolerance = 0.05;
    double inlier_ratio = 0.9;
    int iterations = 40;
    Eigen::MatrixXd T_best;
};

//* MotionDistortedRansac
/**
* \brief This class estimates the linear velocity and angular velocity of the sensor in the body-frame.
*
* Assuming constant velocity, the motion vector can be used to estimate the transform between any two pairs of points
* if the delta_t between those points is known.
*
* A single transform between the two pointclouds can also be retrieved.
*
* All operations are done in SE(3) even if the input is 2D. The output motion and transforms are in 3D.
*/
class MotionDistortedRansac {
public:
    MotionDistortedRansac(Eigen::MatrixXd p1, Eigen::MatrixXd p2, std::vector<int64_t> t1, std::vector<int64_t> t2,
        double tolerance_, double inlier_ratio_, int iterations_) : tolerance(tolerance_),
        inlier_ratio(inlier_ratio_), iterations(iterations_) {
        const int dim = p1.rows();
        assert(p1.cols() == p2.cols() && p1.rows() == p2.rows() && p1.cols() >= p1.rows() && (dim == 2 || dim == 3));
        p1bar = Eigen::MatrixXd::Zero(4, p1.cols());
        p2bar = Eigen::MatrixXd::Zero(4, p2.cols());
        p1bar.block(3, 0, 1, p1.cols()) = Eigen::MatrixXd::Ones(1, p1.cols());
        p2bar.block(3, 0, 1, p2.cols()) = Eigen::MatrixXd::Ones(1, p2.cols());
        if (dim == 2) {
            p1bar.block(0, 0, 2, p1.cols()) = p1;
            p2bar.block(0, 0, 2, p2.cols()) = p2;
        } else if (dim == 3) {
            p1bar.block(0, 0, 3, p1.cols()) = p1;
            p2bar.block(0, 0, 3, p2.cols()) = p2;
        }
        R_pol << pow(0.25, 2), 0, 0, 0, 0, pow(0.0157, 2), 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
        delta_ts = std::vector<double>(p1.cols(), 0.0);
        for (uint i = 0; i < p1bar.cols(); ++i) {
            int64_t delta_t = t2[i] - t1[i];
            delta_ts[i] = double(delta_t) / 1000000.0;
            if (delta_ts[i] > max_delta_t) {
                max_delta_t = delta_ts[i];
            }
            if (delta_ts[i] < min_delta_t) {
                min_delta_t = delta_ts[i];
            }
        }
        double delta_diff = (max_delta_t - min_delta_t) / (num_transforms - 1);
        for (int i = 0; i < num_transforms; ++i) {
            delta_vec.push_back(min_delta_t + i * delta_diff);
        }
    }
    void setTolerance(double tolerance_) {tolerance = tolerance_;}
    void setInlierRatio(double inlier_ratio_) {inlier_ratio = inlier_ratio_;}
    void setMaxIterations(int iterations_) {iterations = iterations_;}
    void setMaxGNIterations(int iterations_) {max_gn_iterations = iterations_;}
    void setConvergenceThreshold(double eps) {epsilon_converge = eps;}
    void correctForDoppler(bool doppler_) {doppler = doppler_;}
    void getTransform(double delta_t, Eigen::MatrixXd &Tf);
    void getMotion(Eigen::VectorXd &w) {w = w_best;}
    void setDopplerParameter(double beta_) {beta = beta_;}

    /*!
       \brief Computes the ego-motion vector that best aligns the two pointclouds
    */
    double computeModel();

    /*!
       \brief Retrieves the set of point pairs which are inliers given the current motion estimate.
    */
    void getInliers(Eigen::VectorXd wbar, std::vector<int> &inliers);

private:
    Eigen::MatrixXd p1bar, p2bar;
    std::vector<double> delta_ts;
    double tolerance = 0.05;
    double inlier_ratio = 0.9;
    int iterations = 40;
    int max_gn_iterations = 10;
    double epsilon_converge = 0.0001;
    double error_converge = 0.01;
    int dim = 2;
    double beta = -0.049;  // beta = (f_t / (df / dt))
    double r_observable_sq = 0.0625;
    bool doppler = false;
    int num_transforms = 21;
    double max_delta_t = 0.0;
    double min_delta_t = 0.5;
    std::vector<double> delta_vec;
    Eigen::VectorXd w_best = Eigen::VectorXd::Zero(6);
    Eigen::Matrix4d R_pol = Eigen::Matrix4d::Identity();
    Eigen::MatrixXd get_jacobian(Eigen::Vector4d gbar);
    Eigen::MatrixXd get_inv_jacobian(Eigen::Vector4d gbar);
    Eigen::VectorXd to_cylindrical(Eigen::VectorXd gbar);
    Eigen::VectorXd from_cylindrical(Eigen::VectorXd ybar);

    /*!
       \brief Given two sets of point pairs (p1small, p2small), this function computes the motion of the sensor
       (linear and angular velocity) in the body frame using nonlinear least squares.
       \pre It's very important that the delt_t_local is accurate. Note that each azimuth in the radar scan is time
       stamped, this should be used to get the more accurate time differences.
    */
    void get_motion_parameters(std::vector<int> subset, Eigen::VectorXd &wbar);

    /*!
       \brief This function also computes the motion of the sensor, but it uses the linear least squares version of
       MDRANSAC. This is a lot less accurate, but it computes the motion vector in a single step.
    */
    void get_motion_parameters2(Eigen::MatrixXd& p1small, Eigen::MatrixXd& p2small, std::vector<double> delta_t_local,
        Eigen::VectorXd &wbar);

    /*!
       \brief Retrieve the number of inliers corresponding to body motion vector wbar. (6 x 1)
    */
    int getNumInliers(Eigen::VectorXd wbar);

    /*!
       \brief Given a body motion vector wbar (6 x 1), adjust the position of point p to account
       for the Doppler distortion which may be present in the data.
    */
    void dopplerCorrection(Eigen::VectorXd wbar, Eigen::VectorXd &p);
};

/*!
   \brief Retrieve a 4x4 homogeneous transformation matrix T_enu_sensor for this ground truth vector.
   \param gt [GPSTime,x,y,z,vel_x,vel_y,vel_z,roll,pitch,heading,ang_vel_z]
*/
Eigen::Matrix4d getTransformFromGT(std::vector<double> gt);

/*!
   \brief This function removes motion distortion from a cloud of points.
   \param pc The input point cloud should be 4 x N, as in homogeneous coordinates (x, y, z, 1) (z = 0 for 2D data)
   \param times Timestamps associated with each point, in seconds.
   \param T_enu_sensor A global transformation from the nominal sensor position to the ENU frame,
        used to transform the velocity vector from the ENU frame to the sensor frame.
   \param gt [GPSTime,x,y,z,vel_x,vel_y,vel_z,roll,pitch,heading,ang_vel_z]
   \param t_ref 0 (radar) --> use first time as reference, -1 (lidar) --> use last time as reference.
*/
void removeMotionDistortion(Eigen::MatrixXd &pc, std::vector<float> &times, Eigen::Matrix4d T_enu_sensor,
    std::vector<double> gt, int t_ref);

// Return the inverse of a 4x4 homogeneous transformation matrix
Eigen::Matrix4d get_inverse_tf(Eigen::Matrix4d T);


#include "scan_matching_skeleton/transform.h"
#include <cmath>
#include "ros/ros.h"
#include <Eigen/Geometry>
#include <complex>

using namespace std;

void transformPoints(const vector<Point>& points, Transform& t, vector<Point>& transformed_points) {
  transformed_points.clear();
  for (int i = 0; i < points.size(); i++) {
    transformed_points.push_back(t.apply(points[i]));
    //printf("%f %transformed_points.back().r, transformed_points.back().theta);
  }
}

// returns the largest real root to ax^3 + bx^2 + cx + d = 0
complex<float> get_cubic_root(float a, float b, float c, float d) {
  //std::cout<< "a= " << a<< ";  b= " << b<< ";  c= " << c<< ";  d= " << d<<";"<<std::endl;
  // Reduce to depressed cubic
  float p = c/a - b*b/(3*a*a);
  float q = 2*b*b*b/(27*a*a*a) + d/a - b*c/(3*a*a);

  // std::cout<<"p = "<<p<<";"<<std::endl;
  // std::cout<<"q = "<<q<<";"<<std::endl;

  complex<float> xi(-.5, sqrt(3)/2);

  complex<float> inside = sqrt(q*q/4 + p*p*p/27);

  complex<float> root;

  for (float k = 0; k < 3; ++k) {
    // get root for 3 possible values of k
    root = -b/(3*a) + pow(xi, k) * pow(-q/2.f + inside, 1.f/3.f) + pow(xi, 2.f*k) * pow(-q/2.f - inside, 1.f/3.f);
    //std::cout<<"RootTemp: "<< root<<std::endl;
    if (root.imag() != 0) { return root; }
  }

  return root;
}

// returns the largest real root to ax^4 + bx^3 + cx^2 + dx + e = 0
float greatest_real_root(float a, float b, float c, float d, float e) {
  // Written with inspiration from: https://en.wikipedia.org/wiki/Quartic_function#General_formula_for_roots
  //std::cout<< "a= " << a<< ";  b= " << b<< ";  c= " << c<< ";  d= " << d<< ";  e= " << e<<";"<<std::endl;

  // Reduce to depressed Quadratic
  float p = (8*a*c-3*b*b)/(8*a*a);
  float q = (b*b*b-4*a*b*c+8*a*a*d)/(8*a*a*a);
  float r = (-3*b*b*b*b+256*a*a*a*e-64*a*a*b*d+16*a*b*b*c)/(256*a*a*a*a);

  // std::cout<<"p = "<<p<<";"<<std::endl;
  // std::cout<<"q = "<<q<<";"<<std::endl;
  // std::cout<<"r = "<<r<<";"<<std::endl;

  // Ferrari's Solution for Quartics: 8m^3 + 8pm^2 + (2p^2-8r)m - q^2 = 0
  complex<float> m = get_cubic_root(8, 8*p, 2*p*p-8*r, -q*q);

  complex<float> root1 = -b/(4*a) + ( sqrt(2.f*m) + sqrt(-(2*p + 2.f*m + sqrt(2.f)*q/sqrt(m))))/2.f;
  complex<float> root2 = -b/(4*a) + ( sqrt(2.f*m) - sqrt(-(2*p + 2.f*m + sqrt(2.f)*q/sqrt(m))))/2.f;
  complex<float> root3 = -b/(4*a) + (-sqrt(2.f*m) + sqrt(-(2*p + 2.f*m - sqrt(2.f)*q/sqrt(m))))/2.f;
  complex<float> root4 = -b/(4*a) + (-sqrt(2.f*m) - sqrt(-(2*p + 2.f*m - sqrt(2.f)*q/sqrt(m))))/2.f;

  vector<complex<float>> roots { root1, root2, root3, root4 };

  float max_real_root = 0.f;

  for (complex<float> root: roots) {
    if(root.imag()==0){
    max_real_root = max(max_real_root, root.real());
  }
  //std::cout<<"Max real root:" << max_real_root<<std::endl;

  return max_real_root;
}}

void updateTransform(vector<Correspondence>& corresponds, Transform& curr_trans) {
  // Written with inspiration from: https://github.com/AndreaCensi/gpc/blob/master/c/gpc.c
  // You can use the helper functions which are defined above for finding roots and transforming points as and when needed.
  // use helper functions and structs in transform.h and correspond.h
  // input : corresponds : a struct vector of Correspondene struct object defined in correspond.
  // input : curr_trans : A Transform object refernece
  // output : update the curr_trans object. Being a call by reference function, Any changes you make to curr_trans will be reflected in the calling function in the scan_match.cpp program/

// You can change the number of iterations here. More the number of iterations, slower will be the convergence but more accurate will be the results. You need to find the right balance.
int number_iter = 1;

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

  //fill in the values of the matrics
  Eigen::MatrixXf M_i(2, 4);
  Eigen::Matrix2f C_i;
  Eigen::Vector2f pi_i;

  // Fill in the values for the matrices
  Eigen::Matrix4f M, W;
  Eigen::MatrixXf g(4, 1);
  M << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
  W << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
  g << 0, 0, 0, 0;

  // Define sub-matrices A, B, D from M
  Eigen::Matrix2f A, B, D;

  //define S and S_A matrices from the matrices A B and D
  Eigen::Matrix2f S;
  Eigen::Matrix2f S_A;

  //find the coefficients of the quadratic function of lambda
  float pow_2; float pow_1; float pow_0;

  // find the value of lambda by solving the equation formed. You can use the greatest real root function
  float lambda;

  //find the value of x which is the vector for translation and rotation
  Eigen::Vector4f x;

  // Convert from x to new transform

  float theta = atan2(x(3), x(2));
  curr_trans= Transform(x(0), x(1), theta);
}
}


/**
 * @file discontinuousgalerkin1d.cc
 * @brief NPDE homework "DiscontinuousGalerkin1D" code
 * @author Oliver Rietmann
 * @date 22.05.2019
 * @copyright Developed at ETH Zurich
 */

#include "discontinuousgalerkin1d.h"

#include <Eigen/Core>
#include <Eigen/SparseCore>

namespace DiscontinuousGalerkin1D {

/* SAM_LISTING_BEGIN_1 */
Eigen::SparseMatrix<double> compBmat(int Ml, int Mr, double h) {
  const int N = 2 * (Ml + Mr + 1);
  Eigen::SparseMatrix<double> A(N, N);
  // Tell Eigen that the matrix is diagonal, which allows efficient
  // initialization
  A.reserve(Eigen::VectorXi::Ones(N));
  for (int i = 0; i < N; i += 2) {
    A.insert(i, i) = h;
    A.insert(i + 1, i + 1) = h * h * h / 12.0;
  }
  return A;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
double Feo(double v, double w) {
  double result;
  auto f = [](double u) { return u * (1.0 - u); };
  auto I = [](double s) {
    double y = 0.25 - s * (1.0 - s);
    return s < 0.5 ? y : -y;
  };
  result = 0.5 * (f(v) + f(w)) - 0.5 * (I(v) - I(w));
  return result;
}
/* SAM_LISTING_END_2 */

/* SAM_LISTING_BEGIN_3 */
Solution solveTrafficFlow() {
  int Ml = 40;
  int Mr = 40;
  int N_half = Mr + Ml + 1;
  int N = 2 * N_half;

  double h = 0.05;
  double tau = h / 3;
  double T = 1.0;
  unsigned int m = (unsigned int)(T / tau);

  // Spatial mesh
  Eigen::VectorXd x = Eigen::VectorXd::LinSpaced(N_half, -2.0, 2.0);
  // Initial condition according to \prbeqref{eq:u0}. Note that we just
  // initialize the cell averages; a more refined implementation would also
  // sample the local first moments of $u_0$.
  Eigen::VectorXd mu0 = Eigen::VectorXd::Zero(N);
  auto u0 = [](double x) { return 0.0 <= x && x <= 1.0 ? 1.0 : 0.0; };
  for (int i = 0; i < N_half; ++i) {
    mu0(2 * i) = u0(x(i));
  }
  // Flux function
  auto f = [](double u) { return u * (1.0 - u); };
  // Perform fully discrete evolution
  Eigen::VectorXd mu = dgcl(mu0, f, Feo, T, Ml, Mr, h, m);
  // Retrieve cell averages
  Eigen::VectorXd u(N_half);
  for (int i = 0; i < N_half; ++i) {
    u(i) = mu(2 * i);
  }

  return Solution(std::move(x), std::move(u));
}
/* SAM_LISTING_END_3 */

}  // namespace DiscontinuousGalerkin1D


#include "mesh_param.h"

#include <Eigen/Geometry>
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>
#include <Eigen/Eigenvalues>
#include <iostream>
#include <igl/grad.h>
#include <igl/slice.h>
#include <igl/cat.h>
#include <igl/doublearea.h>
#include <igl/harmonic.h>
#include <igl/lscm.h>
#include <igl/boundary_loop.h>
#include <igl/map_vertices_to_circle.h>
#include <set>

using namespace std;
using namespace Eigen;

MatrixXd interpolate_field
(
    const MatrixXd& V,          // Vertices of the mesh
    const MatrixXi& F,          // Faces
    const MatrixXi& TT,         // Adjacency triangle-triangle
    const VectorXi& b,          // Constrained faces id
    const MatrixXd& bc          // Cosntrained faces representative vector
)
{
    // Lots of the codes below are based on the codes in tutorial_nrosy.cpp
    // However, there are a lot of modifications

    // Starter code, same as template
    assert(b.size() > 0); // One constraint is necessary to make the solution unique
    Matrix<double,Eigen::Dynamic,3> T1(F.rows(),3), T2(F.rows(),3);
    // Compute the local reference systems for each face
    for (unsigned i=0;i<F.rows();++i)
    {
        Vector3d e1 =  V.row(F(i, 1)) - V.row(F(i, 0));
        Vector3d e2 =  V.row(F(i, 2)) - V.row(F(i, 0));
        T1.row(i) = e1.normalized();
        T2.row(i) = T1.row(i).cross(T1.row(i).cross(e2)).normalized();
    }

    // Build the sparse matrix, with an energy term for each edge
    std::vector< Triplet<std::complex<double> > > t;
    // The b vector is all zeroes so we do not need it here
    // std::vector< Triplet<std::complex<double> > > tb;

    // Handle the free part of matrix A; same as template
    unsigned count = 0;
    for (unsigned f=0;f<F.rows();++f)
    {
        for (unsigned ei=0;ei<F.cols();++ei)
        {
            // Look up the opposite face
            int g = TT(f,ei);
            // If it is a boundary edge, it does not contribute to the energy
            if (g == -1) continue;
            // Avoid to count every edge twice
            if (f > g) continue;
            // Compute the complex representation of the common edge
            Vector3d e  = (V.row(F(f,(ei+1)%3)) - V.row(F(f,ei)));
            Vector2d vef = Vector2d(e.dot(T1.row(f)),e.dot(T2.row(f))).normalized();
            std::complex<double> ef(vef(0),vef(1));
            Vector2d veg = Vector2d(e.dot(T1.row(g)),e.dot(T2.row(g))).normalized();
            std::complex<double> eg(veg(0),veg(1));
            // Add the term conj(f)^n*ui - conj(g)^n*uj to the energy matrix
            t.push_back(Triplet<std::complex<double> >(count,f,    std::conj(ef)));
            t.push_back(Triplet<std::complex<double> >(count,g,-1.*std::conj(eg)));
            ++count;
        }
    }

    // Since tb is not used, comment out the codes related to tb
    // double lambda = 10e6;
    double lambda = 0;
    for (unsigned r=0; r<b.size(); ++r)
    {
        int f = b(r);
        // Vector3d v = bc.row(r);
        // std::complex<double> c(v.dot(T1.row(f)),v.dot(T2.row(f)));
        t.push_back(Triplet<std::complex<double> >(count,f, sqrt(lambda)));
        // tb.push_back(Triplet<std::complex<double> >(count,0, c * std::complex<double>(sqrt(lambda),0)));
        ++count;
    }

    const unsigned fr = F.rows();
    const unsigned bs = b.size();

    // Solve the linear system
    // Here the solving is based on the equation
    // Aff * u = - Afc * xc
    // (#f is the number of total faces)
    // (#b is number of constrained faces)
    // (so #f-#b is the number of free faces)
    // Where Aff is (#f-#b)*(#f-#b) matrix
    //       u is our answer, (#f-#b)*#1 vector
    //       Afc is (#f-#b)*#b matrix
    //       xc is #b*#1 vector
    // Therefore, the right part is (#f-#b)*#1 vector
    // We need A to be a matrix of #count*#f
    // So that Q=A.adjoint()*A is #f*#f
    // Aff is the (#f-#b)*(#f-#b) part of Q
    // Afc is the (#f-#b)*#b part of Q
    typedef SparseMatrix<std::complex<double>> SparseMatrixXcd;
    // SparseMatrixXcd A(count,F.rows());
    SparseMatrixXcd A(count,fr); // #count*#f
    A.setFromTriplets(t.begin(), t.end());
    VectorXcd xc(bs,1); // #b*#1
    for (unsigned r=0; r<b.size(); ++r) {
        int f = b(r);
        Vector3d v = bc.row(r);
        std::complex<double> c(v.dot(T1.row(f)),v.dot(T2.row(f)));
        xc(r) = c;
    }

    #ifdef DEBUG_3
    for (unsigned r=0; r<b.size(); ++r) {
        int f = b(r);
        cout << "representation for vector on face " << f << ": ";
        cout << bc.row(r) << ", " << xc(r) << ", ";
        cout << T1.row(f) * xc(r).real() + T2.row(f) * xc(r).imag() << endl;
    }
    #endif

    SparseMatrixXcd Q = A.adjoint()*A; // #f*#f

    // Get a set of constrained faces
    set<int> constrained_set;
    for (unsigned r=0; r<b.size(); ++r) {
        int f = b(r);
        constrained_set.insert(f);
    }

    // Get the vector representing free faces
    VectorXi vf(fr-bs);
    unsigned findex = 0;
    for (unsigned f=0;f<F.rows();++f) {
        if (constrained_set.find(f) == constrained_set.end()) {
            vf(findex) = f;
            findex++;
        }
    }

    #ifdef DEBUG_5
    cout << "constrained faces: " << endl;
    cout << b << endl;
    cout << xc << endl;
    cout << "--------------" << endl;
    cout << "free faces: " << endl;
    cout << vf << endl;
    #endif

    // Construct Aff
    SparseMatrixXcd Aff(fr-bs, fr-bs); // (#f-#b)*(#f-#b)
    igl::slice(Q, vf, vf, Aff);

    // Construct Afc
    SparseMatrixXcd Afc(fr-bs, bs); // (#f-#b)*#b
    igl::slice(Q, vf, b, Afc);

    MatrixXcd mb = (Afc * xc) * -1; // (#f-#b)*#1
    // SparseMatrixXcd b(count,1);
    // b.setFromTriplets(tb.begin(), tb.end());
    SimplicialLDLT< SparseMatrixXcd > solver;
    solver.compute(Aff);
    // solver.compute(A.adjoint()*A);
    assert(solver.info()==Success);
    MatrixXcd u = solver.solve(mb);
    // MatrixXcd u = solver.solve(A.adjoint()*MatrixXcd(b));
    assert(solver.info()==Success);

    // Finally, rearrange u and store values in ua
    VectorXcd ua(fr);
    for (unsigned ui = 0; ui < u.size(); ui++) {
        ua(vf(ui)) = u(ui);
    }
    for (unsigned r=0; r<b.size(); ++r) {
        ua(b(r)) = xc(r);
    }

    #ifdef DEBUG_3
    cout << "final insertion locations and values: ";
    for (int i = 0; i < bs; i++) {
        cout << b(i) << "," << xc(i) << " ";
    }
    cout << endl;
    #endif

    // Debugging informations
    #ifdef DEBUG_2
    cout << "--------------" << endl;
    cout << "V: " << V.rows() << " * " << V.cols() << endl;
    cout << "F: " << F.rows() << " * " << F.cols() << endl;
    cout << "TT: " << TT.rows() << " * " << TT.cols() << endl;
    cout << "b: " << b.rows() << " * " << b.cols() << endl;
    cout << "vf: " << vf.rows() << " * " << vf.cols() << endl;
    cout << "bc: " << bc.rows() << " * " << bc.cols() << endl;
    cout << "T1: " << T1.rows() << " * " << T1.cols() << endl;
    cout << "T2: " << T2.rows() << " * " << T2.cols() << endl;
    cout << "t: " << t.size() << endl;
    cout << "count = " << count << endl;
    cout << "A: " << A.rows() << " * " << A.cols() << endl;
    cout << "Q: " << Q.rows() << " * " << Q.cols() << endl;
    cout << "xc: " << xc.rows() << " * " << xc.cols() << endl;
    cout << "Aff: " << Aff.rows() << " * " << Aff.cols() << endl;
    cout << "Afc: " << Afc.rows() << " * " << Afc.cols() << endl;
    cout << "mb: " << mb.rows() << " * " << mb.cols() << endl;
    cout << "u: " << u.rows() << " * " << u.cols() << endl;
    cout << "ua: " << ua.rows() << " * " << ua.cols() << endl;
    cout << "--------------" << endl;
    cout << "Q nonzeros: " << Q.nonZeros() << endl;
    cout << "Aff nonzeros: " << Aff.nonZeros() << endl;
    cout << "Afc nonzeros: " << Afc.nonZeros() << endl;
    cout << "--------------" << endl;
    #endif

    // Convert the interpolated polyvector into Euclidean vectors
    MatrixXd R(F.rows(),3);
    for (int f=0; f<ua.size(); ++f) {
        R.row(f) = T1.row(f) * ua(f).real() + T2.row(f) * ua(f).imag();
    }

    #ifdef DEBUG_3
    cout << "final insertion locations and coordinates: ";
    for (int i = 0; i < bs; i++) {
        cout << b(i) << "," << R.row(b(i)) << " ";
    }
    cout << endl;
    #endif

    #ifdef DEBUG_3
    cout << "original locations and coordinates: ";
    for (int i = 0; i < bs; i++) {
        cout << b(i) << "," << bc.row(i) << " ";
    }
    cout << endl;
    #endif

    return R;
}

MatrixXd get_scalar_field
(
    const MatrixXd& V,             // Vertices of the mesh
    const MatrixXi& F,             // Faces
    const MatrixXd& R,             // Vector field
    const SparseMatrix<double>& G  // Gradient operator
)
{
    // Construct matrix A (areas)
    VectorXd A1(F.rows());
    igl::doublearea(V,F,A1);
    SparseMatrix<double> A(F.rows()*3, F.rows()*3); // #3f*#3f
    for (unsigned f = 0; f < F.rows(); ++f) {
        A.insert(f,f) = A1(f);
        A.insert(f+F.rows(),f+F.rows()) = A1(f);
        A.insert(f+F.rows()*2,f+F.rows()*2) = A1(f);
    }

    // Construct matrix u (from R)
    VectorXd u(F.rows()*3); // #3f*#1
    for (unsigned f = 0; f < F.rows(); ++f) {
        u(f) = R(f,0);
        u(f+F.rows()) = R(f,1);
        u(f+2*F.rows()) = R(f,2);
    }

    // Construct K
    SparseMatrix<double> K = G.transpose() * A * G; // #v*#v
    // Construct b
    VectorXd b = G.transpose() * A * u; // #v*#1

    // Row/col removal
    SparseMatrix<double> Kff = K.bottomRightCorner(K.rows()-1, K.cols()-1);
    // SparseMatrix<double> Kfc = K.bottomLeftCorner(K.rows()-1, 1);
    // VectorXd sc(1);
    // sc << 0;
    // VectorXd bt = b.bottomRows(b.rows()-1) - Kfc * sc;
    VectorXd bt = b.bottomRows(b.rows()-1);

    #ifdef DEBUG_7
    cout << "--------------" << endl;
    cout << "V: " << V.rows() << " * " << V.cols() << endl;
    cout << "F: " << F.rows() << " * " << F.cols() << endl;
    cout << "R: " << R.rows() << " * " << R.cols() << endl;
    cout << "G: " << G.rows() << " * " << G.cols() << endl;
    cout << "A: " << A.rows() << " * " << A.cols() << endl;
    cout << "u: " << u.rows() << " * " << u.cols() << endl;
    cout << "K: " << K.rows() << " * " << K.cols() << endl;
    cout << "b: " << b.rows() << " * " << b.cols() << endl;
    cout << "--------------" << endl;
    cout << "G nonzeros: " << G.nonZeros() << endl;
    cout << "A nonzeros: " << A.nonZeros() << endl;
    cout << "K nonzeros: " << K.nonZeros() << endl;
    #endif

    // Solve the system
    SimplicialLDLT<SparseMatrix<double>> solver;
    // solver.compute(K);
    // assert(solver.info() == Success);
    // MatrixXd s = solver.solve(b);
    // assert(solver.info() == Success);
    solver.compute(Kff);
    assert(solver.info() == Success);
    MatrixXd sf = solver.solve(bt);
    assert(solver.info() == Success);
    VectorXd s(K.rows());
    s(0) = 0;
    for (unsigned i = 1; i < K.rows(); i++) {
        s(i) = sf(i-1);
    }

    // Debugging informations
    #ifdef DEBUG_7
    cout << "s: " << s.rows() << " * " << s.cols() << endl;
    cout << "--------------" << endl;
    #endif

    return s;
}

MatrixXd harmonic_param
(
    const MatrixXd& V,             // Vertices of the mesh
    const MatrixXi& F              // Faces
)
{
    // Find the open boundary
    VectorXi bnd;
    igl::boundary_loop(F, bnd);
    // Map the boundary to a circle
    MatrixXd bnd_uv;
    igl::map_vertices_to_circle(V, bnd, bnd_uv);
    // Harmonic parameterizations
    MatrixXd V_uv;
    igl::harmonic(V,F,bnd,bnd_uv,1,V_uv);
    V_uv *= 8;

    #ifdef DEBUG_9
    cout << "--------------" << endl;
    cout << "V: " << V.rows() << " * " << V.cols() << endl;
    cout << "F: " << F.rows() << " * " << F.cols() << endl;
    cout << "bnd: " << bnd.rows() << " * " << bnd.cols() << endl;
    cout << "bnd_uv: " << bnd_uv.rows() << " * " << bnd_uv.cols() << endl;
    cout << "V_uv: " << V_uv.rows() << " * " << V_uv.cols() << endl;
    cout << "--------------" << endl;
    #endif

    return V_uv;
}

MatrixXd lscm_param
(
    const MatrixXd& V,             // Vertices of the mesh
    const MatrixXi& F              // Faces
)
{
    // Fix two points on the boundary
    VectorXi bnd, b(2,1);
    igl::boundary_loop(F,bnd);
    b(0) = bnd(0);
    b(1) = bnd(round(bnd.size()/2));
    MatrixXd bc(2,2);
    bc << 0,0,1,0;
    // LSCM parametrizations
    MatrixXd V_uv;
    igl::lscm(V,F,b,bc,V_uv);
    V_uv *= 8;

    #ifdef DEBUG_9
    cout << "--------------" << endl;
    cout << "V: " << V.rows() << " * " << V.cols() << endl;
    cout << "F: " << F.rows() << " * " << F.cols() << endl;
    cout << "bnd: " << bnd.rows() << " * " << bnd.cols() << endl;
    cout << "V_uv: " << V_uv.rows() << " * " << V_uv.cols() << endl;
    cout << "--------------" << endl;
    #endif

    return V_uv;
}


/*
 *  Copyright (c) 2015-, Filippo Basso <bassofil@gmail.com>
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *     1. Redistributions of source code must retain the above copyright
 *        notice, this list of conditions and the following disclaimer.
 *     2. 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.
 *     3. Neither the name of the copyright holder(s) 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.
 */

#ifndef UNIPD_CALIBRATION_IMPL_CALIBRATION_ALGORITHMS_CERES_PLANE_FIT_HPP_
#define UNIPD_CALIBRATION_IMPL_CALIBRATION_ALGORITHMS_CERES_PLANE_FIT_HPP_

#include <calibration_algorithms/plane_fit.h>
#include <Eigen/Eigenvalues>

namespace unipd
{
namespace calib
{

template <typename ScalarT_>
  Plane3_<ScalarT_>
  plane_fit (const Cloud3_<ScalarT_> & points)
  {
    assert(points.elements() > 1);

    Point3_<ScalarT_> centroid = Point3_<ScalarT_>::Zero();
    typename Cloud3_<ScalarT_>::Container diff(3, points.elements());
    Size1 count = 0;
    for (Size1 i = 0; i < points.elements(); ++i)
    {
      if (points[i].allFinite())
      {
        centroid += points[i];
        diff.col(count++) = points[i];
      }
    }
    centroid /= count;
    diff.conservativeResize(3, count);
    diff.colwise() -= centroid;

    Eigen::Matrix<ScalarT_, 3, 3> covariance_matrix = diff * diff.transpose() / ScalarT_(count - 1);
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<ScalarT_, 3, 3> > solver(covariance_matrix, Eigen::ComputeEigenvectors);

    Point3_<ScalarT_> eigen_vector = solver.eigenvectors().col(0);
    return Plane3_<ScalarT_>(eigen_vector, -eigen_vector.dot(centroid));
  }

} // namespace calib
} // namespace unipd
#endif // UNIPD_CALIBRATION_IMPL_CALIBRATION_ALGORITHMS_CERES_PLANE_FIT_HPP_


#include "tetelem.hpp"
#include <Eigen/LU>

#define ZERO_DET_EPS 1e-14

TetElem::TetElem(std::string& log_buf)
    : log_buf_(log_buf)
{}

bool TetElem::Init(const uint i_elem,
                   const Mat &node_mat,
                   const real e_modulus,
                   const real nu)
{
    node_mat_ = node_mat;
    e_modulus_ = e_modulus;
    nu_ = nu;

    A_ = Mat::Zero(4, 4);
    A_ << Vec::Ones(4, 1), node_mat_.block(0, 0, 4, 3);
    V6_ = A_.determinant();

    if (abs(V6_) == 0.0)
    {
        log_buf_ = "Zero determinant for the " + std::to_string(i_elem) + "th element.";
        return false;
    }

    const Mat tmp = V6_ * A_.inverse().transpose();
    a_ = tmp.col(0);
    b_ = tmp.col(1);
    c_ = tmp.col(2);
    d_ = tmp.col(3);

    D_ = Mat::Zero(6, 6);
    D_(0, 0) = D_(1, 1) = D_(2, 2) = e_modulus*(1.0 - nu) / ((1.0 + nu) * (1.0 - 2*nu));
    D_(0, 1) = D_(0, 2) = D_(1, 2) =
    D_(1, 0) = D_(2, 0) = D_(2, 1) = e_modulus*nu / ((1.0 + nu) * (1.0 - 2*nu));
    D_(3, 3) = D_(4, 4) = D_(5, 5) = e_modulus / (2.0 + 2.0*nu);

    return true;
}

Mat TetElem::BuildStifMat() const
{
    Mat B = Mat::Zero(6, 3*4);
    BuildShapeFuncDerMat(B);

    return (V6_/6.) * B.transpose() * D_ * B;
}

void TetElem::BuildShapeFuncDerMat(Mat& B) const
{
    for(int i = 0; i < 4; ++i)
    {
        B(0, 0 + 3*i) = b_[i];
        B(1, 1 + 3*i) = c_[i];
        B(2, 2 + 3*i) = d_[i];
        B(3, 0 + 3*i) = c_[i];
        B(3, 1 + 3*i) = b_[i];
        B(4, 1 + 3*i) = d_[i];
        B(4, 2 + 3*i) = c_[i];
        B(5, 0 + 3*i) = d_[i];
        B(5, 2 + 3*i) = b_[i];
    }

    B /= V6_;
}

void TetElem::BuildShapeFuncDerRow(Vec& row, const int idx) const
{
    switch(idx)
    {
    case 0: for(int i=0;i<4;++i) row(0 + 3*i) = b_[i]; break;
    case 1: for(int i=0;i<4;++i) row(1 + 3*i) = c_[i]; break;
    case 2: for(int i=0;i<4;++i) row(2 + 3*i) = d_[i]; break;
    case 3: for(int i=0;i<4;++i){row(0 + 3*i) = c_[i]; row(1 + 3*i) = b_[i];} break;
    case 4: for(int i=0;i<4;++i){row(1 + 3*i) = d_[i]; row(2 + 3*i) = c_[i];} break;
    case 5: for(int i=0;i<4;++i){row(0 + 3*i) = d_[i]; row(2 + 3*i) = b_[i];} break;
    default: return;
    }

    row /= V6_;
}

Mat TetElem::BuildIsoMatHookeTensor(const real e_modulus,
                                    const real nu)
{
    Mat m = Mat::Zero(6, 6);
    m(0, 0) = m(1, 1) = m(2, 2) = 1 - nu;
    m(3, 3) = m(4, 4) = m(5, 5) = 0.5 - nu;
    m(0, 1)=m(1, 0) = m(0, 2)=m(2, 0) = m(1, 2)=m(1, 2) = nu;

    return e_modulus / ((1 + nu)*(1 - 2*nu)) * m;
}


#include <iostream>
#include <Eigen/Dense>
#include "admmSolver.hpp"

int main(int argc, char **argv)
{
    Eigen::VectorXf mu = Eigen::VectorXf(4);
    const float regularization = 0.1f;
    const float volatility_target = 0.05; // 5% max vol

    // Vector of asset expected returns
    mu << 0.1, 0.3, -0.5, 0.6;

    Eigen::MatrixXf Sigma(4,4);
    // Sample covariance matrix (positive-definite)
    Sigma << 4.2,   0.3,    0.1,    1.25,
             0.3,   5.95,   0.56,   1.56,
             0.1,   0.56,   4.45,   0.25,
             1.25,  1.56,   0.25,   4.56;

    Eigen::VectorXf x0 = Eigen::VectorXf::Zero(4);

    // Regularized markowitz optimization

    proxopp::ptfVolConstrainedL2 optim(mu, Sigma, x0, volatility_target, regularization);
    optim.setVerbose(1);

    Eigen::VectorXf ptf = optim.solve();
    std::cout << "Solution: \n";
    std::cout << ptf << std::endl;
    std::cout << "(ex-ante) Volatility :" << ptf.dot(Sigma*ptf) << std::endl;
    return 0;
}


#include "slamdunk/include/transformation_estimation.h"
#include "slamdunk/include/pretty_printer.h"

#include <iostream>
#include <boost/random/variate_generator.hpp>
#include <boost/random/uniform_int.hpp>
#include <Eigen/SVD>
#include <Eigen/LU> // required for determinant computation

#include <android/log.h>

#define  LOG_TAG4	"slamdunk_app"
#define  LOGI4(...)	__android_log_print(ANDROID_LOG_INFO, LOG_TAG4, __VA_ARGS__)

void SLAM_DUNK_API slamdunk::estimateTransformationSVD(const std::vector<Eigen::Vector3f>& reference,
                                                const std::vector<Eigen::Vector3f>& query,
                                                const std::vector< std::pair<unsigned, unsigned> >& ref_query_ids,
                                                Eigen::Isometry3d& queryToRef)
{
  const unsigned no_of_matches = ref_query_ids.size();
  Eigen::Vector3f mean_ref(0,0,0), mean_qry(0,0,0);
  for(unsigned rqidx = 0; rqidx < no_of_matches; ++rqidx)
  {
    mean_ref += reference[ ref_query_ids[rqidx].first ];
    mean_qry += query[ ref_query_ids[rqidx].second ];
  }
  mean_ref /= (float)no_of_matches;
  mean_qry /= (float)no_of_matches;

  Eigen::Matrix3d H = Eigen::Matrix3d::Zero();
  for(unsigned rqidx = 0; rqidx < no_of_matches; ++rqidx)
  {
    H.noalias() += (query[ ref_query_ids[rqidx].second ] - mean_qry).cast<double>() *
                   (reference[ ref_query_ids[rqidx].first ] - mean_ref).cast<double>().transpose();
  }
  // SVD
  Eigen::JacobiSVD<Eigen::Matrix3d> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix3d V = svd.matrixV();
  Eigen::Matrix3d R = V * svd.matrixU().transpose();
  if(R.determinant() < 0.0)
  {
    V.col(2) = V.col(2)*-1.0;
    R.noalias() = V * svd.matrixU().transpose();
  }

  queryToRef.setIdentity();
  queryToRef.rotate(R);
  queryToRef.translation() = (mean_ref.cast<double>() - R*mean_qry.cast<double>());
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////
bool slamdunk::RANSAC::findInliers(const std::vector<Eigen::Vector3f>& reference,
                                const std::vector<Eigen::Vector3f>& query,
                                const std::vector< std::pair<unsigned, unsigned> >& ref_query_ids)
{
  if(ref_query_ids.size() < 3)
  {
    if(m_verbose)
      std::cout << SLAM_DUNK_WARNING_STR("Less than 3 matches provided") << std::endl;
    m_inlier_indices.clear();
    return false;
  }

  const unsigned no_of_matches = ref_query_ids.size();
  if(m_deterministic)
    m_rnd_eng.seed(42u);
  boost::variate_generator< boost::mt19937&, boost::uniform_int<unsigned> > rnd_gen(m_rnd_eng, boost::uniform_int<unsigned>(0, no_of_matches-1));
  unsigned current_max_iterations = m_max_iterations;

  unsigned a, b, c;
  Eigen::Matrix3d ref_mtx, qry_mtx;
  Eigen::Isometry3d current_transformation = Eigen::Isometry3d::Identity();
  unsigned best_score = 0;

  const double no_of_matches_inverse = 1.0/(double)no_of_matches;
  const double lognum = std::log(1-m_probability);

  /*std::stringstream sst;
  sst << "RANSAC MATCHES NUMBER: " << no_of_matches;
  LOGI4(sst.str().data());*/

  unsigned it = 0;
  int k = 0;
  for(; it < current_max_iterations; ++it)
  {
    a = rnd_gen();
    do b = rnd_gen(); while(a == b);
    do c = rnd_gen(); while(c == b || c == a);

    /*std::stringstream ss;
    ss << "RANSAC IDS: (" << a << ", " << b << ", " << c << ")";
    LOGI4(ss.str().data());*/

    ref_mtx.row(0) = reference[ ref_query_ids[a].first ].cast<double>();
    ref_mtx.row(1) = reference[ ref_query_ids[b].first ].cast<double>();
    ref_mtx.row(2) = reference[ ref_query_ids[c].first ].cast<double>();
    qry_mtx.col(0) = query[ ref_query_ids[a].second ].cast<double>();
    qry_mtx.col(1) = query[ ref_query_ids[b].second ].cast<double>();
    qry_mtx.col(2) = query[ ref_query_ids[c].second ].cast<double>();

    /*Eigen::Vector3d ref1 = reference[ ref_query_ids[a].first ].cast<double>();
    Eigen::Vector3d ref2 = reference[ ref_query_ids[b].first ].cast<double>();
    Eigen::Vector3d ref3 = reference[ ref_query_ids[c].first ].cast<double>();
    Eigen::Vector3d query1 = query[ ref_query_ids[a].second ].cast<double>();
    Eigen::Vector3d query2 = query[ ref_query_ids[b].second ].cast<double>();
    Eigen::Vector3d query3 = query[ ref_query_ids[c].second ].cast<double>();

    std::stringstream ssx1;
    ssx1 << "REF1: (" << ref1.x() << ", " << ref1.y() << ", " << ref1.z() << ")";
    LOGI4(ssx1.str().data());
    std::stringstream ssx2;
    ssx2 << "REF2: (" << ref2.x() << ", " << ref2.y() << ", " << ref2.z() << ")";
    LOGI4(ssx2.str().data());
    std::stringstream ssx3;
    ssx3 << "REF3: (" << ref3.x() << ", " << ref3.y() << ", " << ref3.z() << ")";
    LOGI4(ssx3.str().data());
    std::stringstream ssx4;
    ssx4 << "QUERY1: (" << query1.x() << ", " << query1.y() << ", " << query1.z() << ")";
    LOGI4(ssx4.str().data());
    std::stringstream ssx5;
    ssx5 << "QUERY2: (" << query2.x() << ", " << query2.y() << ", " << query2.z() << ")";
    LOGI4(ssx5.str().data());
    std::stringstream ssx6;
    ssx6 << "QUERY3: (" << query3.x() << ", " << query3.y() << ", " << query3.z() << ")";
    LOGI4(ssx6.str().data());*/

    // compute centroids
    const Eigen::Vector3d cRef = (ref_mtx.row(0)+ref_mtx.row(1)+ref_mtx.row(2))*(0.33333333333333333333);
    const Eigen::Vector3d cQry = (qry_mtx.col(0)+qry_mtx.col(1)+qry_mtx.col(2))*(0.33333333333333333333);

    ref_mtx.row(0) -= cRef;
    ref_mtx.row(1) -= cRef;
    ref_mtx.row(2) -= cRef;
    qry_mtx.col(0) -= cQry;
    qry_mtx.col(1) -= cQry;
    qry_mtx.col(2) -= cQry;
    const Eigen::Matrix3d H = qry_mtx*ref_mtx;

    // SVD
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3d V = svd.matrixV();
    Eigen::Matrix3d R = V * svd.matrixU().transpose();
    if(R.determinant() < 0.0)
    {
      V.col(2) = V.col(2)*-1.0;
      R.noalias() = V * svd.matrixU().transpose();
    }
    current_transformation = Eigen::Isometry3d(R);
    current_transformation.translation() = cRef-R*cQry;

    // voting
    unsigned current_score = 0;
    for(unsigned rqidx = 0; rqidx < no_of_matches; ++rqidx)
      if((current_transformation * query[ ref_query_ids[rqidx].second ].cast<double>()
            - reference[ ref_query_ids[rqidx].first ].cast<double>()).squaredNorm() <= m_squared_threshold)
        ++current_score;  // TODO: weight the actual distance

    // update
    if(current_score > best_score)
    {
      best_score = current_score;
      m_query2ref = current_transformation;

      if(best_score == no_of_matches)
        current_max_iterations = 0;
      else
        current_max_iterations = std::min((double)m_max_iterations, std::ceil(lognum / std::log(1.0 - std::pow((double)best_score * no_of_matches_inverse, 3.0))));
    }
  }

  if(m_verbose)
    std::cout << SLAM_DUNK_INFO_STR("No of iterations: " << it) << std::endl;

  if(best_score < 4)
  {
    if(m_verbose)
      std::cout << SLAM_DUNK_WARNING_STR("Empty inlier set") << std::endl;
    m_inlier_indices.clear();
    return false;
  }

  // store indices
  m_inlier_indices.clear();
  m_inlier_indices.reserve(best_score);
  for(unsigned rqidx = 0; rqidx < no_of_matches; ++rqidx)
    if((m_query2ref * query[ ref_query_ids[rqidx].second ].cast<double>()
          - reference[ ref_query_ids[rqidx].first ].cast<double>()).squaredNorm() <= m_squared_threshold)
      m_inlier_indices.push_back(rqidx);

  return true;
}


// functions to calculate X^2 (Pearson's cumulative test statistic)
// and significance (1 - P(X^2/2, k/2)) for k categories.

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

#include <iostream>
#include <cmath>
#include <vector>
#include "ChiSquared.hxx"

double ChiSquared::pearsonCTS(const std::vector<int> & S,
		  const std::vector<int> & X)
{
  // The inputs S[i] and X[i] are in the form of "number of samples in
  // category [i] for the "standard" case (the histogram that fits
  // the null hypothesis) and the sample to be tested.

  // first make the p[i] (normative probability of an occurance in
  // bucket [i].
  int num_Ssamps = 0;
  int N = 0;
  for(int i = 0; i < S.size(); i++) {
    num_Ssamps += S[i];
    N += X[i];
  }

  std::vector<double> p;
  double rtsamps = 1.0 / ((double) num_Ssamps);
  for(int i = 0; i < S.size(); i++) {
    double v = (double) S[i];
    p.push_back(v * rtsamps);
  }

  // check
  double sum_c = 0.0;
  for(double pi: p) {
    sum_c += pi;
  }
  std::cerr << "sum_c = " << sum_c << " (1-sum_c) = " << (1.0 - sum_c) << std::endl;

  double X2 = -1.0 * ((double) N);

  for(int i = 0; i < S.size(); i++) {
    double m = N * p[i];
    double xi = ((double) X[i]);

    X2 += (xi * xi / m);
  }

  return X2;
}

double ChiSquared::test(const std::vector<int> & S, const std::vector<int> X)
{
  int k = S.size();

  return 1.0 - boost::math::gamma_p(((double) k) * 0.5, pearsonCTS(S, X) * 0.5);
}