//
// Created by robin jodon on 2020-04-16.
//
// Test file to understand how to use CLP
//

#include <catch2/catch.hpp>
#include <iostream>
#include <Eigen/Sparse>

#include "Interlocking/InterlockingSolver_Clp.h"
#include "IO/JsonIOReader.h"
#include "ClpSimplex.hpp"

typedef Eigen::SparseMatrix<double> SpMat;
typedef Eigen::Triplet<double> T;
typedef Eigen::Matrix<double, 3, 1> Vector3d;

/**
 * @brief Simple test case of CLP simplex solver
 *        We are solving the following LP problem
 *
 *        max 3x + 4y + 2z
 *    s.t.    2x           >= 4     E1
 *             x      + 2z >= 8     E2
 *                 3y +  z >= 6     E3
 *             (x,y,z)     >= 0     E4
 *
 *        Solution is: x,y,z = (2,1,3)
 *                     max   = 16
 */
TEST_CASE("CLP simplex - Simple LP problem") {
    // Mock
    shared_ptr<InputVarList> varList = make_shared<InputVarList>();
    InitVar(varList.get());
    shared_ptr<ContactGraph<double>> dat = make_shared<ContactGraph<double>>(varList);
    InterlockingSolver_Clp<double> solver(dat, varList);
    shared_ptr<typename InterlockingSolver<double>::InterlockingData> data;
    //
    int num_col = 3;
    int num_row = 3;
    int num_var = 3;

    Vector3d objective_function(3, 4, 2);   // coefficients in objective function
    Vector3d rhs(4, 8, 6);                  // the right hand side vector resulting from the constraints
    SpMat A(num_row, num_col);                      // coefficient matrix

    // Define A
    std::vector<T> coef;
    //
    coef.emplace_back(T(0, 0, 2));
    coef.emplace_back(T(1, 0, 1));
    coef.emplace_back(T(1, 2, 2));
    coef.emplace_back(T(2, 1, 3));
    coef.emplace_back(T(2, 2, 1));

    A.setFromTriplets(coef.begin(), coef.end());

    CoinPackedMatrix matrix(true, num_row, num_col, A.nonZeros(), A.valuePtr(), A.innerIndexPtr(), A.outerIndexPtr(), A.innerNonZeroPtr());

    // boundaries for rows and columns values

    auto *objective(new double[num_var]);           // objective function params
    auto *constraintLower(new double[num_row]);     // E1...E3 lower bounds
    auto *constraintUpper(new double[num_row]);     // E1...E3 upper bounds
    auto *varLower(new double[num_var]);            // E4
    auto *varUpper(new double[num_var]);            // E4

    // E4
    for (size_t id = 0; id < num_var; id++) {
        varLower[id] = 0;
        varUpper[id] = COIN_DBL_MAX;
    }

    // Objective func + Ei for i in [1,2,3]
    for (size_t id = 0; id < num_row; id++) {
        constraintLower[id] = -COIN_DBL_MAX;
        constraintUpper[id] = rhs[id];
        objective[id] = -objective_function[id];  // simplex minimises by default
    }

    ClpSimplex model;
    CoinMessageHandler handler;
    handler.setLogLevel(0);
    model.passInMessageHandler(&handler);
    model.newLanguage(CoinMessages::us_en);
    model.loadProblem(matrix, varLower, varUpper, objective, constraintLower, constraintUpper);
    model.setPrimalTolerance(1e-9);
    model.primal();
    // Get the solution
    const double obj_solution = model.rawObjectiveValue();
    double *solution = model.primalColumnSolution();
    double *row_solution = model.primalRowSolution();

    SECTION("CLP simplex - Solutions checkup") {
        REQUIRE(solution[0] == Approx(2.0).epsilon(1E-9));
        REQUIRE(solution[1] == Approx(1.0).epsilon(1E-9));
        REQUIRE(solution[2] == Approx(3.0).epsilon(1E-9));
    }

    SECTION("CLP simplex - Constraints checkup") {
        REQUIRE(row_solution[0] == Approx(4.0).epsilon(1E-9));
        REQUIRE(row_solution[1] == Approx(8.0).epsilon(1E-9));
        REQUIRE(row_solution[2] == Approx(6.0).epsilon(1E-9));
    }

    SECTION("CLP simplex - Problem checkup") {
        REQUIRE(model.isProvenOptimal());
        REQUIRE_FALSE(model.isProvenPrimalInfeasible());
    }
}


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

using big_int = boost::multiprecision::cpp_int;

big_int ipow(big_int base, big_int exp) {
  big_int result(1);
  while (exp) {
    if (exp & 1) {
      result *= base;
    }
    exp >>= 1;
    base *= base;
  }
  return result;
}

big_int ackermann(unsigned m, unsigned n) {
  static big_int (*ack)(unsigned, big_int) =
      [](unsigned m, big_int n)->big_int {
    switch (m) {
    case 0:
      return n + 1;
    case 1:
      return n + 2;
    case 2:
      return 3 + 2 * n;
    case 3:
      return 5 + 8 * (ipow(big_int(2), n) - 1);
    default:
      return n == 0 ? ack(m - 1, big_int(1)) : ack(m - 1, ack(m, n - 1));
    }
  };
  return ack(m, big_int(n));
}

int main() {
  for (unsigned m = 0; m < 4; ++m) {
    for (unsigned n = 0; n < 10; ++n) {
      std::cout << "A(" << m << ", " << n << ") = " << ackermann(m, n) << "\n";
    }
  }

  std::cout << "A(4, 1) = " << ackermann(4, 1) << "\n";

  std::stringstream ss;
  ss << ackermann(4, 2);
  auto text = ss.str();
  std::cout << "A(4, 2) = (" << text.length() << " digits)\n"
            << text.substr(0, 80) << "\n...\n"
            << text.substr(text.length() - 80) << "\n";
}


/*
 * Copyright 2009-2015 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include <votca/tools/linalg.h>
#include <boost/numeric/ublas/matrix_proxy.hpp>

#include "mkl.h"
#include "mkl_lapacke.h"

namespace votca { namespace tools {

using namespace std;

void linalg_cholesky_decompose( ub::matrix<double> &A){
    // Cholesky decomposition using MKL
    // input matrix A will be changed

    // LAPACK variables
    MKL_INT info;
    MKL_INT n = A.size1();
    char uplo = 'L';

    // pointer for LAPACK
    double * pA = const_cast<double*>(&A.data().begin()[0]);
    info = LAPACKE_dpotrf( LAPACK_ROW_MAJOR , uplo , n, pA, n );
    if ( info != 0 )
        throw std::runtime_error("Matrix not symmetric positive definite");
}

void linalg_cholesky_decompose( ub::matrix<float> &A){
    // Cholesky decomposition using MKL
    // input matrix A will be changed

    // LAPACK variables
    MKL_INT info;
    MKL_INT n = A.size1();
    char uplo = 'L';

    // pointer for LAPACK
    float * pA = const_cast<float*>(&A.data().begin()[0]);
    info = LAPACKE_spotrf( LAPACK_ROW_MAJOR , uplo , n, pA, n );
    if ( info != 0 )
        throw std::runtime_error("Matrix not symmetric positive definite");
}

void linalg_cholesky_solve( ub::vector<double> &x, ub::matrix<double> &A, ub::vector<double> &b ){
    /* calling program should catch the error error code
     * thrown by LAPACKE_dpotrf and take
     * necessary steps
     */

    // LAPACK variables
    MKL_INT info;
    MKL_INT n = A.size1();
    char uplo = 'L';

    // pointer for LAPACK LU factorization of input matrix
    double * pA = const_cast<double*>(&A.data().begin()[0]); // input array

    // get LU factorization
    info = LAPACKE_dpotrf( LAPACK_ROW_MAJOR , uplo , n, pA, n );

    if ( info != 0 )
        throw std::runtime_error("Matrix not symmetric positive definite");

    MKL_INT nrhs = 1;

    // pointer of LAPACK LU solver
    double * pb = const_cast<double*>(&b.data()[0]);
    info = LAPACKE_dpotrs(LAPACK_ROW_MAJOR, uplo, n, nrhs, pA, n, pb, n );

    // on output, b contains solution
    x = b;
}

}}


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

// Copyright (c) 2011 Barend Gehrels, Amsterdam, the Netherlands.

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

//[transform
//` Shows how points can be transformed using the default strategy

#include <iostream>
#include <boost/geometry.hpp>

int main()
{
    namespace bg = boost::geometry;

    // Select a point near the pole (theta=5.0, phi=15.0)
    bg::model::point<long double, 2, bg::cs::spherical<bg::degree> > p1(15.0, 5.0);

    // Transform from degree to radian. Default strategy is automatically selected,
    // it will convert from degree to radian
    bg::model::point<long double, 2, bg::cs::spherical<bg::radian> > p2;
    bg::transform(p1, p2);

    // Transform from degree (lon-lat) to 3D (x,y,z). Default strategy is automatically selected,
    // it will consider points on a unit sphere
    bg::model::point<long double, 3, bg::cs::cartesian> p3;
    bg::transform(p1, p3);

    std::cout
        << "p1: " << bg::dsv(p1) << std::endl
        << "p2: " << bg::dsv(p2) << std::endl
        << "p3: " << bg::dsv(p3) << std::endl;

    return 0;
}

//]

//[transform_output
/*`
Output:
[pre
p1: (15, 5)
p2: (0.261799, 0.0872665)
p3: (0.084186, 0.0225576, 0.996195)
]
*/
//]


#include <Eigen/Dense>
#include "gtest/gtest.h"
#include "lsq.h"

TEST(LSQ, TestRunnable){
    Eigen::Matrix2f A;
    A << 1, 2, 2, 1;
    Eigen::Vector2f b;
    b << 5, 4;
    Eigen::Vector2f expected;
    expected << 1, 2;
    LSQ* lsq = new LSQ(A, b);
    Eigen::Vector2f x = lsq->solve();
    EXPECT_TRUE(expected.isApprox(x));
}


/*
 * Introduction to the header template.
 * For each function FUNC, the file name shall be FUNC.hpp in src/DeemaAlomair folder.
 * Every FUNC shall be replaced with implemented function name.
 * The type of a CFS with FLOAT is CFST<FLOAT>
 * The code shall follow c++17 standard.
 * Only the c++ standard libraries are allowed.
 * C-style code is highly not recommended.
 */

#ifndef FSL_FUNC_H
#define FSL_FUNC_H

#include <vector>
#include <Eigen/Dense>
/*Other library inclusion is here*/
namespace FSL{
    template <class FLOAT>
    void LinSolve(size_t n, const std::vector<std::vector<FLOAT>>& M, const std::vector<FLOAT>& y, std::vector<FLOAT>& x)
    {
        Eigen::Matrix<FLOAT, Eigen::Dynamic, Eigen::Dynamic> ME(n, n);
        for(size_t i = 0; i < n; ++i)
        {
            for(size_t j = 0; j < n; ++j)
            {
                ME(i, j) = M[i][j];
            }
        }
        Eigen::Matrix<FLOAT, Eigen::Dynamic, 1> xE(n), yE(n);
        for(size_t i =0; i<n; ++i)
        {
            yE(i) = y[i];
        }
        xE = ME.colPivHouseholderQr().solve(yE);
        x.resize(n);
        for (size_t i=0; i< n; ++i)
        {
            x[i]=xE(i);
        }
    }
}
#endif

/*
 * Type N in MIS shall be translated to size_t in C++.
 * This type is already introduced in CFSData.h
 */


#include "pch.h"
#include <Eigen/Dense>
#include "ReverseDistortionEstimator.h"
#include "photogrammetry.h"

ReverseDistortionEstimator::ReverseDistortionEstimator(const Camera& cam)
{
	//solves for the inverse distortion (like opencv)
	//see: https://docs.opencv.org/3.4.6/d9/d0c/group__calib3d.html

	int x_spacing = cam.W / 10;
	int y_spacing = cam.H / 10;
	vector<std::array<double, 2> > grid_points;
	vector<std::array<double, 2> > grid_points_undistorted;
	vector<std::array<double, 2> > dist_corrections;
	grid_points.reserve(200);

	//calculating the estimation grid:
	for (int x = 0; x <= cam.W; x += x_spacing)
	{
		for (int y = 0; y <= cam.H; y += y_spacing)
		{
			grid_points.push_back({
				static_cast<double>(x),
				static_cast<double>(y) });
		}
	}
	grid_points.shrink_to_fit();

	//transformation from column/row coordinates to image-centered coordinates
	std::transform(grid_points.begin(), grid_points.end(), grid_points.begin(),
		[&](std::array<double,2>& p ){
		return std::array<double, 2>{
			p[0] = p[0] - cam.W / 2.0 + 0.5,
			p[1] = cam.H / 2.0 - p[1] - 0.5}; });

	//transformation from image-centered coordinates to fiducial coordiantes
	std::transform(grid_points.begin(), grid_points.end(), grid_points.begin(),
		[&](std::array<double, 2>& p) {
		return std::array<double, 2>{
			p[0] = p[0] - cam.InternalOrientation[1],
			p[1] = p[1] - cam.InternalOrientation[2]}; });

	grid_points_undistorted.resize(grid_points.size());
	dist_corrections.resize(grid_points.size());

	std::transform(grid_points.begin(), grid_points.end(), dist_corrections.begin(),
		[&](std::array<double, 2>& p) {
		return std::array<double, 2>{
			fT_xDistCorrection(cam.RadialDistortion, cam.TangentialDistortion, p[0], p[1]),
			fT_yDistCorrection(cam.RadialDistortion, cam.TangentialDistortion, p[0], p[1])}; });

	for (size_t i = 0, S = grid_points.size(); i < S; i++)
	{
		grid_points_undistorted.at(i)[0] = grid_points.at(i)[0] - dist_corrections.at(i)[0];
		grid_points_undistorted.at(i)[1] = grid_points.at(i)[1] - dist_corrections.at(i)[1];
	}

	//solving the reversed distortion using Eieng linear algerbra tools:
	//each coordinate generates one equation:
	Eigen::MatrixXd A(2 * grid_points_undistorted.size(), 5);
	Eigen::VectorXd L(2 * grid_points_undistorted.size());
	Eigen::VectorXd X(5);
	Eigen::VectorXd V(2 * grid_points_undistorted.size());

	for (size_t i = 0, S = grid_points_undistorted.size(); i < S; i++)
	{
		//we are estimating in the normalized space so w have to use normalized values
		double x = grid_points_undistorted.at(i)[0] /cam.InternalOrientation[0];
		double y = -grid_points_undistorted.at(i)[1] /cam.InternalOrientation[0];
		double dx = dist_corrections.at(i)[0] / cam.InternalOrientation[0];
		double dy = -dist_corrections.at(i)[1] / cam.InternalOrientation[0];
		double rr = x * x + y * y;

		A(2 * i, 0) = x * rr;
		A(2 * i, 1) = x * rr*rr;
		A(2 * i, 2) = x * rr*rr*rr;
		A(2 * i, 3) = 2 * x * y;
		A(2 * i, 4) = rr + 2 * x*x;
		L(2 * i) = dx;

		A(2 * i + 1, 0) = y * rr;
		A(2 * i + 1, 1) = y * rr*rr;
		A(2 * i + 1, 2) = y * rr*rr*rr;
		A(2 * i + 1, 3) = rr + 2 * y * y;
		A(2 * i + 1, 4) = 2 * x * y;
		L(2 * i + 1) = dy;
	}

	X = A.colPivHouseholderQr().solve(L);
	V = L - A * X;
	RMSE = std::sqrt((1.0 /(L.rows()-1)) * V.transpose()*V);

	for (int i = 0; i < 5; i++) DistortionYDown.at(i) = X(i);

}

