#include <mex.h>
#include <math.h>
#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;
using namespace std;

double amips_param;
double energy_min;
double energy_max = 10;
double c1_param;
double c2_param;
double d1_param;

Vector3d color_map(double v)
{
	Vector3d c;

    c[0] = c[1] = c[2] = 1;

	double dv;

	if (v < energy_min)
		v = energy_min;
	if (v > energy_max)
		v = energy_max;
	dv = energy_max - energy_min;

	if (v < (energy_min + 0.25 * dv)) {
		c[0] = 0;
		c[1] = 4 * (v - energy_min) / dv;
	}
	else if (v < (energy_min + 0.5 * dv)) {
		c[0] = 0;
		c[2] = 1 + 4 * (energy_min + 0.25 * dv - v) / dv;
	}
	else if (v < (energy_min + 0.75 * dv)) {
		c[0] = 4 * (v - energy_min - 0.5 * dv) / dv;
		c[2] = 0;
	}
	else {
		c[1] = 1 + 4 * (energy_min + 0.75 * dv - v) / dv;
		c[2] = 0;
	}

	return c;
}

void set_energy_min(int type)
{

    switch(type)
    {
        case 0: //arap
            energy_min = 0;
            break;

        case 1: // mips
            energy_min = 2;
            break;

        case 2: // iso
            energy_min = 4;
            break;

        case 3: // amips
            energy_min = exp(amips_param * 2);
            break;

        case 4: // conf
            energy_min = 1;
            break;

        case 5:
            energy_min = -c1_param - 2 * c2_param;
            break;
    }

}

double energy_value(int type, Vector2d S)
{

    double value = 0;
    double J;
    double l1;
    double l2;

    switch(type)
    {
        case 0: //arap
            value = (S[0] - 1) * (S[0] - 1) + (S[1] - 1) * (S[1] - 1);
            break;

        case 1: // mips
            value = S[0] / S[1] + S[1] / S[0];
            break;

        case 2: // iso
            value = S[0] * S[0] + 1.0 / (S[0] * S[0]) + S[1] * S[1] + 1.0 / (S[1] * S[1]);
            break;

        case 3: // amips
            value = exp(amips_param * (S[0] / S[1] + S[1] / S[0]));
            break;

        case 4: // conf
            value = S[0] / S[1];
            value *= value;
            break;

        case 5: // gmr
            J = S[0] * S[1];
            l1 = S[0] * S[0] + S[1] * S[1];
            l2 = S[0] * S[0] * S[1] * S[1];

            value = c1_param * (pow(J, -2.0 / 3.0) * l1 - 3) + c2_param * (pow(J, -4.0 / 3.0) * l2 - 3) + d1_param * (J - 1) * (J - 1);
            break;

        case 6: // olg

            value = max(S[0], 1.0 / S[1]);
            break;
    }

    return value;

}

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

    mxArray *output_mex;
    const int *dims;
    double *tri_num, *X_g_inv, *tri_areas, *obj_tri, *q_target, *type, *amips_s, *c1, *c2, *d1;
    double *output;

    tri_num = mxGetPr(prhs[0]);
    X_g_inv = mxGetPr(prhs[1]);
    tri_areas = mxGetPr(prhs[2]);
    obj_tri = mxGetPr(prhs[3]);
    q_target = mxGetPr(prhs[4]);
    type = mxGetPr(prhs[5]);
    amips_s = mxGetPr(prhs[6]);
    c1 = mxGetPr(prhs[7]);
    c2 = mxGetPr(prhs[8]);
    d1 = mxGetPr(prhs[9]);

    int tri_n = tri_num[0];
    int energy_type = type[0];
    amips_param = amips_s[0];
    c1_param = c1[0];
    c2_param = c2[0];
    d1_param = d1[0];

    output_mex = plhs[0] = mxCreateDoubleMatrix(tri_n, 1, mxREAL);

    output = mxGetPr(output_mex);

    int tri[3];
    double tri_area;

    Matrix2d B, X_f, A;
    Vector2d S;
    Matrix2d U, V;

    for(int i = 0; i < tri_n; i++)
    {
        //mexPrintf("%d %d %d\n", (int)obj_tri[i], (int)obj_tri[i + tri_n], (int)obj_tri[i + 2 * tri_n]);
        tri[0] = obj_tri[i] - 1;
        tri[1] = obj_tri[i + tri_n] - 1;
        tri[2] = obj_tri[i + 2 * tri_n] - 1;
        tri_area = tri_areas[i];

        B(0, 0) = X_g_inv[i];
        B(0, 1) = X_g_inv[i + tri_n * 2];
        B(1, 0) = X_g_inv[i + tri_n];
        B(1, 1) = X_g_inv[i + tri_n * 3];

        X_f(0, 0) = q_target[2 * tri[1]] - q_target[2 * tri[0]];
        X_f(1, 0) = q_target[2 * tri[1] + 1] - q_target[2 * tri[0] + 1];
        X_f(0, 1) = q_target[2 * tri[2]] - q_target[2 * tri[0]];
        X_f(1, 1) = q_target[2 * tri[2] + 1] - q_target[2 * tri[0] + 1];

        A = X_f * B;

        if(A.determinant() <= 0)
        {
            //mexPrintf("element inverted");
        }

        JacobiSVD<Matrix2d> svd(A, ComputeFullU | ComputeFullV);

        S = svd.singularValues();
        U = svd.matrixU();
        V = svd.matrixV();

        output[i] = energy_value(energy_type, S);

    }

    return;

}


#include "maxwellJuttner.h"

#include <fmath/mathFunctions.h>
#include <fmath/physics.h>
#include <boost/math/special_functions/bessel.hpp>

double f_norm(double g, double norm_temp)
{
	double beta = sqrt(1.0-1.0/P2(g));

	return P2(g)*beta*exp(-g/norm_temp);

}

double maxwellRel(double gamma, double norm_temp, double norm)
{

	double beta = sqrt(1.0-1.0/(gamma*gamma));

	double K2 =  boost::math::cyl_bessel_k(2, 1.0/norm_temp); //bessk(2, 1.0/norm_temp);

	double dist_g = gamma*gamma*beta*exp(-gamma/norm_temp) / (K2*norm_temp);

	return  dist_g*norm;

}


/**
    @file Project_1.cpp

    @author Terence Henriod

    Project 1: Bayesion Minimum Error Classification

    @brief The driver program for use of a Bayesian Minimum Error Classifier.

    @version Original Code 1.00 (3/8/2014) - T. Henriod

    UNOFFICIALLY:
    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    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
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.

Compilation notes:
g++ -I /home/thenriod/Desktop/cpp_libs/Eigen_lib/ Project_1.cpp

*/

/*==============================================================================
=======     HEADER FILES     ===================================================
==============================================================================*/
#include <cmath>
#include <iostream>

#include "bayes_classifier.cpp"
#include <Eigen/Dense>  // -I /home/thenriod/Desktop/cpp_libs/Eigen_lib

using namespace std;

/*==============================================================================
=======     USER DEFINED TYPES     =============================================
==============================================================================*/

/*==============================================================================
=======     CONSTANTS / MACROS     =============================================
==============================================================================*/

/*==============================================================================
=======     GLOBAL VARIABLES     ===============================================
==============================================================================*/
  // none

/*==============================================================================
=======     FUNCTION PROTOTYPES     ============================================
==============================================================================*/

/*==============================================================================
=======     MAIN FUNCTION     ==================================================
==============================================================================*/

/**
main

The main driver

@param

@return

@pre
-#

@post
-#

@code
@endcode
*/

int main( int argc, char** argv )
{
  // variables
  BayesClassifier problem_solver;
  double part_A_prior_probability = 0.5;
  double part_B_prior_probability = 0.3;
  Eigen::Vector2d part_one_mean_one;
    part_one_mean_one << 1.502, 1.484;
  Eigen::Vector2d part_one_mean_two;
    part_one_mean_two << 2.499, 2.497;
  Eigen::Vector2d part_two_mean_one;
    part_two_mean_one << 1, 2;
  Eigen::Vector2d part_two_mean_two;
    part_two_mean_two << 1, 4;
  Eigen::Matrix2d part_one_covariance_one;
    part_one_covariance_one << 1.099, 0,
                               0, 1.099;
  Eigen::Matrix2d part_one_covariance_two;
    part_one_covariance_two << 1.813, 0,
                               0, 1.813;
  Eigen::Matrix2d part_two_covariance_one;
    part_two_covariance_one << 1, 0,
                               0, 1;
  Eigen::Matrix2d part_two_covariance_two;
    part_two_covariance_two << 3, 0,
                               0, 2;

  // setup for problem 1
  problem_solver.setMean( part_one_mean_one, CLASS_ONE );
  problem_solver.setMean( part_one_mean_two, CLASS_TWO );
  problem_solver.setCovariance( part_one_covariance_one, CLASS_ONE );
  problem_solver.setCovariance( part_one_covariance_two, CLASS_TWO );
  problem_solver.setPriorProbabilities( part_A_prior_probability );
  problem_solver.setAssumptionCase( CASE_ONE );

  // solve 1.a
//  problem_solver.performAnalysis( "P1_data.txt", "test_1A_output.txt" );
problem_solver.performAnalysis( "Bebis11.txt", "Bebis1A.txt" );

  // solve 1.b
  problem_solver.setPriorProbabilities( part_B_prior_probability );
//  problem_solver.performAnalysis( "P1_data.txt", "test_1B_output.txt" );
problem_solver.performAnalysis( "Bebis11.txt", "Bebis1B.txt" );

  // setup for problem 2
  problem_solver.setMean( part_two_mean_one, CLASS_ONE );
  problem_solver.setMean( part_two_mean_two, CLASS_TWO );
  problem_solver.setCovariance( part_two_covariance_one, CLASS_ONE );
  problem_solver.setCovariance( part_two_covariance_two, CLASS_TWO );
  problem_solver.setPriorProbabilities( part_A_prior_probability );
  problem_solver.setAssumptionCase( CASE_THREE );

  // solve 2.a
//  problem_solver.performAnalysis( "P2_data.txt", "test_2A_output.txt" );
problem_solver.performAnalysis( "Bebis21.txt", "Bebis2A.txt" );

  // solve 2.b
  problem_solver.setPriorProbabilities( part_B_prior_probability );
//  problem_solver.performAnalysis( "P2_data.txt", "test_2B_output.txt" );
problem_solver.performAnalysis( "Bebis21.txt", "Bebis2B.txt" );

  // end program
  return 0;
}

