#include <Engine/MeshEdit/ASAP.h>

#include <Engine/Primitive/TriMesh.h>

#include <Eigen/Sparse>
#include <Eigen/LU>
#include <Eigen/SVD>

using namespace Ubpa;

using namespace std;
using namespace Eigen;

bool ASAP::Iterate()
{
	iter_count_++;
	cout << iter_count_ << "th iteration" << endl;
	UpdatePara();
	UpdateTriMesh();
	return true;
}

void ASAP::UpdatePara()
{
	MatrixXf B(nV, 2);
	for (size_t i = 0; i < nV; i++)
	{
		B.row(i) = RowVector2f::Zero();
	}

	for (size_t t = 0; t < nT; t++)
	{
		auto triangle = heMesh->Polygons().at(t);
		auto edge = triangle->HalfEdge();
		Matrix<float, 3, 2> xt = flat_tri_[t];
		Matrix<float, 3, 2> ut = Matrix<float, 3, 2>();
		ut.row(0) = para_solution_.row(heMesh->Index(edge->Origin()));
		ut.row(1) = para_solution_.row(heMesh->Index(edge->End()));
		ut.row(2) = para_solution_.row(heMesh->Index(edge->Next()->End()));
		Matrix2f J = Jacobian(xt, ut);
		JacobiSVD<Matrix2f> svd(J, ComputeThinU | ComputeThinV);
		Matrix2f U = svd.matrixU();
		Matrix2f V = svd.matrixV();
		Vector2f A = svd.singularValues();
		Matrix2f A2 = Matrix2f::Zero();
		A2(0, 0) = (A(0) + A(1)) / 2;
		A2(1, 1) = (A(0) + A(1)) / 2;
		Matrix2f L = U * V;

		for (int k = 0; k < 3; k++, edge = edge->Next())
		{
			auto vi = edge->Origin();
			auto vj = edge->End();
			auto vk = edge->Next()->End();
			int i = heMesh->Index(vi);
			int j = heMesh->Index(vj);
			vecf3 eki = vi->pos - vk->pos;
			vecf3 ekj = vj->pos - vk->pos;
			float cot_theta = eki.cos_theta(ekj) / eki.sin_theta(ekj);
			//B.row(i) += (para_solution_.row(i) - para_solution_.row(j)) * V.transpose() * V * cot_theta;
			B.row(i) += (xt.row(k) - xt.row((k + 1) % 3)) * L * cot_theta;
			//B.row(j) += (para_solution_.row(j) - para_solution_.row(i)) * V.transpose() * V * cot_theta;
			B.row(j) += (xt.row((k + 1) % 3) - xt.row(k)) * L * cot_theta;
		}
	}

	B.row(start) = RowVector2f::Zero();
	B.row(end) = RowVector2f(1, 1);
	para_solution_ = para_solver_.solve(B);
}


//
// Copyright (c) 2012 Juan Palacios juan.palacios.puyana@gmail.com
// This file is part of minimathlibs.
// Subject to the BSD 2-Clause License
// - see < http://opensource.org/licenses/BSD-2-Clause>
//

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE TestMatrixOps
#include <boost/test/unit_test.hpp>

#include <cmath>
#include <cstdlib>
#include <iostream>
#include <limits>
#include "minimath/matrix.hpp"
#include "minimath/matrix_ops.hpp"
#include "Defines.h"

typedef minimath::matrix<double,3,1> Point;

namespace
{

minimath::matrix<double, 3> rotation3DX(double angle)
{
  minimath::matrix<double, 3> m = minimath::identity_matrix();
  const double c = std::cos(angle);
  const double s = std::sin(angle);
  m(1,1) = m(2,2) = c;
  m(1,2) = m(2,1) = s;
  m(2,1)*= -1;
  return m;
}

minimath::matrix<double, 3> rotation3DY(double angle)
{
  minimath::matrix<double, 3> m = minimath::identity_matrix();
  const double c = std::cos(angle);
  const double s = std::sin(angle);
  m(0,0) = m(2,2) = c;
  m(0,2) = m(2,0) = s;
  m(2,0)*= -1;
  return m;
}

minimath::matrix<double, 3> rotation3DZ(double angle)
{
  minimath::matrix<double, 3> m = minimath::identity_matrix();
  const double c = std::cos(angle);
  const double s = std::sin(angle);
  m(0,0) = m(1,1) = c;
  m(1,0) = m(0,1) = s;
  m(0,1)*= -1;
  return m;
}

struct FixturePoints
{
    FixturePoints()
    {
        p100_(0,0) = 1;
        p010_(1,0) = 1;
        p001_(2,0) = 1;
        p110_(0,0) = 1;
        p110_(1,0) = 1;
        p011_(1,0) = 1;
        p011_(2,0) = 1;
        p101_(0,0) = 1;
        p101_(2,0) = 1;
        p111_ += 1;

    }

    Point p100_;
    Point p010_;
    Point p001_;
    Point p110_;
    Point p011_;
    Point p101_;
    Point p111_;
};

} // anonymous namespace

BOOST_FIXTURE_TEST_SUITE(TestMatrixOps, FixturePoints)

BOOST_AUTO_TEST_CASE(testXRotations)
{
  for (int i = 1; i<9; ++i)
  {
    minimath::matrix<double,3> rot = rotation3DX(PI/i);
    minimath::matrix<double,3> orig;
    minimath::setColumn(orig, p100_, 0);
    minimath::setColumn(orig, p010_, 1);
    minimath::setColumn(orig, p111_, 2);

    minimath::matrix<double,3> prime = rot*orig;
    bool success = true;
    minimath::matrix<double,3> rot2 = minimath::transformation(orig, prime, success);
    if (success)
    {
      BOOST_CHECK(minimath::equal(rot, rot2, 1));
    } else {
      std::cout << "\nInversion failed for angle: PI/" << i;
      std::cout <<"\nOriginal matrix:\n " << orig << "\n";
    }
  }
}

BOOST_AUTO_TEST_CASE(testYRotations)
{
  for (int i = 1; i<9; ++i)
  {
    minimath::matrix<double,3> rot = rotation3DY(PI/i);
    minimath::matrix<double,3> orig;
    minimath::setColumn(orig, p100_, 0);
    minimath::setColumn(orig, p010_, 1);
    minimath::setColumn(orig, p111_, 2);

    minimath::matrix<double,3> prime = rot*orig;
    bool success = true;
    minimath::matrix<double,3> rot2 = minimath::transformation(orig, prime, success);
    if (success)
    {
      BOOST_CHECK(minimath::equal(rot, rot2, 1));
    } else {
      std::cout << "\nInversion failed for angle: PI/" << i;
      std::cout <<"\nOriginal matrix:\n " << orig << "\n";
    }
  }
}

BOOST_AUTO_TEST_CASE(testZRotations)
{
  for (int i = 1; i<9; ++i)
  {
    minimath::matrix<double,3> rot = rotation3DZ(PI/i);
    minimath::matrix<double,3> orig;
    minimath::setColumn(orig, p100_, 0);
    minimath::setColumn(orig, p010_, 1);
    minimath::setColumn(orig, p111_, 2);

    minimath::matrix<double,3> prime = rot*orig;
    bool success = true;
    minimath::matrix<double,3> rot2 = minimath::transformation(orig, prime, success);
    if (success)
    {
      BOOST_CHECK(minimath::equal(rot, rot2, 1));
    } else {
      std::cout << "\nInversion failed for angle: PI/" << i;
      std::cout <<"\nOriginal matrix:\n " << orig << "\n";
    }
  }
}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include "mtao/optimization/line_search.hpp"

#include <Eigen/Dense>

struct QuadraticFunc {
    using Scalar = double;
    using Vector = Eigen::VectorXd;

    Scalar objective(const Vector& p) const {
        return p.transpose() * (A * p + b) + c;
    }
    Vector gradient(const Vector& p) const {
        Vector ret = 2 * A * p + b;
        return ret;
    }

    //Vector descent_direction(const Vector& p) const { return -gradient(p).normalized(); }
    Vector descent_direction(const Vector& p) const {

        if(trinary) {
        return (-gradient(p)).unaryExpr([](double v) -> double {
                if(v > 0) { return 1; }
                else if(v < 0) { return -1; }
                else { return 0; }
                });
        } else {
            return -gradient(p).normalized();
        }

    }
    bool trinary = false;
    Eigen::MatrixXd A;
    Eigen::VectorXd b;
    Scalar c;

};

using namespace mtao::optimization;

int main(int argc, char * argv[]) {
    std::cout << "Eigen threads: " << Eigen::nbThreads() << std::endl;
    QuadraticFunc func;
    int N = 600;
    Eigen::MatrixXd A(N,N);
    Eigen::VectorXd b(N);

    if constexpr(true) {
        A.setIdentity();
        b.setOnes(N);

        func.A = A.transpose() * A;
        func.b = -2 * A.transpose() * b;
        func.c = b.dot(b);
    } else {

        A.setRandom();
        b.setRandom();
        func.A = A.transpose() * A;
        func.b = b;
        func.c = 5;
    }

    auto run = [&](auto&& opt) {
        opt.set_position(b);
        std::cout << "Optimal energy: " << opt.objective() << std::endl;
        std::cout << "Optimal gradient norm: " << opt.gradient().norm() << std::endl;
        opt.set_position(Eigen::VectorXd::Random(N));
        std::cout << "Initial energy: " << opt.objective() << std::endl;
        int iters = opt.run();
        std::cout << "Final energy: " << opt.objective() << " in " << iters << " iterations."<< std::endl;
        std::cout << std::endl;
        //std::cout << "position: " << opt.position().transpose() << std::endl;
    };
    auto run_run = [&]() {
    {
        std::cout << "Backtracking / armijo conditions" << std::endl;
        auto opt = make_backtracking_line_search(func);
        run(opt);
    }
    {
        std::cout << "weak wolfe conditions" << std::endl;
        auto opt = make_wolfe_line_search(func);
        opt.set_weak();
        run(opt);
    }
    {
        std::cout << "Strong wolfe conditions" << std::endl;
        auto opt = make_wolfe_line_search(func);
        opt.set_strong();
        run(opt);
    }
    };
    run_run();
    std::cout << std::endl;
    std::cout << std::endl;
    std::cout << std::endl;
    func.trinary = true;
    std::cout << "Using a `trinary` which is lazy for something in the roughly right direction" << std::endl;
    run_run();
}


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

// Note: Matrices unit tests have been split in different files since
// building them with eigen3 eats a lot of RAM and may be a problem while
// compiling in small systems.

#include <gtest/gtest.h>
#include <mrpt/io/CMemoryStream.h>
#include <mrpt/math/CMatrixD.h>
#include <mrpt/math/CMatrixFixed.h>
#include <mrpt/math/matrix_serialization.h>  // serialization of matrices
#include <mrpt/random.h>
#include <mrpt/serialization/CArchive.h>
#include <Eigen/Dense>

using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::random;
using namespace std;

const double dat_A[] = {4, 5, 8, -2, 1, 3};
const double dat_B[] = {2, 6, 9, 8};
const double dat_Cok[] = {53, 64, -2, 32, 29, 30};

#define CHECK_AND_RET_ERROR(_COND_, _MSG_) EXPECT_FALSE(_COND_) << _MSG_;

TEST(Matrices, DynMat_size)
{
	CMatrixDouble A(3, 2, dat_A);
	EXPECT_EQ(A.rows(), 3);
	EXPECT_EQ(A.cols(), 2);
	EXPECT_EQ(A.size(), 6U);
}

TEST(Matrices, A_times_B_dyn)
{
	// Dyn. size, double.
	CMatrixDouble A(3, 2, dat_A);
	CMatrixDouble B(2, 2, dat_B);
	CMatrixDouble C = CMatrixDouble(A * B);
	CMatrixDouble C_ok(3, 2, dat_Cok);
	CMatrixDouble err = C - C_ok;
	EXPECT_NEAR(0, fabs(err.sum()), 1e-5)
		<< "A:   " << A << "B:   " << B << "A*B: " << C << endl;
}

TEST(Matrices, A_times_B_fix)
{
	// Fix. size, double.
	CMatrixFixed<double, 3, 2> A(dat_A);
	CMatrixFixed<double, 2, 2> B(dat_B);
	CMatrixFixed<double, 3, 2> C, C_ok(dat_Cok), Err;

	C = A * B;
	Err = C.asEigen() - CMatrixFixed<double, 3, 2>(C_ok).asEigen();

	EXPECT_NEAR(0, fabs(Err.asEigen().array().sum()), 1e-5);
}

TEST(Matrices, SerializeCMatrixD)
{
	CMatrixDouble A(3, 2, dat_A);
	CMatrixFixed<double, 3, 2> fA;

	CMatrixD As = CMatrixD(A);

	mrpt::io::CMemoryStream membuf;
	auto arch = mrpt::serialization::archiveFrom(membuf);
	arch << As;
	membuf.Seek(0);
	arch >> fA;

	EXPECT_NEAR(0, fabs((CMatrixDouble(fA) - A).sum()), 1e-9);

	try
	{
		// Now, if we try to de-serialize into the wrong type, we should get an
		// exception:
		membuf.Seek(0);
		CMatrixFixed<double, 2, 2> fB;
		arch >> fB;  // Wrong size!

		GTEST_FAIL() << "Exception not launched when it was expected!";
	}
	catch (...)
	{  // OK, exception occurred, as expected
	}
}

TEST(Matrices, EigenVal2x2dyn)
{
	const double dat_C1[] = {14.6271, 5.8133, 5.8133, 16.8805};
	CMatrixDouble C1(2, 2, dat_C1);

	CMatrixDouble C1_V;
	std::vector<double> C1_Ds;
	C1.eig(C1_V, C1_Ds);

	CMatrixDouble C1_D;
	C1_D.setDiagonal(C1_Ds);

	CMatrixDouble C1_RR =
		CMatrixDouble(C1_V.asEigen() * C1_D.asEigen() * C1_V.transpose());
	EXPECT_NEAR((C1_RR - C1).sum_abs(), 0, 1e-4);
}