ReverseDistortionEstimator::~ReverseDistortionEstimator()
{
}


/*****************************************************************************
 * where.cpp     Blitz++ 1D Array example, illustrating where(X,Y,Z)
 *               expressions.
 *****************************************************************************/

#include <blitz/array.h>

BZ_USING_NAMESPACE(blitz)

int main()
{
//  Vector<int> x = Range(-3,+3);   // [ -3 -2 -1  0  1  2  3 ]
    Array<int,1> x(Range(-3,+3));
    x = tensor::i;

    // The where(X,Y,Z) function is similar to the X ? Y : Z operator.
    // If X is logical true, then Y is returned; otherwise, Z is
    // returned.

//  Vector<int> y = where(abs(x) > 2, x+10, x-10);
    Array<int,1> y(x.shape());
    y = where(abs(x) > 2, x+10, x-10);

    // The above statement is transformed into something resembling:
    //
    // for (unsigned i=0; i < 7; ++i)
    //     y[i] = (abs(x[i]) > 2) ? (x[i]+10) : (x[i]-10);
    //
    // The first expression (abs(x) > 2) can involve the usual
    // comparison and logical operators: < > <= >= == != && ||

    cout << x << endl
         << y << endl;

    return 0;
}


#include "mnist_loader.h"
#include "node.h"

#include <iostream>
#include <memory>
#include <Eigen/Core>

#define EIGEN_MPL2_ONLY

#define INPUT_SIZE  784
#define HIDDEN_SIZE 50
#define OUTPUT_SIZE 10

const double VAR_DIFF  = 1.0e-4;
const double MOD_RATIO = 1.0e-2;

namespace {
	Eigen::VectorXf ReLu(Eigen::VectorXf &val)
	{
		Eigen::VectorXf ret(val.rows());

		for(int i = 0 ; i < val.rows() ; i++)
		{
			ret[i] = fmax(0, val[i]);
		}

		return ret;
	}

	Eigen::VectorXf Softmax(Eigen::VectorXf &val)
	{
		Eigen::VectorXf ret(val.rows());

		double sum = 0;
		double *exp_array = new double[val.rows()];

		for(int i = 0 ; i < val.rows() ; i++)
		{
			exp_array[i] = exp(val[i]);
			sum += exp_array[i];
		}

		// printf("softmax sum = %f\n", sum);
		for(int i = 0 ; i < val.rows() ; i++)
		{
			ret[i] = exp_array[i] / sum;
		}

		delete[] exp_array;

		return ret;
	}

	Eigen::VectorXf MakeOnehotVector(int size, int index)
	{
		Eigen::VectorXf ret = Eigen::VectorXf::Zero(size);
		ret[index] = 1;
		return ret;
	}

	float SquareError(Eigen::VectorXf vec1, Eigen::VectorXf vec2)
	{
		assert(vec1.rows() == vec2.rows());
		assert(vec1.cols() == vec2.cols());
		assert(vec1.cols() == 1);

		float error = 0;
		for(int i = 0 ; i < vec1.rows() ; i++)
		{
			float v = vec1[i] - vec2[i];
			error += v * v;
		}

		return error;
	}

	int MaxIndex(Eigen::VectorXf vec)
	{
		int max_index = 0;
		for(int i = 0 ; i < vec.rows() ; i++)
		{
			if(vec[max_index] < vec[i])
			{
				max_index = i;
			}
		}
		return max_index;
	}
};

Eigen::VectorXf Array2VectorXf(uint8_t *array, int size)
{
	Eigen::VectorXf ret(size);

	// this conversion needs due to different data format
	for(int i = 0 ; i < size ; i++)
	{
		ret[i] = array[i];
	}

	return ret;
}

Eigen::VectorXf Predict(Node &node1, Node &node2, Eigen::VectorXf input)
{
	Eigen::VectorXf temp;

	temp = node1.Calc(input);
	temp = ReLu(temp);
	temp = node2.Calc(temp);
	temp = Softmax(temp);

	return temp;
}

double CalcError(Node &node1, Node &node2, Eigen::VectorXf input, Eigen::VectorXf ans)
{
	Eigen::VectorXf output     = Predict(node1, node2, input);
	double          error_rate = SquareError(output, ans);

	return error_rate;
}