/*==============================================================================
=======     FUNCTION IMPLEMENTATIONS     =======================================
==============================================================================*/


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2015 Rokko Developers https://github.com/t-sakashita/rokko
*
* Distributed under the Boost Software License, Version 1.0. (See accompanying
* file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*
*****************************************************************************/

#include <rokko/rokko.hpp>
#include <rokko/lapack.h>
#include <boost/lexical_cast.hpp>

typedef rokko::localized_vector<double> vector_t;
typedef rokko::localized_matrix<std::complex<double>, rokko::matrix_col_major> matrix_t;

int main(int argc, char *argv[]) {
  int info;
  int m = 6;
  int n = 4;
  if (argc > 2) {
    m = boost::lexical_cast<int>(argv[1]);
    n = boost::lexical_cast<int>(argv[2]);
  }
  std::cout << "m = " << m << "\nn = " << n << std::endl;
  int k = std::min(m, n);

  // generate random martix
  matrix_t mat = matrix_t::Random(m, n);
  std::cout << "Input random matrix A:\n" << mat << std::endl;

  // singular value decomposition
  matrix_t a = mat; // 'a' will be destroyed by dgesvd
  vector_t s(k), superb(k);
  matrix_t u(m, k), vt(k, n);
  info = LAPACKE_zgesvd(LAPACK_COL_MAJOR, 'S', 'S', m, n, &a(0, 0), m, &s(0),
                         &u(0, 0), m, &vt(0, 0), k, &superb(0));
  std::cout << "U:\n" << u << std::endl;
  std::cout << "S:\n" << s << std::endl;
  std::cout << "Vt:\n" << vt << std::endl;

  // check correctness of SVD
  matrix_t smat = matrix_t::Zero(k, k);
  for (int i = 0; i < k; ++i) smat(i, i) = s(i);
  matrix_t check = u * smat * vt;
  std::cout << "U * S * Vt:\n" << check << std::endl;
  std::cout << "| A - U * S * Vt | = " << (mat - check).norm() << std::endl;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2020-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Nikita Kaskov <nbering@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_MATH_UNITY_ROOT_HPP
#define CRYPTO3_MATH_UNITY_ROOT_HPP

#include <type_traits>
#include <complex>

#include <boost/math/constants/constants.hpp>
#include <nil/crypto3/algebra/fields/params.hpp>

namespace nil {
    namespace crypto3 {
        namespace math {

            template<typename FieldType>
            constexpr typename std::enable_if<std::is_same<typename FieldType::value_type, std::complex<double>>::value,
                                              typename FieldType::value_type>::type
                unity_root(const std::size_t n) {
                const double PI = boost::math::constants::pi<double>();

                return typename FieldType::value_type(cos(2 * PI / n), sin(2 * PI / n));
            }

            template<typename FieldType>
            constexpr
                typename std::enable_if<!std::is_same<typename FieldType::value_type, std::complex<double>>::value,
                                        typename FieldType::value_type>::type
                unity_root(const std::size_t n) {

                typedef typename FieldType::value_type value_type;

                const std::size_t logn = std::ceil(std::log2(n));

                if (n != (1u << logn)) {
                    throw std::invalid_argument("expected n == (1u << logn)");
                }
                if (logn > algebra::fields::arithmetic_params<FieldType>::s) {
                    throw std::invalid_argument("expected logn <= arithmetic_params<FieldType>::s");
                }

                value_type omega = value_type(algebra::fields::arithmetic_params<FieldType>::root_of_unity);
                for (std::size_t i = algebra::fields::arithmetic_params<FieldType>::s; i > logn; --i) {
                    omega *= omega;
                }

                return omega;
            }
        }    // namespace math
    }        // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_MATH_UNITY_ROOT_HPP


/** @file main.cpp Demonstrates arbitrary-precision factorial and Fibonacci
  * functions using Boost.Multiprecision.
  *
  * @note This implementation uses output parameters to allow reuse of
  * previously allocated memory.  The technique also supports the potential
  * future use of custom allocators, since the caller of each function
  * allocates and configures the result object.  Moreover, the use of in-place
  * operators for assignments avoids the creation of unnecessary temporary
  * objects.
  *
  * @todo Memoize functions.  They currently require O(n) time per call.
  */

#include <boost/multiprecision/cpp_int.hpp>
#include <iostream>

using big_int = boost::multiprecision::cpp_int;

using std::swap;

void factorial(big_int* r, int n)
{
    for (*r = 1; n > 1; --n) {
        *r *= n;
    }
}

void fibonacci(big_int* r, int n)
{
    *r = 1;
    for (big_int s = 1; n > 0; --n) {
        swap(*r, s);
        s += *r;
    }
}

int main()
{
    big_int r;
    for (int i = 0; i < 100; ++i) {
        factorial(&r, i);
        std::cout << i << ' ' << r << ' ';
        fibonacci(&r, i);
        std::cout << r << '\n';
    }
}


//  (C) Copyright Nick Thompson 2018.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_TOOLS_BIVARIATE_STATISTICS_HPP
#define BOOST_MATH_TOOLS_BIVARIATE_STATISTICS_HPP

#include <iterator>
#include <tuple>
#include <boost/math/tools/assert.hpp>
#include <boost/math/tools/header_deprecated.hpp>

BOOST_MATH_HEADER_DEPRECATED("<boost/math/statistics/bivariate_statistics.hpp>");

namespace boost{ namespace math{ namespace tools {

template<class Container>
auto means_and_covariance(Container const & u, Container const & v)
{
    using Real = typename Container::value_type;
    using std::size;
    BOOST_MATH_ASSERT_MSG(size(u) == size(v), "The size of each vector must be the same to compute covariance.");
    BOOST_MATH_ASSERT_MSG(size(u) > 0, "Computing covariance requires at least one sample.");

    // See Equation III.9 of "Numerically Stable, Single-Pass, Parallel Statistics Algorithms", Bennet et al.
    Real cov = 0;
    Real mu_u = u[0];
    Real mu_v = v[0];

    for(size_t i = 1; i < size(u); ++i)
    {
        Real u_tmp = (u[i] - mu_u)/(i+1);
        Real v_tmp = v[i] - mu_v;
        cov += i*u_tmp*v_tmp;
        mu_u = mu_u + u_tmp;
        mu_v = mu_v + v_tmp/(i+1);
    }

    return std::make_tuple(mu_u, mu_v, cov/size(u));
}

template<class Container>
auto covariance(Container const & u, Container const & v)
{
    auto [mu_u, mu_v, cov] = boost::math::tools::means_and_covariance(u, v);
    return cov;
}

template<class Container>
auto correlation_coefficient(Container const & u, Container const & v)
{
    using Real = typename Container::value_type;
    using std::size;
    BOOST_MATH_ASSERT_MSG(size(u) == size(v), "The size of each vector must be the same to compute covariance.");
    BOOST_MATH_ASSERT_MSG(size(u) > 0, "Computing covariance requires at least two samples.");

    Real cov = 0;
    Real mu_u = u[0];
    Real mu_v = v[0];
    Real Qu = 0;
    Real Qv = 0;

    for(size_t i = 1; i < size(u); ++i)
    {
        Real u_tmp = u[i] - mu_u;
        Real v_tmp = v[i] - mu_v;
        Qu = Qu + (i*u_tmp*u_tmp)/(i+1);
        Qv = Qv + (i*v_tmp*v_tmp)/(i+1);
        cov += i*u_tmp*v_tmp/(i+1);
        mu_u = mu_u + u_tmp/(i+1);
        mu_v = mu_v + v_tmp/(i+1);
    }

    // If both datasets are constant, then they are perfectly correlated.
    if (Qu == 0 && Qv == 0)
    {
        return Real(1);
    }
    // If one dataset is constant and the other isn't, then they have no correlation:
    if (Qu == 0 || Qv == 0)
    {
        return Real(0);
    }

    // Make sure rho in [-1, 1], even in the presence of numerical noise.
    Real rho = cov/sqrt(Qu*Qv);
    if (rho > 1) {
        rho = 1;
    }
    if (rho < -1) {
        rho = -1;
    }
    return rho;
}

}}}
#endif


#include <Eigen/Dense>
#include <Eigen/Geometry>

namespace dr {

/// Interpolate linearly between two vectors.
/**
 * At factor 0, the first vector is returned, at factor 1 the second.
 */
template<typename Derived1, typename Derived2>
auto interpolateVector(
	Eigen::MatrixBase<Derived1> const & a, ///< The first vector.
	Eigen::MatrixBase<Derived2> const & b, ///< The second vector.
	double factor                          ///< The interpolation factor.
) -> decltype(a + factor * (b - a)) {
	return a + factor * (b - a);
}

/// Interpolate spherical linearly between two rotations.
/**
 * At factor 0, the first rotation is returned, at factor 1 the second.
 */
template<typename T>
Eigen::AngleAxis<T> interpolateRotation(
	Eigen::AngleAxis<T> const & a, ///< The first rotation.
	Eigen::AngleAxis<T> const & b, ///< The second rotation.
	double factor                  ///< The interpolation factor.
) {
	Eigen::AngleAxis<T> difference = b * a.inverse();
	difference.angle() *= factor;
	return difference * a;
}

/// Interpolate spherical linearly between two rotations.
/**
 * At factor 0, the first rotation is returned, at factor 1 the second.
 */
template<typename T>
Eigen::Quaternion<T> interpolateRotation(
	Eigen::Quaternion<T> const & a, ///< The first rotation.
	Eigen::Quaternion<T> const & b, ///< The second rotation.
	double factor                   ///< The interpolation factor.
) {
	return a.slerp(factor, b);
}

/// Interpolate spherical linearly between two rotations.
/**
 * At factor 0, the first rotation is returned, at factor 1 the second.
 */
template<typename Derived1, typename Derived2>
Eigen::Quaternion<typename Eigen::MatrixBase<Derived1>::Scalar> interpolateRotation(
	Eigen::MatrixBase<Derived1> const & a, ///< The first rotation.
	Eigen::MatrixBase<Derived2> const & b, ///< The second rotation.
	double factor                          ///< The interpolation factor.
) {
	using Scalar1 = typename Eigen::MatrixBase<Derived1>::Scalar;
	using Scalar2 = typename Eigen::MatrixBase<Derived2>::Scalar;
	return interpolateRotation<Scalar1>(Eigen::Quaternion<Scalar1>(a), Eigen::Quaternion<Scalar2>(b), factor);
}

/// Interpolate spherical linearly between two isometries.
/**
 * At factor 0, the first isometry is returned, at factor 1 the second.
 * The translation will be interpolated linearly and the rotation spherial linearly.
 */
Eigen::Isometry3d interpolateIsometry(
	Eigen::Isometry3d const & a, ///< The first isometry.
	Eigen::Isometry3d const & b, ///< The second isometry.
	double factor                ///< The interpolation factor.
) {
	return Eigen::Translation3d{interpolateVector(a.translation(), b.translation(), factor)}
		* interpolateRotation(Eigen::Quaterniond{a.rotation()}, Eigen::Quaterniond{b.rotation()}, factor);
}

}


#include "Constraint.h"
#include "AttachmentConstraint.h"
#include <Eigen/Dense>
#include <iostream>
using namespace FEM;
AttachmentConstraint::
AttachmentConstraint(const double& stiffness,int i0,const Eigen::Vector2d& p)
	:Constraint(stiffness),mi0(i0),mp(p)
{

}

double
AttachmentConstraint::
EvalPotentialEnergy(const Eigen::VectorXd& x)
{
	Eigen::Vector2d x_p0 = x.block<2,1>(mi0*2,0) - mp;

    return 0.5*mStiffness*x_p0.squaredNorm();
}
void
AttachmentConstraint::
EvalGradient(const Eigen::VectorXd& x, Eigen::VectorXd& gradient)
{
	Eigen::Vector2d x_p0 = x.block<2,1>(mi0*2,0) - mp;
	gradient.block<2,1>(mi0*2,0) += mStiffness*x_p0;
}

void
AttachmentConstraint::
EvalHessian(const Eigen::VectorXd& x, const Eigen::VectorXd& dx, Eigen::VectorXd& dg)
{
	//Compute H*x
	dg.block<2,1>(mi0*2,0) += mStiffness*dx.block<2,1>(mi0*2,0);
}

void
AttachmentConstraint::
EvaluateDVector(int index, const Eigen::VectorXd& x,Eigen::VectorXd& d)
{
	d.block<2,1>(2*index,0) = mp;
}
void
AttachmentConstraint::
EvaluateJMatrix(int index, std::vector<Eigen::Triplet<double>>& J_triplets)
{
	J_triplets.push_back(Eigen::Triplet<double>(2*mi0, 2*index, mStiffness));
	J_triplets.push_back(Eigen::Triplet<double>(2*mi0+1, 2*index+1, mStiffness));
}
void
AttachmentConstraint::
EvaluateLMatrix(std::vector<Eigen::Triplet<double>>& L_triplets)
{
	L_triplets.push_back(Eigen::Triplet<double>(2*mi0+0, 2*mi0+0, mStiffness));
	L_triplets.push_back(Eigen::Triplet<double>(2*mi0+1, 2*mi0+1, mStiffness));
}

int
AttachmentConstraint::
GetDof()
{
	return 1;
}
int
AttachmentConstraint::
GetNumHessianTriplets()
{
	return 2;
}

ConstraintType
AttachmentConstraint::
GetType()
{
	return ConstraintType::ATTACHMENT;
}
void
AttachmentConstraint::
AddOffset(const int& offset)
{
	mi0+=offset;
}

Eigen::Vector2d&
AttachmentConstraint::
GetP()
{
	return mp;
}
int&
AttachmentConstraint::
GetI0()
{
	return mi0;
}


// =========================================================================
// @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
// =========================================================================

#include <iostream>

#include <boost/program_options.hpp>

#include <PUJ_ML/Helpers/CSV.h>
#include <PUJ_ML/Model/Linear.h>
#include <PUJ_ML/Optimizer/GradientDescent.h>

// -- Types
using TScalar = double;
using TModel = PUJ_ML::Model::Linear< TScalar >;
using TMatrix = TModel::TMatrix;
namespace po = boost::program_options;

// -- Main --
int main( int argc, char** argv )
{
  std::string csv = "[NO INPUT FILE]";
  TScalar alpha = 1e-2;
  TScalar lambda = 0;
  TScalar epsilon = std::numeric_limits< TScalar >::epsilon( );
  unsigned long long epochs = 10000;
  unsigned long long debug_step = 100;
  bool use_LASSO = false;

  po::options_description desc( "Allowed parameters" );
  desc.add_options( )
    ( "help,h", "Help message" )
    ( "alpha,a", po::value( &alpha )->default_value( alpha ), "Learning rate" )
    ( "lambda,l", po::value( &lambda )->default_value( lambda ), "Regularization" )
    ( "LASSO", po::bool_switch( &use_LASSO )->default_value( use_LASSO ), "Use LASSO?" )
    ( "epsilon,e", po::value( &epsilon )->default_value( epsilon ), "Epsilon" )
    ( "epochs", po::value( &epochs )->default_value( epochs ), "Epochs" )
    ( "debug_step", po::value( &debug_step )->default_value( debug_step ), "Debug step" )
    ( "csv", po::value( &csv )->default_value( csv ), "Input file" )
    ;

  po::variables_map vm;
  po::store( po::parse_command_line( argc, argv, desc ), vm );
  po::notify( vm );
  if( vm.count( "help" ) )
  {
    std::cerr << desc << std::endl;
    return( EXIT_FAILURE );
  } // end if

  auto D = PUJ_ML::Helpers::CSV::Read< TMatrix >( csv, true, "," );
  unsigned long long p = 1;
  unsigned long long m = D.rows( );
  unsigned long long n = D.cols( ) - p;
  TMatrix X = D.block( 0, 0, m, n );
  TMatrix Y = D.block( 0, n, m, p );

  TModel model;
  model.SetParameters( TModel::TCol::Zero( n + 1 ) );
  TModel::Cost cost( &model, X, Y );

  PUJ_ML::Optimizer::GradientDescent< TModel > opt;
  opt.SetCost( cost );
  opt.SetLearningRate( alpha );
  opt.SetRegularizationCoefficient( lambda );
  if( use_LASSO ) opt.SetRegularizationToLASSO( );
  else            opt.SetRegularizationToRidge( );
  opt.SetEpsilon( epsilon );
  opt.SetNumberOfEpochs( epochs );
  opt.SetDebugStep( debug_step );

  TScalar final_cost;
  unsigned long long final_epochs;
  opt.SetDebug(
    [&]( unsigned long long i, TScalar J, bool show ) -> bool
    {
      if( show )
        std::cout << i << " " << J << std::endl;
      final_cost = J;
      final_epochs = i;
      return( false );
    }
    );
  opt.Fit( );

  std::cout << "---------------------" << std::endl;
  std::cout << "Fitted model : " << model << std::endl;
  std::cout << "Final cost   : " << final_cost << std::endl;
  std::cout << "Final epochs : " << final_epochs << std::endl;
  std::cout << "---------------------" << std::endl;

  /* TODO
     std::cout << X.colwise( ).minCoeff( ) << std::endl;
     std::cout << X.colwise( ).maxCoeff( ) << std::endl;
     TModel::TCol L = TModel::TCol::LinSpaced( 100, -10, 10 );
  */

  return( EXIT_SUCCESS );
}

// eof - $RCSfile$


/**
 * @file radauthreetimestepping.cc
 * @brief NPDE homework RadauThreeTimestepping
 * @author Erick Schulz, edited by Oliver Rietmann
 * @date 08/04/2019
 * @copyright Developed at ETH Zurich
 */

#include "radauthreetimestepping.h"

#include <lf/assemble/assemble.h>
#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/utils/utils.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <Eigen/SparseLU>
#include <cmath>
#include <iostream>
#include <unsupported/Eigen/KroneckerProduct>

namespace RadauThreeTimestepping {

/**
 * @brief Implementation of the right hand side (time dependent) source vector
 * for the parabolic heat equation
 * @param dofh A reference to the DOFHandler
 * @param time The time at which to evaluate the source vector
 * @returns The source vector at time `time`
 */
/* SAM_LISTING_BEGIN_1 */
Eigen::VectorXd rhsVectorheatSource(const lf::assemble::DofHandler &dofh,
                                    double time) {
  // Dimension of finite element space
  const lf::uscalfe::size_type N_dofs(dofh.NumDofs());
  // Right-hand side vector has to be set to zero initially
  Eigen::VectorXd phi(N_dofs);
#if SOLUTION
  // Functor for computing the source function at 2d coordinates
  auto f = [time](Eigen::Vector2d x) -> double {
    const double PI = 3.14159265358979323846;
    Eigen::Vector2d v(std::cos(time * PI), std::sin(time * PI));
    return ((x - 0.5 * v).norm() < 0.5) ? 1.0 : 0.0;
  };
  auto mesh_p = dofh.Mesh();  // pointer to current mesh
  phi.setZero();

  /* Assembling right-hand side source vector */
  // Initialize object taking care of local computations on all cells.
  TrapRuleLinFEElemVecProvider<decltype(f)> elvec_builder(f);
  // Computing right hand side vector
  // Invoke assembly on cells (codim == 0 as first agrument)
  lf::assemble::AssembleVectorLocally(0, dofh, elvec_builder, phi);

  /* Enforce the zero Dirichlet boundary conditions */
  // Obtain an array of boolean flags for the vertices of the mesh: 'true'
  // indicates that the vertex lies on the boundary. This predicate will
  // guarantee that the computations are carried only on the boundary vertices
  auto bd_flags{lf::mesh::utils::flagEntitiesOnBoundary(mesh_p, 2)};
  // Creating predicate that will guarantee that the computations are carried
  // only on the vertices of the mesh using the boundary flags
  auto vertices_predicate =
      [&bd_flags](const lf::mesh::Entity &vertex) -> bool {
    return bd_flags(vertex);
  };
  // Assigning zero to the boundary values of phi
  for (const lf::mesh::Entity *vertex : mesh_p->Entities(2)) {
    if (bd_flags(*vertex)) {
      auto dof_idx = dofh.GlobalDofIndices(*vertex);
      LF_ASSERT_MSG(
          dofh.NumLocalDofs(*vertex) == 1,
          "Too many global indices were returned for a vertex entity!");
      phi(dof_idx[0]) = 0.0;
    }
  }
#else
  //====================
  // Your code goes here
  //====================
#endif
  return phi;
}
/* SAM_LISTING_END_1 */

/**
 * @brief Heat evolution solver: the solver obtains the
 * discrete evolution operator from the Radau3MOLTimestepper class and
 * repeatedly iterates its applicaiton starting from the initial condition
 * @param dofh The DOFHandler object
 * @param m is total number of steps until final time final_time (double)
 * @param final_time The duration for which to solve the PDE
 * @returns The solution at the final timestep
 */
/* SAM_LISTING_BEGIN_6 */
Eigen::VectorXd solveHeatEvolution(const lf::assemble::DofHandler &dofh,
                                   unsigned int m, double final_time) {
  Eigen::VectorXd discrete_heat_sol(dofh.NumDofs());
#if SOLUTION
  double tau = final_time / m;                          // step size
  const lf::uscalfe::size_type N_dofs(dofh.NumDofs());  // dim. of FE space

  std::cout << "*********************************************************"
            << std::endl;
  std::cout << "\n>>> SolveHeatEvolution: m = " << m << ", N = " << N_dofs
            << std::endl;
  /* Setting up the problem information */
  // Precomputing the required data for the Runge-Kutta method
  // Assemble the Runge-Kutta Radau IIA 2-stages method solver (order 3)
  Radau3MOLTimestepper radau_solver(dofh);
  // Starting with the zero initial condition vector
  Eigen::VectorXd discrete_solution_cur =
      radau_solver.discreteEvolutionOperator(0.0, tau,
                                             Eigen::VectorXd::Zero(N_dofs));

  std::cout << "\n>> Iterating the action of discreteEvolutionOperator"
            << std::endl;
  /* Evolving the parabolic heat system */
  // While less elegant, we use a current and next step solution vector in the
  // iteration to stay away from potential harming aliasing effects of putting
  // an Eigen::Vector on both sides of an assignment statement.
  Eigen::VectorXd discrete_solution_next;
  for (int i = 1; i < m; i++) {
    discrete_solution_next = radau_solver.discreteEvolutionOperator(
        i * tau, tau, discrete_solution_cur);
    discrete_solution_cur = discrete_solution_next;
  }
  discrete_heat_sol = discrete_solution_cur;
#else
  //====================
  // Your code goes here
  //====================
#endif
  return discrete_heat_sol;
}
/* SAM_LISTING_END_6 */

/* Implementing member function Eval of class LinFEMassMatrixProvider*/
Eigen::Matrix<double, 3, 3> LinFEMassMatrixProvider::Eval(
    const lf::mesh::Entity &tria) {
  Eigen::Matrix<double, 3, 3> elMat;
#if SOLUTION
  // Throw error in case no triangular cell
  LF_VERIFY_MSG(tria.RefEl() == lf::base::RefEl::kTria(),
                "Unsupported cell type " << tria.RefEl());
  // Compute the area of the triangle cell
  const double area = lf::geometry::Volume(*(tria.Geometry()));
  // Assemble the mass element matrix over the cell
  // clang-format off
  elMat << 2.0, 1.0, 1.0,
           1.0, 2.0, 1.0,
           1.0, 1.0, 2.0;
  // clang-format on
  elMat *= area / 12.0;
#else
  //====================
  // Your code goes here
  //====================
#endif
  return elMat;  // return the local mass element matrix
}

/* Implementing constructor of class Radau3MOLTimestepper */
/* SAM_LISTING_BEGIN_4 */
Radau3MOLTimestepper::Radau3MOLTimestepper(const lf::assemble::DofHandler &dofh)
    : dofh_(dofh) {
#if SOLUTION
  std::cout << "\n>> Constructing SRadau3MOLTimestepper " << std::endl;
  auto mesh_p = dofh.Mesh();  // pointer to current mesh

  // Instantiating Galerkin matrices to be pre-computed
  // Dimension of finite element space
  const lf::uscalfe::size_type N_dofs(dofh.NumDofs());
  // Matrices in triplet format holding Galerkin matrices, zero initially.
  lf::assemble::COOMatrix<double> A_COO(N_dofs,
                                        N_dofs);  // element matrix Laplace
  lf::assemble::COOMatrix<double> M_COO(N_dofs,
                                        N_dofs);  // element mass matrix

  std::cout << "> Initializing the Galerking local matrices builders"
            << std::endl;
  // Initialize classes containing the information required for the
  // local computations of the Galerkin matrices. Simple implementations of
  // LinFEMassMatrixProvider and TrapRuleLinFEElemVecProvider adapted to this
  // particular problem was written to spare some of the overhead calculations
  // involved in the use of the more general LehrFEM++ matrices providers.
  lf::uscalfe::LinearFELaplaceElementMatrix elLapMat_builder;
  LinFEMassMatrixProvider elMassMat_builder;

  std::cout << "> Assembling Galerking matrices in COO format" << std::endl;
  // Compute the Galerkin matrices
  // Invoke assembly on cells (co-dimension = 0 as first argument)
  // Information about the mesh and the local-to-global map is passed through
  // a Dofhandler object, argument 'dofh'. This function call adds triplets to
  // the internal COO-format representation of the sparse matrices A and M.
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, elLapMat_builder, A_COO);
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, elMassMat_builder, M_COO);

  // Enforcing zero Dirichlet boundary conditions
  // Obtain an array of boolean flags for the vertices of the mesh: 'true'
  // indicates that the vertex lies on the boundary.
  auto bd_flags{lf::mesh::utils::flagEntitiesOnBoundary(mesh_p, 2)};
  // Index predicate for the selectvals FUNCTOR of dropMatrixRowsColumns
  auto bdy_vertices_selector = [&bd_flags, &dofh](unsigned int idx) -> bool {
    return bd_flags(dofh.Entity(idx));
  };
  dropMatrixRowsColumns(bdy_vertices_selector, A_COO);
  dropMatrixRowsColumns(bdy_vertices_selector, M_COO);

  std::cout << "> Converting triplets to sparse matrices" << std::endl;
  // Creating the private Galerkin stiffness and mass matrices
  A_ = A_COO.makeSparse();
  Eigen::SparseMatrix<double> M = M_COO.makeSparse();

  // Runge-Kutta matrices defining the 2-stage Radau timestepping. In the
  // Butcher tableau, this corresponds to c = (1/3 1)^T (top-left column
  // vector), b^T = (3/4 1/4) (bottom-right row vector), U_11 = 5/12, U_12 =
  // -1/12, U_21 = 3/4, U_22 = 1/4 (top-right block); values are fixed in
  // time
  // clang-format off
    U_ << 5.0/12.0, -1.0/12.0,
              0.75,      0.25;
    c_ << 1.0/3.0, 1.0;
    b_ << 0.75, 0.25;
  // clang-format on
  // Precomputing the kronecker products involved in the implicit linear system
  // for the increments of the RADAU-2 method
  M_Kp_ = Eigen::kroneckerProduct(Eigen::Matrix<double, 2, 2>::Identity(), M);
  A_Kp_ = Eigen::kroneckerProduct(U_, A_);
#else
  //====================
  // Your code goes here
  // Add any additional members you need in the header file
  //====================
#endif
}
/* SAM_LISTING_END_4 */

/* Implementation of Radau3MOLTimestepper member functions */
// The function discreteEvolutionOperator() returns the discretized evolution
// operator as obtained from the Runge-Kutta Radau IIA 2-stages method using the
// Butcher table as stored in the Radau3MOLTimestepper class
/* SAM_LISTING_BEGIN_5 */
Eigen::VectorXd Radau3MOLTimestepper::discreteEvolutionOperator(
    double time, double tau, const Eigen::VectorXd &mu) const {
  Eigen::VectorXd discrete_evolution_operator(dofh_.NumDofs());
#if SOLUTION
  // Dimension of finite element space
  const lf::uscalfe::size_type N_dofs(dofh_.NumDofs());
  LF_VERIFY_MSG(N_dofs == mu.size(),
                "Dimension mismatch between the number of degrees of freedom "
                "and the dimension of the argument vector.");

  // Building the linear system for the implicitely defined increments
  // Assembling the right hand side using block initialization
  Eigen::VectorXd linSys_rhs(2 * N_dofs);
  Eigen::VectorXd rhs_subtraction_term = A_ * mu;  // precomputation
  linSys_rhs << rhsVectorheatSource(dofh_, time + c_[0] * tau) -
                    rhs_subtraction_term,
      rhsVectorheatSource(dofh_, time + tau) - rhs_subtraction_term;

  // Implicit Runge-Kutta methods lead to systems of equations that must be
  // solved in order to obtained the increments.
  Eigen::SparseMatrix<double> linSys_mat;
  // Assembling the system right hand side matrix using the (unfortunately
  // officially not supported) Eigen Kronecker product
  linSys_mat = M_Kp_ + tau * A_Kp_;
  LF_VERIFY_MSG(linSys_mat.rows() == linSys_mat.cols(),
                "The linSys_mat Eigen matrix is not squared.");
  Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
  solver.compute(linSys_mat);
  LF_VERIFY_MSG(solver.info() == Eigen::Success, "LU decomposition failed");

  // Solve linear system using Eigen's sparse direct elimination
  Eigen::VectorXd k_vec = solver.solve(linSys_rhs);
  LF_VERIFY_MSG(solver.info() == Eigen::Success, "Solving LSE failed");

  // Compute action of the discrete evolution operator on argument vec
  discrete_evolution_operator = mu + tau * (b_[0] * k_vec.topRows(N_dofs) +
                                            b_[1] * k_vec.bottomRows(N_dofs));
#else
  //====================
  // Your code goes here
  //====================
#endif
  return discrete_evolution_operator;
}
/* SAM_LISTING_END_5 */

}  // namespace RadauThreeTimestepping


#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include "g2o/EXTERNAL/ceres/autodiff.h"
#include <g2o/core/auto_differentiation.h>

#include <g2o/core/robust_kernel_impl.h>
#include <iostream>

#include "common.h"
#include <sophus/se3.hpp>
#include <sophus/so3.hpp>
#include <ceres/rotation.h>
#include <Eigen/Dense>

using namespace Sophus;
using namespace Eigen;
using namespace std;

class VertexCamera: public g2o::BaseVertex<9, Eigen::Matrix<double, 9, 1>>      //  here the camera is parameterized as a 9-vector (angle axis, t, f, k1, k2)
{
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    virtual void setToOriginImpl() override {
        _estimate = Eigen::Matrix<double, 9, 1>::Zero();
    }

    virtual void oplusImpl(const double *update) override {
        _estimate += Eigen::Map<const Eigen::Matrix<double, 9, 1>>(update);
    }

    virtual bool read(std::istream&) override {}
    virtual bool write(std::ostream&) const override {}
};

class VertexLandmark: public g2o::BaseVertex<3, Eigen::Vector3d>
{
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    virtual void setToOriginImpl() override {
        _estimate = Eigen::Vector3d::Zero();
    }

    virtual void oplusImpl(const double *update) override {
        _estimate += Eigen::Map<const Eigen::Vector3d>(update);
    }

    virtual bool read(std::istream&) override {}
    virtual bool write(std::ostream&) const override {}
};

class EdgeReprojection: public g2o::BaseBinaryEdge<2, Eigen::Vector2d, VertexCamera, VertexLandmark>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    EdgeReprojection() { }

    template <class T>
    bool operator() (const T* camera, const T* landmark, T* residuals) const
    {
        T Xc[3];
        ceres::AngleAxisRotatePoint(camera, landmark, Xc);
        Xc[0] += camera[3];
        Xc[1] += camera[4];
        Xc[2] += camera[5];

        T Xp[2];
        Xp[0] = Xc[0] / Xc[2];
        Xp[1] = Xc[1] / Xc[2];

        T n2 = Xp[0] * Xp[0] + Xp[1] * Xp[1];
        T r = T(1.0) + n2 * (camera[7] + n2 * camera[8]);

        T uv[2];
        uv[0] = -Xp[0] * camera[6] * r;
        uv[1] = -Xp[1] * camera[6] * r;

        residuals[0] = T(_measurement[0]) - uv[0];
        residuals[1] = T(_measurement[1]) - uv[1];
        return true;
    }

    virtual bool read(std::istream&) override {}
    virtual bool write(std::ostream&) const override {}

    G2O_MAKE_AUTO_AD_FUNCTIONS  // use autodiff

};

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

    if (argc != 2) {
        cout << "usage: bundle_adjustment_g2o bal_data.txt" << endl;
        return 1;
    }

    BALProblem dataset(argv[1]);
    dataset.Normalize();
    dataset.Perturb(0.1, 0.5, 0.5);
    dataset.WriteToPLYFile("initial_pc.ply");

    std::cout << "\n";
    std::cout << "nb cameras: " << dataset.num_cameras() << std::endl;
    std::cout << "nb landmarks: " << dataset.num_points() << std::endl;
    std::cout << "nb observations: " << dataset.num_observations() << std::endl;
    std::cout << "nb parameters: " << dataset.num_parameters() << std::endl;
    std::cout << "check: " << dataset.num_cameras() * 9 + dataset.num_points()*3 << std::endl;

    // pose dimension 9, landmark is 3
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<9, 3>> BlockSolverType;
    typedef g2o::LinearSolverCSparse<BlockSolverType::PoseMatrixType> LinearSolverType;

    auto solver = new g2o::OptimizationAlgorithmLevenberg(
        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>())
    );
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);

    auto* cameras = dataset.mutable_cameras();
    std::vector<VertexCamera*> camera_vertices;
    for (int i = 0; i < dataset.num_cameras(); ++i)
    {
        auto *c = new VertexCamera();
        c->setId(i);
        c->setEstimate(Eigen::Map<Eigen::Matrix<double, 9, 1>>(cameras + (i*dataset.camera_block_size())));
        optimizer.addVertex(c);
        camera_vertices.push_back(c);
    }

    auto* landmarks = dataset.mutable_points();
    std::vector<VertexLandmark*> landmark_vertices;
    for (int i = 0; i < dataset.num_points(); ++i)
    {
        auto* l = new VertexLandmark();
        l->setId(dataset.num_cameras() + i);
        l->setEstimate(Eigen::Map<Eigen::Vector3d>(landmarks + i*dataset.point_block_size()));
        l->setMarginalized(true);
        optimizer.addVertex(l);
        landmark_vertices.push_back(l);
    }

    auto* observations = dataset.observations();
    auto* cam_indices = dataset.camera_index();
    auto* landmark_indices = dataset.point_index();
    for (int i = 0; i < dataset.num_observations(); ++i)
    {
        auto* e = new EdgeReprojection();
        e->setVertex(0, camera_vertices[cam_indices[i]]);
        e->setVertex(1, landmark_vertices[landmark_indices[i]]);
        e->setMeasurement(Eigen::Map<const Eigen::Vector2d>(observations + i*2));
        e->setInformation(Eigen::Matrix2d::Identity());
        optimizer.addEdge(e);
    }

    optimizer.initializeOptimization();
    optimizer.optimize(40);

    for (int i = 0; i < dataset.num_cameras(); ++i)
    {
        Eigen::Matrix<double, 9, 1> in = camera_vertices[i]->estimate();
        double *out = cameras + i * dataset.camera_block_size();
        for (int i = 0; i < 9; ++i)
        {
            out[i] = in[i];
        }
    }
    for (int i = 0; i < dataset.num_points(); ++i)
    {
        Eigen::Vector3d X = landmark_vertices[i]->estimate();
        landmarks[i*3] = X.x();
        landmarks[i*3+1] = X.y();
        landmarks[i*3+2] = X.z();
    }

    dataset.WriteToPLYFile("after_ba_g2o.ply");

    return 0;
}