TEST(Matrices, eig_symmetric)
{
	// Test by looking only at the lower triangular-part:
	{
		const double dat_C[] = {14.6271, 0, 5.8133, 16.8805};
		CMatrixDouble22 C(dat_C);

		CMatrixDouble22 eig_vecs;
		std::vector<double> eig_vals;
		C.eig_symmetric(eig_vecs, eig_vals);

		EXPECT_EQ(eig_vals.size(), 2UL);
		EXPECT_NEAR(eig_vals[0], 9.83232131811656, 1e-4);
		EXPECT_NEAR(eig_vals[1], 21.67527868188344, 1e-4);
	}
}

TEST(Matrices, EigenVal3x3dyn)
{
	const double dat_C1[] = {8, 6, 1, 6, 9, 4, 1, 4, 10};
	CMatrixDouble C1(3, 3, dat_C1);

	CMatrixDouble C1_V;
	std::vector<double> C1_Ds;
	C1.eig(C1_V, C1_Ds);

	CMatrixDouble C1_D;
	C1_D.setDiagonal(C1_Ds);

	CMatrixDouble C1_RR =
		CMatrixDouble(C1_V.asEigen() * C1_D.asEigen() * C1_V.transpose());
	EXPECT_NEAR((C1_RR - C1).sum_abs(), 0, 1e-4);
}

TEST(Matrices, EigenVal2x2fix)
{
	const double dat_C1[] = {14.6271, 5.8133, 5.8133, 16.8805};
	CMatrixDouble22 C1(dat_C1);

	CMatrixDouble22 C1_V;
	std::vector<double> C1_Ds;
	C1.eig(C1_V, C1_Ds);

	CMatrixDouble22 C1_D;
	C1_D.setDiagonal(C1_Ds);

	CMatrixDouble22 C1_RR =
		CMatrixDouble22(C1_V.asEigen() * C1_D.asEigen() * C1_V.transpose());
	EXPECT_NEAR((C1_RR - C1).sum_abs(), 0, 1e-4);
}

TEST(Matrices, EigenVal3x3fix)
{
	const double dat_C1[] = {8, 6, 1, 6, 9, 4, 1, 4, 10};
	CMatrixDouble33 C1(dat_C1);

	CMatrixDouble33 C1_V;
	std::vector<double> C1_Ds;
	C1.eig(C1_V, C1_Ds);

	CMatrixDouble33 C1_D;
	C1_D.setDiagonal(C1_Ds);

	CMatrixDouble33 C1_RR =
		CMatrixDouble33(C1_V.asEigen() * C1_D.asEigen() * C1_V.transpose());
	EXPECT_NEAR((C1_RR - C1).sum_abs(), 0, 1e-4);
}


/*
// Copyright (c) 2000-2009, Texas Engineering Experiment Station (TEES), a
// component of the Texas A&M University System.

// All rights reserved.

// The information and source code contained herein is the exclusive
// property of TEES and may not be disclosed, examined or reproduced
// in whole or in part without explicit written authorization from TEES.
*/

////////////////////////////////////////////////////////////////////////////////
/// @file
/// Solution of the Euler Problem #3
/// (link).
////////////////////////////////////////////////////////////////////////////////

#include <stapl/utility/do_once.hpp>
#include <stapl/array.hpp>
#include <stapl/views/array_view.hpp>
#include <stapl/algorithm.hpp>
#include <boost/lexical_cast.hpp>

////////////////////////////////////////////////////////////////////////////////
/// @brief Returns truncated integer square root of given number.
/// @tparam IntType Integral type of the number.
////////////////////////////////////////////////////////////////////////////////
template <typename IntType>
inline IntType sqrt_int(IntType n)
{
  return static_cast<IntType>( std::sqrt(n) );
}

////////////////////////////////////////////////////////////////////////////////
/// @brief Functor that returns the larger of @p n and @p x/n that is both a
///        factor of @p x and a prime number (0 if neither satisfies these
///        conditions).
/// @tparam IntType Integral type of the dividend @p x.
////////////////////////////////////////////////////////////////////////////////
template <typename IntType>
struct largest_prime_factors_filter
{
  static_assert(std::is_integral<IntType>::value, "Integer required.");

  largest_prime_factors_filter(IntType x)
    : m_x(x)
  { }

  IntType operator()(IntType n)
  {
    if ((m_x % n) != 0)   // n is not a factor of x
      return 0;

    if (is_prime(m_x/n))  // x/n (which is larger than n) is prime
      return m_x/n;

    return is_prime(n) ? n : 0;
  }

  void define_type(stapl::typer& t)
  {
    t.member(m_x);
  }

private:
  IntType m_x;

  static bool is_prime(IntType n)
  {
    IntType sqrt_n = sqrt_int(n);

    for (IntType i = 2; i <= sqrt_n; ++i)
    {
      if ((n % i) == 0)
        return false;
    }

    return true;
  }
};

stapl::exit_code stapl_main(int argc, char** argv)
{
  using value_t = std::size_t;

  if (argc < 2)
  {
    stapl::do_once( [] {
      std::cout << "Run as: mpirun -n <ncpu> pe_3a <number>" << std::endl;
    });
    return EXIT_FAILURE;
  }

  stapl::counter<stapl::default_timer> exec_timer;
  exec_timer.start();

  // Read the dividend from command line.
  value_t num = boost::lexical_cast<value_t> (argv[1]);

  // Create array container for storage and a view into it.
  stapl::array<value_t> a(sqrt_int(num)-1);
  auto a_vw = stapl::make_array_view(a);

  // Fill the array with consecutive values from 2 to sqrt(num).
  stapl::iota(a_vw, 2);

  // Replace numbers in the array that are either not factors of num or not
  // prime by 0; also replace any prime factor x by the greater value num/x if
  // the latter is also prime.
  stapl::transform(a_vw, a_vw, largest_prime_factors_filter<value_t>(num));

  // Compute and print the maximum of all numbers in the array.
  value_t max_prime_factor = stapl::max_value(a_vw);

  double t = exec_timer.stop();

  stapl::do_once( [&] {
    std::cout << "Computation finished (time taken: " << t << " s)."
              << std::endl
              << "Largest prime factor of " << num << " is: "
              << max_prime_factor << std::endl;
  });

  return EXIT_SUCCESS;
}


#include <Eigen>

#include "warp.h"

void llcv_calc_persp_transform(float *matrixData, int matrixDataSize, bool rowMajor, const vector<cv::Point> sourcePoints, const vector<cv::Point> destPoints) {

  // Set up matrices a and b so we can solve for x from ax = b
  // See http://xenia.media.mit.edu/~cwren/interpolator/ for a
  // good explanation of the basic math behind this.

  typedef Eigen::Matrix<float, 8, 8> Matrix8x8;
  typedef Eigen::Matrix<float, 8, 1> Matrix8x1;

  Matrix8x8 a;
  Matrix8x1 b;

  for(int i = 0; i < 4; i++) {
    a(i, 0) = sourcePoints[i].x;
    a(i, 1) = sourcePoints[i].y;
    a(i, 2) = 1;
    a(i, 3) = 0;
    a(i, 4) = 0;
    a(i, 5) = 0;
    a(i, 6) = -sourcePoints[i].x * destPoints[i].x;
    a(i, 7) = -sourcePoints[i].y * destPoints[i].x;

    a(i + 4, 0) = 0;
    a(i + 4, 1) = 0;
    a(i + 4, 2) = 0;
    a(i + 4, 3) = sourcePoints[i].x;
    a(i + 4, 4) = sourcePoints[i].y;
    a(i + 4, 5) = 1;
    a(i + 4, 6) = -sourcePoints[i].x * destPoints[i].y;
    a(i + 4, 7) = -sourcePoints[i].y * destPoints[i].y;

    b(i, 0) = destPoints[i].x;
    b(i + 4, 0) = destPoints[i].y;
  }

  // Solving ax = b for x, we get the values needed for our perspective
  // matrix. Table of options on the eigen site at
  // /dox/TutorialLinearAlgebra.html#TutorialLinAlgBasicSolve
  //
  // We use householderQr because it places no restrictions on matrix A,
  // is moderately fast, and seems to be sufficiently accurate.
  //
  // partialPivLu() seems to work as well, but I am wary of it because I
  // am unsure of A is invertible. According to the documenation and basic
  // performance testing, they are both roughly equivalent in speed.
  //
  // - @burnto

  Matrix8x1 x = a.householderQr().solve(b);

  // Initialize matrixData
  for (int i = 0; i < matrixDataSize; i++) {
    matrixData[i] = 0.0f;
  }
  int matrixSize = (matrixDataSize >= 16) ? 4 : 3;

  // Initialize a 4x4 eigen matrix. We may not use the final
  // column/row, but that's ok.
  Eigen::Matrix4f perspMatrix = Eigen::Matrix4f::Zero();

  // Assign a, b, d, e, and i
  perspMatrix(0, 0) = x(0, 0); // a
  perspMatrix(0, 1) = x(1, 0); // b
  perspMatrix(1, 0) = x(3, 0); // d
  perspMatrix(1, 1) = x(4, 0); // e
  perspMatrix(2, 2) = 1.0f;    // i

  // For 4x4 matrix used for 3D transform, we want to assign
  // c, f, g, and h to the fourth col and row.
  // So we use an offset for thes values
  int o = matrixSize - 3; // 0 or 1
  perspMatrix(0, 2 + o) = x(2, 0); // c
  perspMatrix(1, 2 + o) = x(5, 0); // f
  perspMatrix(2 + o, 0) = x(6, 0); // g
  perspMatrix(2 + o, 1) = x(7, 0); // h
  perspMatrix(2 + o, 2 + o) = 1.0f; // i

  // Assign perspective matrix to our matrixData buffer,
  // swapping row versus column if needed, and taking care not to
  // overflow if user didn't provide a large enough matrixDataSize.
  for(int c = 0; c < matrixSize; c++) {
    for(int r = 0; r < matrixSize; r++) {
      int index = rowMajor ? (c + r * matrixSize) : (r + c * matrixSize);
      if (index < matrixDataSize) {
        matrixData[index] = perspMatrix(r, c);
      }
    }
  }
  // TODO - instead of copying final values into matrixData return array, do one of:
  // (a) assign directly into matrixData, or
  // (b) use Eigen::Mat so that assignment goes straight into underlying matrixData
}

void llcv_unwarp(const Mat& input, const vector<cv::Point>& source_points, const cv::Rect& to_rect, Mat& output)
{
    float matrix[16];
	vector<cv::Point> dest_points;
    dest_points.push_back(cv::Point(to_rect.x, to_rect.y));
    dest_points.push_back(cv::Point(to_rect.x + to_rect.width, to_rect.y));
    dest_points.push_back(cv::Point(to_rect.x, to_rect.y + to_rect.height));
    dest_points.push_back(cv::Point(to_rect.x + to_rect.width, to_rect.y + to_rect.height));

    // Calculate row-major matrix
    llcv_calc_persp_transform(matrix, 9, true, source_points, dest_points);
    Mat cv_persp_mat = Mat(3, 3, CV_32FC1);
    for (int r = 0; r < 3; r++) {
        float* ptr = cv_persp_mat.ptr<float>(r);
        for (int c = 0; c < 3; c++) {
            ptr[c] =  matrix[3 * r + c];
        }
    }

    warpPerspective(input, output, cv_persp_mat, input.size(), CV_INTER_LINEAR + CV_WARP_FILL_OUTLIERS);
}


#pragma once
#include <Eigen/Core>
#include <numeric>

namespace mtao::linear_algebra {
    // M is matrix, B is initial vector, Q is the basis for the basis
    // N is the dimension of the subspace

    namespace internal {
        template <typename Derived, typename BDerived, typename QDerived, typename HDerived>
            auto arnoldi(const Eigen::MatrixBase<Derived>& M, const Eigen::MatrixBase<BDerived>& b, Eigen::PlainObjectBase<QDerived>& Q, Eigen::PlainObjectBase<HDerived>& H) {
                const int N = H.rows() - 1;
                using Scalar = typename Derived::Scalar;
                constexpr Scalar eps = std::numeric_limits<Scalar>::epsilon();
                using Vec = Eigen::Matrix<Scalar, Derived::RowsAtCompileTime, 1>;

                Q.setZero();
                H.setZero();
                Vec v;

                Q.col(0) = b.normalized();
                for(int j = 0; j <= N; ++j) {

                    auto q = Q.col(j);
                    v = M * q;
                    for(int k = 0; k <= j; ++k) {
                        auto l = Q.col(k);
                        const Scalar s = l.dot(v);
                        H(k,j) = s;
                        v -= s * l;
                    }
                    if(j < N) {
                        const Scalar n = H(j+1,j) = v.norm();
                        if(n > eps) {
                            v /= n;
                            Q.col(j+1) = v;
                            continue;
                        }
                    }
                    return;
                }
            }
    }