int main()
{
	std::vector<std::shared_ptr<Image> > train_image_list;
	std::vector<uint8_t>                 train_label_list;
	std::vector<std::shared_ptr<Image> > test_image_list;
	std::vector<uint8_t>                 test_label_list;

	Eigen::initParallel();
	Eigen::setNbThreads(4);

	printf("[INFO] load image...\n");
	std::shared_ptr<MnistLoader> loader = std::make_shared<MnistLoader>();
	if(loader->LoadImage("../train-images-idx3-ubyte", train_image_list) == false)
	{
		printf("[ERROR] load data error.\n");
	}

	if(loader->LoadLabel("../train-labels-idx1-ubyte", train_label_list) == false)
	{
		printf("[ERROR] load data error.\n");
	}

	if(loader->LoadImage("../t10k-images-idx3-ubyte", test_image_list) == false)
	{
		printf("[ERROR] load data error.\n");
	}

	if(loader->LoadLabel("../t10k-labels-idx1-ubyte", test_label_list) == false)
	{
		printf("[ERROR] load data error.\n");
	}

	Node node1(INPUT_SIZE, HIDDEN_SIZE);
	if(node1.Load("weight_node1") == false)
	{
		printf("[INFO] node1 weight value initialize with random value.\n");
	}
	Node node2(HIDDEN_SIZE, OUTPUT_SIZE);
	if(node2.Load("weight_node2") == false)
	{
		printf("[INFO] node2 weight value initialize with random value.\n");
	}

	printf("[INFO] training...\n");
	sranddev();
	// for(int image_index = 0 ; image_index < train_image_list.size() ; image_index++)
	// for(int image_index = 0 ; image_index < 100 ; image_index++)
	for(int train_count = 0 ; train_count < 100 ; train_count++)
	{
		int image_index = rand() % train_image_list.size();

//		if(image_index % 100 == 0)
//		{
//			printf("[INFO] exec %d/%d\n", image_index, (int)train_label_list.size());
//		}

		std::shared_ptr<Image> image = train_image_list[image_index];
		Eigen::VectorXf        input = Array2VectorXf(image->image, image->image_size);
		int                    ans   = train_label_list[image_index];
		Eigen::VectorXf        ans_vec = MakeOnehotVector(OUTPUT_SIZE, ans);
//		printf("image_index = %d, ans = %d\n", image_index, ans);

		input.normalize();

		double *node1_mod = new double[node1.GetMaxWeightIndex()];
		double *node2_mod = new double[node2.GetMaxWeightIndex()];

		for(int i = 0 ; i < node1.GetMaxWeightIndex() ; i++)
		{
			double dyp = 0;
			double dyn = 0;

			node1.PushWeightDiff(i, VAR_DIFF);
			dyp = CalcError(node1, node2, input, ans_vec);
			node1.PopWeightDiff();

			node1.PushWeightDiff(i, -VAR_DIFF);
			dyn = CalcError(node1, node2, input, ans_vec);
			node1.PopWeightDiff();

			node1_mod[i] = (dyp - dyn) / (2.0 * VAR_DIFF);

//			printf("dyp=%f, dyn=%f, mod=%f\n", dyp, dyn, node1_mod[i]);
		}

		for(int i = 0 ; i < node2.GetMaxWeightIndex() ; i++)
		{
			double dyp = 0;
			double dyn = 0;

			node2.PushWeightDiff(i, VAR_DIFF);
			dyp = CalcError(node1, node2, input, ans_vec);
			node2.PopWeightDiff();

			node2.PushWeightDiff(i, -VAR_DIFF);
			dyn = CalcError(node1, node2, input, ans_vec);
			node2.PopWeightDiff();

			node2_mod[i] = (dyp - dyn) / (2.0 * VAR_DIFF);

//			printf("dyp=%lf, dyn=%lf, mod=%lf\n", dyp, dyn, node2_mod[i]);
		}

		for(int i = 0 ; i < node1.GetMaxWeightIndex() ; i++)
		{
			node1.AddWeight(i, node1_mod[i] * -MOD_RATIO);
		}

		for(int i = 0 ; i < node2.GetMaxWeightIndex() ; i++)
		{
			node2.AddWeight(i, node2_mod[i] * -MOD_RATIO);
		}

		delete[] node1_mod;
		delete[] node2_mod;

		//return 1;

//		if(image_index == 10)
//		{
//			// optimization test
//			return 1;
//		}
	}

	node1.Save("weight_node1");
	node2.Save("weight_node2");

	printf("[INFO] test...\n");
	std::vector<uint8_t> result_list;
	for(int image_index = 0 ; image_index < test_image_list.size() ; image_index++)
	{
		std::shared_ptr<Image> image = test_image_list[image_index];
		Eigen::VectorXf        input = Array2VectorXf(image->image, image->image_size);

		input.normalize();
		Eigen::VectorXf output = Predict(node1, node2, input);

		int result = MaxIndex(output);
		result_list.push_back(result);

	}

	assert(result_list.size() == test_label_list.size());
	int success_count = 0;
	int fail_count    = 0;
	for(int i = 0 ; i < result_list.size() ; i++)
	{
//		printf("result = %d, test = %d,", result_list[i], test_label_list[i]);
		if(result_list[i] == test_label_list[i])
		{
			success_count++;
		}else
		{
			fail_count++;
		}
	}
	float accuracy = success_count / (float)(success_count + fail_count);
	std::cout << "accuracy = " << accuracy << std::endl;

	return 0;
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_EPS_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_EPS_HPP_INCLUDED

/*!
  @ingroup group-constant
  @defgroup constant-Eps Eps (function template)

  Generates a constant equals to the [machine epsilon](https://en.wikipedia.org/wiki/Machine_epsilon).

  @headerref{<boost/simd/constant/eps.hpp>}

  @par Description

  1.  @code
      template<typename T> T Eps();
      @endcode

  2.  @code
      template<typename T> T Eps( boost::simd::as_<T> const& target );
      @endcode

  Generates a value of type @c T equals to the difference between 1 and the next representable
  value of type @c T.

  @par Parameters

  | Name                | Description                                                         |
  |--------------------:|:--------------------------------------------------------------------|
  | **target**          | a [placeholder](@ref type-as) value encapsulating the constant type |

  @par Return Value
  A value of type @c T that evaluates to:

  | Type       | `double`       | `float`       | Integer |
  |:-----------|:---------------|---------------|---------|
  | **Values** | \f$2^{-52}\f$  | \f$2^{-23}\f$ |    1    |

  @par Requirements
  - **T** models Value
**/

#include <boost/simd/constant/scalar/eps.hpp>
#include <boost/simd/constant/simd/eps.hpp>

#endif


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

int main(int argc, char **argv)
{
  // 100x100 Random Matrix
  Eigen::MatrixXd Rand = Eigen::MatrixXd::Random(100, 100);
  Eigen::MatrixXd A = Rand * Rand.transpose();
  Eigen::VectorXd b_ = Eigen::VectorXd::Random(100);
  Eigen::VectorXd x_ = Eigen::VectorXd::Zero(100);

  clock_t t_start = clock();

  // LU Decomposition
  x_ = A.partialPivLu().solve(b_);  // A must be Invertible
  std::cout << "time of partialPivLu() is " << 1000 * (clock() - t_start) / (double) CLOCKS_PER_SEC << "ms" << std::endl;
  // std::cout << "x = " << x_.transpose() << std::endl;
  t_start = clock();

  x_ = A.fullPivLu().solve(b_);
  std::cout << "time of fullPivLu() is " << 1000 * (clock() - t_start) / (double) CLOCKS_PER_SEC << "ms" << std::endl;
  // std::cout << "x = " << x_.transpose() << std::endl;
  t_start = clock();

  // QR Decomposition
  x_ = A.householderQr().solve(b_);
  std::cout << "time of householderQr() is " << 1000 * (clock() - t_start) / (double) CLOCKS_PER_SEC << "ms" << std::endl;
  // std::cout << "x = " << x_.transpose() << std::endl;
  t_start = clock();

  x_ = A.colPivHouseholderQr().solve(b_);
  std::cout << "time of colPivHouseholderQr() is " << 1000 * (clock() - t_start) / (double) CLOCKS_PER_SEC << "ms" << std::endl;
  // std::cout << "x = " << x_.transpose() << std::endl;
  t_start = clock();

  x_ = A.fullPivHouseholderQr().solve(b_);
  std::cout << "time of fullPivHouseholderQr() is " << 1000 * (clock() - t_start) / (double) CLOCKS_PER_SEC << "ms" << std::endl;
  // std::cout << "x = " << x_.transpose() << std::endl;
  t_start = clock();

  // Cholesky Decomposition
  x_ = A.llt().solve(b_); // A must be Positive Definite
  std::cout << "time of llt() is " << 1000 * (clock() - t_start) / (double) CLOCKS_PER_SEC << "ms" << std::endl;
  // std::cout << "x = " << x_.transpose() << std::endl;
  t_start = clock();

  x_ = A.ldlt().solve(b_);  // A must be Positive or Negative Semidefinite
  std::cout << "time of ldlt() is " << 1000 * (clock() - t_start) / (double) CLOCKS_PER_SEC << "ms" << std::endl;
  // std::cout << "x = " << x_.transpose() << std::endl;
  t_start = clock();

  return EXIT_SUCCESS;
}


#include <boost/test/unit_test.hpp>
#include "algorithms/math/greatest_common_divisor.hpp"

BOOST_AUTO_TEST_SUITE(TestGreatestCommonDivisor)

BOOST_AUTO_TEST_CASE(numbers)
{
    BOOST_CHECK(Algo::Math::GreatestCommonDivisor::Calc(1, 1) == 1);
    BOOST_CHECK(Algo::Math::GreatestCommonDivisor::Calc(5, 1) == 1);
    BOOST_CHECK(Algo::Math::GreatestCommonDivisor::Calc(5, 15) == 5);
    BOOST_CHECK(Algo::Math::GreatestCommonDivisor::Calc(18, 35) == 1);
    BOOST_CHECK(Algo::Math::GreatestCommonDivisor::Calc(28851538, 1183019) == 17657);
    BOOST_CHECK(Algo::Math::GreatestCommonDivisor::Calc(3918848, 1653264) == 61232);
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE AlgorithmTest
#include <boost/test/unit_test.hpp>
#include <poplibs_support/Algorithm.hpp>

using namespace poplibs_support;

BOOST_AUTO_TEST_CASE(BalancedPartitionZeroPartitions) {
  // The correct answer is 0 of either size of partition as
  // ceil(1/0) and floor(1/0) are both infinity.
  auto p = balancedPartition(1u, 0u);
  BOOST_CHECK_EQUAL(p.first, 0u);
  BOOST_CHECK_EQUAL(p.second, 0u);
}

BOOST_AUTO_TEST_CASE(BalancedPartitionZeroSize) {
  // Any answer is correct here so just check this works.
  BOOST_CHECK_NO_THROW(balancedPartition(0u, 1u));
}

BOOST_AUTO_TEST_CASE(BalancedPartitionNLessThanD) {
  auto p = balancedPartition(1u, 2u);
  // We expect n partitions of size 1 and d - n partitions of size 0.
  BOOST_CHECK_EQUAL(p.first, 1u);
  BOOST_CHECK_EQUAL(p.second, 1u);

  p = balancedPartition(3u, 5u);
  BOOST_CHECK_EQUAL(p.first, 3u);
  BOOST_CHECK_EQUAL(p.second, 2u);
}

BOOST_AUTO_TEST_CASE(BalancedPartitionDividesEqually) {
  auto p = balancedPartition(20u, 5u);
  // We expect the non-zero answer in the first return even
  // though mathematically the answer being in the second return would
  // be correct.
  BOOST_CHECK_EQUAL(p.first, 5u);
  BOOST_CHECK_EQUAL(p.second, 0u);
}

BOOST_AUTO_TEST_CASE(BalancedPartitionDividesUnequally) {
  constexpr unsigned n = 20u;

  // For all divisors which give ceil(n/d)=2 and floor(n/d)=1
  for (unsigned d = n / 2 + 1; d < n; ++d) {
    auto p = balancedPartition(n, d);
    BOOST_CHECK_EQUAL(p.first * ceildiv(n, d) + p.second * floordiv(n, d), n);
    BOOST_CHECK_EQUAL(p.first + p.second, d);
  }
}


//
// Dynamics of a single pendulum.
// State space is 2-d [\theta, \dot\theta]: angle, angular velocity
// If \theta=\pi is straight up, \theta=0 is straight down
//

# pragma once

#include <Eigen/Dense>

#include <experiments/simulators/simulator_utils.hh>

namespace simulators
{
namespace pendulum
{

template<int dim>
using Vector = simulators::Vector<dim>;

enum State
{
    THETA = 0,
    THETADOT,
    STATE_DIM
};

enum Control
{
    TORQUE = 0,
    CONTROL_DIM
};

// Useful for holding the parameters of the pendulum, including integration timestep.
class Pendulum
{
public:
    Pendulum(const double dt, const double damping_coeff, const double length);
    // Discrete time dynamics.
    Vector<STATE_DIM> operator()(const Vector<STATE_DIM>& x, const Vector<CONTROL_DIM>& u);
private:
    double dt_;
    double damping_coeff_;
    double length_;

};

// Returns a function that can return x_{t+1} = f(x_t, u_t) for a chosen dt.
// Underlying uses RK4 integration.
ilqr::DynamicsFunc make_discrete_dynamics_func(const double dt, const double length, const double damping_coeff);

// Defines pendulum dynamics as xdot = f(x,u).
//Eigen::VectorXd continuous_dynamics(const Eigen::VectorXd& state, const Eigen::VectorXd& control,
//                                    const double length, const double damping_coeff);

Vector<STATE_DIM> continuous_dynamics(const Vector<STATE_DIM>& x, const Vector<CONTROL_DIM>& u,
                                    const double length, const double damping_coeff);
} // namespace pendulum
} // namespace simulators


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// 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 SMOOTH__SO3_HPP_
#define SMOOTH__SO3_HPP_

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "internal/lie_group_base.hpp"
#include "internal/macro.hpp"
#include "internal/so3.hpp"
#include "lie_group.hpp"
#include "map.hpp"

namespace smooth {

// \cond
template<typename Scalar>
class SO2;
// \endcond

/**
 * @brief Base class for SO3 Lie group types
 *
 * Internally represented as a member of \f$ \mathbb{S}^3 \f$ (unit quaternions).
 *
 * Memory layout
 * -------------
 *
 * - Group:    \f$ \mathbf{x} = [q_x, q_y, q_z, q_w] \f$ (same as Eigen quaternion).
 * - Tangent:  \f$ \mathbf{a} = [\omega_x, \omega_y, \omega_z] \f$
 *
 * Constraints
 * -----------
 *
 * - Group:   \f$ q_x^2 + q_y^2 + q_z^2 + q_w^2 = 1, q_w >= 0 \f$
 * - Tangent: \f$ -\pi < \omega_x, \omega_y, \omega_z \leq \pi \f$
 *
 * Lie group matrix form
 * ---------------------
 *
 * 3x3 rotation matrix
 *
 * Lie algebra matrix form
 * -----------------------
 *
 * \f[
 * \mathbf{a}^\wedge =
 * \begin{bmatrix}
 *   0 & -\omega_z & \omega_y \\
 *  \omega_z &   0 & -\omega_x \\
 *  -\omega_y &   \omega_x & 0 \\
 * \end{bmatrix} \in \mathbb{R}^{3 \times 3}
 * \f]
 */
template<typename _Derived>
class SO3Base : public LieGroupBase<_Derived>
{
  using Base = LieGroupBase<_Derived>;

protected:
  SO3Base() = default;

public:
  SMOOTH_INHERIT_TYPEDEFS;

  /**
   * @brief Access quaterion.
   */
  Eigen::Map<Eigen::Quaternion<Scalar>> quat() requires is_mutable
  {
    return Eigen::Map<Eigen::Quaternion<Scalar>>(static_cast<_Derived &>(*this).data());
  }

  /**
   * @brief Access const quaterion.
   */
  Eigen::Map<const Eigen::Quaternion<Scalar>> quat() const
  {
    return Eigen::Map<const Eigen::Quaternion<Scalar>>(static_cast<const _Derived &>(*this).data());
  }

  /**
   * @brief Return euler angles.
   *
   * @param i1, i2, i3 euler angle axis convention (\f$0=x, 1=y, 2=z\f$).
   *        Default values correspond to ZYX rotation.
   *
   * Returned angles a1, a2, a3 are s.t. rotation is described by
   * \f$ Rot_{i_1}(a_1) * Rot_{i_2}(a_2) * Rot_{i_3}(a_3). \f$
   * where \f$ Rot_i(a) \f$ rotates an angle \f$a\f$ around the \f$i\f$:th axis.
   */
  Eigen::Vector3<Scalar>
  eulerAngles(Eigen::Index i1 = 2, Eigen::Index i2 = 1, Eigen::Index i3 = 0) const
  {
    return quat().toRotationMatrix().eulerAngles(i1, i2, i3);
  }

  /**
   * @brief Rotation action on 3D vector.
   */
  template<typename EigenDerived>
  Eigen::Vector3<Scalar> operator*(const Eigen::MatrixBase<EigenDerived> & v) const
  {
    return quat() * v;
  }

  /**
   * @brief Project to SO2.
   *
   * This keeps the yaw/z axis component of the rotation.
   *
   * @note SO2 header must be included.
   */
  SO2<Scalar> project_so2() const
  {
    using std::atan2;

    const auto q = quat();
    Scalar yaw   = atan2(
      Scalar(2) * (q.w() * q.z() + q.x() * q.y()),
      Scalar(1) - Scalar(2) * (q.y() * q.y() + q.z() * q.z()));
    return SO2<Scalar>(yaw);
  }
};

// STORAGE TYPE TRAITS

// \cond
template<typename _Scalar>
class SO3;
// \endcond

// \cond
template<typename _Scalar>
struct liebase_info<SO3<_Scalar>>
{
  static constexpr bool is_mutable = true;

  using Impl   = SO3Impl<_Scalar>;
  using Scalar = _Scalar;

  template<typename NewScalar>
  using PlainObject = SO3<NewScalar>;
};
// \endcond

/**
 * @brief Storage implementation of SO3 Lie group.
 *
 * @see SO3Base for group API.
 */
template<typename _Scalar>
class SO3 : public SO3Base<SO3<_Scalar>>
{
  using Base = SO3Base<SO3<_Scalar>>;

  SMOOTH_GROUP_API(SO3);

public:
  /**
   * @brief Construct from quaternion.
   *
   * @param quat Eigen quaternion.
   *
   * @note Input is normalized inside constructor.
   */
  template<typename Derived>
  SO3(const Eigen::QuaternionBase<Derived> & quat) : coeffs_(quat.normalized().coeffs())
  {
    if (coeffs_(3) < 0) { coeffs_ *= Scalar(-1); }
  }

  /**
   * @brief SO3 representing rotation around the x axis
   * @param angle angle of rotation [rad]
   */
  static SO3 rot_x(const Scalar & angle)
  {
    using std::cos, std::sin;

    SO3 ret;
    ret.coeffs() << sin(angle / 2), Scalar(0), Scalar(0), cos(angle / 2);
    if (ret.coeffs()(3) < 0) { ret.coeffs() *= Scalar(-1); }
    return ret;
  }

  /**
   * @brief SO3 representing rotation around the y axis
   * @param angle angle of rotation [rad]
   */
  static SO3 rot_y(const Scalar & angle)
  {
    using std::cos, std::sin;

    SO3 ret;
    ret.coeffs() << Scalar(0), sin(angle / 2), Scalar(0), cos(angle / 2);
    if (ret.coeffs()(3) < 0) { ret.coeffs() *= Scalar(-1); }
    return ret;
  }

  /**
   * @brief SO3 representing rotation around the z axis
   * @param angle angle of rotation [rad]
   */
  static SO3 rot_z(const Scalar & angle)
  {
    using std::cos, std::sin;

    SO3 ret;
    ret.coeffs() << Scalar(0), Scalar(0), sin(angle / 2), cos(angle / 2);
    if (ret.coeffs()(3) < 0) { ret.coeffs() *= Scalar(-1); }
    return ret;
  }
};

// \cond
template<typename _Scalar>
struct liebase_info<Map<SO3<_Scalar>>> : public liebase_info<SO3<_Scalar>>
{};
// \endcond

/**
 * @brief Memory mapping of SO3 Lie group.
 *
 * @see SO3Base for group API.
 */
template<typename _Scalar>
class Map<SO3<_Scalar>> : public SO3Base<Map<SO3<_Scalar>>>
{
  using Base = SO3Base<Map<SO3<_Scalar>>>;

  SMOOTH_MAP_API();
};

// \cond
template<typename _Scalar>
struct liebase_info<Map<const SO3<_Scalar>>> : public liebase_info<SO3<_Scalar>>
{
  static constexpr bool is_mutable = false;
};
// \endcond

/**
 * @brief Const memory mapping of SO3 Lie group.
 *
 * @see SO3Base for group API.
 */
template<typename _Scalar>
class Map<const SO3<_Scalar>> : public SO3Base<Map<const SO3<_Scalar>>>
{
  using Base = SO3Base<Map<const SO3<_Scalar>>>;

  SMOOTH_CONST_MAP_API();
};

using SO3f = SO3<float>;   ///< SO3 with float scalar representation
using SO3d = SO3<double>;  ///< SO3 with double scalar representation

}  // namespace smooth

#endif  // SMOOTH__SO3_HPP_


#pragma once
#include <Eigen/Dense>

namespace mdk {
    using VRef = Eigen::Vector3d const&;

    inline double angle(VRef v1, VRef v2, VRef v3) {
        auto u1 = v2 - v1, u2 = v2 - v3;
        return acos(u1.dot(u2) / (u1.norm() * u2.norm()));
    }

    inline double dihedral(VRef v1, VRef v2, VRef v3, VRef v4) {
        auto u1 = v2 - v1, u2 = v3 - v2, u3 = v4 - v3;
        auto d1 = u1.cross(u2), d2 = u2.cross(u3);

        if (d1.isZero() || d2.isZero()) return 0.0;
        double phi = acos(d1.dot(d2) / (d1.norm() * d2.norm()));
        if (d1.dot(u3) < 0.) phi = -phi;
        return phi;
    }

    double asphericity(Vectors const& v) {
        Vector mean = v.vectorwise().mean();

        Eigen::Matrix3d mit;
        double crg;
        for (size_t i = 0; i < v.size(); ++i) {
            Vector diff = v.col(i) - mean;
            crg += diff.squaredNorm();

            Vector sq_diff = diff.array() * diff.array();
            mit(0,0) += sq_diff(1) + sq_diff(2);
            mit(1,1) += sq_diff(2) + sq_diff(0);
            mit(2,2) += sq_diff(0) + sq_diff(1);
            mit(0,1) -= diff(0) * diff(1);
            mit(1,2) -= diff(1) * diff(2);
            mit(0,2) -= diff(0) * diff(2);
        }
        mit(1,0) = mit(0,1);
        mit(2,1) = mit(1,2);
        mit(2,0) = mit(0,2);
        crg = sqrt(crg / v.size());

        auto eigenvaluesC = mit.eigenvalues();
        double eigenvalues[3] = {
            sqrt(eigenvaluesC(0).real() / v.size()),
            sqrt(eigenvaluesC(1).real() / v.size()),
            sqrt(eigenvaluesC(2).real() / v.size())
        };

        std::sort(eigenvalues, eigenvalues + 3);
        return eigenvalues[1] - 0.5 * (eigenvalues[0] + eigenvalues[2]);
    }
}


/*
Copyright 2012-2018 Tamas Bolner

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

  http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

#include <cinttypes>
#include <Eigen/Dense>
#include "SimplexSolver.h"
#include "exception.h"

using namespace Eigen;

/**
 * Constructor
 *
 * @param int mode This can be one of these: SIMPLEX_MINIMIZE, SIMPLEX_MAXIMIZE
 * @param const VectorXd &objectiveFunction The coefficients of the objective function.
 * @param const MatrixXd &constraints Full matrix for the constraints. Contains also the righthand-side values.
 * @returns SimplexSolver
 */
SimplexSolver::SimplexSolver(int mode, const VectorXd &objectiveFunction, const MatrixXd &constraints) {
    int64_t constantColumn, temp;
    this->foundSolution = false;
    this->optimum = 0;
    this->numberOfVariables = objectiveFunction.rows();

    /*
        Validate input parameters
    */
    if (mode != SIMPLEX_MINIMIZE && mode != SIMPLEX_MAXIMIZE) {
        throw FException("SimplexSolver: invalid value for the 'mode' parameter.");
    }

    if (objectiveFunction.rows() < 1) {
        throw FException("SimplexSolver: The coefficient vector of the objective function must contain at least one row.");
    }

    if (constraints.rows() < 1) {
        throw FException("SimplexSolver: The constraint matrix must contain at least one row.");
    }

    if (constraints.cols() != objectiveFunction.rows() + 1) {
        throw FException("SimplexSolver: The constraint matrix has %d columns, but should have %d, because the coefficient vector of the objective function has %d rows.",
            constraints.cols(), objectiveFunction.rows() + 1, objectiveFunction.rows());
    }

    for (int i = 0; i < this->numberOfVariables; i++) {
        if (objectiveFunction(i) == 0) {
            throw FException("SimplexSolver: One of the coefficients of the objective function is zero.");
        }
    }

    temp = constraints.cols() - 1;
    for (int i = 0; i < constraints.rows(); i++) {
        if (constraints(i, temp) < 0) {
            throw FException("SimplexSolver: All righthand-side coefficients of the constraint matrix must be non-negative.");
        }
    }

    /*
        Build tableau
    */
    if (mode == SIMPLEX_MAXIMIZE) {
        // Maximize
        this->tableau.resize(constraints.rows() + 1, this->numberOfVariables + constraints.rows() + 1);

        this->tableau <<    -objectiveFunction.transpose(),                   MatrixXd::Zero(1, constraints.rows() + 1),
                            constraints.leftCols(this->numberOfVariables),    MatrixXd::Identity(constraints.rows(), constraints.rows()), constraints.rightCols(1);
    } else {
        // Minimize: construct the Dual problem
        this->tableau.resize(this->numberOfVariables + 1, this->numberOfVariables + constraints.rows() + 1);

        this->tableau <<    -constraints.rightCols(1).transpose(),                        MatrixXd::Zero(1, this->numberOfVariables + 1),
                            constraints.leftCols(this->numberOfVariables).transpose(),    MatrixXd::Identity(this->numberOfVariables, this->numberOfVariables), objectiveFunction;
    }

    /*
        Simplex algorithm
    */
    if (mode == SIMPLEX_MAXIMIZE) {
        // Maximize original problem
        if (!this->simplexAlgorithm(this->numberOfVariables)) {
            return;    // No solution
        }
    } else {
        // Maximize the dual of the minimization problem
        if (!this->simplexAlgorithm(constraints.rows())) {
            return;    // No solution
        }
    }

    /*
        Fetch solution
    */
    constantColumn = this->tableau.cols() - 1;
    this->solution.resize(this->numberOfVariables);

    if (mode == SIMPLEX_MAXIMIZE) {
        // Maximize
        for (int i = 0; i < this->numberOfVariables; i++) {
            temp = this->getPivotRow(i);

            if (temp > 0) {
                // Basic variable
                this->solution(i) = this->tableau(temp, constantColumn);
            } else {
                // Non-basic variable
                this->solution(i) = 0;
            }
        }
        this->foundSolution = true;
        this->optimum = this->tableau(0, constantColumn);
    } else {
        // Minimize
        for (int i = 0; i < this->numberOfVariables; i++) {
            this->solution(i) = this->tableau(0, constraints.rows() + i);
        }
        this->foundSolution = true;
        this->optimum = this->tableau(0, constantColumn);
    }
}

/**
 * Returns true if a solution has been found.
 * Return false otherwise.
 *
 * @returns boolean
 */
bool SimplexSolver::hasSolution() {
    return this->foundSolution;
}

/**
 * Returns the maximum/minimum value of
 * the objective function.
 *
 * @returns double
 */
double SimplexSolver::getOptimum() {
    return this->optimum;
}

/**
 * Returns a vector with the variable
 * values for the solution.
 *
 * return VectorXd
 */
VectorXd SimplexSolver::getSolution() {
    return this->solution;
}

/**
 * Searches for the pivot row in the given column, by calculating the ratios.
 * Tries to find smallest non-negative ratio.
 * Returns -1 if all possible pivots are 0 or if the ratios are negative.
 * Deals with cases like this:  0/negative < 0/positive
 *
 * @param int64_t column
 * @returns int64_t Returns the number of the pivot row, or -1 if found none.
 */
int64_t SimplexSolver::findPivot_min(int64_t column) {
    int64_t minIndex = -1;
    int64_t constantColumn = this->tableau.cols() - 1;
    double minRatio = 0;
    double minConstant = 0;    // For the "0/negative < 0/positive" difference the constants have to be tracked also.
    double ratio;
    int64_t rowNum = this->tableau.rows();

    for (int i = 1; i < rowNum; i++) {
        if (this->tableau(i, column) == 0) {
            continue;
        }

        ratio = this->tableau(i, constantColumn) / this->tableau(i, column);
        if (ratio < 0) {
            //The ratio must be non-negative
            continue;
        }

        if (minIndex == -1) {
            // First pivot candidate
            minIndex = i;
            minRatio = ratio;
            minConstant = this->tableau(i, constantColumn);
        } else {
            if (ratio == 0 && ratio == minRatio) {
                // 0/negative < 0/positive
                if (this->tableau(i, constantColumn) < minConstant) {
                    minIndex = i;
                    minRatio = ratio;
                    minConstant = this->tableau(i, constantColumn);
                }
            } else if (ratio < minRatio) {
                minIndex = i;
                minRatio = ratio;
                minConstant = this->tableau(i, constantColumn);
            }
        }
    }

    return minIndex;
}

/**
 * Iterates through the this->tableau matrix to solve the problem.
 *
 * @param int64_t variableNum The number of variables (dimensions). (different for the minimization problem)
 * @returns bool Returns true if a solution has been found. Returns false otherwise.
 */
bool SimplexSolver::simplexAlgorithm(int64_t variableNum) {
    MatrixXd::Index pivotColumn;
    int64_t pivotRow;

    while (true) {
        /*
            Find pivot column, check for halt condition
        */
        this->tableau.row(0).leftCols(variableNum).minCoeff(&pivotColumn);
        if (this->tableau(0, pivotColumn) >= 0) {
            //Found no negative coefficient
            break;
        }

        /*
            Find pivot row
        */
        pivotRow = this->findPivot_min(pivotColumn);
        if (pivotRow == -1) {
            //no solution
            return false;
        }

        /*
            Do pivot operation
        */
        this->tableau.row(pivotRow) /= this->tableau(pivotRow, pivotColumn);
        this->tableau(pivotRow, pivotColumn) = 1;    // For possible precision issues
        for (int i = 0; i < this->tableau.rows(); i++) {
            if (i == pivotRow) continue;

            this->tableau.row(i) -= this->tableau.row(pivotRow) * this->tableau(i, pivotColumn);
            this->tableau(i, pivotColumn) = 0;    // For possible precision issues
        }
    }

    return true;
}

/**
 * If the given column has only one coefficient with value 1 (except in topmost row), and all other
 * coefficients are zero, then returns the row of the non-zero value.
 * Otherwise return -1.
 * This method is used in the final step of maximization, when we read
 * the solution from the tableau.
 *
 * @param int64_t column
 * @returns int64_t
 */
int64_t SimplexSolver::getPivotRow(int64_t column) {
    int64_t one_row = -1;

    for (int i = 1; i < this->tableau.rows(); i++) {
        if (this->tableau(i, column) == 1) {
            if (one_row >= 0) {
                return -1;
            } else {
                one_row = i;
                continue;
            }
        } else if (this->tableau(i, column) != 0) {
            return -1;
        }
    }

    return one_row;
}


#include <iostream>
#include <vector>
#include <Eigen/Eigen>
#include <cmath>

// #include <Python.h>
// #include "matplotlibcpp.h"

// namespace plt = matplotlibcpp;
using namespace std;
using namespace Eigen;

void ucitajTacke(vector<pair<float, float>> &tacke, int duzina) {
	pair<float, float> tacka;
	for (int i = 0; i < duzina; i++) {
		cin >> tacka.first >> tacka.second;
		tacke.push_back(tacka);
	}
}

MatrixXf kreirajMatricuIx3(vector<pair<float, float>> &tacke) {
	MatrixXf Mat(tacke.size(), 3);

	int i = 0;
	while (i < tacke.size()) {
		Mat(i, 0) = tacke[i].first;
		Mat(i, 1) = tacke[i].second;
		Mat(i, 2) = 1;
		i++;
	}
	/*
		| t[0][p1] t[0][p2] 1 |
		| t[1][p1] t[1][p2] 1 |
		| t[2][p1] t[3][p2] 1 |
		| t[4][p1] t[4][p2] 1 |
		| ........			  |
	*/
	return Mat;
}

// Svaka tacka i njena slika iz vektora pravi ovu matricu 2 x 9
// Za broj tacaka n, pravi matricu 2n x 9
MatrixXf kreirajMatricu2nx9(vector<pair<float, float>> &pocetne, vector<pair<float, float>> &projektovane) {

	MatrixXf matrix2nx9(2 * pocetne.size(), 9);

	auto i = pocetne.begin();
	auto j = projektovane.begin();
	int k = 0;

	while (i < pocetne.end()) {
		matrix2nx9(k, 0) = 0;
		matrix2nx9(k, 1) = 0;
		matrix2nx9(k, 2) = 0;
		matrix2nx9(k, 3) = -i->first;
		matrix2nx9(k, 4) = -i->second;
		matrix2nx9(k, 5) = -1;
		matrix2nx9(k, 6) = j->second * i->first;
		matrix2nx9(k, 7) = j->second * i->second;
		matrix2nx9(k, 8) = j->second;

		matrix2nx9(k + 1, 0) = i->first;
		matrix2nx9(k + 1, 1) = i->second;
		matrix2nx9(k + 1, 2) = 1;
		matrix2nx9(k + 1, 3) = 0;
		matrix2nx9(k + 1, 4) = 0;
		matrix2nx9(k + 1, 5) = 0;
		matrix2nx9(k + 1, 6) = -(j->first * i->first);
		matrix2nx9(k + 1, 7) = -(j->first * i->second);
		matrix2nx9(k + 1, 8) = -(j->first);

		i++;
		j++;
		k += 2;
		/// |  0	 0    0  -b[0] -b[1] -1   (p[1]*b[0])   (p[1]*b[1])   p[1]	|		b - par pocetnih koordinata tacaka
		///	| p[0]  p[1]  1   0     0     0  -(p[0]*b[0])  -(p[0]*b[1])  -p[0]	|		p - par projektovanih koordinata tacaka
		///	|	.	.	.	.	.	.	.	.	.	.	.	.	.	.		.	|
		/// |	.	.	.	.	.	.	.	.	.	.	.	.	.	.		.	|
		/// |	.	.	.	.	.	.	.	.	.	.	.	.	.	.		.	|
	}

	return matrix2nx9;
}

MatrixXf resiP(MatrixXf Mat, Vector3f vec, int k) {
	Vector3f stepen = ((Mat.transpose()).inverse()) * vec;

	int m = stepen.size();
	int n = k;
	MatrixXf P(n, m);
	for (int i = 0; i < n; i++) {
		for (int j = 0; j < m; j++) {
			P(i, j) = stepen(i) * Mat(i, j);
		}
	}
	return P.transpose();
}

MatrixXf naivniAlgoritam(vector<pair<float, float>> pocetne, vector<pair<float, float>> projektovane) {

	float x, y;

	x = pocetne.back().first;
	y = pocetne.back().second;
	Vector3f v(x, y, 1);

	x = projektovane.back().first;
	y = projektovane.back().second;
	Vector3f vp(x, y, 1);

	pocetne.pop_back();
	projektovane.pop_back();

	MatrixXf M = kreirajMatricuIx3(pocetne);
	MatrixXf Mp = kreirajMatricuIx3(projektovane);

	MatrixXf P1 = resiP(M, v, pocetne.size());
	MatrixXf P2 = resiP(Mp, vp, projektovane.size());

	return P2 * P1.inverse();
}

MatrixXf DLTAlgoritam(vector<pair<float, float>> pocetne, vector<pair<float, float>> projektovane) {

	int br_tacaka = pocetne.size();

	MatrixXf matrix2nx9 = kreirajMatricu2nx9(pocetne, projektovane);
	// SVD razbija matricu na proizvode 3 matrice
	JacobiSVD<MatrixXf> Svd(matrix2nx9, ComputeFullV);

	MatrixXf Mat(2 * br_tacaka, 9);
	MatrixXf matrica_poslednje_kolone(3, 3);
	Mat = Svd.matrixV();	// Uzimamo 3. matricu V iz A = USV*

	int br_kolona = Mat.cols();
	// Uzimamo poslednju kolonu iz dekompozicije
	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			matrica_poslednje_kolone(i, j) = Mat(i * 3 + j, br_kolona - 1);
		}
	}

	return matrica_poslednje_kolone;
}