#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  MatrixXcd X = MatrixXcd::Random(4,4);
MatrixXcd A = X + X.adjoint();
cout << "Here is a random self-adjoint 4x4 matrix:" << endl << A << endl << endl;

Tridiagonalization<MatrixXcd> triOfA(A);
MatrixXd T = triOfA.matrixT();
cout << "The tridiagonal matrix T is:" << endl << T << endl << endl;

cout << "We can also extract the diagonals of T directly ..." << endl;
VectorXd diag = triOfA.diagonal();
cout << "The diagonal is:" << endl << diag << endl;
VectorXd subdiag = triOfA.subDiagonal();
cout << "The subdiagonal is:" << endl << subdiag << endl;

  return 0;
}


/**
 * @date Tue Jan 18 17:07:26 2011 +0100
 * @author André Anjos <andre.anjos@idiap.ch>
 * @author Laurent El Shafey <Laurent.El-Shafey@idiap.ch>
 *
 * @brief Principal Component Analysis implemented with Singular Value
 * Decomposition or using the Covariance Method. Both are implemented using
 * LAPACK. Implementation.
 *
 * Copyright (C) Idiap Research Institute, Martigny, Switzerland
 */

#include <algorithm>
#include <blitz/array.h>
#include <boost/format.hpp>
#include <bob.math/stats.h>
#include <bob.math/svd.h>
#include <bob.math/eig.h>

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