    template <typename Derived, typename BDerived>
        auto arnoldi(const Eigen::MatrixBase<Derived>& M, const Eigen::MatrixBase<BDerived>& B, int N) {
            assert(M.rows() == M.cols());

            using Scalar = typename Derived::Scalar;
            using Mat = Eigen::Matrix<Scalar,Derived::RowsAtCompileTime,Eigen::Dynamic>;
            using DMat = Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>;

            Mat Q(M.rows(),N);
            DMat H(N,N);

            internal::arnoldi(M,B,Q,H);
            return std::make_tuple(Q,H);
        }
    template <int N, typename Derived, typename BDerived>
        auto arnoldi(const Eigen::MatrixBase<Derived>& M, const Eigen::MatrixBase<BDerived>& B) {
            assert(M.rows() == M.cols());

            using Scalar = typename Derived::Scalar;
            using Mat = Eigen::Matrix<Scalar,Eigen::Dynamic,N>;
            using HMat = Eigen::Matrix<Scalar,N,N>;

            Mat Q(M.rows(),N);
            HMat H(N,N);
            internal::arnoldi(M,B,Q,H);
            return std::make_tuple(Q,H);
        }
    template <typename Derived>
        auto arnoldi(const Eigen::MatrixBase<Derived>& M, int N) {
            using Scalar = typename Derived::Scalar;
            auto B = Eigen::Matrix<Scalar,Derived::RowsAtCompileTime, 1>::Random(M.rows()) ;
            return arnoldi(M,B,N);
        }
    template <int N, typename Derived>
        auto arnoldi(const Eigen::MatrixBase<Derived>& M) {
            using Scalar = typename Derived::Scalar;
            auto B = Eigen::Matrix<Scalar,Derived::RowsAtCompileTime, 1>::Random(M.rows()) ;
            return arnoldi<N>(M,B);
        }
}


// STL includes
#include <iostream>
#include <vector>
#include <unordered_set>
#include <climits>

// BGL includes
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/maximum_weighted_matching.hpp>
#include <boost/graph/max_cardinality_matching.hpp>

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS> graph;
typedef boost::graph_traits<graph>::vertex_descriptor vertex_desc;

using namespace std;

int maximum_matching(const graph &G) {
  int n = boost::num_vertices(G);
  std::vector<vertex_desc> mate_map(n);  // exterior property map
  // const vertex_desc NULL_VERTEX = boost::graph_traits<weighted_graph>::null_vertex();

  boost::edmonds_maximum_cardinality_matching(G,
    boost::make_iterator_property_map(mate_map.begin(), boost::get(boost::vertex_index, G)));
  int matching_size = boost::matching_size(G,
    boost::make_iterator_property_map(mate_map.begin(), boost::get(boost::vertex_index, G)));
  return matching_size;
  // int min_weight = INT_MAX;
  // edge_desc e;
  // for (int i = 0; i < n; ++i) {
  //   // mate_map[i] != NULL_VERTEX: the vertex is matched
  //   // i < mate_map[i]: visit each edge in the matching only once
  //   // cout << i << " " << mate_map[i] << endl;
  //   e = edge(i,mate_map[i],G).first;
  //   min_weight = min(min_weight, weights[e]);
  // }
  // return min_weight;
}

void testcase()
{
  int n; cin >> n;
  int c; cin >> c;
  int f; cin >> f;
  graph G(n);
  vector<unordered_set<string>> characteristics(n);
  for(int i = 0; i < n; i++) {
    characteristics[i] = unordered_set<string>(0);
    for(int k = 0; k < c; k++) {
      string s; cin >> s;
      characteristics[i].insert(s);
    }
    for(int j = 0; j < i; j++) {
      int common = 0;
      for(auto s : characteristics[j]) {
        if(characteristics[i].count(s) > 0) common++;
      }
      // cout << i << " " << j << " " << common << " ";
      if(common > f) {
        // cout << i << " " << j << endl;
        boost::add_edge(i, j, G);
      }
      // cout << e << " " << weights[e] << endl;
    }
  }

  int matching_size = maximum_matching(G);
  // std::cout << matching_size << endl;
  if(matching_size == n / 2) {
    cout << "not optimal" << endl;
  } else {
    cout << "optimal" << endl;
  }
}

int main() {
  std::ios_base::sync_with_stdio(false); // Always!
  int t; cin >> t;
  while(t--) testcase();
}


#define __USE_MATH_DEFINES
#include <cmath>
#include <Eigen/Dense>
#include "../matplotlibcpp.h"
namespace plt = matplotlibcpp;

void waves(const unsigned n) {
  Eigen::MatrixXd X(n, n);
  for (unsigned i = 0; i < n; ++i) {
    for (unsigned j = 0; j < n; ++j) {
      X(i, j) = sin(3.0 * M_PI * i / n) * cos(20.0 * M_PI * j / n);
    }
  }
  plt::figure();
  plt::imshow(X, {{"cmap", "Spectral"}});
  plt::colorbar();
  plt::show();
}

int main() {
  waves(200);
  return 0;
}


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

using boost::multiprecision::cpp_int;

std::vector<cpp_int> take_input();

struct linear_combination {
    cpp_int quotient = 0;
    cpp_int x = 0;
    cpp_int y = 0;
};

void euclidean_gcd(cpp_int a, cpp_int b, std::vector<linear_combination> &combinations);

cpp_int
solve_congruence_system(cpp_int a, cpp_int m, cpp_int b, cpp_int n);

int main() {
    std::vector<cpp_int> input = take_input();

    cpp_int x = 0;
    cpp_int product = 1;
    std::cout << input.size() << std::endl;

    for (int i = 0; i < input.size(); i++) {
        std::cout << i << std::endl;
        cpp_int &m = input.at(i);
        x = solve_congruence_system(x, product, m - i, m);
        product *= m;
        while (x > product) {
            x -= product;
        }
        while (x < 0) {
            x += product;
        }
        std::cout << x % 17 << std::endl;
    }

    std::cout << x << std::endl;

//    100 - 6 * 15 = 10
//    15 - 1 * (100 - 6 * 15) = 5
//    15 + 6 * 15
    return 0;
}

void euclidean_gcd(cpp_int a, cpp_int b, std::vector<linear_combination> &combinations) {
    if (b % a == 0) {
        return;
    } else {
        linear_combination temp;
        temp.quotient = b / a;
        if (combinations.empty()) {
            temp.x = -temp.quotient;
            temp.y = 1;
        } else if (combinations.size() == 1) {
            temp.x = 1 + combinations.at(0).quotient * temp.quotient;
            temp.y = -temp.quotient;
        } else {
            temp.x = combinations.at(combinations.size() - 2).x -
                     temp.quotient * (combinations.at(combinations.size() - 1).x);
            temp.y = combinations.at(combinations.size() - 2).y -
                     temp.quotient * (combinations.at(combinations.size() - 1).y);
        }
        combinations.push_back(temp);
        euclidean_gcd(b % a, a, combinations);
        return;
    }
}

cpp_int solve_congruence_system(cpp_int a, cpp_int m, cpp_int b, cpp_int n) {

    std::vector<linear_combination> combinations;
    euclidean_gcd(m, n, combinations);

    cpp_int s = 0;
    cpp_int t = 0;
    if (!combinations.empty()) {
        s = combinations.at(combinations.size() - 1).x;
        t = combinations.at(combinations.size() - 1).y;
    }

    cpp_int x = b;
    while (x > m * n) {
        x -= m * n;
    }
    x *= m;
    while (x > m * n) {
        x -= m * n;
    }
    x *= s;
    while (x > m * n) {
        x -= m * n;
    }

    cpp_int y = a;
    while (y > m * n) {
        y -= m * n;
    }
    y *= n;
    while (y > m * n) {
        y -= m * n;
    }
    y *= t;
    y -= (y - m * n) / (m * n) * (m * n);
    while (y > m * n) {
        y -= m * n;
    }

    cpp_int z = x + y;
    while (z < 0) {
        z += m * n;
    }

    return x + y;
}

std::vector<cpp_int> take_input() {
    std::string line;
    std::string number;
    std::vector<cpp_int> numbers;
    std::cin.ignore(10000, '\n');
    std::getline(std::cin, line);

    for (char c : line) {
        if (c != ',') {
            number.append(1, c);
        } else {
            if (number == "x") {
                numbers.push_back(1);
            } else {
                numbers.push_back(stoi(number));
            }
            number = "";
        }
    }

    if (number == "x") {
        numbers.push_back(1);
    } else {
        numbers.push_back(stoi(number));
    }

    return numbers;
}

// 5 = 1 * 3 + 2
// 3 = 1 * 2 + 1
// 2 = 1 * 1 + 1
// 1 = 1 * 1


#include <boost/numeric/ublas/matrix.hpp>
#include <iostream>
#include <boost/numeric/ublas/io.hpp>

typedef boost::numeric::ublas::matrix<float> Matrix44;

int main()
{
	Matrix44 matrix(4, 4);

	for (int i = 0; i < 4; ++i) {
		for (int j = 0; j < 4; ++j) {
			matrix(i, j) = i * 4 + j;
		}
	}

	std::cout << matrix << std::endl;

	return 0;
}


/**
 * @file   quaternion_algebra.hpp
 * @author Paul Furgale <paul.furgale@utoronto.ca>
 * @date   Sun Nov 21 19:20:37 2010
 *
 * @brief  Quaternion algebra from the paper Barfoot T D, Forbes J R, and Furgale P T. “Pose Estimation using Linearized
 * Rotations and Quaternion Algebra”. Acta Astronautica, 2010. doi:10.1016/j.actaastro.2010.06.049.
 *
 *
 */

#ifndef SM_QUATERNION_ALGEBRA_HPP
#define SM_QUATERNION_ALGEBRA_HPP
#include <Eigen/Core>
#include <Eigen/Geometry>

namespace sm {
namespace kinematics {

Eigen::Matrix3d quat2r(Eigen::Vector4d const& q);
Eigen::Vector4d r2quat(Eigen::Matrix3d const& C);
Eigen::Vector4d r2quat(Eigen::Matrix3d const& C);
Eigen::Vector4d axisAngle2quat(Eigen::Vector3d const& a);

template <typename Scalar_>
Eigen::Matrix<Scalar_, 3, 1> quat2AxisAngle(Eigen::Matrix<Scalar_, 4, 1> const& q);
extern template Eigen::Matrix<double, 3, 1> quat2AxisAngle(Eigen::Matrix<double, 4, 1> const& q);
extern template Eigen::Matrix<float, 3, 1> quat2AxisAngle(Eigen::Matrix<float, 4, 1> const& q);
inline Eigen::Vector3d quat2AxisAngle(Eigen::Vector4d const& q) { return quat2AxisAngle<>(q); }

Eigen::Matrix4d quatPlus(Eigen::Vector4d const& q);
Eigen::Vector4d qplus(Eigen::Vector4d const& q, Eigen::Vector4d const& p);
Eigen::Matrix4d quatOPlus(Eigen::Vector4d const& q);
Eigen::Vector4d qoplus(Eigen::Vector4d const& q, Eigen::Vector4d const& p);
Eigen::Vector4d quatInv(Eigen::Vector4d const& q);
Eigen::Vector3d quatRotate(Eigen::Vector4d const& q_a_b, Eigen::Vector3d const& v_b);
Eigen::Vector4d quatRandom();
Eigen::Vector4d quatIdentity();
void invertQuat(Eigen::Vector4d& q);
Eigen::Vector3d qeps(Eigen::Vector4d const& q);
double qeta(Eigen::Vector4d const& q);
// For estimation functions to handle a constraint-sensitive minimal parameterization for a quaternion update
Eigen::Matrix<double, 4, 3> quatJacobian(Eigen::Vector4d const& q);
Eigen::Vector4d updateQuat(Eigen::Vector4d const& q, Eigen::Vector3d const& dq);
Eigen::Matrix<double, 3, 4> quatS(Eigen::Vector4d q);
Eigen::Matrix<double, 4, 3> quatInvS(Eigen::Vector4d q);

inline Eigen::Vector3d qlog(const Eigen::Vector4d& q) { return quat2AxisAngle(q); }
inline Eigen::Vector4d qexp(const Eigen::Vector3d& theta) { return axisAngle2quat(theta); }

/// \brief do spherical linear interpolation between q0 and q1 for times t = [0.0,1.0]
Eigen::Vector4d qslerp(const Eigen::Vector4d& q0, const Eigen::Vector4d& q1, double t);

/// \brief do linear interpolation between p0 and p1 for times t = [0.0,1.0]
Eigen::VectorXd lerp(const Eigen::VectorXd& p0, const Eigen::VectorXd& p1, double t);

/// \brief Jacobian of the quat log function evaluated at p
Eigen::Matrix<double, 3, 4> quatLogJacobian(const Eigen::Vector4d& p);

/// \brief Jacobian of the quat exp function evaluated at vec
template <typename Scalar_ = double>
Eigen::Matrix<Scalar_, 4, 3> quatExpJacobian(const Eigen::Matrix<Scalar_, 3, 1>& vec);

template <typename Scalar_ = double>
Eigen::Matrix<Scalar_, 3, 4> quatLogJacobian2(const Eigen::Matrix<Scalar_, 4, 1>& p);

template <typename Scalar_ = double>
const Eigen::Matrix<Scalar_, 4, 3>& quatV();

template <typename Scalar_ = double>
Eigen::Matrix<Scalar_, 3, 3> logDiffMat(const Eigen::Matrix<Scalar_, 3, 1>& vec);

template <typename Scalar_ = double>
Eigen::Matrix<Scalar_, 3, 3> expDiffMat(const Eigen::Matrix<Scalar_, 3, 1>& vec);

extern template const Eigen::Matrix<double, 4, 3>& quatV();
extern template const Eigen::Matrix<float, 4, 3>& quatV();
extern template Eigen::Matrix<double, 4, 3> quatExpJacobian(const Eigen::Matrix<double, 3, 1>& vec);
extern template Eigen::Matrix<float, 4, 3> quatExpJacobian(const Eigen::Matrix<float, 3, 1>& vec);
extern template Eigen::Matrix<double, 3, 4> quatLogJacobian2(const Eigen::Matrix<double, 4, 1>& p);
extern template Eigen::Matrix<float, 3, 4> quatLogJacobian2(const Eigen::Matrix<float, 4, 1>& p);
extern template Eigen::Matrix<double, 3, 3> logDiffMat(const Eigen::Matrix<double, 3, 1>& vec);
extern template Eigen::Matrix<float, 3, 3> logDiffMat(const Eigen::Matrix<float, 3, 1>& vec);
extern template Eigen::Matrix<double, 3, 3> expDiffMat(const Eigen::Matrix<double, 3, 1>& vec);
extern template Eigen::Matrix<float, 3, 3> expDiffMat(const Eigen::Matrix<float, 3, 1>& vec);
}  // namespace kinematics
}  // namespace sm