float izrSrednjeRastojanje(vector<pair<float, float>> tacke, pair<float, float> tezite) {

	float prosecno_rastojanje = 0;
	int n = tacke.size();
	for (auto i = tacke.begin(); i < tacke.end(); i++) {
		float medju_rastojanje = sqrt(pow(i->first - tezite.first, 2) + pow(i->second - tezite.second, 2));	// sqrt((x2-x1)^2 + (y2-y1)^2)
		prosecno_rastojanje += medju_rastojanje;
	}

	return prosecno_rastojanje / n;
}

Vector3f izrCentroidu(vector<pair<float, float>> tacke) {

	float tx = 0;
	float ty = 0;
	int n = tacke.size();

	// Sabiramo tezine koordinata
	for (auto i = tacke.begin(); i < tacke.end(); i++) {
		tx += i->first;
		ty += i->second;
	}

	// centroida = (tx/n, ty/n, 1)
	// homogene
	Vector3f centroida;
	centroida << tx / n, ty / n, 1;

	/*
	// Test: teziste
	cout << "Teziste je : " << centroida;
	cout << endl;
	*/

	return centroida;
}

MatrixXf izrMatricuNormalizacije(vector<pair<float, float>> tacke) {
	/*
	1. Izracunati teziste sistema tacaka
	2. Translirati teziste u koordinatni pocetak (matrica T)
	3. Skalirati tacke tako da prosecna udaljenost tacke od koordinatnog pocetka bude sqrt(2) (matrica homotetije S)
	4. Matrica normalizacije je jednaka S * T
	*/
	Vector3f vteziste = izrCentroidu(tacke);
	pair<float, float> t(vteziste(0), vteziste(1));

	// Matrica T: Translira teziste ovog skupa tacaka u O(0,0)
	MatrixXf T(3, 3);
	T << 1, 0, -vteziste(0),
		 0, 1, -vteziste(1),
		 0, 0,	    1;
	// cout << T << endl;
	float srednje_rastojanje = izrSrednjeRastojanje(tacke, t);
	float k_skaliranja = sqrt(2) / srednje_rastojanje;
	// Matrica S: Skalira, tako da prosecno rastojanje tacaka od koordinatnog pocetka bude jednak sqrt(2)
	MatrixXf S(3, 3);
	S << k_skaliranja,		0,			0,
			0,			k_skaliranja,	0,
			0,				0,			1;
	// cout << S << endl;
	// Prvo transliramo pa skaliramo
	return S * T;
}