namespace bob { namespace learn { namespace linear {

  PCATrainer::PCATrainer(bool use_svd)
    : m_use_svd(use_svd), m_safe_svd(false)
  {
  }

  PCATrainer::PCATrainer(const PCATrainer& other)
    : m_use_svd(other.m_use_svd), m_safe_svd(other.m_safe_svd)
  {
  }

  PCATrainer::~PCATrainer() {}

  PCATrainer& PCATrainer::operator= (const PCATrainer& other) {
    if (this != &other) {
      m_use_svd = other.m_use_svd;
      m_safe_svd = other.m_safe_svd;
    }
    return *this;
  }

  bool PCATrainer::operator== (const PCATrainer& other) const {

    return m_use_svd == other.m_use_svd &&
      m_safe_svd == other.m_safe_svd;

  }

  bool PCATrainer::operator!= (const PCATrainer& other) const {

    return !(this->operator==(other));

  }

  /**
   * Sets up the machine calculating the PC's via the Covariance Matrix
   */
  static void pca_via_covmat(Machine& machine,
      blitz::Array<double,1>& eigen_values, const blitz::Array<double,2>& X,
      int rank) {

    /**
     * computes the covariance matrix (X-mu)(X-mu)^T / (len(X)-1) and then solves
     * the generalized eigen-value problem taking into consideration the
     * covariance matrix is symmetric (and, by extension, hermitian).
     */
    blitz::Array<double,1> mean(X.extent(1));
    blitz::Array<double,2> Sigma(X.extent(1), X.extent(1));
    bob::math::scatter_(X, Sigma, mean);
    Sigma /= (X.extent(0)-1); //unbiased variance estimator

    blitz::Array<double,2> U(X.extent(1), X.extent(1));
    blitz::Array<double,1> e(X.extent(1));
    bob::math::eigSym_(Sigma, U, e);
    e.reverseSelf(0);
    U.reverseSelf(1);

    /**
     * sets the linear machine with the results:
     */
    machine.setInputSubtraction(mean);
    machine.setInputDivision(1.0);
    machine.setBiases(0.0);
    if (e.size() == eigen_values.size()) {
      eigen_values = e;
      machine.setWeights(U);
    }
    else {
      eigen_values = e(blitz::Range(0,rank-1));
      machine.setWeights(U(blitz::Range::all(), blitz::Range(0,rank-1)));
    }

  }

  /**
   * Sets up the machine calculating the PC's via SVD
   */
  static void pca_via_svd(Machine& machine, blitz::Array<double,1>& eigen_values,
      const blitz::Array<double,2>& X, int rank, bool safe_svd) {

    // removes the empirical mean from the training data
    blitz::Array<double,2> data(X.extent(1), X.extent(0));
    blitz::Range a = blitz::Range::all();
    for (int i=0; i<X.extent(0); ++i) data(a,i) = X(i,a);

    // computes the mean of the training data
    blitz::secondIndex j;
    blitz::Array<double,1> mean(X.extent(1));
    mean = blitz::mean(data, j);

    // applies the training data mean
    for (int i=0; i<X.extent(0); ++i) data(a,i) -= mean;

    /**
     * computes the singular value decomposition using lapack
     *
     * note: Lapack already organizes the U,Sigma,V**T matrixes so that the
     * singular values in Sigma are organized by decreasing order of magnitude.
     * You **don't** need sorting after this.
     */
    const int rank_1 = (rank == (int)X.extent(1))? X.extent(1) : X.extent(0);
    blitz::Array<double,2> U(X.extent(1), rank_1);
    blitz::Array<double,1> sigma(rank_1);
    bob::math::svd_(data, U, sigma, safe_svd);

    /**
     * sets the linear machine with the results:
     *
     * note: eigen values are sigma^2/X.extent(0) diagonal
     *       eigen vectors are the rows of U
     */
    machine.setInputSubtraction(mean);
    machine.setInputDivision(1.0);
    machine.setBiases(0.0);
    blitz::Range up_to_rank(0, rank-1);
    machine.setWeights(U(a,up_to_rank));

    //weight normalization (if necessary):
    //norm_factor = blitz::sum(blitz::pow2(V(all,i)))

    // finally, we set also the eigen values in this version
    eigen_values = (blitz::pow2(sigma)/(X.extent(0)-1))(up_to_rank);
  }

  void PCATrainer::train(Machine& machine, blitz::Array<double,1>& eigen_values,
      const blitz::Array<double,2>& X) const {

    // data is checked now and conforms, just proceed w/o any further checks.
    const int rank = output_size(X);

    // Checks that the dimensions are matching
    if (machine.inputSize() != (size_t)X.extent(1)) {
      boost::format m("Number of features at input data set (%d columns) does not match machine input size (%d)");
      m % X.extent(1) % machine.inputSize();
      throw std::runtime_error(m.str());
    }
    if (machine.outputSize() != (size_t)rank) {
      boost::format m("Number of outputs of the given machine (%d) does not match the maximum covariance rank, i.e., min(#samples-1,#features) = min(%d, %d) = %d");
      m % machine.outputSize() % (X.extent(0)-1) % X.extent(1) % rank;
      throw std::runtime_error(m.str());
    }
    if (eigen_values.extent(0) != rank) {
      boost::format m("Number of eigenvalues on the given 1D array (%d) does not match the maximum covariance rank, i.e., min(#samples-1,#features) = min(%d,%d) = %d");
      m % eigen_values.extent(0) % (X.extent(0)-1) % X.extent(1) % rank;
      throw std::runtime_error(m.str());
    }

    if (m_use_svd) pca_via_svd(machine, eigen_values, X, rank, m_safe_svd);
    else pca_via_covmat(machine, eigen_values, X, rank);
  }