#endif /* SM_QUATERNION_ALGEBRA_HPP */


#pragma once

#include <vector>
#include <cassert>

#include <iostream>
#include <iomanip>

#include <Eigen/Dense>

//! \file rkintegrator.hpp Solution for Problem 1a, implementing RkIntegrator class

//! \brief Implements a Runge-Kutta explicit solver for a given Butcher tableau for autonomous ODEs
//! \tparam State a type representing the space in which the solution lies, e.g. R^d, represented by e.g. Eigen::VectorXd.
template <class State>
class RKIntegrator {
public:
    //! \brief Constructor for the RK method.
    //! Performs size checks and copies A and b into internal storage
    //! \param[in] A matrix containing coefficents of Butcher tableau, must be (strictly) lower triangular (no check)
    //! \param[in] b vector containing coefficients of lower part of Butcher tableau
    RKIntegrator(const Eigen::MatrixXd & A, const Eigen::VectorXd & b) {
        // TODO: implement size checks and initialize internal data
    }

    //! \brief Perform the solution of the ODE
    //! Solve an autonomous ODE y' = f(y), y(0) = y0, using a RK scheme given in the Butcher tableau provided in the
    //! constructor. Performs N equidistant steps upto time T with initial data y0
    //! \tparam Function type for function implementing the rhs function. Must have State operator()(State x)
    //! \param[in] f function handle for rhs in y' = f(y), e.g. implemented using lambda funciton
    //! \param[in] T final time T
    //! \param[in] y0 initial data y(0) = y0 for y' = f(y)
    //! \param[in] N number of steps to perform. Step size is h = T / N. Steps are equidistant.
    //! \return vector containing all steps y^n (for each n) including initial and final value
    template <class Function>
    std::vector<State> solve(const Function &f, double T, const State & y0, unsigned int N) const {
        // TODO: implement solver from 0 to T, calling function step appropriately
    }

private:

    //! \brief Perform a single step of the RK method for the solution of the autonomous ODE
    //! Compute a single explicit RK step y^{n+1} = y_n + \sum ... starting from value y0 and storing next value in y1
    //! \tparam Function type for function implementing the rhs. Must have State operator()(State x)
    //! \param[in] f function handle for ths f, s.t. y' = f(y)
    //! \param[in] h step size
    //! \param[in] y0 initial state
    //! \param[out] y1 next step y^{n+1} = y^n + ...
    template <class Function>
    void step(const Function &f, double h, const State & y0, State & y1) const {
        // TODO: implement a single step of the RK method using provided Butcher scheme
    }

    //! TODO: put here suitable internal data storage
};

Nummerical Methods for CSE/PS12/solutions_ps12/rkintegrator_template.hpp

Nummerical Methods for CSE/PS12/templates_ps12/rkintegrator_template.hpp


/* Copyright (c) 2012, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See LICENSE.txt or
 * http://www.opensource.org/licenses/mit-license.php */

#pragma once

#include <boost/random/mersenne_twister.hpp>
//#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/gamma_distribution.hpp>

#include <armadillo>
#include <time.h>

using namespace arma;

class GammaRnd
{
public:
  GammaRnd(double alpha, double beta) // alpha = shape; beta = scale
    : mGen(time(0)),mAlpha(alpha), mBeta(beta), mGamma(mAlpha)
  {};

  double draw(void)
  {
    return mBeta*mGamma(mGen);
  };
  void draw(Col<double>& c)
  {
    for (uint32_t i=0; i<c.n_elem; ++i)
      c(i)=draw();
  };

  void draw(Row<double>& c)
  {
    for (uint32_t i=0; i<c.n_elem; ++i)
      c(i)=draw();
  };

private:
  boost::mt19937 mGen;
  double mAlpha;
  double mBeta;
  boost::gamma_distribution<> mGamma;
};

class RandInt
{
public:
  RandInt(uint32_t limLower, uint32_t limUpper)
    : mGen(time(0)),  mDist(limLower,limUpper-1) // so we generate numbers in the range( upper - lower)
  {};

  uint32_t draw(void)
  {
    return mDist(mGen);
  };

  void draw(Col<uint32_t>& c)
  {
    for (uint32_t i=0; i<c.n_rows; ++i)
    {
      c(i)=mDist(mGen);
    }
  }
  Col<uint32_t> draw(uint32_t N)
  {
    Col<uint32_t> c(N);
    draw(c);
    return c;
  }

private:
  boost::mt19937 mGen;
  boost::uniform_int<> mDist;
};

class RandDisc
{
public:
  RandDisc() : mGen(time(0))
  { };

  double draw(void)
  {
    return mDist(mGen);
  };

  uint32_t draw(const Col<double>& pdf)
  {
    Col<double> cdf=cumsum(pdf);
    double r=mDist(mGen);
    for (uint32_t i=0; i<pdf.n_rows; ++i)
      if (r<cdf(i)){return i;}
    return pdf.n_rows-1;
  };

private:
  Col<double> mPdf;
  boost::mt19937 mGen;
  boost::uniform_01<> mDist;
};

uint32_t sampleDiscLogProb(RandDisc& rndDisc, colvec l)
{
  //    cout<<"max(l)="<<l.max()<<" min(l)="<<l.min()<<endl;
  double lmax=l.max();
  double lmin=l.min();
  for(uint32_t i=0; i<l.n_elem; ++i)
    if(!is_finite(l(i)))
      l(i)=0.0;
    else
      l(i)=exp(l(i) + (lmax - lmin)*0.5);
  //    cout<<"l(exp) ="<<l.t()<<endl;
  //    cout<<"l(norm) ="<<l.t()/sum(l)<<endl;
  return rndDisc.draw(l/sum(l));
};


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

#include "matrix_gemm.h"
#include "common/pixel_benchmark.h"
#include "common/pixel_check.h"

#include <opencv2/opencv.hpp>
#include <Eigen/Dense>

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

typedef struct Matrix {
    size_t rows;
    size_t cols;
    float* data;
} Matrix;

/*
1 2     1  2    7  10
3 4     3  4    15 22
*/

static void print_matrix(float* data, uint32_t rows, uint32_t cols) {
    size_t idx=0;
    for (uint32_t i=0; i<rows; i++){
        for (uint32_t j=0; j<cols; j++) {
            printf("%.2f, ", data[idx]);
            idx++;
        }
        printf("\n");
    }
}

static void matrix_gemm_f32_opencv(float* mA, float* mB, float* mC, const uint32_t M, const uint32_t K, const uint32_t N)
{
    cv::Mat matA = cv::Mat(M, K, CV_32FC1, mA);
    cv::Mat matB = cv::Mat(K, N, CV_32FC1, mB);
    cv::Mat matC = cv::Mat(M, N, CV_32FC1, mC);
    matC = matA * matB;
    // cout << "matA:\n" << matA << endl;
    // cout << "matB:\n" << matB << endl;
    // cout << "matC:\n" << matC << endl;
}

static void matrix_gemm_f32_eigen(float* mA, float* mB, float* mC, const uint32_t M, const uint32_t K, const uint32_t N)
{
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> eA(mA, M, K);
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> eB(mB, K, N);
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> eC(mC, M, N);
    eC = eA * eB;
    // cout << "eA:\n" << eA << endl;
    // cout << "eB:\n" << eB << endl;
    // cout << "eC:\n" << eC << endl;
}

static void gemm_tiny_debug()
{
    uint32_t m = 3;
    uint32_t k = 2;
    uint32_t n = 4;

    Matrix mA;
    mA.rows = m;
    mA.cols = k;

    Matrix mB;
    mB.rows = k;
    mB.cols = n;

    mA.data = (float*)malloc(mA.rows*mA.cols * sizeof(float));
    mB.data = (float*)malloc(mB.rows*mB.cols * sizeof(float));

    mA.data[0] = 2;
    mA.data[1] = -6;
    mA.data[2] = 3;
    mA.data[3] = 5;
    mA.data[4] = 1;
    mA.data[5] = -1;

    mB.data[0] = 4;
    mB.data[1] = -2;
    mB.data[2] = -4;
    mB.data[3] = -5;
    mB.data[4] = -7;
    mB.data[5] = -3;
    mB.data[6] = 6;
    mB.data[7] = 7;

    Matrix mC;
    mC.rows = m;
    mC.cols = n;
    mC.data = (float*)malloc(mC.rows*mC.cols * sizeof(float));

    matrix_gemm_f32_order_opt(mA.data, mB.data, mC.data, m, k, n);

    printf("--- matrix A:\n");
    print_matrix(mA.data, mA.rows, mA.cols);

    printf("--- matrix B:\n");
    print_matrix(mB.data, mB.rows, mB.cols);

    printf("--- matrix C:\n");
    print_matrix(mC.data, mC.rows, mC.cols);

}

static void prepare_big(uint32_t& M, uint32_t& K, uint32_t& N, cv::Mat& matA, cv::Mat& matB)
{
    cv::Mat image = cv::imread("colorhouse.png");

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

    cv::Size transposed_size;
    transposed_size.height = size.width;
    transposed_size.width = size.height;

    std::vector<cv::Mat> channels;
    cv::split(image, channels);
    cv::Mat b_channels = channels[0];
    cv::Mat g_channels = channels[1];
    cv::Mat r_channels = channels[2];

    // matA's dim: height * width
    b_channels.convertTo(matA, CV_32FC1);

    matB = b_channels.t(); // matB's dim: width * height
    matB.convertTo(matB, CV_32FC1);

    M = height;
    K = width;
    N = height;
}

static void prepare_small(uint32_t& M, uint32_t& K, uint32_t& N, cv::Mat& matA, cv::Mat& matB)
{
    M = 2;
    K = 3;
    N = 2;

    /*
    0  1  2
    1  2  3
    */
    float* data = NULL;
    matA = cv::Mat(M, K, CV_32FC1);
    data = (float*)matA.data;
    data[0] = 1.2;
    data[1] = 2.3;
    data[2] = 3.4;
    data[3] = 4.5;
    data[4] = 5.6;
    data[5] = 6.7;

    cout << "matA:" << endl << matA << endl;

    /*
    1 2
    3 4
    5 6
    */
    matB = cv::Mat(K, N, CV_32FC1);
    data = (float*)matB.data;
    data[0] = 1.1;
    data[1] = 2.2;
    data[2] = 3.3;
    data[3] = 4.5;
    data[4] = 5.5;
    data[5] = 6.6;

    cout << "matB:" << endl << matB << endl;
}

static void gemm_f32_test() {
    //--------------------------------
    cv::Mat matA;
    cv::Mat matB;
    uint32_t M, K, N;
    prepare_big(M, K, N, matA, matB);
    //prepare_small(M, K, N, matA, matB);

    printf("-- after prepare_small:\n");
    // cout << "matA:" << endl << matA << endl;
    // cout << "matB:" << endl << matB << endl;

    float* mA = (float*)matA.data;
    float* mB = (float*)matB.data;

    size_t buf_size = M*N * sizeof(float);
    float* mC_naive = (float*)malloc(buf_size);
    float* mC_eigen = (float*)malloc(buf_size);
    float* mC_opencv = (float*)malloc(buf_size);
    float* mC_order_opt = (float*)malloc(buf_size);
    float* mC_order_opt2 = (float*)malloc(buf_size);
    float* mC_asimd = (float*)malloc(buf_size);
    for(uint32_t i=0; i<M*N; i++) {
        mC_naive[i] = 0;
        mC_eigen[i] = 0;
        mC_opencv[i] = 0;
        mC_order_opt[i] = 0;
        mC_order_opt2[i] = 0;
        mC_asimd[i] = 0;
    }

    double t_start, t_cost;

    // eigen
    t_start = pixel_get_current_time();
    matrix_gemm_f32_eigen(mA, mB, mC_eigen, M, K, N);
    t_cost = pixel_get_current_time() - t_start;
    printf("matrix gemm, eigen,     time cost %.2lf ms\n", t_cost);

    // opencv
    t_start = pixel_get_current_time();
    matrix_gemm_f32_opencv(mA, mB, mC_opencv, M, K, N);
    t_cost = pixel_get_current_time() - t_start;
    printf("matrix gemm, opencv,    time cost %.2lf ms\n", t_cost);

    // naive
    t_start = pixel_get_current_time();
    matrix_gemm_f32_naive(mA, mB, mC_naive, M, K, N);
    t_cost = pixel_get_current_time() - t_start;
    printf("matrix gemm, naive,     time cost %.2lf ms\n", t_cost);

    // order opt 调整维度顺序
    t_start = pixel_get_current_time();
    matrix_gemm_f32_order_opt(mA, mB, mC_order_opt, M, K, N);
    t_cost = pixel_get_current_time() - t_start;
    printf("matrix gemm, order opt, time cost %.2lf ms\n", t_cost);

    // order opt2 调整维度顺序2
    t_start = pixel_get_current_time();
    matrix_gemm_f32_order_opt2(mA, mB, mC_order_opt2, M, K, N);
    t_cost = pixel_get_current_time() - t_start;
    printf("matrix gemm, order opt2,time cost %.2lf ms\n", t_cost);

    // asimd
    t_start = pixel_get_current_time();
    matrix_gemm_f32_asimd(mA, mB, mC_asimd, M, K, N);
    t_cost = pixel_get_current_time() - t_start;
    printf("matrix gemm, asimd,     time cost %.2lf ms\n", t_cost);

    // validate if different implementations result match
    int mis_eigen = 0;
    int mis_opencv = 0;
    int mis_order_opt = 0;
    int mis_order_opt2 = 0;
    int mis_asimd = 0;
    uint32_t len = M * N;

    //float* mC_gt = mC_order_opt;
    float* mC_gt = mC_naive;
    float epsilon = 1e-3;
    for (uint32_t i=0; i<len; i++) {
        if (!nearly_equal_absolutely(mC_gt[i], mC_eigen[i], epsilon)) {
            mis_eigen++;
        }
        if (!nearly_equal_absolutely(mC_gt[i], mC_opencv[i], epsilon)) {
            mis_opencv++;
        }
        if (!nearly_equal_absolutely(mC_gt[i], mC_order_opt[i], epsilon)) {
            mis_order_opt++;
        }
        if (!nearly_equal_absolutely(mC_gt[i], mC_order_opt2[i], epsilon)) {
            mis_order_opt2++;
        }
        if (!nearly_equal_absolutely(mC_gt[i], mC_asimd[i], epsilon)) {
            mis_asimd++;
        }
    }

    printf("mis_eigen=%d, mis_opencv=%d, mis_order_opt=%d, mis_order_opt2=%d, mis_asimd=%d\n",
        mis_eigen, mis_opencv, mis_order_opt, mis_order_opt2, mis_asimd);

    FILE* fout = NULL;

    fout = fopen("opencv.txt", "w");
    for (uint32_t i=0; i<len; i++) {
        fprintf(fout, "%f\n", mC_opencv[i]);
    }
    fclose(fout);

    fout = fopen("eigen.txt", "w");
    for (uint32_t i=0; i<len; i++) {
        fprintf(fout, "%f\n", mC_eigen[i]);
    }
    fclose(fout);

    fout = fopen("order_opt.txt", "w");
    for (uint32_t i=0; i<len; i++) {
        fprintf(fout, "%f\n", mC_order_opt[i]);
    }
    fclose(fout);

    fout = fopen("naive.txt", "w");
    for (uint32_t i=0; i<len; i++) {
        fprintf(fout, "%f\n", mC_naive[i]);
    }
    fclose(fout);
}