MatrixXf nDLTAlgoritam(vector<pair<float, float>> pocetne, vector<pair<float, float>> projektovane) {
	/*
	1. Normalizuj tacke i slike:	nx[i] = T * x[i], npx[i] = Tp * px[i]
	2. Uradi DLT na:				nx[i] <-> npx[i]
	3. Denormalizuj resenje:		H = Tp_inv * nH * T
	*/

	MatrixXf T = izrMatricuNormalizacije(pocetne);
	MatrixXf s_tacka = (T * kreirajMatricuIx3(pocetne).transpose()).transpose();	// 3x3 * nx3.transp
	//	cout << T << endl << endl << s_tacka << endl << endl;

	MatrixXf Tp = izrMatricuNormalizacije(projektovane);
	MatrixXf p_tacka = (Tp * kreirajMatricuIx3(projektovane).transpose()).transpose();	// 3x3 * nx3.transp
	//	cout << Tp << endl << endl << p_tacka << endl << endl;

	int n = pocetne.size();
	vector<pair<float, float>> s;
	for (int i = 0; i < n; i++) {
		s.push_back(pair<float, float>(s_tacka(i, 0), s_tacka(i, 1)));
		//	cout << s_tacka(i, 0) << " " << s_tacka(i, 1) << endl;
	}

	vector<pair<float, float>> p;
	for (int i = 0; i < n; i++) {
		p.push_back(pair<float, float>(p_tacka(i, 0), p_tacka(i, 1)));
		//	cout << p_tacka(i, 0) << " " << p_tacka(i, 1) << endl;
	}

	/*
	////	---- Plot ----
	for (int i = 0; i < n; i++) {
		plt::plot(s_tacka(i,0), s_tacka(i, 1), 'go');
		plt::plot(p_tacka(i, 0), p_tacka(i, 1), 'g+');
	}
	*/

	MatrixXf H = DLTAlgoritam(s, p);
	return Tp.inverse() * H * T;
}

int main() {
	int opcija;
	cout << "Izaberite opciju: " << endl << "1. Rucni unos koordinata" << endl << "2. Izabir koordinata pomocu misa" << endl << ":::  ";
	cin >> opcija;
	if (opcija == 1) {				// Rucni unos koordinata
		// Unos tacaka
		int br_tacaka;
		cout << "Unesite broj korespodencija: ";
		cin >> br_tacaka;
		if (br_tacaka < 0) {
			cout << "greska: Neispravan unos! (ocekivan broj veci od 0)" << endl;
			return -1;

		}

		vector<pair<float, float>> pocetne_tacke;
		vector<pair<float, float>> projektovane_tacke;

		cout << "Ucitajte ulazne tacke kao parove x, y: " << endl;
		ucitajTacke(pocetne_tacke, br_tacaka);
		cout << "Ucitajte izlazne tacke kao parove x, y: " << endl;
		ucitajTacke(projektovane_tacke, br_tacaka);
		cout << endl;
		if (br_tacaka == 4) {
			cout << "--- Naivni algoritam: ---" << endl;
			cout << naivniAlgoritam(pocetne_tacke, projektovane_tacke) << endl;

			/*
			////	---- Plot ----
			for (int i = 0; i < br_tacaka; i++) {
				plt::plot(pocetne_tacke[i].first, pocetne_tacke[i].second, 'ro');
				plt::plot(projektovane_tacke[i].first, projektovane_tacke[i].second, 'r+');
			}
			*/

		}

		cout << "--- DLT algoritam: ---" << endl;
		cout << DLTAlgoritam(pocetne_tacke, projektovane_tacke) << endl;

		cout << "---Normalizovani DLT algoritam: ---" << endl;
		cout << nDLTAlgoritam(pocetne_tacke, projektovane_tacke);

		//	ERROR: Error	C1083	Cannot open include file : 'Python.h' : No such file or directory
		//	VS ne cita Python header iako je ubacen u Additional Include Libraries, kao i postaveljen kao header u src dir

		/*
		////	---- Plot ----
		plt::figure_size(1200, 780);
		plt::show();
		*/

	}
	else if (opcija == 2) {			// Izabir koordinata pomocu misa

		cout << "Nije zavrseno!";
		return 0;

	}
	else {							// Greska
		cout << "greska: Izabrana je nepostojeca opcija!";
		return -1;
	}

	return 0;
}

02. Calculating projections and Removing distortion/removing_distortion.cpp


//  Copyright John Maddock 2007.
//  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)

// Note that this file contains quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

//[policy_eg_10

/*`

To understand how the rounding policies for
the discrete distributions can be used, we'll
use the 50-sample binomial distribution with a
success fraction of 0.5 once again, and calculate
all the possible quantiles at 0.05 and 0.95.

Begin by including the needed headers:

*/

#include <iostream>
#include <boost/math/distributions/binomial.hpp>

/*`

Next we'll bring the needed declarations into scope, and
define distribution types for all the available rounding policies:

*/

using namespace boost::math::policies;
using namespace boost::math;

typedef binomial_distribution<
            double,
            policy<discrete_quantile<integer_round_outwards> > >
        binom_round_outwards;

typedef binomial_distribution<
            double,
            policy<discrete_quantile<integer_round_inwards> > >
        binom_round_inwards;

typedef binomial_distribution<
            double,
            policy<discrete_quantile<integer_round_down> > >
        binom_round_down;

typedef binomial_distribution<
            double,
            policy<discrete_quantile<integer_round_up> > >
        binom_round_up;

typedef binomial_distribution<
            double,
            policy<discrete_quantile<integer_round_nearest> > >
        binom_round_nearest;

typedef binomial_distribution<
            double,
            policy<discrete_quantile<real> > >
        binom_real_quantile;

/*`

Now let's set to work calling those quantiles:

*/

int main()
{
   std::cout <<
      "Testing rounding policies for a 50 sample binomial distribution,\n"
      "with a success fraction of 0.5.\n\n"
      "Lower quantiles are calculated at p = 0.05\n\n"
      "Upper quantiles at p = 0.95.\n\n";

   std::cout << std::setw(25) << std::right
      << "Policy"<< std::setw(18) << std::right
      << "Lower Quantile" << std::setw(18) << std::right
      << "Upper Quantile" << std::endl;

   // Test integer_round_outwards:
   std::cout << std::setw(25) << std::right
      << "integer_round_outwards"
      << std::setw(18) << std::right
      << quantile(binom_round_outwards(50, 0.5), 0.05)
      << std::setw(18) << std::right
      << quantile(binom_round_outwards(50, 0.5), 0.95)
      << std::endl;

   // Test integer_round_inwards:
   std::cout << std::setw(25) << std::right
      << "integer_round_inwards"
      << std::setw(18) << std::right
      << quantile(binom_round_inwards(50, 0.5), 0.05)
      << std::setw(18) << std::right
      << quantile(binom_round_inwards(50, 0.5), 0.95)
      << std::endl;

   // Test integer_round_down:
   std::cout << std::setw(25) << std::right
      << "integer_round_down"
      << std::setw(18) << std::right
      << quantile(binom_round_down(50, 0.5), 0.05)
      << std::setw(18) << std::right
      << quantile(binom_round_down(50, 0.5), 0.95)
      << std::endl;

   // Test integer_round_up:
   std::cout << std::setw(25) << std::right
      << "integer_round_up"
      << std::setw(18) << std::right
      << quantile(binom_round_up(50, 0.5), 0.05)
      << std::setw(18) << std::right
      << quantile(binom_round_up(50, 0.5), 0.95)
      << std::endl;

   // Test integer_round_nearest:
   std::cout << std::setw(25) << std::right
      << "integer_round_nearest"
      << std::setw(18) << std::right
      << quantile(binom_round_nearest(50, 0.5), 0.05)
      << std::setw(18) << std::right
      << quantile(binom_round_nearest(50, 0.5), 0.95)
      << std::endl;

   // Test real:
   std::cout << std::setw(25) << std::right
      << "real"
      << std::setw(18) << std::right
      << quantile(binom_real_quantile(50, 0.5), 0.05)
      << std::setw(18) << std::right
      << quantile(binom_real_quantile(50, 0.5), 0.95)
      << std::endl;
}

/*`

Which produces the program output:

[pre
Testing rounding policies for a 50 sample binomial distribution,
with a success fraction of 0.5.

Lower quantiles are calculated at p = 0.05

Upper quantiles at p = 0.95.

Testing rounding policies for a 50 sample binomial distribution,
with a success fraction of 0.5.

Lower quantiles are calculated at p = 0.05

Upper quantiles at p = 0.95.

                   Policy    Lower Quantile    Upper Quantile
   integer_round_outwards                18                31
    integer_round_inwards                19                30
       integer_round_down                18                30
         integer_round_up                19                31
    integer_round_nearest                19                30
                     real            18.701            30.299
]

*/