  void PCATrainer::train(Machine& machine, const blitz::Array<double,2>& X) const {
    blitz::Array<double,1> throw_away_eigen_values(output_size(X));
    train(machine, throw_away_eigen_values, X);
  }

  size_t PCATrainer::output_size (const blitz::Array<double,2>& X) const {
    return (size_t)std::min(X.extent(0)-1,X.extent(1));
  }

}}}


#pragma once

#include <boost/multiprecision/cpp_dec_float.hpp>
#include <cudd/cplusplus/cuddObj.hh>
#include <vector>

#include "number_representation.hpp"

namespace abo::error_metrics {

/**
 * @brief Computes the maximum relative difference between f and f_hat for any input
 * It is defined as the maximum of |f(x) - f_hat(x)| / max(1, |f(x)|) over all inputs x
 * As it is not symmetric, it is not a metric in the mathematical sense
 * The computation is performed with ADDs and might be quite slow
 * @param mgr The BDD object manager
 * @param f The original function
 * @param f_hat The approximated function. Must have the same number of bits as f
 * @param num_rep The number representation for f and f_hat
 * @return the maximum relative difference of the inputs
 */
double wcre_add(const Cudd& mgr, const std::vector<BDD>& f,
              const std::vector<BDD>& f_hat,
              const abo::util::NumberRepresentation num_rep
                = abo::util::NumberRepresentation::BaseTwo);

/**
 * @brief Computes bounds on the maximum relative value of f in relation to g
 * It is defined as the maximum of |f(x)| / max(1, |g(x)|) over all inputs x
 * This function returns a range in which the actual relative value is guaranteed to lie
 * The computed bounds are always within a factor of 2, meaning that the maximum bound
 * returned by this function is at most twice the minimum bound
 * @param mgr The BDD object manager
 * @param f The function to compute the maximum relative value of (must be an unsigned integer)
 * @param g The function to use as a relation. Must have the same number of bits as f (must be an
 * unsigned integer)
 * @return {min, max}, the lower and upper bound on the maximum relative value
 */
std::pair<boost::multiprecision::cpp_dec_float_100,
            boost::multiprecision::cpp_dec_float_100>
maximum_relative_value_bounds(const Cudd& mgr, const std::vector<BDD>& f,
                              const std::vector<BDD>& g);

/**
 * @brief Computes bounds on the maximum relative difference between f and f_hat
 * It is defined as the maximum of |f(x) - f_hat(x)| / max(1, |f(x)|) over all inputs x
 * As it is not symmetric, it is not a metric in the mathematical sense
 * This function returns a range in which the actual maximum relative error is guaranteed to lie
 * The computed bounds are always within a factor of 2, meaning that the maximum error
 * returned by this function is at most twice the minimum error
 * @param mgr The BDD object manager
 * @param f The original function
 * @param f_hat The approximated function. Must have the same number of bits as f
 * @param num_rep The number representation for f and f_hat
 * @return {min, max}, the lower and upper bound on the maximum relative error
 */
std::pair<boost::multiprecision::cpp_dec_float_100,
            boost::multiprecision::cpp_dec_float_100>
    wcre_bounds(
        const Cudd& mgr,
        const std::vector<BDD>& f,
        const std::vector<BDD>& f_hat,
        const abo::util::NumberRepresentation num_rep
            = abo::util::NumberRepresentation::BaseTwo);

/**
 * @brief Computes the maximum relative difference between f and f_hat for any input
 * It is defined as the maximum of |f(x) - f_hat(x)| / max(1, |f(x)|) over all inputs x
 * As it is not symmetric, it is not a metric in the mathematical sense
 * The computation is performed with BDDs using a binary search to find the maximum value
 * If the correct value is found, the search is aborted. Otherwise, it is run until the desired
 * precision is reached
 * @param mgr The BDD object manager
 * @param f The original function
 * @param f_hat The approximated function. Must have the same number of bits as f
 * @param num_extra_bits The number of additional bits used during the search to represent values
 * smaller than one
 * @param precision The desired precision of the result if the correct value is not found during the
 * binary search Do not set it lower than 2^-num_extra_bits
 * @param num_rep The number representation for f and f_hat
 * @return the maximum relative difference of the inputs
 */
double wcre_search(
        const Cudd& mgr, const std::vector<BDD>& f,
        const std::vector<BDD>& f_hat,
        unsigned int num_extra_bits = 16,
        double precision = 0.0001,
        const abo::util::NumberRepresentation num_rep
            = abo::util::NumberRepresentation::BaseTwo);

/**
 * @brief Computes the maximum relative difference between f and f_hat for any input
 * It is defined as the maximum of |f(x) - f_hat(x)| / max(1, |f(x)|) over all inputs x
 * As it is not symmetric, it is not a metric in the mathematical sense
 * The computation is performed with BDDs using a binary search to find the maximum value
 * If the correct value is found, the search is aborted. Otherwise, it is run until the desired
 * precision is reached
 * @param mgr The BDD object manager
 * @param f The original function
 * @param f_hat The approximated function. Must have the same number of bits as f
 * @param samples The number of random input samples drawn in each iteration
 * @param num_rep The number representation for f and f_hat
 * @return the maximum relative difference of the inputs as a fraction [numerator, denominator]
 */
std::pair<long, long> wcre_randomized_search(
        const Cudd& mgr, const std::vector<BDD>& f,
        const std::vector<BDD>& f_hat,
        unsigned int samples = 1,
        const abo::util::NumberRepresentation num_rep
        = abo::util::NumberRepresentation::BaseTwo);
/**
 * @brief Computes the maximum relative difference between f and f_hat for any input
 * It is defined as the maximum of |f(x) - f_hat(x)| / max(1, |f(x)|) over all inputs x
 * As it is not symmetric, it is not a metric in the mathematical sense
 * The computation is performed with BDDs using a symbolic division and might be quite slow
 * @param mgr The BDD object manager
 * @param f The original function
 * @param f_hat The approximated function. Must have the same number of bits as f
 * @param num_extra_bits The number of additional fixed precision bits to use during the division
 * As the result of each division is not an integer, the result is described as a fixed point number
 * with exactly num_extra_bits bits with a lower significance than one. Roughly correlates the the
 * precision of the result
 * @param num_rep The number representation for f and f_hat
 * @return the maximum relative difference of the inputs
 */
boost::multiprecision::cpp_dec_float_100 wcre_symbolic_division(
        const Cudd& mgr, const std::vector<BDD>& f,
        const std::vector<BDD>& f_hat,
        unsigned int num_extra_bits = 16,
        const abo::util::NumberRepresentation num_rep
            = abo::util::NumberRepresentation::BaseTwo);

} // namespace abo::error_metrics


/*
   Copyright (C) 2016-2021 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>
                              Chihiro Kondo <chihiro.kondo@phys.s.u-tokyo.ac.jp>

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

// Density of state of square lattice Ising model

#pragma once

#include <cmath>
#include <limits>
#include <vector>
#include <boost/math/constants/constants.hpp>
#include <boost/math/differentiation/autodiff.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <lattice/graph.hpp>

namespace ising {
namespace dos {
namespace square {

typedef unsigned long uint_t;

std::vector<uint_t> count(uint_t Lx, uint_t Ly) {
  auto basis = lattice::basis::simple(2);
  auto unitcell = lattice::unitcell(2);
  unitcell.add_site(lattice::coordinate(0, 0), 0);
  unitcell.add_bond(0, 0, lattice::offset(1, 0), 0);
  unitcell.add_bond(0, 0, lattice::offset(0, 1), 0);
  auto graph = lattice::graph(basis, unitcell, lattice::extent(Lx, Ly));
  if (graph.num_bonds() >= std::numeric_limits<uint_t>::digits)
    throw std::range_error("Error: system size is too large\n");

  std::vector<uint_t> dos(graph.num_bonds() + 1, 0);
  uint_t num_states = 1 << graph.num_sites();
  for (uint_t c = 0; c < num_states; ++c) {
    uint_t energy = 0;
    for (uint_t b = 0; b < graph.num_bonds(); ++b) {
      uint_t ci = (c >> graph.source(b)) & 1;
      uint_t cj = (c >> graph.target(b)) & 1;
      energy += (ci ^ cj);
    }
    ++dos[energy];
  }
  return dos;
}

// Density of state of square lattice Ising model

// Ref: P. Beale, Phys. Rev. Lett. 76, 78-81 (1996)

namespace {

template<typename T>
T zero(const T& x) { return 0 * x; }

template<typename T>
T one(const T& x) { return 1 + zero(x); }

template<typename T>
T power_n(const T& x, unsigned n) {
  auto res = one(x);
  for (unsigned i = 0; i < n; ++i) res *= x;
  return res;
}

template<typename T>
T alpha(const T& x, const T& beta, unsigned n, unsigned k) {
  using std::cos;
  auto pi = boost::math::constants::pi<typename T::root_type>();
  return power_n(1 + x * x, 2) - beta * cos(pi * k / n);
}

}

using namespace boost::math;
using namespace boost::math::differentiation;
namespace mp = boost::multiprecision;
typedef mp::cpp_int int_type;

template<unsigned Order, unsigned Digits10, class ExponentType = boost::int32_t>
std::vector<mp::cpp_int> finite(uint_t m, uint_t n) {
  typedef mp::number<mp::cpp_dec_float<Digits10, ExponentType>> real_type;
  std::cout << std::setprecision(std::numeric_limits<real_type>::max_digits10);

  auto const x = make_fvar<real_type, Order>(0);
  auto const xpo = make_fvar<real_type, Order>(-1);
  auto const xmo = make_fvar<real_type, Order>(+1);

  auto beta = 2 * x * xpo * xmo;
  auto beta_m = power_n(beta, m);
  auto c0 = power_n(xmo, m) + power_n(x * xpo, m);
  auto s0 = power_n(xmo, m) - power_n(x * xpo, m);
  auto cn = power_n(xpo, m) + power_n(x * xmo, m);
  auto sn = power_n(xpo, m) - power_n(x * xmo, m);

  auto z1 = zero(x);
  auto z2 = zero(x);
  auto z3 = zero(x);
  auto z4 = zero(x);
  if ((n & 1) == 0) {
    z1 = one(x) / 2;
    z2 = one(x) / 2;
    z3 = c0 * cn / 2;
    z4 = s0 * sn / 2;
  } else {
    z1 = cn / 2;
    z2 = sn / 2;
    z3 = c0 / 2;
    z4 = s0 / 2;
  }
  for (unsigned k = 1; k < n; ++k) {
    auto ak = alpha(x, beta, n, k);
    auto v = zero(x);
    for (unsigned j = 0; j <= m; j += 2)
      v += binomial_coefficient<real_type>(m, j)
        * power_n(ak * ak - beta * beta, j/2) * power_n(ak, m-j);
    if ((k & 1) == 1) {
      z1 *= (v + beta_m) / power_n(real_type(2), m - 1);
      z2 *= (v - beta_m) / power_n(real_type(2), m - 1);
    } else {
      z3 *= (v + beta_m) / power_n(real_type(2), m - 1);
      z4 *= (v - beta_m) / power_n(real_type(2), m - 1);
    }
  }
  auto zmn = z1 + z2 + z3 + z4;

  std::vector<int_type> dos;
  for (unsigned i = 0; i <= 2 * m * n; ++i) dos.push_back(int_type(zmn.at(i) + 0.01));
  auto sum = std::accumulate(dos.begin(), dos.end(), int_type(0));
  if (sum != power_n(int_type(2), m * n)) {
    std::cerr << "Error: result check failed\n";
    throw(0);
  }
  return dos;
}

}
}
}


// HelloWorld.cpp : main project file.
//#define _CRTDBG_MAP_ALLOC
//#include <stdlib.h>
//#include <crtdbg.h>
#include "stdafx.h"
#include <NTL/ZZ_pXFactoring.h>
#include "Shamir.h"
#include "string.h"
#include "ShamirShare.h"
#include "BenalohLeichter.h"
//#include "AccessStructure.h"
//#include "Trustee.h"
#include "ISecretShare.h"
#include "NonInteractiveChaumPedersen.h"
#include "NTLHelper.h"
#include "PrimeGenerator.h"
#include <vector>
#include "Schoenmakers.h"
#include "PublicKeyEncryption.h"
#include <tuple>
//#include "vld.h"
using namespace System;
using namespace std;
using namespace NTL;
using namespace System::Collections::Generic;
using namespace SecretSharingCore::Algorithms;
using namespace SecretSharingCore::Algorithms::PVSS;
using namespace SecretSharingCore::Algorithms::PKE;
using namespace SecretSharingCore::Common;
using namespace SecretSharingCore::Algorithms::GeneralizedAccessStructure;
using namespace SecretSharing::OptimalThreshold::Models;
using namespace SecretSharing::OptimalThreshold;
using namespace SecretSharingCore::ZKProtocols;
using namespace SecretSharingCore;

void MarshalString(String ^ s, string& os)
{
	using namespace Runtime::InteropServices;
	const char* chars =
		(const char*)(Marshal::StringToHGlobalAnsi(s)).ToPointer();
	os = chars;
	Marshal::FreeHGlobal(IntPtr((void*)chars));
}

void runByteChunkShare(){
	int k = 3;
	int n = 10;
	Byte chunkSize = 16;
	String^ secret = "1234567812345678";
	array<Byte>^ bytes = Encoding::UTF8->GetBytes(secret->ToCharArray());
	Shamir^ secretshare = gcnew Shamir();
#ifdef calcPrimeTime
	double a = 0;
	List<IShareCollection^>^ shares = secretshare->DivideSecret(k, n, bytes,chunkSize,a);
#else
	List<IShareCollection^>^ shares = secretshare->DivideSecret(k, n, bytes, chunkSize);
#endif
	//List<IShareCollection^>^ sharesStr = secretshare->DivideSecret(k, n,secret);
	delete bytes;
	for (int i = 0; i < k; i++)
	{
		IShareCollection^ col = shares[i];
		//IShareCollection^ colstr = sharesStr[i];

		Console::WriteLine(col->ToString());
		//Console::WriteLine(colstr->ToString());

	/*	for (int j = 0; j < col->GetCount(); j++)
		{
			IShare^ share = col->GetShare(j);
			IShare^ shareStr = colstr->GetShare(j);
			Console::WriteLine(share->ToString());
			Console::WriteLine(shareStr->ToString());
		}*/
	}

	List<IShareCollection^>^ recshares = shares->GetRange(0, k);
	array<Byte>^ recoveredSecret = secretshare->ReconstructSecret(recshares, chunkSize);
	//List<IShareCollection^>^ recsharesstr = sharesStr->GetRange(0, k);
	//String^ recoveredSecretstr = secretshare->ReconstructSecret(recsharesstr);

	Console::WriteLine("Secret:"+Encoding::UTF8->GetString(recoveredSecret));
	for (int i = 0; i < recshares->Count; i++)
	{
		delete recshares[i];
	}
	//Console::WriteLine("SecretStr:" + recoveredSecretstr);
}