int main() {
    //gemm_tiny_debug();
    gemm_f32_test();

    return 0;
}


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

  This source file is part of the Avogadro project.

  Copyright (C) 2010 Eric C. Brown

  This source code is released under the New BSD License, (the "License").

  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 "qtaimmathutilities.h"

#include <Eigen/Eigenvalues>
#include <Eigen/QR>
#include <cmath>

namespace Avogadro {
namespace QtPlugins {
namespace QTAIMMathUtilities {

Matrix<qreal, 3, 1> eigenvaluesOfASymmetricThreeByThreeMatrix(
  const Matrix<qreal, 3, 3>& A)
{
  SelfAdjointEigenSolver<Matrix<qreal, 3, 3>> eigensolver(A);
  return eigensolver.eigenvalues();
}

Matrix<qreal, 3, 3> eigenvectorsOfASymmetricThreeByThreeMatrix(
  const Matrix<qreal, 3, 3>& A)
{
  SelfAdjointEigenSolver<Matrix<qreal, 3, 3>> eigensolver(A);
  return eigensolver.eigenvectors();
}

Matrix<qreal, 4, 1> eigenvaluesOfASymmetricFourByFourMatrix(
  const Matrix<qreal, 4, 4>& A)
{
  SelfAdjointEigenSolver<Matrix<qreal, 4, 4>> eigensolver(A);
  return eigensolver.eigenvalues();
}

Matrix<qreal, 4, 4> eigenvectorsOfASymmetricFourByFourMatrix(
  const Matrix<qreal, 4, 4>& A)
{
  SelfAdjointEigenSolver<Matrix<qreal, 4, 4>> eigensolver(A);
  return eigensolver.eigenvectors();
}

qint64 signOfARealNumber(qreal x)
{
  if (x > 0.)
    return 1;
  else if (x == 0.)
    return 0;
  else
    return -1;
}

qint64 signatureOfASymmetricThreeByThreeMatrix(const Matrix<qreal, 3, 3>& A)
{
  SelfAdjointEigenSolver<Matrix<qreal, 3, 3>> eigensolver(A);
  Matrix<qreal, 3, 1> eigenvalues = eigensolver.eigenvalues();

  return signOfARealNumber(eigenvalues(0)) + signOfARealNumber(eigenvalues(1)) +
         signOfARealNumber(eigenvalues(2));
}

qreal ellipticityOfASymmetricThreeByThreeMatrix(const Matrix<qreal, 3, 3>& A)
{
  SelfAdjointEigenSolver<Matrix<qreal, 3, 3>> eigensolver(A);
  Matrix<qreal, 3, 1> eigenvalues = eigensolver.eigenvalues();

  return (eigenvalues(0) / eigenvalues(1)) - 1.0;
}

qreal distance(const Matrix<qreal, 3, 1>& a, const Matrix<qreal, 3, 1>& b)
{
  return sqrt(pow(a(0) - b(0), 2) + pow(a(1) - b(1), 2) + pow(a(2) - b(2), 2));
}

Matrix<qreal, 3, 1> sphericalToCartesian(const Matrix<qreal, 3, 1>& rtp,
                                         const Matrix<qreal, 3, 1>& x0y0z0)
{
  qreal r = rtp(0);
  qreal theta = rtp(1);
  qreal phi = rtp(2);

  qreal x0 = x0y0z0(0);
  qreal y0 = x0y0z0(1);
  qreal z0 = x0y0z0(2);

  qreal costheta = cos(theta);
  qreal cosphi = cos(phi);
  qreal sintheta = sin(theta);
  qreal sinphi = sin(phi);

  Matrix<qreal, 3, 1> xyz(r * cosphi * sintheta + x0,
                          r * sintheta * sinphi + y0, r * costheta + z0);

  return xyz;
}

Matrix<qreal, 3, 1> sphericalToCartesian(const Matrix<qreal, 3, 1>& rtp)
{
  Matrix<qreal, 3, 1> x0y0z0(0., 0., 0.);

  return sphericalToCartesian(rtp, x0y0z0);
}

Matrix<qreal, 3, 1> cartesianToSpherical(const Matrix<qreal, 3, 1>& xyz,
                                         const Matrix<qreal, 3, 1>& x0y0z0)
{
  qreal x = xyz(0);
  qreal y = xyz(1);
  qreal z = xyz(2);

  qreal x0 = x0y0z0(0);
  qreal y0 = x0y0z0(1);
  qreal z0 = x0y0z0(2);

  qreal xshift = x - x0;
  qreal yshift = y - y0;
  qreal zshift = z - z0;

  qreal length = sqrt(pow(xshift, 2) + pow(yshift, 2) + pow(zshift, 2));

  Matrix<qreal, 3, 1> rtp;

  if (length == 0.)
    rtp << x0, y0, z0;
  else if (xshift == 0. && yshift == 0.)
    rtp << length, acos(zshift / length), 0.;
  else
    rtp << length, acos(zshift / length), atan2(xshift, yshift);

  return rtp;
}

Matrix<qreal, 3, 1> cartesianToSpherical(const Matrix<qreal, 3, 1>& xyz)
{
  Matrix<qreal, 3, 1> x0y0z0(0., 0., 0.);

  return cartesianToSpherical(xyz, x0y0z0);
}

// Cerjan-Miller-Baker-Popelier Methods
//
// Based on:
// Popelier, P.L.A. Comput. Phys. Comm. 1996, 93, 212.

Matrix<qreal, 3, 1> minusThreeSignatureLocatorGradient(
  const Matrix<qreal, 3, 1>& g, const Matrix<qreal, 3, 3>& H)
{
  Matrix<qreal, 3, 1> value;

  Matrix<qreal, 3, 1> b = eigenvaluesOfASymmetricThreeByThreeMatrix(H);
  Matrix<qreal, 3, 3> U = eigenvectorsOfASymmetricThreeByThreeMatrix(H);

  Matrix<qreal, 3, 1> F = U.transpose() * g;

  Matrix<qreal, 4, 4> A;
  A << b(0), 0., 0., F(0), 0., b(1), 0., F(1), 0., 0., b(2), F(2), F(0), F(1),
    F(2), 0.;

  Matrix<qreal, 4, 1> eval = eigenvaluesOfASymmetricFourByFourMatrix(A);

  Matrix<qreal, 3, 1> lambda;
  lambda << eval(3), eval(3), eval(3);

  Matrix<qreal, 3, 1> denom;
  denom = b - lambda;

  for (qint64 i = 0; i < 3; ++i)
    if (denom(i) < SMALL)
      denom(i) = denom(i) + SMALL;

  Matrix<qreal, 3, 1> h;
  h << 0., 0., 0.;

  for (qint64 j = 0; j < 3; ++j)
    for (qint64 i = 0; i < 3; ++i)
      h(j) = h(j) + (-F(i) * U(j, i)) / denom(i);

  value = h;

  return value;
}

Matrix<qreal, 3, 1> minusOneSignatureLocatorGradient(
  const Matrix<qreal, 3, 1>& g, const Matrix<qreal, 3, 3>& H)
{
  Matrix<qreal, 3, 1> value;

  Matrix<qreal, 3, 1> b = eigenvaluesOfASymmetricThreeByThreeMatrix(H);
  Matrix<qreal, 3, 3> U = eigenvectorsOfASymmetricThreeByThreeMatrix(H);

  Matrix<qreal, 3, 1> F = U.transpose() * g;

  Matrix<qreal, 3, 3> A;
  A << b(0), 0., F(0), 0., b(1), F(1), F(0), F(1), 0.;

  Matrix<qreal, 3, 1> eval = eigenvaluesOfASymmetricThreeByThreeMatrix(A);

  Matrix<qreal, 3, 1> lambda;
  lambda << eval(2), eval(2),
    (0.5) * (b(2) - sqrt(pow(b(2), 2) + 4.0 * pow(F(2), 2)));

  Matrix<qreal, 3, 1> denom;
  denom = b - lambda;

  for (qint64 i = 0; i < 3; ++i)
    if (denom(i) < SMALL)
      denom(i) = denom(i) + SMALL;

  Matrix<qreal, 3, 1> h;
  h << 0., 0., 0.;

  for (qint64 j = 0; j < 3; ++j)
    for (qint64 i = 0; i < 3; ++i)
      h(j) = h(j) + (-F(i) * U(j, i)) / denom(i);

  value = h;

  return value;
}

Matrix<qreal, 3, 1> plusOneSignatureLocatorGradient(
  const Matrix<qreal, 3, 1>& g, const Matrix<qreal, 3, 3>& H)
{
  Matrix<qreal, 3, 1> value;

  Matrix<qreal, 3, 1> b = eigenvaluesOfASymmetricThreeByThreeMatrix(H);
  Matrix<qreal, 3, 3> U = eigenvectorsOfASymmetricThreeByThreeMatrix(H);

  Matrix<qreal, 3, 1> F = U * g;

  Matrix<qreal, 3, 3> A;
  A << b(1), 0., F(1), 0., b(2), F(2), F(1), F(2), 0.;

  Matrix<qreal, 3, 1> eval = eigenvaluesOfASymmetricThreeByThreeMatrix(A);

  Matrix<qreal, 3, 1> lambda;
  lambda << eval(2), eval(2),
    (0.5) * (b(0) + sqrt(pow(b(0), 2) + 4.0 * pow(F(0), 2)));

  Matrix<qreal, 3, 1> denom;
  denom = b - lambda;

  for (qint64 i = 0; i < 3; ++i)
    if (denom(i) < SMALL)
      denom(i) = denom(i) + SMALL;

  Matrix<qreal, 3, 1> h;
  h << 0., 0., 0.;

  for (qint64 j = 0; j < 3; ++j)
    for (qint64 i = 0; i < 3; ++i)
      h(j) = h(j) + (-F(i) * U(i, j)) / denom(i);

  value = h;

  return value;
}

Matrix<qreal, 3, 1> plusThreeSignatureLocatorGradient(
  const Matrix<qreal, 3, 1>& g, const Matrix<qreal, 3, 3>& H)
{
  Matrix<qreal, 3, 1> value;

  Matrix<qreal, 3, 1> b = eigenvaluesOfASymmetricThreeByThreeMatrix(H);
  Matrix<qreal, 3, 3> U = eigenvectorsOfASymmetricThreeByThreeMatrix(H);

  Matrix<qreal, 3, 1> F = U * g;

  Matrix<qreal, 4, 4> A;
  A << b(0), 0., 0., F(0), 0., b(1), 0., F(1), 0., 0., b(2), F(2), F(0), F(1),
    F(2), 0.;

  Matrix<qreal, 4, 1> eval = eigenvaluesOfASymmetricFourByFourMatrix(A);

  Matrix<qreal, 3, 1> lambda;
  lambda << eval(0), eval(0), eval(0);

  Matrix<qreal, 3, 1> denom;
  denom = b - lambda;

  for (qint64 i = 0; i < 3; ++i)
    if (denom(i) < SMALL)
      denom(i) = denom(i) + SMALL;

  Matrix<qreal, 3, 1> h;
  h << 0., 0., 0.;

  for (qint64 j = 0; j < 3; ++j)
    for (qint64 i = 0; i < 3; ++i)
      h(j) = h(j) + (-F(i) * U(i, j)) / denom(i);

  value = h;

  return value;
}

} // namespace QTAIMMathUtilities
} // namespace QtPlugins
} // namespace Avogadro


#include <math.h>
#include <Eigen/Geometry>

#include "stiffness_checker/Util.h"