//] ends quickbook import


#include <iostream>
#include <Eigen\Dense>

using namespace Eigen;
using namespace std;

void main() {
	{
		MatrixXf M1(3, 3); // column-major storage
		M1 << 1, 2, 3, 4, 5, 6, 7, 8, 9;
		cout << "M1: \n" << M1 << endl;
		Map<RowVectorXf> v1(M1.data(), M1.size());
		cout << "v1: \n" << v1 << endl;

		Matrix<float, Dynamic, Dynamic, RowMajor> M2(M1);
		cout << "M2: \n" << M2 << endl;
		Map<RowVectorXf> v2(M2.data(), M2.size());
		cout << "v2: \n" << v2 << endl;
	}
	{
		MatrixXf M1(2, 6);
		M1 << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12;
		cout << "M1: \n" << M1 << endl;
		Map<VectorXf> v1(M1.data(), M1.size());
		cout << "v1: \n" << v1 << endl;
		Map<MatrixXf> M2(M1.data(), 6, 2);
		Map<VectorXf> v2(M2.data(), M2.size());
		cout << "M2: \n" << M2 << endl;
		cout << "v2: \n" << v2 << endl;
	}
	{
		// Slicing
		RowVectorXf v = RowVectorXf::LinSpaced(20, 0, 19);
		cout << "Input: \n" << v << endl;
		Map<RowVectorXf, 0, InnerStride<2>> v2(v.data(), v.size() / 2);
		cout << "Even: \n" << v2 << endl;
	}
	{
		MatrixXf M1 = MatrixXf::Random(3, 8);
		cout << "Column major input: \n" << M1 << endl;
		Map<MatrixXf, 0, OuterStride<>> M2(M1.data(), M1.rows(),
										  (M1.cols() + 2) / 3,
										   OuterStride<>(M1.outerStride() * 3));
		cout << "1 column over 3: \n" << M2 << endl;

		typedef Matrix<float, Dynamic, Dynamic, RowMajor> RowMajorMatrixXf;
		RowMajorMatrixXf M3(M1);
		cout << "Row major input:" << endl << M3 << "\n";
		Map<RowMajorMatrixXf, 0, Stride<Dynamic, 3> > M4(M3.data(), M3.rows(), (M3.cols() + 2) / 3,
			Stride<Dynamic, 3>(M3.outerStride(), 3));
		cout << "1 column over 3:" << endl << M4 << "\n";
	}
	system("pause");
}


#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "matrix_transpose.h"

#include <opencv2/opencv.hpp>
#include "common/pixel_benchmark.h"
#include <Eigen/Dense>

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

static void matrix_transpose_u8_opencv(unsigned char* src, uint32_t height, uint32_t width, unsigned char* dst)
{
    cv::Mat src_mat(height, width, CV_8UC1, src);
    cv::Mat dst_mat(width, height, CV_8UC1, dst);
    dst_mat = src_mat.t();
}

static void matrix_transpose_u8_eigen(unsigned char* src, uint32_t height, uint32_t width, unsigned char* dst)
{
    Eigen::Map<Eigen::Matrix<unsigned char, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> eSrc(src, height, width);
    Eigen::Map<Eigen::Matrix<unsigned char, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> eDst(dst, width, height);
    eDst = eSrc.transpose();

    // cout << "eSrc:" << endl << eSrc << endl;
    // cout << "eDst:" << endl << eDst << endl;
}

static void matrix_transpose_f32_opencv(float* src, uint32_t height, uint32_t width, float* dst)
{
    cv::Mat src_mat(height, width, CV_32FC1, src);
    cv::Mat dst_mat(width, height, CV_32FC1, dst);
    dst_mat = src_mat.t();
}

static void matrix_transpose_f32_eigen(float* src, uint32_t height, uint32_t width, float* dst)
{
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> eSrc(src, height, width);
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> eDst(dst, width, height);
    eDst = eSrc.transpose();

    // cout << "eSrc:" << endl << eSrc << endl;
    // cout << "eDst:" << endl << eDst << endl;
}

static void prepare_big(uint32_t& height, uint32_t& width, cv::Mat& gray)
{
    cv::Mat image = cv::imread("river_bank2.png");

    // 为了检查边界是否处理正确，resize到不是4的倍数的尺寸
    // cv::Size dsize;
    // dsize.height = image.rows - 1;
    // dsize.width = image.cols - 1;
    // cv::resize(image, image, dsize);

    // cv::Size dsize;
    // dsize.height = 1024;
    // dsize.width = 1024;
    // cv::resize(image, image, dsize);

    cv::Size size = image.size();
    printf("image info: height=%d, width=%d\n", size.height, size.width);

    gray = cv::Mat(size, CV_8UC1);
    cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY);
    height = size.height;
    width = size.width;
}

static void prepare_small(uint32_t& height, uint32_t& width, cv::Mat& gray)
{
    height = 3;
    width = 5;
    gray = cv::Mat(height, width, CV_8UC1);

    unsigned char* data = gray.data;
    for (uint32_t i=0; i<height*width; i++) {
        data[i] = i%256;
    }
}

static void matrix_transpose_f32_test()
{
    uint32_t height;
    uint32_t width;
    cv::Mat gray;
    prepare_big(height, width, gray);
    //prepare_small(height, width, gray);

    // u8 to float
    gray.convertTo(gray, CV_32FC1);

    float* src = (float*)gray.data;
    double t_start, t_cost;
    uint32_t len = height * width;
    float* dst_naive = (float*)malloc(sizeof(float)*len);
    float* dst_order_opt = (float*)malloc(sizeof(float)*len);
    float* dst_opencv = (float*)malloc(sizeof(float)*len);
    float* dst_eigen = (float*)malloc(sizeof(float)*len);
    float* dst_partition4x4 = (float*)malloc(sizeof(float)*len);
    float* dst_partition4x4_asimd = (float*)malloc(sizeof(float)*len);
    float* dst_partition8x8 = (float*)malloc(sizeof(float)*len);
    float* dst_partition8x8_asimd = (float*)malloc(sizeof(float)*len);

    // naive
    t_start = pixel_get_current_time();
    matrix_transpose_f32_naive(src, height, width, dst_naive);
    t_cost = pixel_get_current_time() - t_start;
    printf("matrix transpose f32, naive,         time cost %.2lf ms\n", t_cost);

    // order_opt
    t_start = pixel_get_current_time();
    matrix_transpose_f32_order_opt(src, height, width, dst_order_opt);
    t_cost = pixel_get_current_time() - t_start;
    printf("matrix transpose f32, order_opt,     time cost %.2lf ms\n", t_cost);

    // opencv
    t_start = pixel_get_current_time();
    matrix_transpose_f32_opencv(src, height, width, dst_opencv);
    t_cost = pixel_get_current_time() - t_start;
    printf("matrix transpose f32, opencv,        time cost %.2lf ms\n", t_cost);

    // eigen
    t_start = pixel_get_current_time();
    matrix_transpose_f32_eigen(src, height, width, dst_eigen);
    t_cost = pixel_get_current_time() - t_start;
    printf("matrix transpose f32, eigen,         time cost %.2lf ms\n", t_cost);

    // partition, 4x4
    t_start = pixel_get_current_time();
    matrix_transpose_f32_partition(src, height, width, dst_partition4x4, 4);
    t_cost = pixel_get_current_time() - t_start;
    printf("matrix transpose f32, partition4x4,  time cost %.2lf ms\n", t_cost);

    // partition, 4x4, asimd
    t_start = pixel_get_current_time();
    matrix_transpose_f32_partition_asimd(src, height, width, dst_partition4x4_asimd, 4);
    t_cost = pixel_get_current_time() - t_start;
    printf("matrix transpose f32, partition4x4 asimd, time cost %.2lf ms\n", t_cost);

    // partition, 8x8
    t_start = pixel_get_current_time();
    matrix_transpose_f32_partition(src, height, width, dst_partition8x8, 8);
    t_cost = pixel_get_current_time() - t_start;
    printf("matrix transpose f32, partition8x8,  time cost %.2lf ms\n", t_cost);

    // partition, 8x8, asimd
    t_start = pixel_get_current_time();
    matrix_transpose_f32_partition_asimd(src, height, width, dst_partition8x8_asimd, 8);
    t_cost = pixel_get_current_time() - t_start;
    printf("matrix transpose f32, partition8x8 asimd, time cost %.2lf ms\n", t_cost);

    int mis_order_opt = 0;
    int mis_opencv = 0;
    int mis_eigen = 0;
    int mis_partition4x4 = 0;
    int mis_partition4x4_asimd = 0;
    int mis_partition8x8 = 0;
    int mis_partition8x8_asimd = 0;

    for (uint32_t i=0; i<len; i++) {
        if (dst_naive[i]!=dst_order_opt[i]) {
            mis_order_opt++;
        }
        if (dst_naive[i]!=dst_opencv[i]) {
            mis_opencv++;
        }
        if (dst_naive[i]!=dst_eigen[i]) {
            mis_eigen++;
        }
        if (dst_naive[i]!=dst_partition4x4[i]) {
            mis_partition4x4++;
            printf("dst_naive[%d]=%f, dst_partition4x4[%d]=%f\n", i, dst_naive[i], i, dst_partition4x4[i]);
        }
        if (dst_naive[i]!=dst_partition4x4_asimd[i]) {
            mis_partition4x4_asimd++;
        }
        if (dst_naive[i]!=dst_partition8x8[i]) {
            mis_partition8x8++;
            printf("dst_naive[%d]=%f, dst_partition4x4[%d]=%f\n", i, dst_naive[i], i, dst_partition8x8[i]);
        }
        if (dst_naive[i]!=dst_partition8x8_asimd[i]) {
            mis_partition8x8_asimd++;
        }
    }

    printf("mis_order_opt=%d, mis_opencv=%d, mis_eigen=%d, \nmis_partition4x4=%d, mis_partition4x4_asimd=%d, mis_partition8x8=%d, mis_partition8x8_asimd=%d\n",
        mis_order_opt, mis_opencv, mis_eigen, mis_partition4x4, mis_partition4x4_asimd, mis_partition8x8, mis_partition8x8_asimd);
}

static void matrix_transpose_u8_test()
{
    uint32_t height;
    uint32_t width;
    cv::Mat gray;
    prepare_big(height, width, gray);
    //prepare_small(height, width, gray);

    unsigned char* src = gray.data;
    double t_start, t_cost;
    uint32_t len = height * width;
    unsigned char* dst_naive = (unsigned char*)malloc(sizeof(unsigned char)*len);
    unsigned char* dst_order_opt = (unsigned char*)malloc(sizeof(unsigned char)*len);
    unsigned char* dst_opencv = (unsigned char*)malloc(sizeof(unsigned char)*len);
    unsigned char* dst_eigen = (unsigned char*)malloc(sizeof(unsigned char)*len);
    unsigned char* dst_partition8x8 = (unsigned char*)malloc(sizeof(unsigned char)*len);
    unsigned char* dst_partition8x8_asimd = (unsigned char*)malloc(sizeof(unsigned char)*len);
    unsigned char* dst_partition16x16 = (unsigned char*)malloc(sizeof(unsigned char)*len);
    unsigned char* dst_partition16x16_asimd = (unsigned char*)malloc(sizeof(unsigned char)*len);

    // naive
    t_start = pixel_get_current_time();
    matrix_transpose_u8_naive(src, height, width, dst_naive);
    t_cost = pixel_get_current_time() - t_start;
    printf("matrix transpose u8, naive,         time cost %.2lf ms\n", t_cost);

    // order_opt
    t_start = pixel_get_current_time();
    matrix_transpose_u8_order_opt(src, height, width, dst_order_opt);
    t_cost = pixel_get_current_time() - t_start;
    printf("matrix transpose u8, order_opt,     time cost %.2lf ms\n", t_cost);

    // opencv
    t_start = pixel_get_current_time();
    matrix_transpose_u8_opencv(src, height, width, dst_opencv);
    t_cost = pixel_get_current_time() - t_start;
    printf("matrix transpose u8, opencv,        time cost %.2lf ms\n", t_cost);

    // eigen
    t_start = pixel_get_current_time();
    matrix_transpose_u8_eigen(src, height, width, dst_eigen);
    t_cost = pixel_get_current_time() - t_start;
    printf("matrix transpose u8, eigen,         time cost %.2lf ms\n", t_cost);

    // partition, 8x8
    t_start = pixel_get_current_time();
    matrix_transpose_u8_partition(src, height, width, dst_partition8x8, 8);
    t_cost = pixel_get_current_time() - t_start;
    printf("matrix transpose u8, partition8x8,  time cost %.2lf ms\n", t_cost);

    // partition, 8x8, asimd
    t_start = pixel_get_current_time();
    matrix_transpose_u8_partition_asimd(src, height, width, dst_partition8x8_asimd, 8);
    t_cost = pixel_get_current_time() - t_start;
    printf("matrix transpose u8, partition8x8 asimd, time cost %.2lf ms\n", t_cost);

    // partition, 16x16
    t_start = pixel_get_current_time();
    matrix_transpose_u8_partition(src, height, width, dst_partition16x16, 16);
    t_cost = pixel_get_current_time() - t_start;
    printf("matrix transpose u8, partition16x16,  time cost %.2lf ms\n", t_cost);

    // partition, 16x16, asimd
    t_start = pixel_get_current_time();
    matrix_transpose_u8_partition_asimd(src, height, width, dst_partition16x16_asimd, 16);
    t_cost = pixel_get_current_time() - t_start;
    printf("matrix transpose u8, partition16x16 asimd, time cost %.2lf ms\n", t_cost);

    int mis_order_opt = 0;
    int mis_opencv = 0;
    int mis_eigen = 0;
    int mis_partition8x8 = 0;
    int mis_partition8x8_asimd = 0;
    int mis_partition16x16 = 0;
    int mis_partition16x16_asimd = 0;

    for (uint32_t i=0; i<len; i++) {
        if (dst_naive[i]!=dst_order_opt[i]) {
            mis_order_opt++;
        }
        if (dst_naive[i]!=dst_opencv[i]) {
            mis_opencv++;
        }
        if (dst_naive[i]!=dst_eigen[i]) {
            mis_eigen++;
        }
        if (dst_naive[i]!=dst_partition8x8[i]) {
            mis_partition8x8++;
        }
        if (dst_naive[i]!=dst_partition8x8_asimd[i]) {
            mis_partition8x8_asimd++;
        }
        if (dst_naive[i]!=dst_partition16x16[i]) {
            mis_partition16x16++;
        }
        if (dst_naive[i]!=dst_partition16x16_asimd[i]) {
            mis_partition16x16_asimd++;
        }
    }

    printf("mis_order_opt=%d, mis_opencv=%d, mis_eigen=%d, \nmis_partition8x8=%d, mis_partition8x8_asimd=%d, mis_partition16x16=%d, mis_partition16x16_asimd=%d\n",
        mis_order_opt, mis_opencv, mis_eigen, mis_partition8x8, mis_partition8x8_asimd, mis_partition16x16, mis_partition16x16_asimd);
}