void PrintIShares(List<IShare^>^ shares){
	for (int j = 0; j < shares-> Count; j++)
	{
		Console::WriteLine(shares[j]->ToString());
	}
}

int main(array<System::String ^> ^args)
{
	//PrimeGenerator^ pg = gcnew PrimeGenerator();
	//ZZ p = ZZ(263);
	//ZZ_p::init(p);
	//cout<< pg->IsGeneratorOfP(p, ZZ_p(5));
	//Console::Read();

	Schoenmakers^ sch = gcnew Schoenmakers();
	sch->SelectPrimeAndGenerators(1);
	ZZ_p g = sch->Getg();
	ZZ_p G = sch->GetG();
	ZZ q = sch->Getq();
	cout <<"g:"<<g <<'\n';
	cout << "G:" << G << '\n';
	cout << "q:" << q << '\n';

	int n = 5;
	// generate n keypairs
	vector<ZZ_p> publickeys;
	vector<tuple<ZZ_p, ZZ_p>> keypairs;
	PublicKeyEncryption^ pke = gcnew PublicKeyEncryption();
	for (int i = 0; i < n; i++)
	{
		tuple<ZZ_p,ZZ_p> pair= pke->GenerateKeyPair(q, G);
		keypairs.push_back(pair);
		//cout <<"x:"<< get<0>(pair)<<" y:"<<get<1>(pair)<<'\n';
		publickeys.push_back(get<1>(pair));
	}

	int t = 3;

	vector<ZZ_p> encryptedShares;
	vector<ZZ_p> commitments;
	vector<ZZ_p> c,r;
	ZZ_p secret;
	//provide public keys to schoenmakers
	sch->SetPublicKeys(publickeys);

	List<array<Byte>^>^ commitsList = gcnew List<array<Byte>^>();
	array<Byte>^ secretB;
	array<Byte>^ U;
	array<Byte>^ sigma = Encoding::UTF8->GetBytes("4");
	List<SchoenmakersShare^>^ shares =  sch->Distribute(t, n,sigma, commitsList, secretB,U);
	List<SchoenmakersShare^>^ poolesshares = gcnew List<SchoenmakersShare^> ();
	int i = 0;
	for each (SchoenmakersShare^ share in shares)
	{
		SchoenmakersShare^ pooledshare = share;
		sch->PoolShare(gcnew Tuple<array<Byte>^, array<Byte>^>(NTLHelper::ZZpToByte(get<0>(keypairs.at(i))), NTLHelper::ZZpToByte(get<1>(keypairs.at(i)))), pooledshare);
		poolesshares->Add(pooledshare);
		i++;
	}
	array<Byte>^ reconed = sch->Reconstruct(t, poolesshares, U);
	//Console::Read();

	sch->Distribute(t, n, encryptedShares, commitments, r, c,secret);

	for (int i = 0; i < n; i++)
	{
		bool verified = sch->VerifyDistributedShare(i+1, r.at(i), c.at(i), encryptedShares.at(i), commitments, publickeys.at(i));
		cout << "share i:" << i+1 << " verified status:" << verified<<'\n';
		//if (!verified) throw gcnew Exception("Failed to verify the share!");
	}
	vector<ZZ_p> rshares;
	vector<ZZ_p> cshares;
	vector<ZZ_p> S;
	for (int i = 0; i < encryptedShares.size(); i++)
	{
		ZZ_p rshare, cshare;
		//each party pool his share and compute zero knowledge r and c
		ZZ_p Si = sch->PoolShare(get<0>(keypairs.at(i)), get<1>(keypairs.at(i)), encryptedShares.at(i), rshare, cshare);
		rshares.push_back(rshare);
		cshares.push_back(cshare);
		S.push_back(Si);
	}
	//dealer, at this point can verify decrypted shares
	bool verifyConstruction = sch->VerifyPooledShares(t, S, encryptedShares, rshares, cshares);
	cout << "Construction verified:" << verifyConstruction<<'\n';

	if (verifyConstruction){
		ZZ_p secret = sch->Reconstruct(t,S);
		cout <<"secret:" <<secret<<'\n';
	}
	Console::Read();
	return 0;

	again:
	array<Byte>^ Prime =   NTLHelper::NumberToZZByte(17);
	NTLHelper::InitZZ_p(Prime);

	array<Byte>^ Base1 = NTLHelper::NumberToZZpByte(3);
	array<Byte>^ Base2 = NTLHelper::NumberToZZpByte(5);
    array<Byte>^ Result1 = NTLHelper::NumberToZZpByte((long)pow(3,7));
	array<Byte>^ Result2 = NTLHelper::NumberToZZpByte((long)pow(5,7));
	array<Byte>^ Secret = NTLHelper::NumberToZZpByte(7);
	array<Byte>^ R;
	array<Byte>^ C;

	NonInteractiveChaumPedersen^ NICP = gcnew NonInteractiveChaumPedersen(Prime);
	NICP->ComputeProofs(Base1,Base2,Result1,Result2,Secret,R,C);
	bool Proved = NICP ->VerifyProofs(Base1,Base2,Result1,Result2,R,C);
	cout << "Proved:" << Proved<<'\n';
	Console::Read();
	goto again;

	/*ZZ prime = ZZ(17);
	ZZ_p::init(prime);
	NonInteractiveChaumPedersen^ nicp = gcnew NonInteractiveChaumPedersen(prime);
	ZZ_p base1 = ZZ_p(3);
	ZZ_p base2 = ZZ_p(5);
	ZZ secret = ZZ(7);
	ZZ_p result1 = power(base1, secret);
	ZZ_p result2 = power(base2, secret);
	ZZ_p c;
	ZZ_p r;
	nicp->ComputeProofs(base1, base2, result1, result2, to_ZZ_p(secret), r, c);

	cout <<"r: "<< r<<'\n';
	cout << "c: " << c << '\n';

	bool proved = nicp->VerifyProofs(base1, base2, result1, result2, r, c);
	cout << "proved:" << proved<<'\n';
	Console::Read();

	goto again;*/

	/*BenalohLeichter^ benaloh = gcnew BenalohLeichter();
	AccessStructure^ access = gcnew AccessStructure("p1^p2^p3,p2^p3^p4,p1^p3^p4,p1^p2^p4");
	access = ThresholdHelper::OptimiseAccessStructure(access, true);
	array<Byte>^ secretBytes = Encoding::UTF8->GetBytes("12345678");
	List<IShareCollection^>^ shares =  benaloh->DivideSecret(secretBytes, access);
	array<Byte>^ reconSecretBytes = benaloh->ReconstructSecret(shares[0]);
	Console::WriteLine(Encoding::UTF8->GetString(reconSecretBytes));
*/
	//_CrtSetReportMode(_CRT_ERROR, _CRTDBG_MODE_DEBUG);

	/*Vec<ZZ_p> y = vec_ZZ_p();
	Vec<ZZ_p> x = vec_ZZ_p();

	ZZ_p::init(ZZ(199));
	ZZ_p y1 = ZZ_p(68);
	ZZ_p y2 = ZZ_p(2);
	ZZ_p y3 = ZZ_p(92);

		x.append(ZZ_p(1));
		x.append(ZZ_p(2));
		x.append(ZZ_p(3));

		y.append(y1);
		y.append(y2);
		y.append(y3);

	ZZ_pX interpolatedf = interpolate(x, y);
	cout << "interpol g(x):" << interpolatedf;*/

	//runByteChunkShare();

	//IShare^ sharezz = gcnew ShamirShare(1, &ZZ_p(2));

	/*
	int k = 4;
	int n = 10;
	String^ secret = "1234";

	Shamir^ secretshare = gcnew Shamir();
	List<IShareCollection^>^ shares = secretshare->DivideSecret(k, n, secret);

	for (int i = 0; i < shares->Count; i++)
	{
		IShareCollection^ col = shares[i];
		Console::WriteLine(col->ToString());
		for (int j = 0; j < col->GetCount(); j++)
		{
			IShare^ share = col->GetShare(j);
			Console::WriteLine(share->ToString());
		}
	}

	List<IShareCollection^>^ frecshares = shares->GetRange(0, k - 1);
	String^ frecoveredSecret = secretshare ->ReconstructSecret(frecshares);
	Console::WriteLine("recovered secret with k-1 shares:{0} secret:{1}", frecshares->Count, frecoveredSecret);

	List<IShareCollection^>^ recshares = shares->GetRange(0, k);
	String^ recoveredSecret = secretshare->ReconstructSecret(recshares);
	Console::WriteLine("recovered secret with k shares:{0} secret:{1}", recshares->Count, recoveredSecret);*/

	/*
	int a1 = 166;
	int a2 = 94;

	int p = 1613;

	ZZ_p::init(ZZ(p));

	ZZ_pX f;
	f.SetLength( k-1);
	SetCoeff(f, 0, ZZ_p(secret));
	SetCoeff(f, 1, ZZ_p(a1));
	SetCoeff(f, 2, ZZ_p(a2));

	cout << f;

	ZZ_p d1 = eval(f, ZZ_p(1));
	ZZ_p d2 = eval(f, ZZ_p(2));
	ZZ_p d3 = eval(f, ZZ_p(3));
	ZZ_p d4 = eval(f, ZZ_p(4));
	ZZ_p d5 = eval(f, ZZ_p(5));
	ZZ_p d6 = eval(f, ZZ_p(6));

	cout << '\n' << d1 << '\n' << d2 << '\n' << d3 << '\n' << d4 << '\n' << d5 << '\n' << d6;

	Vec<ZZ_p> x = vec_ZZ_p();
	for (int i = 1; i < 4; i++)
	{
		x.append(ZZ_p(i));
	}

	Vec<ZZ_p> y = vec_ZZ_p();
	y.append(d1);
	y.append(d2);
	y.append(d3);
	ZZ_pX interpolatedf = interpolate(x, y);
	cout << "secret is:" << eval(interpolatedf, ZZ_p(0));

	/*ZZ p;
	GenPrime(p, 10);
	long val = RandomPrime_long(8);
	cout << p << "/n";
	cout << val;
	cin >> p;
	ZZ_p::init(p);

	ZZ_pX f;
	cin >> f;

	Vec< Pair< ZZ_pX, long > > factors;

	CanZass(factors, f);  // calls "Cantor/Zassenhaus" algorithm

	cout << factors << "\n";
	*/	//_CrtDumpMemoryLeaks();
}


// solving A * X = B
// using driver function gesv()

#include <cstddef>
#include <iostream>
#include <vector>
#include <boost/numeric/bindings/lapack/gesv.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/std_vector.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace lapack = boost::numeric::bindings::lapack;

using std::size_t;
using std::cout;
using std::endl;

typedef ublas::matrix<double, ublas::column_major> m_t;

int main() {

  cout << endl;

  size_t n = 5;
  m_t a (n, n);   // system matrix

  size_t nrhs = 2;
  m_t x (n, nrhs), b (n, nrhs);  // b -- right-hand side matrix

  init_symm (a);
  //     [n   n-1 n-2  ... 1]
  //     [n-1 n   n-1  ... 2]
  // a = [n-2 n-1 n    ... 3]
  //     [        ...       ]
  //     [1   2   ...  n-1 n]

  m_t aa (a); // copy of a, because a is `lost' after gesv()

  ublas::matrix_column<m_t> xc0 (x, 0), xc1 (x, 1);
  for (int i = 0; i < xc0.size(); ++i) {
    xc0 (i) = 1.;
    xc1 (i) = 2.;
  }
  b = prod (a, x);

  print_m (a, "A");
  cout << endl;
  print_m (b, "B");
  cout << endl;

  lapack::gesv (a, b);  // solving the system, b contains x

  print_m (b, "X");
  cout << endl;

  x = prod (aa, b);
  print_m (x, "B = A X");

  cout << endl;

}


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