namespace conmech
{
namespace stiffness_checker
{

void createLocalStiffnessMatrix(const double &L, const double &A, const int &dim,
                                const double &Jx, const double &Iy, const double &Iz,
                                const double &E, const double &G, const double &mu,
                                Eigen::MatrixXd &K_eL)
{
  // TODO: add 2D case
  assert(3 == dim);

  switch(dim)
  {
    case 2:
      assert(false && "2D local stiffness not implemented.");
      return;
    case 3:
    {
      K_eL = Eigen::MatrixXd::Zero(12,12);

      // see: [Matrix Structural Analysis, McGuire et al., 2rd edition]
      // P73 - eq(4.34)
      Eigen::MatrixXd K_block(6,6);
      K_block.setZero();
      Eigen::VectorXd diag(6);

      // block_00 and block_11
      K_block(1,5) = 6*Iz / std::pow(L,2);
      K_block(2,4) = - 6*Iy / std::pow(L,2);
      K_block = K_block.eval() +  K_block.transpose().eval();
      K_eL.block<6,6>(0,0) = K_block;
      K_eL.block<6,6>(6,6) = -K_block;

      diag[0] = A/L;
      diag[1] = 12*Iz / std::pow(L,3);
      diag[2] = 12*Iy / std::pow(L,3);
      diag[3] = Jx / (2*(1+mu)*L);
      diag[4] = 4*Iy / L;
      diag[5] = 4*Iz / L;
      K_eL.block<6,6>(0,0) += Eigen::MatrixXd(diag.asDiagonal());
      K_eL.block<6,6>(6,6) += Eigen::MatrixXd(diag.asDiagonal());

      // block_01 and block_10
      K_block.setZero();

      K_block(1,5) = 6*Iz / std::pow(L,2);
      K_block(2,4) = - 6*Iy / std::pow(L,2);
      K_block = K_block.eval() - K_block.transpose().eval();
      K_eL.block<6,6>(0,6) = K_block;
      K_eL.block<6,6>(6,0) = -K_block;

      diag[0] = -A/L;
      diag[1] = -12*Iz / std::pow(L,3);
      diag[2] = -12*Iy / std::pow(L,3);
      diag[3] = -Jx / (2*(1+mu)*L);
      diag[4] = 2*Iy / L;
      diag[5] = 2*Iz / L;
      K_eL.block<6,6>(0,6) += Eigen::MatrixXd(diag.asDiagonal());
      K_eL.block<6,6>(6,0) += Eigen::MatrixXd(diag.asDiagonal());

      K_eL *= E;
    }
  }
}

/**
 * @brief Get the Global to Local Rotation Matrix object
 * Calculates a 3x3 matrix to tranform the global xyz axis to the element local axis.
 * TODO: add info on the axis convention, we have different conventions with compas_fea (abaqus)
 *
 * The coordinate transformation matrix can be used to:
 *  - transform frame element end forces from the element (local) coordinate system
 *    to the structure (global) coordinate system
 *  - transfrom end displacements from the structural (global) coordinate system
 *    to the element (local) coordinate system,
 *  - transform the frame element stiffness and mass matrices
 *    from element (local) coordinates to structral (global) coordinates.
 * Symbolically, the return matrix R = {local}_R_{global}
 *
 * @param[in] end_vert_u
 * @param[in] end_vert_v
 * @param[out] rot_m 3x3 Eigen matrix, transforming global axis to local coordinate frame
 * @param[in] rot_y2x optional rotation of local y axis around the local x axis, defaults to zero
 */
void getGlobal2LocalRotationMatrix(
    const Eigen::VectorXd & end_vert_u,
    const Eigen::VectorXd & end_vert_v,
    Eigen::Matrix3d& rot_m,
    const double& rot_y2x)
{
  assert(end_vert_u.size() == end_vert_v.size() && "vert dimension not agree!");
  assert(end_vert_u.size() == 2 || end_vert_u.size() == 3);
  int dim = end_vert_u.size();

  // length of the element
  double L = (end_vert_v - end_vert_u).norm();
  // TODO: make tol as a common shared const
  assert(L < 1e6 && "vertices too close, might be duplicated pts.");

  // by convention, the new x axis is along the element's direction
  // directional cosine of the new x axis in the global world frame
  double c_x, c_y;
  c_x = (end_vert_v[0] - end_vert_u[0]) / L;
  c_y = (end_vert_v[1] - end_vert_u[1]) / L;

  Eigen::Matrix3d R = Eigen::Matrix3d::Zero();

  if (3 == dim)
  {
    double c_z = (end_vert_v[2] - end_vert_u[2]) / L;
    auto rot_axis = Eigen::AngleAxisd(rot_y2x, Eigen::Vector3d::UnitZ());

    if (abs(c_z) == 1.0)
    {
      // the element is parallel to global z axis
      // cross product is not defined, in this case
      // it's just a rotation about the global z axis
      // in x-y plane
      R(0, 2) = -c_z;
      R(1, 1) = 1;
      R(2, 0) = c_z;
    }
    else
    {
      // local x_axis = element's vector
      auto new_x = Eigen::Vector3d(c_x, c_y, c_z);

      // local y axis = cross product with global z axis
      Eigen::Vector3d new_y = -new_x.cross(Eigen::Vector3d::UnitZ());
      new_y.normalize();

      auto new_z = new_x.cross(new_y);

      R.block<3, 1>(0, 0) = new_x;
      R.block<3, 1>(0, 1) = new_y;
      R.block<3, 1>(0, 2) = new_z;
    }
    // This is essential!
    R = R * rot_axis;
    rot_m = R.transpose();
  }
  else
  {
    // 2D rotational matrix
    R(0,0) = c_x;
    R(0,1) = c_y;
    R(1,0) = -c_y;
    R(1,1) = c_x;
    R(2,2) = 1;

    auto rot_axis = Eigen::AngleAxisd(rot_y2x, Eigen::Vector3d::UnitZ());
    assert((R - rot_axis.toRotationMatrix()).norm() > 1e-3);

    rot_m = R;
  }
}

void getNodePoints(const Eigen::MatrixXd& Vertices, const int& end_u_id, const int& end_v_id,
  Eigen::VectorXd& end_u, Eigen::VectorXd& end_v)
{
  end_u = Eigen::VectorXd(3);
  end_u << Vertices(end_u_id, 0), Vertices(end_u_id, 1), Vertices(end_u_id, 2);
  end_v = Eigen::VectorXd(3);
  end_v << Vertices(end_v_id, 0), Vertices(end_v_id, 1), Vertices(end_v_id, 2);
}

} // namespace stiffness_checker
} // namespace conmech


#include <iostream>
#include <Eigen/Dense>
#include <vector>
#include <array>
#include <string>
#include <cmath>

#include "../include/input_parser.h"
#include "../include/ply.h"
#include "../include/laminate.h"

using std::cin; using std::cout; using std::endl;
using std::string;
using std::vector;
using Eigen::Matrix; using Eigen::Matrix3d; using Eigen::Vector3d;

// Get the mid-plane strain of the laminate.
void solve_mid_strain(laminate& lam, Matrix<double, 6, 1>& load_vector);

// Get the stresses and strains for each sampling point of the laminate.
void solve_stress_strain_profile(laminate& lam, double pt_spacing);

// Construct laminate from a vector of ply, the input load, and the spacing
// between sampling points.
laminate::laminate(vector<ply>& ply_vector, Matrix<double, 6, 1>& load_vector,
                    double pt_spacing):
        ply_vector_(ply_vector), load_vector_(load_vector) {
    height_ = 0.;
    for (auto it = ply_vector_.begin(); it != ply_vector_.end(); it++) {
        height_ += it->thickness_;
    }

    A_ = Matrix3d::Zero();
    B_ = Matrix3d::Zero();
    D_ = Matrix3d::Zero();
    double bottom_coordinate = -height_/2;
    for (auto it = ply_vector_.begin(); it != ply_vector_.end(); it++) {
        double top_coordinate = bottom_coordinate + it->thickness_;
        A_ = A_ + it->Qbar_ * (top_coordinate - bottom_coordinate);
        B_ = B_ + 1./2 * it->Qbar_
            * (pow(top_coordinate, 2) - pow(bottom_coordinate, 2));
        D_ = D_ + 1./3 * it->Qbar_
            * (pow(top_coordinate, 3) - pow(bottom_coordinate, 3));

        bottom_coordinate = top_coordinate;
    }
    solve_mid_strain(*this, load_vector_);
    solve_stress_strain_profile(*this, pt_spacing);

}

void solve_mid_strain(laminate& lam, Matrix<double, 6, 1>& load_vector) {
    Matrix<double, 6, 6> stiffness = Matrix<double, 6, 6>::Zero();
    stiffness.block<3, 3>(0, 0) = lam.A_;
    stiffness.block<3, 3>(0, 3) = lam.B_;
    stiffness.block<3, 3>(3, 0) = lam.B_;
    stiffness.block<3, 3>(3, 3) = lam.D_;
    Matrix<double, 6, 1> strain_vector =
        stiffness.colPivHouseholderQr().solve(load_vector);
    lam.mid_strain_ = strain_vector.head<3>();
    lam.mid_curvature_ = strain_vector.tail<3>();
}

void solve_stress_strain_profile(laminate& lam, double pt_spacing) {

    lam.profile_pt_.push_back(-lam.height_/2);
    lam.strains_.push_back(
        lam.mid_strain_ + lam.profile_pt_.back() * lam.mid_curvature_);
    vector<ply>::size_type current_layer = 0;
    lam.stresses_.push_back(
        lam.ply_vector_[current_layer].Qbar_ * lam.strains_.back());

    double current_bottom_pt = -lam.height_/2;
    double current_top_pt = current_bottom_pt
                          + lam.ply_vector_[current_layer].thickness_;
    while (lam.profile_pt_.back() <= lam.height_/2) {
        double next_profile_pt = lam.profile_pt_.back() + pt_spacing;
        if (next_profile_pt > current_top_pt) {
            current_bottom_pt = current_top_pt;
            current_layer++;
            current_top_pt = current_bottom_pt
                           + lam.ply_vector_[current_layer].thickness_;
        }
        lam.profile_pt_.push_back(lam.profile_pt_.back() + pt_spacing);
        lam.strains_.push_back(
            lam.mid_strain_ + lam.profile_pt_.back() * lam.mid_curvature_);
        lam.stresses_.push_back(
            lam.ply_vector_[current_layer].Qbar_ * lam.strains_.back());
    }
}


/***************************************************************************
 *  @file       matrix_chain_mutiply.hpp
 *  @author     Alan.W
 *  @date       03  August 2014
 *  @remark     CLRS Algorithms implementation, using C++ templates.
 ***************************************************************************/

//!
//! ex15.2-2
//! Give a recursive algorithm MATRIX-CHAIN-MULTIPLY(A, s, i, j) that actually
//! performs the optimal matrix-chain multiplication, given the sequence of matrices
//! {A1,A2,...An}, the s table computed by MATRIX-CHAIN-ORDER , and the indices i and j .
//! (The initial call would be MATRIX-CHAIN-MULTIPLY (A, s, 1, n))
//!
//  check the lambda in function matrix_chain_multiply.
//!

#ifndef MATRIX_CHAIN_MUTIPLY_HPP
#define MATRIX_CHAIN_MUTIPLY_HPP

#include <vector>
#include <functional>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include "matrix_chain_order.hpp"

namespace ch15 {

//! foward declarations
template<typename Range>
void
build_chain(ch15::Chain<typename Range::value_type>& chain,
            const Range& dimensions,
            const typename Range::value_type& init_val = 0);

template<typename Range>
void
build_dimensions(const ch15::Chain<typename Range::value_type>& chain,
                 Range& dimensions);

template<typename T>
void print_matrix_chain(const ch15::Chain<T>& chain);

template<typename T>
ch15::Matrix<T>
matrix_chain_multiply(const ch15::Chain<T>& chain);

/**
 * @brief build_chain
 * @param chain
 * @param dimensions
 *
 * build a matrix chain using dimemsions stored in a stl container
 */
template<typename Range>
inline void
build_chain(ch15::Chain<typename Range::value_type>& chain,
            const Range& dimensions,
            const typename Range::value_type& init_val)
{
    using ValueType =   typename Range::value_type;

    for(auto it = dimensions.begin(); it != dimensions.end() - 1; ++it)
    {
        auto mat = ch15::Matrix<ValueType>(*it, *(it + 1), init_val);
        chain.push_back(mat);
    }
}

/**
 * @brief print_matrix_chain
 * @param chain
 */
template<typename T>
inline void
print_matrix_chain(const ch15::Chain<T>& chain)
{
    for(const auto& mat : chain)
        std::cout << mat << std::endl << std::endl;
}

/**
 * @brief build_dimensions
 * @param chain
 * @param dimensions
 *
 * build dimensions from matrix chain
 */
template<typename Range>
inline void
build_dimensions(const ch15::Chain<typename Range::value_type>& chain,
                 Range& dimensions)
{
    dimensions.push_back( chain.begin()->size1() );
    for(const auto& mat : chain)
        dimensions.push_back( mat.size2());
}

/**
 * @brief matrix_chain_multiply
 * @param chain
 *
 * @complx  O(n^3)
 * for ex15.2-2
 */
template<typename T>
ch15::Matrix<T>
matrix_chain_multiply(const ch15::Chain<T>& chain)
{
    //! type def for MatrixChainOrder's parameter
    using RangeType =   std::vector<T>;
    using SizeType  =   typename ch15::Matrix<T>::size_type;

    //! build dimensions
    RangeType dimens;
    ch15::build_dimensions(chain, dimens);

    //! build optimal order
    ch15::MatrixChainOrder<RangeType> order(dimens);
    order.build();
    order.print_optimal(1,chain.size());
    std::cout << std::endl;

    //! lambda to do the real job recursively
    //! @note   ex15.2-2
    std::function<ch15::Matrix<T>(SizeType,SizeType)> multiply
            = [&](SizeType head, SizeType tail)
    {
        //! @attention below is the pseudocode for ex15.2-2
        if(head == tail)
            return chain[head - 1];
        else
            return multiply(head, order.s(head - 1,tail - 2))
                   *
                   multiply(order.s(head - 1,tail - 2) + 1, tail);
    };

    //! return the product
    return multiply(1, chain.size());
}

}//namepspace
#endif // MATRIX_CHAIN_MUTIPLY_HPP