static void eigen_test()
{
    using namespace Eigen;

    // Matrix3d Mat1;
    // Mat1 << 1, 2, 3,
    //     4, 6, 8,
    //     7, 9, 9;

    Eigen::Matrix <double, 3, 5, RowMajor> Mat1;
    Mat1 << 1, 2, 3, 4, 5,
            11, 12, 13, 14, 15,
            21, 22, 23, 24, 25;

    cout << "Mat1=\n" << Mat1 << endl;
    cout << "Mat1转置矩阵：\n" << Mat1.transpose() << endl;
    // cout << "Mat1伴随矩阵：\n" << Mat1.adjoint() << endl;
    // cout << "Mat1逆矩阵：\n" << Mat1.inverse() << endl;
    // cout << "Mat1行列式：\n" << Mat1.determinant() << endl;
    // SelfAdjointEigenSolver<Matrix3d>eigensover(Mat1);
    // if (eigensover.info() != Success) abort();
    // cout << "特征值：\n" << eigensover.eigenvalues() << endl;
    // cout << "特征向量：\n" << eigensover.eigenvectors() << endl;
}

static void vtrn_f32_test()
{
    float data0[4] = {0, 1, 2, 3};
    float data1[4] = {32, 33, 34, 35};
    float data2[4] = {64, 65, 66, 67};
    float data3[4] = {96, 97, 98, 99};

    printf("before transpose32x4x4:\n");
    printf("data0: %f %f %f %f\n", data0[0], data0[1], data0[2], data0[3]);
    printf("data1: %f %f %f %f\n", data1[0], data1[1], data1[2], data1[3]);
    printf("data2: %f %f %f %f\n", data2[0], data2[1], data2[2], data2[3]);
    printf("data3: %f %f %f %f\n", data3[0], data3[1], data3[2], data3[3]);

    transpose_f32_4x4_asimd(data0, data1, data2, data3);

    printf("--------------------------------------------------\n");

    printf("after transpose32x4x4:\n");
    printf("data0: %f %f %f %f\n", data0[0], data0[1], data0[2], data0[3]);
    printf("data1: %f %f %f %f\n", data1[0], data1[1], data1[2], data1[3]);
    printf("data2: %f %f %f %f\n", data2[0], data2[1], data2[2], data2[3]);
    printf("data3: %f %f %f %f\n", data3[0], data3[1], data3[2], data3[3]);
}

static void print_u8_array(unsigned char* data, uint32_t len) {
    for (size_t i=0; i<len; i++) {
        printf(" %2d", data[i]);
    }
    printf("\n");
}

static void vtrn_u8_test()
{
    unsigned char data0[8] = {0, 1, 2, 3, 4, 5, 6, 7};
    unsigned char data1[8] = {10, 11, 12, 13, 14, 15, 16, 17};
    unsigned char data2[8] = {20, 21, 22, 23, 24, 25, 26, 27};
    unsigned char data3[8] = {30, 31, 32, 33, 34, 35, 36, 37};
    unsigned char data4[8] = {40, 41, 42, 43, 44, 45, 46, 47};
    unsigned char data5[8] = {50, 51, 52, 53, 54, 55, 56, 57};
    unsigned char data6[8] = {60, 61, 62, 63, 64, 65, 66, 67};
    unsigned char data7[8] = {70, 71, 72, 73, 74, 75, 76, 77};

    printf("before transpose_u8_8x8:\n");
    printf("data0:"); print_u8_array(data0, 8);
    printf("data1:"); print_u8_array(data1, 8);
    printf("data2:"); print_u8_array(data2, 8);
    printf("data3:"); print_u8_array(data3, 8);
    printf("data4:"); print_u8_array(data4, 8);
    printf("data5:"); print_u8_array(data5, 8);
    printf("data6:"); print_u8_array(data6, 8);
    printf("data7:"); print_u8_array(data7, 8);

    transpose_u8_8x8_asimd(data0, data1, data2, data3, data4, data5, data6, data7);

    printf("--------------------------------------------------\n");

    printf("after transpose_u8_8x8:\n");
    printf("data0:"); print_u8_array(data0, 8);
    printf("data1:"); print_u8_array(data1, 8);
    printf("data2:"); print_u8_array(data2, 8);
    printf("data3:"); print_u8_array(data3, 8);
    printf("data4:"); print_u8_array(data4, 8);
    printf("data5:"); print_u8_array(data5, 8);
    printf("data6:"); print_u8_array(data6, 8);
    printf("data7:"); print_u8_array(data7, 8);
}

int main() {

    matrix_transpose_u8_test();
    matrix_transpose_f32_test();
    //eigen_test();
    //vtrn_f32_test();
    //vtrn_u8_test();

    return 0;
}


//
//  Polynomials.cpp
//  kcalg
//
//  Created by knightc on 2019/7/15.
//  Copyright © 2019 knightc. All rights reserved.
//

#include <stdio.h>
#include <fstream>

#include "opentsb/math.h"

#include <NTL/ZZXFactoring.h> //单 x 的整数系数的多项式因式分解
#include <NTL/ZZX.h>  //单 x 的 整数系数函数/多项式
#include <NTL/ZZ_pX.h> //单 x 的 模 p 的整数系数函数/多项式
#include <NTL/ZZ_pXFactoring.h> //单 x 的 模 p 的整数系数多项式因式分解
#include <NTL/GF2X.h>
#include <NTL/GF2XFactoring.h>
#include <NTL/GF2EX.h>
#include <NTL/GF2EXFactoring.h>

using namespace NTL;
using namespace std;

ZZ y_polynomials(const ZZX &f,const ZZ xPoint) {

    ZZ m;
    m= f.rep[0];

    for (long i=1 ; i < f.rep.length(); i++) {
        if (!IsZero(f.rep[i])){
        m+=power(xPoint, i) * f.rep[i];
        }
    }

    return m;
}

void test_polynomials_main() {

    ZZX f;

    //f= a0+a1*x +a2*x^2 + a3 * x^3 +... + an * x^n
    ifstream fin("/Users/kc/git/alg-test/kcalg/kcalg/poly.txt");
    fin >> f;
   // cin >> f;//xcode的输入会自动补齐，但是键盘输入需要手动输入"【 "与 " 】"

    Vec< Pair<ZZX,long> > factors;//pair--系数、指数的对；vec_pair_ZZX_long
    ZZ c;

    factor(c, factors, f);//多项式的因式分解

 //   cout << c << "\n" << factors << "\n";

    //特定多项式输出:分圆多项式
    vec_ZZX phi(INIT_SIZE, 5);
    for (long i = 1; i <= 5; i++) {
        ZZX t;
        t = 1;

        for (long j = 1; j <= i-1; j++)
            if (i % j == 0)
                t *= phi(j);

        phi(i) = (ZZX(INIT_MONO, i) - 1)/t;

     //   cout << phi(i) << "\n";
    }

    //多项式赋值示例
    ZZX t1;
    ZZ t11;
    SetCoeff(t1, 4, 1);    //SetCoeff的优点：自动判断长度是否变化，即保证领项系数非0
    //SetCoeff(t1, 2560, 128); //尝试一下，大数
    t1.rep[2] = 2; //ZZX.rep的缺点：改变系数的同时不判断长度是否变化
    SetCoeff(t1, 0, -1);
    SetCoeff(t1, 5, 0); //领项系数设为 0 时，自动去掉
  //  t1[4]=2; //也可以直接定义系数值

    cout << t1 << "\n";
    ZZ t12;
    t12=12345678901234567890;//long 最大只能 20 位，不能直接转换。但是 ZZ 类型可以无穷大
    t11=y_polynomials(t1, t12);
    cout << t11 << endl;//t12=2 时，正确值为 23

    ZZX t2;
    t2.rep.SetLength(6); //如果没有SetCoeff，使用ZZX.rep之前必须先用该句初始化长度
    t2.rep[4] = 1;
    t2.rep[0] = -1;
//    cout << t2 << "\n"; //注：此时领项系数是0，即第 6 项的系数为 0

    t2.normalize(); //作用：重新调整长度，以保证领项系数非0
 //   cout << t2 << "\n";

    //下面两种从多项式中取系数的值的方法等价
    if (t1.rep[4] == 1 and coeff(t1, 4) == 1 ){
      //  cout << "多项式取值正确" << "\n";
    }
  //  cout << GCD(t1, t2) << endl;

    //mod p polynomials
    ZZ p(65537);
    ZZ_p::init(p);

    ZZ_pX f_px;
    //cin >> f_px;
    SetCoeff(f_px, 4, 2);
    f_px.rep[3]=35;
    SetCoeff(f_px,5,1);//领项系数必须为 1：leadcoeff=1

    vec_pair_ZZ_pX_long factors_px;
    CanZass(factors_px, f_px);  // calls "Cantor/Zassenhaus" algorithm
  //  cout << factors_px << "\n" << f_px << "\n";

    // mod gf2e polynomials
    GF2X P;
    SetCoeff(P, 8, 1);
    SetCoeff(P, 4, 1);
    SetCoeff(P, 3, 1);
    SetCoeff(P, 1, 1);
    SetCoeff(P, 0, 1);
    GF2E::init(P);

    GF2EX f_gf2ex;
    SetCoeff(f_gf2ex, 0, 65537);
   // f_gf2ex.rep[2]= 23;

    vec_pair_GF2EX_long factors_gf2ex;
  //  CanZass(factors_gf2ex, f_gf2ex);
   // cout << factors_gf2ex << "\n" << f_gf2ex << "\n";

}


#include <Eigen/CholmodSupport>
#include <fstream>
#include <iomanip>
#include "../../include/Optimization/LineSearch.h"
#include "../../include/Optimization/NewtonDescent.h"
#include "../../include/timer.h"
// #include "SuiteSparse_config.h"