/*
 Copyright (C) 2020 Klaus Spanderen

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

/*! \file momentbasedgaussianpolynomial.hpp
    \brief Gaussian quadrature defined by the moments of the distribution
*/

#ifndef quantlib_moment_based_gaussian_polynomial_hpp
#define quantlib_moment_based_gaussian_polynomial_hpp

#include <ql/math/comparison.hpp>
#include <ql/math/integrals/gaussianorthogonalpolynomial.hpp>
#include <ql/errors.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <vector>

namespace QuantLib {
    /*! References:
        Gauss quadratures and orthogonal polynomials

        G.H. Gloub and J.H. Welsch: Calculation of Gauss quadrature rule.
        Math. Comput. 23 (1986), 221-230,
        http://web.stanford.edu/class/cme335/spr11/S0025-5718-69-99647-1.pdf

        M. Morandi Cecchi and M. Redivo Zaglia, Computing the coefficients
        of a recurrence formula for numerical integration by moments and
        modified moments.
        http://ac.els-cdn.com/0377042793901522/1-s2.0-0377042793901522-main.pdf?_tid=643d5dca-a05d-11e6-9a56-00000aab0f27&acdnat=1478023545_cf7c87cba4cc9e37a136e68a2564d411
    */

    template <class mp_real>
    class MomentBasedGaussianPolynomial
            : public GaussianOrthogonalPolynomial {
      public:
        MomentBasedGaussianPolynomial();

        Real mu_0() const;
        Real alpha(Size i) const;
        Real beta(Size i) const;

        virtual mp_real moment(Size i) const = 0;

      private:
        mp_real alpha_(Size i) const;
        mp_real beta_(Size i) const;

        mp_real z(Integer k, Integer i) const;

        mutable std::vector<mp_real> b_, c_;
        mutable std::vector<std::vector<mp_real> > z_;
    };

    template <class mp_real> inline
    MomentBasedGaussianPolynomial<mp_real>::MomentBasedGaussianPolynomial()
    : z_(1, std::vector<mp_real>()) {}

    template <class mp_real> inline
    mp_real MomentBasedGaussianPolynomial<mp_real>::z(Integer k, Integer i) const {
        if (k == -1) return mp_real(0.0);

        const Integer rows = z_.size();
        const Integer cols = z_[0].size();

        if (cols <= i) {
            for (Integer l=0; l<rows; ++l)
                z_[l].resize(i+1, std::numeric_limits<mp_real>::quiet_NaN());
        }
        if (rows <= k) {
            z_.resize(k+1, std::vector<mp_real>(
                z_[0].size(), std::numeric_limits<mp_real>::quiet_NaN()));
        }

        if (boost::math::isnan(z_[k][i])) {
            if (k == 0)
                z_[k][i] = moment(i);
            else {
                const mp_real tmp = z(k-1, i+1)
                    - alpha_(k-1)*z(k-1, i) - beta_(k-1)*z(k-2, i);
                z_[k][i] = tmp;
            }
        }

        return z_[k][i];
    };

    template <class mp_real> inline
    mp_real MomentBasedGaussianPolynomial<mp_real>::alpha_(Size u) const {

        if (b_.size() <= u)
            b_.resize(u+1, std::numeric_limits<mp_real>::quiet_NaN());

        if (boost::math::isnan(b_[u])) {
            if (u == 0)
                b_[u] = moment(1);
            else {
                const Integer iu(u);
                const mp_real tmp =
                    -z(iu-1, iu)/z(iu-1, iu-1) + z(iu, iu+1)/z(iu, iu);
                b_[u] = tmp;
            }
        }
        return b_[u];
    }

    template <class mp_real> inline
    mp_real MomentBasedGaussianPolynomial<mp_real>::beta_(Size u) const {
        if (u == 0)
            return mp_real(1.0);

        if (c_.size() <= u)
            c_.resize(u+1, std::numeric_limits<mp_real>::quiet_NaN());

        if (boost::math::isnan(c_[u])) {
            const Integer iu(u);
            const mp_real tmp = z(iu, iu) / z(iu-1, iu-1);
            c_[u] = tmp;
        }
        return c_[u];
    }

    template <> inline
    Real MomentBasedGaussianPolynomial<Real>::alpha(Size u) const {
        return alpha_(u);
    }

    template <class mp_real> inline
    Real MomentBasedGaussianPolynomial<mp_real>::alpha(Size u) const {
        return alpha_(u).template convert_to<Real>();
    }

    template <> inline
    Real MomentBasedGaussianPolynomial<Real>::beta(Size u) const {
        return beta_(u);
    }

    template <class mp_real> inline
    Real MomentBasedGaussianPolynomial<mp_real>::beta(Size u) const {
        mp_real b = beta_(u);
        return b.template convert_to<Real>();
    }

    template <> inline
    Real MomentBasedGaussianPolynomial<Real>::mu_0() const {
        const Real m0 = moment(0);
        QL_REQUIRE(close_enough(m0, 1.0), "zero moment must by one.");

        return moment(0);
    }

    template <class mp_real> inline
    Real MomentBasedGaussianPolynomial<mp_real>::mu_0() const {
        return moment(0).template convert_to<Real>();
    }
}

#endif


#include <SOperations.h>
#include <assert.h>                  // for assert
#include <algorithm>                 // for min
#include <boost/core/enable_if.hpp>  // for enable_if_c<>::type
#include <stdexcept>                 // for out_of_range

using namespace boost ;

//https://stackoverflow.com/questions/35971827/c-boost-rational-class-floor-function
namespace boost {
    template <typename IntType>
    constexpr IntType floor(rational<IntType> const &num) {
        return static_cast<IntType>(num.numerator() / num.denominator());
    }
    template <typename IntType>
    constexpr IntType ceil(rational<IntType> const &num) {
        auto inum = static_cast<IntType>(num.numerator() / num.denominator());
        return (num == inum) ? inum : ((num.numerator() > 0) ? ++ inum : --inum) ;
    }
}

/*
for i in range(0,10):
     if floor(i*delta)==floor((i+1)*delta):
         print B[i-int(floor((i+1)*delta))],
     else:
         print A[int(floor(i*delta))],

deltaC = (deltaA*deltaB)/(deltaA+deltaB)
*/

rational<int> deltaHash(const rational<int> &arg1, const rational<int> &arg2) {
    assert(arg1 + arg2 != 0);
    return (arg1 * arg2) / (arg1 + arg2);
}

bool Hash(const rational<int> &deltaA, const rational<int> &deltaB, const int i, int &retPos) {
    assert(deltaA > 0);
    assert(deltaB > 0);
    const rational<int> delta = deltaB / (deltaA + deltaB) ;
    bool ret = floor(delta * i) == floor(delta * (i + 1));

    if (ret) {
        retPos = i - (floor((i + 1) * delta));        //B
    } else {
        retPos = floor(i * delta);    //A
    }

    return ret ;
}

/*
#hash - begin
delta=deltaB/(deltaA+deltaB)

for i in range(0,24):
    if int(i*delta)==int((i+1)*delta):
        C.append( B[i-int((i+1)*delta)] )
    else:
        C.append( A[int(i*delta)] )
#hash - end

#div - begin
deltaC=(deltaA*deltaB)/(deltaA+deltaB)

deltaA_ = deltaB*deltaC/(deltaB-deltaC)
assert(deltaA_ == deltaA)
from math import ceil
for i in range(0,5) :
    print C[i+int(ceil((i+1)*deltaA/deltaB))],    <--- tu jest to div
#Output: 1 2 3 4 5

#div- end
*/

int Div(const boost::rational<int> &deltaA, const boost::rational<int> &deltaB, const int i) {
    return i + ceil((i + 1) * deltaA / deltaB);
}

/*
#hash - begin
delta=deltaB/(deltaA+deltaB)

for i in range(0,24):
    if int(i*delta)==int((i+1)*delta):
        C.append( B[i-int((i+1)*delta)] )
    else:
        C.append( A[int(i*delta)] )

#hash - end

#mod - begin
deltaC=(deltaA*deltaB)/(deltaA+deltaB)

deltaB_ = deltaA*deltaC/(deltaA-deltaC)
assert(deltaB_ == deltaB)
for i in range(0,10) :
    print C[i+int(i*deltaB/deltaA)],                <--- tu jest mod
#Output: a b c d e f g h i j#

#mod - end
*/

int Mod(const boost::rational<int> &deltaA, const boost::rational<int> &deltaB, const int i) {
    return i + floor(i * deltaB / deltaA);
}

/* Ta funkcja jest taka sama dla obu operacji */

rational<int> deltaDivMod(const rational<int> &arg1, const rational<int> &arg2) {
    assert(arg1 != arg2);

    if (arg1 == arg2) {
        throw std::out_of_range("Delta are equal in DehashDiv - undefinied.");
    }

    return (arg1 * arg2) / abs(arg1 - arg2);
}

/*
from math import ceil
for i in range(0,10):
    if deltaA > deltaB :
        print C[int(ceil(i*deltaA/deltaB))][0],
    else:
        print C[i][0],
*/

rational<int> deltaSubstract(const rational<int> &arg1) {
    return arg1 ;
}
//todo
int Substract(const rational<int> &deltaA, const rational<int> &deltaB, const int i) {
    return ceil(i * deltaA / deltaB);
}

rational<int> deltaAdd(const rational<int> &arg1, const rational<int> &arg2) {
    return std::min(arg1, arg2) ;
}

/*
deltaC = min( deltaA,deltaB )
for i in range(0,10):
     if deltaC == deltaA:
         print str(A[i])+B[int(i*deltaA/deltaB)],
     else:
         print str(A[int(i*deltaB/deltaA)])+B[i],
*/

rational<int> deltaTimemove(const rational<int> &arg1, const rational<int> &arg2) {
    return arg1 ;
}

int agse(int offset, int step) {
    return floor(boost::rational<int> (offset) / boost::rational<int> (step));
}

void SOperations_regtest() {
    boost::rational<int> a(1, 2) ;
    boost::rational<int> b(1, 3) ;
    boost::rational<int> c(2, 3) ;
    boost::rational<int> d(5, 4) ;
    boost::rational<int> e(1) ;
    boost::rational<int> f(0) ;
    boost::rational<int> g(-2, 3);
    boost::rational<int> h(-1);
    boost::rational<int> j(-5, 4);
    assert(floor(a) == 0);
    assert(floor(b) == 0);
    assert(floor(c) == 0);
    assert(floor(d) == 1);
    assert(floor(e) == 1);
    assert(floor(f) == 0);
    assert(floor(g) == 0);
    assert(floor(h) == -1);
    assert(floor(j) == -1);
    assert(ceil(a) == 1);
    assert(ceil(b) == 1);
    assert(ceil(c) == 1);
    assert(ceil(d) == 2);
    assert(ceil(e) == 1);
    assert(ceil(f) == 0);
    assert(ceil(g) == -1);
    assert(ceil(h) == -1);
    assert(ceil(j) == -2);
}


#ifndef USE_CUDA

#include <kernel_vectormath.hpp>
#include <Eigen/Dense>

// C++ Version
namespace Kernel
{
    // sets vf := v
    // vf is a vectorfield
    // v is a vector
    void fill(vectorfield & vf, const Vector3 & v)
    {
        for (unsigned int i=0; i<vf.size(); ++i)
        {
            vf[i] = v;
        }
    }

    void scale(vectorfield & vf, const scalar & sc)
    {
        for (unsigned int i=0; i<vf.size(); ++i)
        {
            vf[i] *= sc;
        }
    }

	// computes the inner product of two vectorfields v1 and v2
	scalar dot(const vectorfield & v1, const vectorfield & v2)
	{
		scalar x = 0;
		for (unsigned int i = 0; i<v1.size(); ++i)
		{
			x += v1[i].dot(v2[i]);
		}
		return x;
	}

    // computes the inner products of vectors in v1 and v2
    // v1 and v2 are vectorfields
    void dot(const vectorfield & v1, const vectorfield & v2, scalarfield & out)
    {
        for (unsigned int i=0; i<v1.size(); ++i)
        {
			out[i] = v1[i].dot(v2[i]);
        }
    }

    // computes the vector (cross) products of vectors in v1 and v2
    // v1 and v2 are vector fields
    void cross(const vectorfield & v1, const vectorfield & v2, vectorfield & out)
    {
        for (unsigned int i=0; i<v1.size(); ++i)
        {
            out[i] = v1[i].cross(v2[i]);
        }
    }

    // out[i] += c*a
    void add_c_a(const scalar & c, const Vector3 & a, vectorfield & out)
    {
        for(unsigned int idx = 0; idx < out.size(); ++idx)
        {
            out[idx] += c*a;
        }
    }

    // out[i] += c*a[i]
    void add_c_a(const scalar & c, const vectorfield & a, vectorfield & out)
    {
        for(unsigned int idx = 0; idx < out.size(); ++idx)
        {
            out[idx] += c*a[idx];
        }
    }