//! test: build_chain
//!     : build_dimensions
//#include <iostream>
//#include <boost/numeric/ublas/io.hpp>
//#include "color.hpp"
//#include "matrix.hpp"
//#include "matrix_chain_mutiply.hpp"
//#include "matrix_chain_order.hpp"

//int main()
//{
//    std::vector<int> v = {30,35,15,5,10,20,25};

//    ch15::Chain<int> chain;
//    ch15::build_chain(chain, v, 2);
//    ch15::print_matrix_chain(chain);

//    std::vector<int> dimens;
//    ch15::build_dimensions(chain, dimens);
//    for(auto d : dimens)
//        std::cout << d << " ";

//    std::cout << color::red("\nend\n");
//    return 0;
//}

//! @test   matrix_chain_multiply
//!         ex15.2-2
//#include <iostream>
//#include <boost/numeric/ublas/io.hpp>
//#include "color.hpp"
//#include "matrix.hpp"
//#include "matrix_chain_mutiply.hpp"
//#include "matrix_chain_order.hpp"

//int main()
//{
//    std::vector<int> v = {30,35,15,5,10,20,25};

//    ch15::Chain<int> chain;
//    ch15::build_chain(chain, v, 2);
//    std::cout << ch15::matrix_chain_multiply(chain) << std::endl;

//    std::cout << color::red("\nend\n");
//    return 0;
//}


#pragma once

/// This file contains definitions of aliases for basic vector classes and functions

#include <Core/RaCore.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Sparse>
#include <functional>
#include <unsupported/Eigen/AlignedVector3>

#include <Core/Math/Math.hpp>
#include <Core/Types.hpp>

namespace Ra {
namespace Core {
namespace Math {
//
// Common vector types
//

inline void print( const MatrixN& matrix );

//
// Geometry types
//

// Todo : storage transform using quaternions ?

//
// Vector Functions
//

/// Component-wise floor() function on a floating-point vector.
template <typename Vector>
inline Vector floor( const Vector& v );

/// Component-wise ceil() function on a floating-point vector.
template <typename Vector>
inline Vector ceil( const Vector& v );

/// Component-wise trunc() function on a floating-point vector
template <typename Vector>
inline Vector trunc( const Vector& v );

/// Component-wise clamp() function on a floating-point vector.

template <typename Derived, typename DerivedA, typename DerivedB>
inline typename Derived::PlainMatrix clamp( const Eigen::MatrixBase<Derived>& v,
                                            const Eigen::MatrixBase<DerivedA>& min,
                                            const Eigen::MatrixBase<DerivedB>& max );
/// Component-wise clamp() function on a floating-point vector.
template <typename Derived>
inline typename Derived::PlainMatrix
clamp( const Eigen::MatrixBase<Derived>& v, const Scalar& min, const Scalar& max );

/// Call std::isnormal on quaternion entries.
template <typename S>
inline bool checkInvalidNumbers( Eigen::Ref<Eigen::Quaternion<S>> q,
                                 const bool FAIL_ON_ASSERT = false ) {
    return checkInvalidNumbers( q.coeffs(), FAIL_ON_ASSERT );
}

/// Call std::isnormal on matrix entry.
/// Dense version
template <typename Matrix_>
inline bool checkInvalidNumbers( Eigen::Ref<const Matrix_> matrix,
                                 const bool FAIL_ON_ASSERT = false );

/// Get two vectors orthogonal to a given vector.
/// \warning fx must be normalized (this is not checked in the function)
inline void
getOrthogonalVectors( const Vector3& fx, Eigen::Ref<Vector3> fy, Eigen::Ref<Vector3> fz );

/// Get the angle between two vectors. Works for types where the cross product is
/// defined (i.e. 2D and 3D vectors).
template <typename Vector_>
inline Scalar angle( const Vector_& v1, const Vector_& v2 );

/// Get the spherical linear interpolation between two unit non-colinear vectors.
/// works for types where the cross-product is defined (i.e. 2D and 3D vectors).
template <typename Vector_>
inline Vector_ slerp( const Vector_& v1, const Vector_& v2, Scalar t );

/// @return the projection of point on the plane define by plane and planeNormal
inline Vector3
projectOnPlane( const Vector3& planePos, const Vector3& planeNormal, const Vector3& point );

/// Get the cotangent of the angle between two vectors. Works for vector types where
/// dot and cross product is defined (2D or 3D vectors).
template <typename Vector_>
inline Scalar cotan( const Vector_& v1, const Vector_& v2 );

/// Get the cosine of the angle between two vectors.
/// \todo use dot instead
template <typename Vector_>
inline Scalar cos( const Vector_& v1, const Vector_& v2 );

/// Normalize a vector and returns its norm before normalization.
/// If the vector's norm is 0, the vector's components will be overwritten by NaNs
template <typename Vector_>
inline Scalar getNormAndNormalize( Vector_& v );

/// Normalize a vector and returns its norm before normalization.
/// If the vector's norm is 0, the vector remains null
template <typename Vector_>
inline Scalar getNormAndNormalizeSafe( Vector_& v );

/// Transform a ray, direction is only transformed by linear part of the
/// transformation, while origin is fully transformed
/// corresponds to t*r
/// \param t : transform matrix
/// \param r : ray to transform
/// \return transoformed ray, origine is translated while direction is only
/// linarly transformed.
template <typename Scalar>
inline Eigen::ParametrizedLine<Scalar, 3>
transformRay( const Eigen::Transform<Scalar, 3, Eigen::Affine>& t,
              const Eigen::ParametrizedLine<Scalar, 3>& r );

inline Matrix4 lookAt( const Vector3& position, const Vector3& target, const Vector3& up );
inline Matrix4 perspective( Scalar fovy, Scalar aspect, Scalar near, Scalar zfar );
inline Matrix4
orthographic( Scalar left, Scalar right, Scalar bottom, Scalar top, Scalar near, Scalar zfar );

//
// Quaternion functions
//

// Define functions for multiplying a quaternion by a scalar
// and adding two quaternions. While Quaternion is supposed to
// represent a unit quaternion (thus a valid rotation), these functions
// are useful for linear interpolation of quaternions.

/// Returns the quaternion q multipled by a scalar factor of k.
inline Quaternion scale( const Quaternion& q, const Scalar k );

/// Returns the sum of two quaternions.
inline Quaternion add( const Quaternion& q1, const Quaternion& q2 );

/// Returns the sum of two quaternions, resolving antipodality by flipping
/// the sign of q2 if q1.q2 is negative. This operation is usually
/// denoted as a circled + sign, forming the basis of the QLERP algorithm.
/// See "Spherical Blend Skinning" (Kavan & Zara 2005) for more details.
inline Quaternion addQlerp( const Quaternion& q1, const Quaternion& q2 );

// Note : the .inl file also define operator+ for quaternions
// and operator * and / between quaternions and scalar.

/// Decompose a given rotation Qin into a swing rotation and a twist rotation.
/// Qswing is a rotation whose axis lies in the XY plane and Qtwist is a rotation about axis Z.
/// such as Qin = Qswing * Qtwist
/// If the rotation is already around axis z, Qswing will be set to identity
/// and Qtwist equal to Qin
inline void getSwingTwist( const Quaternion& in, Quaternion& swingOut, Quaternion& twistOut );

} // namespace Math
} // namespace Core
} // namespace Ra
#include <Core/Math/LinearAlgebra.inl>


#pragma once

#include "BinomialCoefficient.hh"
#include <Eigen/Core>

namespace kt84 {

template <int _DimIn, int _Degree>
struct PolynomialBasisGenT {
    enum { DimOut = PolynomialBasisGenT<_DimIn, _Degree - 1>::DimOut + BinomialCoefficient<_DimIn + _Degree - 1, _Degree>::Value };
    typedef Eigen::Matrix<double, _DimIn , 1> Point;
    typedef Eigen::Matrix<double, DimOut, 1> Basis;
    typedef Eigen::Matrix<double, DimOut, _DimIn> Gradient;

    static Basis basis(const Point& x) {
        const int N = BinomialCoefficient<_DimIn + _Degree - 1, _Degree>::Value;
        Eigen::Matrix<double, N, 1> result_partial;
        int index_out = 0;
        int index_in[_Degree];
        for (int i = 0; i < _Degree; ++i)
            index_in[i] = 0;
        while (true) {
            double d = 1;
            for (int i = 0; i < _Degree; ++i)
                d *= x[index_in[i]];
            result_partial[index_out++] = d;
            int is_complete = true;
            for (int i = _Degree - 1; i >= 0; --i) {
                if (index_in[i] == _DimIn - 1)
                    continue;
                ++index_in[i];
                for (int j = i + 1; j < _Degree; ++j)
                    index_in[j] = index_in[i];
                is_complete = false;
                break;
            }
            if (is_complete)
                break;
        }
        Basis result;
        result << PolynomialBasisGenT<_DimIn, _Degree - 1>::basis(x), result_partial;
        return result;
    }
    static Gradient gradient(const Point& x) {
        const int N = BinomialCoefficient<_DimIn + _Degree - 1, _Degree>::Value;
        Eigen::Matrix<double, N, _DimIn> result_partial;
        int index_out = 0;
        int index_in[_Degree];
        for (int i = 0; i < _Degree; ++i)
            index_in[i] = 0;
        while (true) {
            for (int i = 0; i < _DimIn; ++i) {
                int cnt = 0;
                double d = 1;
                for (int j = 0; j < _Degree; ++j) {
                    if (index_in[j] == i) {
                        ++cnt;
                        continue;
                    }
                    d *= x[index_in[j]];
                }
                result_partial(index_out, i) = cnt == 0 ? 0 : cnt * d * std::pow(x[i], cnt - 1);
            }
            ++index_out;
            int is_complete = true;
            for (int i = _Degree - 1; i >= 0; --i) {
                if (index_in[i] == _DimIn - 1)
                    continue;
                ++index_in[i];
                for (int j = i + 1; j < _Degree; ++j)
                    index_in[j] = index_in[i];
                is_complete = false;
                break;
            }
            if (is_complete)
                break;
        }
        Gradient result;
        result << PolynomialBasisGenT<_DimIn, _Degree - 1>::gradient(x), result_partial;
        return result;
    }
};

template <int _DimIn>
struct PolynomialBasisGenT<_DimIn, 0> {
    enum { DimOut = 1 };
    typedef Eigen::Matrix<double, _DimIn , 1> Point;
    typedef Eigen::Matrix<double, DimOut, 1> Basis;
    typedef Eigen::Matrix<double, DimOut, _DimIn> Gradient;

    static Basis basis(const Point& x) { return Basis::Constant(1); }
    static Gradient gradient(const Point& x) { return Gradient::Zero(); }
};

}


#include "RSA.h"
#include "DataStream.h"
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/miller_rabin.hpp>
#include <utility>
using boost::multiprecision::cpp_int;

RSAClient::RSAClient(size_t _bit_size, int _e) {
    bit_size = _bit_size;
    e = _e;
    cpp_int p = gen_prime(bit_size, _e);
    cpp_int q = gen_prime(bit_size, _e);
    n = p * q;
    cpp_int toit = (p - 1) * (q - 1); // since (p,q) are prime
    do {
        d = inv_mod(e, toit);
        if(!d) e++;
    } while (d == 0);
}

RSA_key RSAClient::get_public_key() {
    return std::make_pair(e, n);
}

DataStream RSAClient::decrypt(const DataStream &cipher) {
    return DataStream(powm(cipher.getCppInt(), d, n), bit_size*2);
}

DataStream encrypt(const DataStream &ds, const RSA_key &key, size_t bit_size) {
    return DataStream(powm(ds.getCppInt(), key.first, key.second), bit_size*2);
}

cpp_int inv_mod(const cpp_int &num, const cpp_int &mod) {
    cpp_int r0 = mod, r1 = num;
    cpp_int t0 = 0, t1 = 1;
    while(r1 != 0) {
        cpp_int q = r0 / r1;
        cpp_int r2 = r0 - q * r1;
        cpp_int t2 = t0 - q * t1;
        r0 = r1;
        r1 = r2;
        t0 = t1;
        t1 = t2;
    }
    if(r0 > 1) {
        return 0;
    }
    return (t0 + mod) % mod;
}

cpp_int gen_prime(size_t bit_size, int no_div) {
    cpp_int prime;
    do {
        if(no_div > 0) {
            do {
                prime = get_random_key(bit_size / 8).getCppInt();
            } while ((prime - 1) % no_div == 0);
        } else {
            prime = get_random_key(bit_size / 8).getCppInt();
        }
    } while(!miller_rabin_test(prime, 100));
    return prime;
}


#include <iostream>
#include <vector>
#include <algorithm>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include "powerscore.h"

void PowerScoreWorker::ComputeEigenValueCentrality(Eigen::MatrixXd A, Eigen::RowVectorXd &powerScores) {
  int dimension = A.rows();
  int idxMax;
  float epsilon = 1e-6;
  Eigen::MatrixXd T(dimension, dimension);
  Eigen::MatrixXd I(dimension, dimension);
  Eigen::MatrixXd Ones(dimension, dimension);
  Eigen::MatrixXd denominator(dimension, dimension);
  Eigen::VectorXd rowSum(dimension);
  I = Eigen::MatrixXd::Identity(dimension, dimension);
  Ones = Eigen::MatrixXd::Zero(dimension, dimension);
  Ones.setOnes(dimension, dimension);
  denominator = Eigen::MatrixXd::Zero(dimension, dimension);
  powerScores = Eigen::RowVectorXd(dimension);
  // Compute the transition matrix T from the aggression matrix A
  // Start constructing T by adding eps to non-diagonal elements of A
  T = A + epsilon * (Ones - I);
  // Compute row sums for normalization to conditional probabilities
  rowSum = T.rowwise().sum();
  denominator = rowSum.replicate(1, dimension);
  // Perform element wise division to get the transition matrix
  T = T.array() / denominator.array();
  // Compute eigenvalues and eigenvectors of the transpose of T (to get left hand eigenvalues)
  Eigen::EigenSolver<Eigen::MatrixXd> es(T.transpose());
  // Find the eigenvalue with the largest absolute value
  es.eigenvalues().array().abs().maxCoeff(&idxMax);
  // Pick the corresponding eigenvector, and compute power score
  powerScores = es.eigenvectors().transpose().row(idxMax).array().abs();
}


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