void OptSolver::newtonSolver(std::function<double(const Eigen::VectorXd&, Eigen::VectorXd*, Eigen::SparseMatrix<double>*, bool)> objFunc, std::function<double(const Eigen::VectorXd&, const Eigen::VectorXd&)> findMaxStep, Eigen::VectorXd& x0, int numIter, double gradTol, double xTol, double fTol, bool disPlayInfo, std::function<void(const Eigen::VectorXd&, double&, double&)> getNormFunc, std::string* savingFolder, std::function<void(Eigen::VectorXd&)> postProcess)
{
	const int DIM = x0.rows();
    Eigen::VectorXd randomVec = x0;
    randomVec.setRandom();
    x0 += 1e-6 * randomVec;
	Eigen::VectorXd grad = Eigen::VectorXd::Zero(DIM);
	Eigen::SparseMatrix<double> hessian;

	Eigen::VectorXd neggrad, delta_x;
	double maxStepSize = 1.0;
	double reg = 1e-8;

	bool isProj = true;
    Timer<std::chrono::high_resolution_clock> totalTimer;
    double totalAssemblingTime = 0;
    double totalSolvingTime = 0;
    double totalLineSearchTime = 0;

    totalTimer.start();
	std::ofstream optInfo;
	if (savingFolder)
	{
		optInfo = std::ofstream((*savingFolder) + "_optInfo.txt");
		optInfo << "Newton solver with termination criterion: " << std::endl;
		std::cout << "gradient tol: " << gradTol << ", function update tol: " << fTol << ", variable update tol: " << xTol << ", maximum iteration: " << numIter << std::endl << std::endl;
	}
	int i = 0;
	for (; i < numIter; i++)
	{
		if(disPlayInfo)
			std::cout << "\niter: " << i << std::endl;
		if(savingFolder)
			optInfo << "\niter: " << i << std::endl;
        Timer<std::chrono::high_resolution_clock> localTimer;
        localTimer.start();
		double f = objFunc(x0, &grad, &hessian, isProj);
        localTimer.stop();
        double localAssTime = localTimer.elapsed<std::chrono::milliseconds>() * 1e-3;
        totalAssemblingTime += localAssTime;

        localTimer.start();
		Eigen::SparseMatrix<double> H = hessian;
		Eigen::SparseMatrix<double> I(DIM, DIM);
		I.setIdentity();
		std::cout << "num of nonzeros: " << H.nonZeros() << ", rows: " << H.rows() << ", cols: " << H.cols() << std::endl;
		Eigen::CholmodSupernodalLLT<Eigen::SparseMatrix<double>> solver(H);

		// Eigen::CholmodSimplicialLLT<Eigen::SparseMatrix<double> > solver(H);

		while (solver.info() != Eigen::Success)
		{
			if (disPlayInfo)
			{
				if (isProj)
					std::cout << "some small perturb is needed to remove round-off error, current reg = " << reg << std::endl;
				else
					std::cout << "Matrix is not positive definite, current reg = " << reg << std::endl;
			}

			H = hessian + reg * I;
			solver.compute(H);
			reg = std::max(2 * reg, 1e-16);

            if(reg > 1e4)
            {
                std::cout << "reg is too large, use SPD hessian instead." << std::endl;
                reg = 1e-6;
                isProj = true;
                f = objFunc(x0, &grad, &hessian, isProj);
            }
		}

		neggrad = -grad;
		delta_x = solver.solve(neggrad);

        localTimer.stop();
        double localSolvingTime = localTimer.elapsed<std::chrono::milliseconds>() * 1e-3;
        totalSolvingTime += localSolvingTime;

		maxStepSize = findMaxStep(x0, delta_x);

        localTimer.start();
		double rate = LineSearch::backtrackingArmijo(x0, grad, delta_x, objFunc, maxStepSize);
        localTimer.stop();
        double localLinesearchTime = localTimer.elapsed<std::chrono::milliseconds>() * 1e-3;
        totalLineSearchTime += localLinesearchTime;

		if (!isProj)
		{
			reg *= 0.5;
			reg = std::max(reg, 1e-16);
		}

		x0 = x0 + rate * delta_x;

		double fnew = objFunc(x0, &grad, NULL, isProj);
		if (disPlayInfo)
		{
			std::cout << "line search rate : " << rate << ", actual hessian : " << !isProj << ", reg = " << reg << std::endl;
			std::cout << "f_old: " << f << ", f_new: " << fnew << ", grad norm: " << grad.norm() << ", delta x: " << rate * delta_x.norm() << ", delta_f: " << f - fnew << std::endl;
			if (getNormFunc)
			{
				double gradz, gradw;
				getNormFunc(grad, gradz, gradw);

				double updatez, updatew;
				getNormFunc(rate * delta_x, updatez, updatew);
				std::cout << "z grad: " << gradz << ", w grad: " << gradw << ", z change: " << updatez << ", w change: " << updatew  << std::endl;
			}
            std::cout << "timing info (in total seconds): " << std::endl;
            std::cout << "assembling took: " << totalAssemblingTime << ", LLT solver took: "  << totalSolvingTime << ", line search took: " << totalLineSearchTime << std::endl;
		}

        if(postProcess)
        {
            postProcess(x0);
            if (disPlayInfo)
            {
                double tmpfnew = objFunc(x0, &grad, NULL, isProj);
                std::cout << "after post process, f_new: " << tmpfnew << ", grad norm: " << grad.norm() << std::endl;
            }

        }

		if (savingFolder)
		{
			optInfo << "line search rate : " << rate << ", actual hessian : " << !isProj << ", reg = " << reg << std::endl;
			optInfo << "f_old: " << f << ", f_new: " << fnew << ", grad norm: " << grad.norm() << ", delta x: " << rate * delta_x.norm() << ", delta_f: " << f - fnew << std::endl;
			if (getNormFunc)
			{
				double gradz, gradw;
				getNormFunc(grad, gradz, gradw);

				double updatez, updatew;
				getNormFunc(rate * delta_x, updatez, updatew);
				optInfo << "z grad: " << gradz << ", w grad: " << gradw << ", z change: " << updatez << ", w change: " << updatew << std::endl;
			}
			optInfo << "timing info (in total seconds): " << std::endl;
			optInfo << "assembling took: " << totalAssemblingTime << ", LLT solver took: " << totalSolvingTime << ", line search took: " << totalLineSearchTime << std::endl;

			if (i % 100 == 0)
			{
				std::string fileName = (*savingFolder) + "intermediate.txt";
				std::ofstream ofs(fileName);
				if (ofs)
				{
					ofs << std::setprecision(std::numeric_limits<long double>::digits10 + 1) << x0 << std::endl;
				}
			}
		}

		if ((f - fnew) / f < 1e-5 || rate * delta_x.norm() < 1e-5 || grad.norm() < 1e-4 || std::abs(f) < 1e-4)
		{
			isProj = false;
			// double f = objFunc(x0, &grad, &hessian, isProj);
			// H = hessian;
			// solver.compute(hessian);
			// double tmpReg = 1e-8;
			// while (solver.info() != Eigen::Success)
			// {
			// 	hessian = H + tmpReg * I;
			// 	solver.compute(hessian);
			// 	tmpReg = std::max(2 * tmpReg, 1e-16);
			// }
			// if(tmpReg > 1e-5)
			// {
			// 	std::cout << "hessian is far away from PD, stick with PD hessian." << std::endl;
			// 	isProj = true;
			// }

		}

		if (rate < 1e-8)
		{
			std::cout << "terminate with small line search rate (<1e-8): L2-norm = " << grad.norm() << std::endl;
			break;
		}

		if (grad.norm() < gradTol)
		{
			std::cout << "terminate with gradient L2-norm = " << grad.norm() << std::endl;
			break;
		}

		if (rate * delta_x.norm() < xTol)
		{
			std::cout << "terminate with small variable change, gradient L2-norm = " << grad.norm() << std::endl;
			break;
		}

		if (f - fnew < fTol)
		{
			std::cout << "terminate with small energy change, gradient L2-norm = " << grad.norm() << std::endl;
			break;
		}
	}
	if (i >= numIter)
		std::cout << "terminate with reaching the maximum iteration, with gradient L2-norm = " << grad.norm() << std::endl;

    double f = objFunc(x0, &grad, &hessian, false);
    Eigen::SparseMatrix<double> I(DIM, DIM), H = hessian;
    I.setIdentity();
    Eigen::CholmodSupernodalLLT<Eigen::SparseMatrix<double>> solver(H);

    reg = 1e-10;
    while (solver.info() != Eigen::Success)
    {
        if (disPlayInfo)
        {
            if (isProj)
                std::cout << "some small perturb is needed to remove round-off error, current reg = " << reg << std::endl;
            else
                std::cout << "Matrix is not positive definite, current reg = " << reg << std::endl;
        }

        H = hessian + reg * I;
        solver.compute(H);
        reg = std::max(2 * reg, 1e-16);
    }
    std::cout << "end up with energy: " << f << ", gradient: " << grad.norm() << std::endl;
    std::cout << "mininal evalues of hessian is between: " << reg / 2 << ", " << reg << std::endl;

    totalTimer.stop();
    if(disPlayInfo)
    {
        std::cout << "total time costed (s): " << totalTimer.elapsed<std::chrono::milliseconds>() * 1e-3 << ", within that, assembling took: " << totalAssemblingTime << ", LLT solver took: "  << totalSolvingTime << ", line search took: " << totalLineSearchTime << std::endl;
    }

	if (savingFolder)
	{
		optInfo << "total time costed (s): " << totalTimer.elapsed<std::chrono::milliseconds>() * 1e-3 << ", within that, assembling took: " << totalAssemblingTime << ", LLT solver took: " << totalSolvingTime << ", line search took: " << totalLineSearchTime << std::endl;

		std::string fileName = (*savingFolder) + "final_res.txt";
		std::ofstream ofs(fileName);
		if (ofs)
		{
			ofs << std::setprecision(std::numeric_limits<long double>::digits10 + 1) << x0 << std::endl;
		}

	}

}


#include "PolyMesh/IOManager.h"
#include <Eigen/Sparse>
#define pi 3.1415926

using namespace acamcad;
using namespace polymesh;
using namespace Eigen;

void BarycentricCoordinates(PolyMesh* mesh)
{
	int v_n = mesh->numVertices();
	std::vector<int> inner_id(v_n);
	int iter;
	int boundary_num, inner_num;

	std::vector<double> u(mesh->numVertices());
	std::vector<double> v(mesh->numVertices());

	iter = 0;
	for (VertexIter v_it = mesh->vertices_begin(); v_it != mesh->vertices_end(); ++v_it)
	{
		if (!mesh->isBoundary(*v_it))
		{
			inner_id[(*v_it)->index()] = iter;
			iter++;
		}
	}
	inner_num = iter;
	boundary_num = v_n - iter;

	MHalfedge* first_heh;
	for (HalfEdgeIter he_it = mesh->halfedge_begin(); he_it != mesh->halfedge_end(); ++he_it)
	{
		if (mesh->isBoundary(*he_it))
		{
			first_heh = *he_it;
			break;
		}
	}
	MHalfedge* iter_heh = first_heh->next();
	iter = 0;
	while (iter_heh != first_heh)
	{
		MVert* from_v = iter_heh->fromVertex();
		u[from_v->index()] = cos(double(2 * pi *iter / boundary_num));
		v[from_v->index()] = sin(double(2 * pi *iter / boundary_num));
		iter_heh = iter_heh->next();
		iter++;
	}
	u[first_heh->fromVertex()->index()] = cos(double(2 * pi *iter / boundary_num));
	v[first_heh->fromVertex()->index()] = sin(double(2 * pi *iter / boundary_num));

	SparseMatrix<double> weight(v_n, v_n);
	std::vector<Triplet<double>> triplet;

	for (FaceIter f_it = mesh->polyfaces_begin(); f_it != mesh->polyfaces_end(); ++f_it)
	{
		MHalfedge* heh = (*f_it)->halfEdge();
		MVert* v0 = heh->fromVertex();
		MVert* v1 = heh->toVertex();
		MHalfedge* next_heh = heh->next();
		MVert* v2 = next_heh->toVertex();

		double l2 = (v0->position() - v1->position()).norm();
		double l1 = (v0->position() - v2->position()).norm();
		double l0 = (v1->position() - v2->position()).norm();

		double angle = acos(dot(v2->position() - v0->position(), v1->position() - v0->position())/(l1*l2));
		triplet.push_back(Triplet<double>(v0->index(), v1->index(), tan(angle*0.5) / l2));
		triplet.push_back(Triplet<double>(v0->index(), v2->index(), tan(angle*0.5) / l1));

		angle = acos(dot(v0->position() - v1->position(), v2->position() - v1->position())/(l2*l0));
		triplet.push_back(Triplet<double>(v1->index(), v0->index(), tan(angle*0.5) / l2));
		triplet.push_back(Triplet<double>(v1->index(), v2->index(), tan(angle*0.5) / l0));

		angle = acos(dot(v0->position() - v2->position(), v1->position() - v2->position())/(l0*l1));
		triplet.push_back(Triplet<double>(v2->index(), v0->index(), tan(angle*0.5) / l1));
		triplet.push_back(Triplet<double>(v2->index(), v1->index(), tan(angle*0.5) / l0));
	}
	weight.setFromTriplets(triplet.begin(), triplet.end());

	SparseMatrix<double> inner(inner_num, inner_num);
	VectorXd ub(inner_num), vb(inner_num);

	triplet.clear();
	for (VertexIter v_it = mesh->vertices_begin(); v_it != mesh->vertices_end(); ++v_it)
	{
		if (!mesh->isBoundary(*v_it))
		{
			double tempu = 0;
			double tempv = 0;
			for (VertexVertexIter vv_it = mesh->vv_iter(*v_it); vv_it.isValid(); ++vv_it)
			{
				if (mesh->isBoundary(*vv_it))
				{
					tempu += u[(*vv_it)->index()] * weight.coeff((*v_it)->index(), (*vv_it)->index());
					tempv += v[(*vv_it)->index()] * weight.coeff((*v_it)->index(), (*vv_it)->index());
				}
				else
				{
					triplet.push_back(Triplet<double>(inner_id[(*v_it)->index()], inner_id[(*vv_it)->index()], -weight.coeff((*v_it)->index(), (*vv_it)->index())));
				}
				triplet.push_back(Triplet<double>(inner_id[(*v_it)->index()], inner_id[(*v_it)->index()], weight.coeff((*v_it)->index(), (*vv_it)->index())));
			}
			ub[inner_id[(*v_it)->index()]] = tempu;
			vb[inner_id[(*v_it)->index()]] = tempv;
		}
	}
	inner.setFromTriplets(triplet.begin(), triplet.end());
	SparseLU<SparseMatrix<double>> solver;
	solver.analyzePattern(inner);
	solver.factorize(inner);
	VectorXd result_u = solver.solve(ub);
	VectorXd result_v = solver.solve(vb);

	for (VertexIter v_it = mesh->vertices_begin(); v_it != mesh->vertices_end(); ++v_it)
	{
		if (!mesh->isBoundary(*v_it))
		{
			(*v_it)->setPosition(result_u[inner_id[(*v_it)->index()]], result_v[inner_id[(*v_it)->index()]], 0);
		}
		else
		{
			(*v_it)->setPosition(u[(*v_it)->index()], v[(*v_it)->index()], 0);
		}
	}
}

int main(int argc, const char **argv)
{
	if (argc != 2)
	{
		std::cout << "========== Hw7 Usage  ==========\n";
		std::cout << std::endl;
		std::cout << "blablabla.exe	test.obj\n";
		std::cout << std::endl;
		std::cout << "=================================================\n";
        return 0;
	}
	std::string mesh_path = argv[1];
	PolyMesh* mesh = new PolyMesh();
	loadMesh(mesh_path, mesh);
	BarycentricCoordinates(mesh);
	writeMesh("result_" + mesh_path, mesh);
    return 0;
}