    // out[i] += c * a*b[i]
    void add_c_dot(const scalar & c, const Vector3 & a, const vectorfield & b, scalarfield & out)
    {
        for(unsigned int idx = 0; idx < out.size(); ++idx)
        {
            out[idx] += c*a.dot(b[idx]);
        }
    }

    // out[i] += c * a[i]*b[i]
    void add_c_dot(const scalar & c, const vectorfield & a, const vectorfield & b, scalarfield & out)
    {
        for(unsigned int idx = 0; idx < out.size(); ++idx)
        {
            out[idx] += c*a[idx].dot(b[idx]);
        }
    }

    // out[i] += c * a x b[i]
    void add_c_cross(const scalar & c, const Vector3 & a, const vectorfield & b, vectorfield & out)
    {
        for(unsigned int idx = 0; idx < out.size(); ++idx)
        {
            out[idx] += c*a.cross(b[idx]);
        }
    }

    // out[i] += c * a[i] x b[i]
    void add_c_cross(const scalar & c, const vectorfield & a, const vectorfield & b, vectorfield & out)
    {
        for(unsigned int idx = 0; idx < out.size(); ++idx)
        {
            out[idx] += c*a[idx].cross(b[idx]);
        }
    }

}

#endif


#include <iostream>
#include <Eigen/Dense>
#include "matplotlibcpp.h"
#include <boost/numeric/odeint.hpp>

using namespace boost::numeric::odeint;

// Define a abbreviation for state type
typedef std::vector< double > state_type;

namespace plt = matplotlibcpp;

const double sigma = 10.0;
const double R = 28.0;
const double b = 8.0 / 3.0;

// the system function can be a classical functions
void lorenz( state_type &x , state_type &dxdt , double t )
{
    dxdt[0] = sigma * ( x[1] - x[0] );
    dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
    dxdt[2] = x[0]*x[1] - b * x[2];
}

int main(int argc, char const *argv[])
{
    state_type x( 3 );
    x[0] = x[1] = x[2] = 10.0;
    const double dt = 0.01;
    integrate_const( runge_kutta4< state_type >() , lorenz , x , 0.0 , 10.0 , dt );

    return 0;
}


#include <iostream>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main()
{
	Matrix3f A;
	A << 1, 2, 1,
	    2, 1, 0,
	    -1, 1, 2;
	cout << A << endl;
	cout << A * A * A * A * A * A * A << endl;
	cout << A.inverse() << endl;
	cout << A.determinant() << endl;

	MatrixXf B = MatrixXf::Random(5, 3);
	cout << B << endl;

	return 0;
}


#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

int main(int, char**)
{
    typedef mtl::dense2D<double>       Matrix;
    typedef mtl::dense_vector<double>  Vector;

    Matrix                             A(4, 4), L(4, 4), U(4, 4), AA(4, 4);
    Vector	       		       v(4);
    double 			       c=1.0;

    for (unsigned i= 0; i < 4; i++)
	for(unsigned j= 0; j < 4; j++) {
	    U[i][j]= i <= j ? c * (i+j+2) : (0);
	    L[i][j]= i > j ? c * (i+j+1) : (i == j ? (1) : (0));
	}

    std::cout << "L is:\n" << L << "U is:\n" << U;
    A= L * U;
    std::cout << "A is:\n" << A;
    AA= adjoint(A);

    for (unsigned i= 0; i < 4; i++)
	v[i]= double(i);

    Vector b( A*v ), b2( adjoint(A)*v );

    Matrix LU(A);
    lu(LU);
    std::cout << "LU decomposition of A is:\n" << LU;

    Matrix B( lu_f(A) );
    std::cout << "LU decomposition of A (as function result) is:\n" << B;

    Vector v1( lu_solve_straight(A, b) );
    std::cout << "v1 is " << v1 << "\n";

    Vector v2( lu_solve(A, b) );
    std::cout << "v2 is " << v2 << "\n";

    mtl::dense_vector<unsigned> P;
    lu(A, P);
    std::cout << "LU with pivoting is \n" << with_format(A, 5, 2) << "Permutation is " << P << "\n";
    Vector v3( lu_apply(A, P, b) );
    std::cout << "v3 is " << v3 << "\n";

    Vector v4(lu_adjoint_apply(A, P, b2));
    std::cout << "v4 is " << v4 << "\n";

    Vector v5(lu_adjoint_solve(AA, b));
    std::cout << "v5 is " << v5 << "\n";

    return 0;
}


//
// Created by senbaikang on 21.05.21.
//

#include <cmath>
#include <config.h>

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

#include "probabilities.h"

double logBetaBinCountsTerm(
    double sup,
    double cov
    )
{
  return std::lgamma(cov + 1.0) -  std::lgamma(sup + 1.0) - std::lgamma(cov - sup + 1.0);
}

double logBetaBinMixedTerm(
    double sup,
    double cov,
    double mean,
    double overDis
    )
{
  return std::lgamma(sup + mean * overDis) + std::lgamma(cov - sup + overDis * (1.0 - mean)) - std::lgamma(cov + overDis);
}

double logBetaBinParamsTerm(
    double mean,
    double overDis
    )
{
  return std::lgamma(overDis) - std::lgamma(mean * overDis) - std::lgamma(overDis * (1.0 - mean));
}

double logBetaBinPDF(
    double sup,
    double cov,
    double mean,
    double overDis
)
{
  if (cov == 0)
    return 0;

  return logBetaBinCountsTerm(sup, cov) +
         logBetaBinMixedTerm(sup, cov, mean, overDis) +
         logBetaBinParamsTerm(mean, overDis);
}

double computeRawWildLogScore(
    Config const &config,
    double altCount,
    double coverage
    )
{
  return logBetaBinPDF(
      altCount,
      coverage,
      config.getEffectiveSeqErrRate() / 3.0,
      config.getWildOverdispersion()
      );
//  return logBetaBinPDF(
//      altCount,
//      coverage,
//      config.getEffectiveSeqErrRate(),
//      config.getWildOverdispersion()
//  );
}

double computeRawHeteroMutLogScore(
    Config const &config,
    double altCount,
    double coverage
    )
{
  return logBetaBinPDF(
      altCount,
      coverage,
      0.5 - config.getEffectiveSeqErrRate() / 3.0,
      config.getMuOverdispersion()
      );
//  return logBetaBinPDF(
//      altCount,
//      coverage,
//      0.5 - (2.0 / 3.0 * config.getEffectiveSeqErrRate()),
//      config.getMuOverdispersion()
//  );
}

double computeRawHomoMutLogScore(
    Config const &config,
    double covMinusSup,
    double coverage
    )
{
  return logBetaBinPDF(
      covMinusSup,
      coverage,
      config.getEffectiveSeqErrRate(),
      config.getWildOverdispersion()
      );
}

// Add two values in real space by first exponentiating
double addLogProb(double x, double y)
{
  double maxScore;
  double minScore;

  if (x > y)
  {
    maxScore = x;
    minScore = y;
  }
  else
  {
    maxScore = y;
    minScore = x;
  }

  return std::log(1.0 + std::exp(minScore - maxScore)) + maxScore;
}

double logNChoose2(u_int32_t numMut)
{
  return log(static_cast<double>(numMut)) + log((static_cast<double>(numMut) - 1.0) / 2.0);
}

double logNChooseK(u_int32_t n, u_int32_t k, double logNChoosekMinusOne)
{
  if (k == 0)
    return 0;

  return logNChoosekMinusOne + log(static_cast<double>(n + 1 - k) / static_cast<double>(k));
}

double logNChooseK(u_int32_t n, u_int32_t k)
{
  if (k == 0)
    return 0;

  return std::lgamma(n + 1) - std::lgamma(k + 1) - std::lgamma(n - k + 1);
}

OptimizeBetaBinMeanOverDis::OptimizeBetaBinMeanOverDis(
    const std::vector<std::pair<u_int32_t, u_int32_t>> &counts
    ):
        counts(counts)
{}

double OptimizeBetaBinMeanOverDis::operator()(const dlib::matrix<double,0,1> &x) const
{
  double result = 0;
  for (const auto & count : this->counts)
    result += logBetaBinPDF(count.first, count.second, x(0), x(1));

  return result;
}

OptimizeBetaBinMeanOverDisDerivates::OptimizeBetaBinMeanOverDisDerivates(
    const std::vector<std::pair<u_int32_t , u_int32_t>> &counts
    ):
    counts(counts)
{}

dlib::matrix<double> OptimizeBetaBinMeanOverDisDerivates::operator()(const dlib::matrix<double,0,1> &x) const
{
  double mean = x(0);
  double overDis = x(1);
  dlib::matrix<double,0,1> res = {0,0};

  double temp = 0;
  unsigned counter = 0;
  for (const auto & count : this->counts)
  {
    unsigned k = count.first;
    unsigned n = count.second;
    temp += overDis *  boost::math::digamma(k + mean * overDis)
            - overDis *  boost::math::digamma(n - k + overDis - overDis * mean);
    ++counter;
  }
  res(0) = counter * (-overDis *  boost::math::digamma(mean * overDis) + overDis *  boost::math::digamma(overDis - overDis * mean)) + temp;

  temp = 0;
  for (const auto & count : this->counts)
  {
    unsigned k = count.first;
    unsigned n = count.second;
    temp += mean * boost::math::digamma(k + mean * overDis) +
            (1.0 - mean) * boost::math::digamma(n - k + overDis - overDis * mean) -
            boost::math::digamma(n + overDis);
  }
  res(1) = counter * (boost::math::digamma(overDis) - mean * boost::math::digamma(mean * overDis) - (1.0 - mean) * boost::math::digamma(overDis - overDis * mean)) + temp;

  return res;
}

OptimizeBetaBinOverDis::OptimizeBetaBinOverDis(
    const std::vector<std::pair<u_int32_t, u_int32_t>> &counts,
    double meanFilter
):
    counts(counts),
    meanFilter(meanFilter)
{}

double OptimizeBetaBinOverDis::operator()(const dlib::matrix<double,0,1> &x) const
{
  double result = 0;
  for (const auto & count : this->counts)
    result += logBetaBinPDF(count.first, count.second, meanFilter, x(0));

  return result;
}

OptimizeBetaBinOverDisDerivates::OptimizeBetaBinOverDisDerivates(
    const std::vector<std::pair<u_int32_t, u_int32_t>> &counts,
    double meanFilter
    ):
    counts(counts),
    meanFilter(meanFilter)
{}

dlib::matrix<double> OptimizeBetaBinOverDisDerivates::operator()(const dlib::matrix<double,0,1> &x) const
{
  double mean = this->meanFilter;
  double overDis = x(0);
  dlib::matrix<double,0,1> res = {0};

  double temp = 0;
  u_int32_t counter = 0;
  for (const auto & count : this->counts)
  {
    unsigned k = count.first;
    unsigned n = count.second;
    temp += mean * boost::math::digamma(k + mean * overDis) +
            (1.0 - mean) * boost::math::digamma(n - k + overDis - overDis * mean) -
            boost::math::digamma(n + overDis);
    ++counter;
  }
  res(0) = counter * (boost::math::digamma(overDis) - mean * boost::math::digamma(mean * overDis) - (1.0 - mean) * boost::math::digamma(overDis - overDis * mean)) + temp;

  return res;
}


/*
 * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
 *
 * This software may be modified and distributed under the terms of the
 * BSD 3-Clause license. See the accompanying LICENSE file for details.
 */

#include <BayesFilters/LTIStateModel.h>

#include <Eigen/Dense>

using namespace bfl;
using namespace Eigen;

LTIStateModel::LTIStateModel(const Ref<const MatrixXd>& transition_matrix, const Ref<const MatrixXd>& noise_covariance_matrix) :
    F_(transition_matrix), Q_(noise_covariance_matrix)
{
    if ((F_.rows() == 0) || (F_.cols() == 0))
        throw std::runtime_error("ERROR::LTISTATEMODEL::CTOR\nERROR:\n\tState transition matrix dimensions cannot be 0.");
    else if ((Q_.rows() == 0) || (Q_.cols() == 0))
        throw std::runtime_error("ERROR::LTISTATEMODEL::CTOR\nERROR:\n\tNoise covariance matrix dimensions cannot be 0.");
    else if (F_.rows() != F_.cols())
        throw std::runtime_error("ERROR::LTISTATEMODEL::CTOR\nERROR:\n\tState transition matrix must be a square matrix.");
    else if (Q_.rows() != Q_.cols())
        throw std::runtime_error("ERROR::LTISTATEMODEL::CTOR\nERROR:\n\tNoise covariance matrix must be a square matrix.");
    else if (F_.rows() != Q_.rows())
        throw std::runtime_error("ERROR::LTISTATEMODEL::CTOR\nERROR:\n\tNumber of rows of the state transition matrix must be the same as the size of the noise covariance matrix.");
}

void LTIStateModel::propagate(const Eigen::Ref<const Eigen::MatrixXd>& cur_states, Eigen::Ref<Eigen::MatrixXd> prop_states)
{
    prop_states = F_ * cur_states;
}

Eigen::MatrixXd LTIStateModel::getNoiseCovarianceMatrix()
{
    return Q_;
}

Eigen::MatrixXd LTIStateModel::getStateTransitionMatrix()
{
    return F_;
}

bool LTIStateModel::setProperty(const std::string& property)
{
    return false;
}

Eigen::MatrixXd LTIStateModel::getJacobian()
{
    return F_;
}