using namespace Eigen;

template <class Matrix>

// The auxiliary function Atimesx computes the function A*x in a smart way, using the particular structure of the matrix A.

void Atimesx(const Matrix & d, const Matrix & a, const Matrix & x, Matrix & Ax)
{
    int n=d.size();
    Ax=(d.array()*x.array()).matrix();
    VectorXd Axcut=Ax.head(n-1);
    VectorXd acut = a.head(n-1);
    VectorXd xcut = x.head(n-1);

    Ax << Axcut + x(n-1)*acut, Ax(n-1)+ acut.transpose()*xcut;
}

// We compute A*A*x by using the function Atimesx twice with 5 dimensional random vectors.

int main(void)
{
    VectorXd a=VectorXd::Random(5);
    VectorXd d=VectorXd::Random(5);
    VectorXd x=VectorXd::Random(5);
    VectorXd Ax(5);

    Atimesx(d,a,x,Ax);
    VectorXd AAx(5);
    Atimesx(d,a,Ax,AAx);
    std::cout << "A*A*x = " << AAx << std::endl;
}

Nummerical Methods for CSE/PS1/solutions_ps1/C++/arrowmatvec2.cpp


// Copyright Paul A. Bristow 2015.

// 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!

// Example of root finding using Boost.Multiprecision.

#ifndef BOOST_MATH_STANDALONE

#include <boost/math/tools/roots.hpp>
//using boost::math::policies::policy;
//using boost::math::tools::newton_raphson_iterate;
//using boost::math::tools::halley_iterate;
//using boost::math::tools::eps_tolerance; // Binary functor for specified number of bits.
//using boost::math::tools::bracket_and_solve_root;
//using boost::math::tools::toms748_solve;

#include <boost/math/special_functions/next.hpp> // For float_distance.
#include <boost/math/special_functions/pow.hpp>
#include <boost/math/constants/constants.hpp>

//[root_finding_multiprecision_include_1
#include <boost/multiprecision/cpp_bin_float.hpp> // For cpp_bin_float_50.
#include <boost/multiprecision/cpp_dec_float.hpp> // For cpp_dec_float_50.
#ifndef _MSC_VER  // float128 is not yet supported by Microsoft compiler at 2013.
#  include <boost/multiprecision/float128.hpp> // Requires libquadmath.
#endif
//] [/root_finding_multiprecision_include_1]

#include <iostream>
// using std::cout; using std::endl;
#include <iomanip>
// using std::setw; using std::setprecision;
#include <limits>
// using std::numeric_limits;
#include <tuple>
#include <utility> // pair, make_pair

// #define BUILTIN_POW_GUESS // define to use std::pow function to obtain a guess.

template <class T>
T cbrt_2deriv(T x)
{ // return cube root of x using 1st and 2nd derivatives and Halley.
  using namespace std;  // Help ADL of std functions.
  using namespace boost::math::tools; // For halley_iterate.

  // If T is not a binary floating-point type, for example, cpp_dec_float_50
  // then frexp may not be defined,
  // so it may be necessary to compute the guess using a built-in type,
  // probably quickest using double, but perhaps with float or long double.
  // Note that the range of exponent may be restricted by a built-in-type for guess.

  typedef long double guess_type;

#ifdef BUILTIN_POW_GUESS
  guess_type pow_guess = std::pow(static_cast<guess_type>(x), static_cast<guess_type>(1) / 3);
  T guess = pow_guess;
  T min = pow_guess /2;
  T max = pow_guess * 2;
#else
  int exponent;
  frexp(static_cast<guess_type>(x), &exponent); // Get exponent of z (ignore mantissa).
  T guess = ldexp(static_cast<guess_type>(1.), exponent / 3); // Rough guess is to divide the exponent by three.
  T min = ldexp(static_cast<guess_type>(1.) / 2, exponent / 3); // Minimum possible value is half our guess.
  T max = ldexp(static_cast<guess_type>(2.), exponent / 3); // Maximum possible value is twice our guess.
#endif

  int digits = std::numeric_limits<T>::digits / 2; // Half maximum possible binary digits accuracy for type T.
  const std::uintmax_t maxit = 20;
  std::uintmax_t it = maxit;
  T result = halley_iterate(cbrt_functor_2deriv<T>(x), guess, min, max, digits, it);
  // Can show how many iterations (updated by halley_iterate).
  // std::cout << "Iterations " << it << " (from max of "<< maxit << ")." << std::endl;
  return result;
} // cbrt_2deriv(x)

template <class T>
struct cbrt_functor_2deriv
{ // Functor returning both 1st and 2nd derivatives.
  cbrt_functor_2deriv(T const& to_find_root_of) : a(to_find_root_of)
  { // Constructor stores value to find root of, for example:
  }

  // using boost::math::tuple; // to return three values.
  std::tuple<T, T, T> operator()(T const& x)
  {
    // Return both f(x) and f'(x) and f''(x).
    T fx = x*x*x - a;                     // Difference (estimate x^3 - value).
    // std::cout << "x = " << x << "\nfx = " << fx << std::endl;
    T dx = 3 * x*x;                       // 1st derivative = 3x^2.
    T d2x = 6 * x;                        // 2nd derivative = 6x.
    return std::make_tuple(fx, dx, d2x);  // 'return' fx, dx and d2x.
  }
private:
  T a;                                    // to be 'cube_rooted'.
}; // struct cbrt_functor_2deriv

template <int n, class T>
struct nth_functor_2deriv
{ // Functor returning both 1st and 2nd derivatives.

  nth_functor_2deriv(T const& to_find_root_of) : value(to_find_root_of)
  { /* Constructor stores value to find root of, for example: */ }

  // using std::tuple; // to return three values.
  std::tuple<T, T, T> operator()(T const& x)
  {
    // Return both f(x) and f'(x) and f''(x).
    using boost::math::pow;
    T fx = pow<n>(x) - value;              // Difference (estimate x^3 - value).
    T dx = n * pow<n - 1>(x);              // 1st derivative = 5x^4.
    T d2x = n * (n - 1) * pow<n - 2 >(x);  // 2nd derivative = 20 x^3
    return std::make_tuple(fx, dx, d2x);   // 'return' fx, dx and d2x.
  }
private:
  T value;                                 // to be 'nth_rooted'.
}; // struct nth_functor_2deriv

template <int n, class T>
T nth_2deriv(T x)
{
  // return nth root of x using 1st and 2nd derivatives and Halley.
  using namespace std;  // Help ADL of std functions.
  using namespace boost::math; // For halley_iterate.

  int exponent;
  frexp(x, &exponent);                                 // Get exponent of z (ignore mantissa).
  T guess = ldexp(static_cast<T>(1.), exponent / n);   // Rough guess is to divide the exponent by three.
  T min = ldexp(static_cast<T>(0.5), exponent / n);    // Minimum possible value is half our guess.
  T max = ldexp(static_cast<T>(2.), exponent / n);     // Maximum possible value is twice our guess.

  int digits = std::numeric_limits<T>::digits / 2;     // Half maximum possible binary digits accuracy for type T.
  const std::uintmax_t maxit = 50;
  std::uintmax_t it = maxit;
  T result = halley_iterate(nth_functor_2deriv<n, T>(x), guess, min, max, digits, it);
  // Can show how many iterations (updated by halley_iterate).
  std::cout << it << " iterations (from max of " << maxit << ")" << std::endl;

  return result;
} // nth_2deriv(x)

//[root_finding_multiprecision_show_1

template <typename T>
T show_cube_root(T value)
{ // Demonstrate by printing the root using all definitely significant digits.
  std::cout.precision(std::numeric_limits<T>::digits10);
  T r = cbrt_2deriv(value);
  std::cout << "value = " << value << ", cube root =" << r << std::endl;
  return r;
}

//] [/root_finding_multiprecision_show_1]

int main()
{
  std::cout << "Multiprecision Root finding Example." << std::endl;
  // Show all possibly significant decimal digits.
  std::cout.precision(std::numeric_limits<double>::digits10);
  // or use   cout.precision(max_digits10 = 2 + std::numeric_limits<double>::digits * 3010/10000);
  //[root_finding_multiprecision_example_1
  using boost::multiprecision::cpp_dec_float_50; // decimal.
  using boost::multiprecision::cpp_bin_float_50; // binary.
#ifndef _MSC_VER  // Not supported by Microsoft compiler.
  using boost::multiprecision::float128;
#endif
  //] [/root_finding_multiprecision_example_1

  try
  { // Always use try'n'catch blocks with Boost.Math to get any error messages.
    // Increase the precision to 50 decimal digits using Boost.Multiprecision
//[root_finding_multiprecision_example_2

      std::cout.precision(std::numeric_limits<cpp_dec_float_50>::digits10);

      cpp_dec_float_50 two = 2; //
      cpp_dec_float_50  r = cbrt_2deriv(two);
      std::cout << "cbrt(" << two << ") = " << r << std::endl;

      r = cbrt_2deriv(2.); // Passing a double, so ADL will compute a double precision result.
      std::cout << "cbrt(" << two << ") = " << r << std::endl;
      // cbrt(2) = 1.2599210498948731906665443602832965552806854248047 'wrong' from digits 17 onwards!
      r = cbrt_2deriv(static_cast<cpp_dec_float_50>(2.)); // Passing a cpp_dec_float_50,
      // so will compute a cpp_dec_float_50 precision result.
      std::cout << "cbrt(" << two << ") = " << r << std::endl;
      r = cbrt_2deriv<cpp_dec_float_50>(2.); // Explicitly a cpp_dec_float_50, so will compute a cpp_dec_float_50 precision result.
      std::cout << "cbrt(" << two << ") = " << r << std::endl;
      // cpp_dec_float_50 1.2599210498948731647672106072782283505702514647015
//] [/root_finding_multiprecision_example_2
     //  N[2^(1/3), 50]  1.2599210498948731647672106072782283505702514647015

      //show_cube_root(2); // Integer parameter - Errors!
      //show_cube_root(2.F); // Float parameter - Warnings!
//[root_finding_multiprecision_example_3
      show_cube_root(2.);
      show_cube_root(2.L);
      show_cube_root(two);

//] [/root_finding_multiprecision_example_3

  }
  catch (const std::exception& e)
  { // Always useful to include try&catch blocks because default policies
    // are to throw exceptions on arguments that cause errors like underflow & overflow.
    // Lacking try&catch blocks, the program will abort without a message below,
    // which may give some helpful clues as to the cause of the exception.
    std::cout <<
      "\n""Message from thrown exception was:\n   " << e.what() << std::endl;
  }
  return 0;
} // int main()

/*

Description: Autorun "J:\Cpp\MathToolkit\test\Math_test\Release\root_finding_multiprecision.exe"
Multiprecision Root finding Example.
cbrt(2) = 1.2599210498948731647672106072782283505702514647015
cbrt(2) = 1.2599210498948731906665443602832965552806854248047
cbrt(2) = 1.2599210498948731647672106072782283505702514647015
cbrt(2) = 1.2599210498948731647672106072782283505702514647015
value = 2, cube root =1.25992104989487
value = 2, cube root =1.25992104989487
value = 2, cube root =1.2599210498948731647672106072782283505702514647015

*/

#endif // BOOST_MATH_STANDALONE


/* based upon http://www.boost.org/doc/libs/1_55_0/libs/multiprecision/doc/html/boost_multiprecision/tut/floats/cpp_dec_float.html
 * 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)
 * Copyright ???? 20??. */

#include <iostream>
#include <utility>

#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>

typedef boost::multiprecision::number<
            boost::multiprecision::backends::cpp_bin_float<
                106,
                boost::multiprecision::backends::digit_base_2,
                void,
                boost::int16_t, -1022, 1023>,
            boost::multiprecision::et_off>
        cpp_bin_float_double_double;

using boost::multiprecision::cpp_dec_float;
using boost::multiprecision::cpp_bin_float_single;
using boost::multiprecision::cpp_bin_float_double;
using boost::multiprecision::cpp_bin_float_double_extended;
using boost::multiprecision::cpp_bin_float_quad;

template <typename T>
void foo(void)
{
    std::cout << "========================" << std::endl;

    /* prints the numerical precision i.e. 64 */
    std::cout << std::numeric_limits<T>::digits10 << std::endl;

    T a = 2;
    //T b = boost::math::constants::pi<T, boost::math::policies::policy<boost::math::policies::digits2<64> > >();
    T b = boost::math::constants::pi<T>();
    T c = exp(a);
    T d = pow(c,c);
    T e = 1./c;

    std::cout << std::setprecision(std::numeric_limits<T>::max_digits10) << a << std::endl;
    std::cout << std::setprecision(std::numeric_limits<T>::max_digits10) << b << std::endl;
    std::cout << std::setprecision(std::numeric_limits<T>::max_digits10) << c << std::endl;
    std::cout << std::setprecision(std::numeric_limits<T>::max_digits10) << d << std::endl;
    std::cout << std::setprecision(std::numeric_limits<T>::max_digits10) << e << std::endl;
}

int main(void)
{
    //foo<boost::multiprecision::cpp_bin_float_single >(); // see bug.cc
    foo<boost::multiprecision::cpp_bin_float_double >();
    foo<boost::multiprecision::cpp_bin_float_double_extended >();
    foo<cpp_bin_float_double_double >();
    foo<boost::multiprecision::cpp_bin_float_quad >();
    foo<boost::multiprecision::number<cpp_dec_float<64> > >();

    return 0;
}