#include "petsc.h"
#include "gmock/gmock.h"
#include "gtest/gtest.h"

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

#include <iostream>
#include <stdlib.h>
#include <cassert>

namespace {

using namespace testing;

typedef Eigen::SparseMatrix<double, Eigen::RowMajor> EigenMatCSR;

extern "C" {
void applicationfunctionfortran_(
		double* lambda,
		int* mx,
		int* my,
		double const* x,
		double* f,
		PetscErrorCode* ierr
	);

void initialguessfortran_(int* mx, int* my, double* lambda, double* x);
}

void create_petsc_matrix_from_eigen_csr(
	EigenMatCSR& mat_csr,
	Mat& jacobian
);

// Compressed Sparse Row Format
struct MatCSR {
	Eigen::Map<Eigen::ArrayXi> row_pointers;
	Eigen::Map<Eigen::ArrayXi> column_indices;
	Eigen::Map<Eigen::ArrayXd> values;

	MatCSR()
	: row_pointers(nullptr, 0)
	, column_indices(nullptr, 0)
	, values(nullptr, 0)
	{}
};

EigenMatCSR create_laplacian_matrix_1d(int size){
	EigenMatCSR mat_csr(size, size);

	mat_csr.insert(0, 0) = 1;
	mat_csr.insert(0, 1) = -1;
	for(int i = 1; i < size-1; ++i){
		mat_csr.insert(i, i - 1) = -1;
		mat_csr.insert(i, i) = 2;
		mat_csr.insert(i, i + 1) = -1;

	}
	mat_csr.insert(size-1, size-1) = 1;
	mat_csr.insert(size-1, size-2) = -1;

	mat_csr.makeCompressed();

	return mat_csr;
}

EigenMatCSR create_laplacian_matrix_2d(int mx, int my){

	auto N = mx * my;
	EigenMatCSR mat_csr(N, N);

	for (int i = 0; i < N; ++i)
	{
		auto rj = i % mx; // column in grid
		auto ri = i / mx; // row in grid
		if (ri != 0) {    // first row does not have neighbor below
			mat_csr.insert(i, i - mx) = 1;
		}
		if (ri != my - 1) { // last row does not have neighbors above
			mat_csr.insert(i, i + mx) = 1;
		}
		if (rj != 0) {     // first column does not have neighbor to left
			mat_csr.insert(i, i - 1) = 1;
		}
		if (rj != mx - 1) {     // last column does not have neighbor to right
			mat_csr.insert(i, i + 1) = 1;
		}
		mat_csr.insert(i, i) = 1;
	}

	mat_csr.makeCompressed();

	return mat_csr;
}

TEST(Eigen, Sparse)
{
	int N = 5;
	auto mat_csr = create_laplacian_matrix_1d(N);

	auto expected_nnz = 3 * (N - 2) + 4;

	auto nnz = mat_csr.nonZeros();
	auto row_pointers = mat_csr.outerIndexPtr();
	auto column_indices = mat_csr.innerIndexPtr();
	auto values = mat_csr.valuePtr();

	ASSERT_TRUE(nnz == expected_nnz);

	MatCSR mat_csr_struct;
	new (&mat_csr_struct.row_pointers) Eigen::Map<Eigen::ArrayXi>(row_pointers, N);
	new (&mat_csr_struct.column_indices) Eigen::Map<Eigen::ArrayXi>(column_indices, nnz);
	new (&mat_csr_struct.values) Eigen::Map<Eigen::ArrayXd>(values, nnz);

	Eigen::ArrayXi const expected_row_pointers = (Eigen::ArrayXi(5) << 0, 2, 5, 8, 11).finished();
	ASSERT_TRUE(mat_csr_struct.row_pointers.isApprox(expected_row_pointers));

	Eigen::ArrayXi const expected_column_indices = (Eigen::ArrayXi(13) << 0, 1, 0, 1, 2, 1, 2, 3, 2, 3, 4, 3, 4).finished();
	ASSERT_TRUE(mat_csr_struct.column_indices.isApprox(expected_column_indices));

	Eigen::ArrayXd const expected_values = (Eigen::ArrayXd(13) << 1, -1, -1, 2, -1, -1, 2, -1, -1, 2, -1, -1, 1).finished();
	ASSERT_TRUE(mat_csr_struct.values.isApprox(expected_values));

	Mat mat_petsc;
	create_petsc_matrix_from_eigen_csr(mat_csr, mat_petsc);
}

/*
   User-defined application context - contains data needed by the
   application-provided call-back routines   FormFunction().
*/
typedef struct {
	PetscReal param;             /* test problem parameter */
	int       mx,my;             /* discretization in x, y directions */
} AppCtx;

// User-defined routines
int FormFunction(SNES,Vec,Vec,void*);
int FormFunctionFortran(SNES,Vec,Vec,void*);
void FormInitialGuess(AppCtx*,Vec, bool);
void initial_guess(int mx, int my, double lambda, double* x);

TEST(Impl, FortranVSCpp)
{
	auto mx = 5;
	auto my = 5;
	auto lambda = 4.75;
	auto N = mx * my;
	Eigen::ArrayXd x_cpp(N);
	Eigen::ArrayXd x_for(N);

	initialguessfortran_(&mx, &my, &lambda, x_for.data());
	initial_guess(mx, my, lambda, x_cpp.data());

	ASSERT_TRUE(x_cpp.isApprox(x_for));
}

class PetscTest : public ::testing::TestWithParam<PetscBool> {
  // You can implement all the usual fixture class members here.
  // To access the test parameter, call GetParam() from class
  // TestWithParam<T>.
};
/* ------------------------------------------------------------------------

    Solid Fuel Ignition (SFI) problem.  This problem is modeled by
    the partial differential equation

            -Laplacian u - lambda*exp(u) = 0,  0 < x,y < 1,

    with boundary conditions

             u = 0  for  x = 0, x = 1, y = 0, y = 1.

    A finite difference approximation with the usual 5-point stencil
    is used to discretize the boundary value problem to obtain a nonlinear
    system of equations.
    The uniprocessor version of this code is snes/examples/tutorials/ex4.c

  ------------------------------------------------------------------------- */
TEST_P(PetscTest, SNES)
{
	SNES             snes;                /* nonlinear solver */
	Vec              x,r;                 /* solution, residual vectors */
	AppCtx           user;                /* user-defined work context */
	int              its;                 /* iterations for convergence */
	int              N,ierr,i,ii,ri,rj;
	ISColoringValue* colors;
	PetscReal        bratu_lambda_max = 6.81,bratu_lambda_min = 0.;
	MatColoring  	 mat_coloring;
	MatFDColoring    fdcoloring;
	ISColoring       iscoloring;
	Mat              J;
	PetscScalar      zero = 0.0;
	PetscBool        use_fortran;
	PetscErrorCode   (*fnc)(SNES,Vec,Vec,void*);

	/*
	 Initialize problem parameters
	*/
	user.mx = 5; user.my = 5; user.param = 6.0;

	ASSERT_FALSE(user.param >= bratu_lambda_max || user.param <= bratu_lambda_min); // Lambda is out of range
	N = user.mx*user.my;

	/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
	 Create nonlinear solver context
	 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
	SNESCreate(PETSC_COMM_WORLD, &snes);

	/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
	 Create vector data structures; set function evaluation routine
	 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
	VecCreateSeq(PETSC_COMM_WORLD, N, &x);
	VecDuplicate(x, &r);

	use_fortran = GetParam();
	if (use_fortran) fnc = FormFunctionFortran;
	else     fnc = FormFunction;

	/*
	 Set function evaluation routine and vector
	*/
	SNESSetFunction(snes,r,fnc,&user);

	auto mat_csr = create_laplacian_matrix_2d(user.mx, user.my);
	create_petsc_matrix_from_eigen_csr(mat_csr, J);
	/*
	   Create the data structure that SNESComputeJacobianDefaultColor() uses
	   to compute the actual Jacobians via finite differences.
	*/
	MatColoringCreate(J, &mat_coloring);
	MatColoringSetType(mat_coloring, MATCOLORINGSL);
	MatColoringSetFromOptions(mat_coloring);
	MatColoringApply(mat_coloring, &iscoloring);
	MatFDColoringCreate(J,iscoloring,&fdcoloring);
	MatFDColoringSetType(fdcoloring, MATMFFD_DS);
	MatFDColoringSetFunction(fdcoloring,(PetscErrorCode (*)(void))fnc,&user);
	MatFDColoringSetFromOptions(fdcoloring);
	MatFDColoringSetUp(J,iscoloring,fdcoloring);
	/*
		Tell SNES to use the routine SNESComputeJacobianDefaultColor()
	  to compute Jacobians.
	*/
	SNESSetJacobian(snes,J,J,SNESComputeJacobianDefaultColor,fdcoloring);
	ISColoringDestroy(&iscoloring);
	MatColoringDestroy(&mat_coloring);

	/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
	 Customize nonlinear solver; set runtime options
	- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */

	// It is good to iterate at least once, otherwise the simulator
	// may not solve the problem at all if the first residual is
	// sufficiently small
	SNESSetForceIteration(snes, PETSC_TRUE);

    SNESSetTolerances(snes, 1e-8, 1e-50, 1e-50, 10, 1e8);

	/*
	 Set runtime options (e.g., -snes_monitor -snes_rtol <rtol> -ksp_type <type>)
	*/
	SNESSetFromOptions(snes);

	/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
	 Evaluate initial guess; then solve nonlinear system
	- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
	/*
	 Note: The user should initialize the vector, x, with the initial guess
	 for the nonlinear solver prior to calling SNESSolve().  In particular,
	 to employ an initial guess of zero, the user should explicitly set
	 this vector to zero by calling VecSet().
	*/
	FormInitialGuess(&user, x, use_fortran);
	SNESSolve(snes,NULL,x);
	SNESGetIterationNumber(snes,&its);
	PetscPrintf(PETSC_COMM_WORLD,"Number of SNES iterations = %D\n",its);

	SNESConvergedReason reason;
	SNESGetConvergedReason(snes, &reason);
	ASSERT_TRUE(reason > 0);

	/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
	 Free work space.  All PETSc objects should be destroyed when they
	 are no longer needed.
	- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
	VecDestroy(&x);
	VecDestroy(&r);
	MatFDColoringDestroy(&fdcoloring);
	MatDestroy(&J);
	SNESDestroy(&snes);
} // TEST_P

INSTANTIATE_TEST_CASE_P(
		FotranTest,
		PetscTest,
		::testing::Values(PETSC_TRUE)
);
INSTANTIATE_TEST_CASE_P(
		CppTest,
		PetscTest,
		::testing::Values(PETSC_FALSE)
);

void FormInitialGuess(AppCtx *user, Vec X, bool use_fortran)
{
	PetscScalar *x;

	auto mx = user->mx;
	auto my = user->my;
	auto lambda = user->param;

	VecGetArray(X,&x);

	initial_guess(mx, my, lambda, x);
	if(use_fortran){
		initialguessfortran_(&mx, &my, &lambda, x);
	}
	else{
		initial_guess(mx, my, lambda, x);
	}

	VecRestoreArray(X,&x);
}

void initial_guess(int mx, int my, double lambda, double* x)
{
	auto hx = 1.0 / (double)(mx - 1.0);
	auto hy = 1.0 / (double)(my - 1.0);
	auto temp1 = lambda / (lambda + 1.0);
	for (int j = 0; j < my; ++j)
	{
		auto temp = double(std::min(j, my - j - 1) * hy);
		for (int i = 0; i < mx; ++i)
		{
			auto row = i + j * mx;
			if (i == 0 || j == 0 || i == mx-1 || j == my-1) {
				x[row] = 0.0;
				continue;
			}
			x[row] = temp1 * std::sqrt( std::min(std::min(i, mx - i - 1) * hx, temp));
		}
	}
}

int FormFunction(SNES snes, Vec X, Vec F, void *ptr)
{
	AppCtx            *user = (AppCtx*)ptr;
	int               ierr,i,j,row,mx,my;
	PetscReal         two = 2.0,one = 1.0,lambda,hx,hy,hxdhy,hydhx,sc;
	PetscScalar       u,uxx,uyy,*f;
	const PetscScalar *x;

	mx = user->mx;            my = user->my;            lambda = user->param;
	hx = one/(PetscReal)(mx-1);  hy = one/(PetscReal)(my-1);
	sc = hx*hy*lambda;        hxdhy = hx/hy;            hydhx = hy/hx;

	/*
	 Get pointers to vector data
	*/
	VecGetArrayRead(X,&x);
	VecGetArray(F,&f);

	/*
	 Compute function over the entire  grid
	*/
	for (j=0; j<my; j++) {
		for (i=0; i<mx; i++) {
			row = i + j*mx;
			if (i == 0 || j == 0 || i == mx-1 || j == my-1) {
				f[row] = x[row];
				continue;
			}
			u      = x[row];
			uxx    = (two*u - x[row-1] - x[row+1])*hydhx;
			uyy    = (two*u - x[row-mx] - x[row+mx])*hxdhy;
			f[row] = uxx + uyy - sc*PetscExpScalar(u);
		}
	}

	/*
	 Restore vectors
	*/
	VecRestoreArrayRead(X,&x);
	VecRestoreArray(F,&f);

	return 0;
}

// FormFunctionFortran - Evaluates nonlinear function, F(x) in Fortran.
int FormFunctionFortran(SNES snes, Vec X, Vec F, void *ptr)
{
	AppCtx            *user = (AppCtx*)ptr;
	int               ierr;
	PetscScalar       *f;
	PetscScalar const *x;

	VecGetArrayRead(X,&x);
	VecGetArray(F,&f);
	applicationfunctionfortran_(&user->param,&user->mx,&user->my,x,f,&ierr);
	VecRestoreArrayRead(X,&x);
	VecRestoreArray(F,&f);

	return 0;
}

void create_petsc_matrix_from_eigen_csr(
	EigenMatCSR& mat_csr,
	Mat& jacobian
)
{
	auto n_rows = mat_csr.rows();
	auto n_cols = mat_csr.cols();
	auto nnz = mat_csr.nonZeros();

	auto row_pointers = mat_csr.outerIndexPtr();
	auto column_indices = mat_csr.innerIndexPtr();
	auto values = mat_csr.valuePtr();

	MatCSR jacobian_mat_csr;
	new (&jacobian_mat_csr.row_pointers) Eigen::Map<Eigen::ArrayXi>(row_pointers, n_rows);
	new (&jacobian_mat_csr.column_indices) Eigen::Map<Eigen::ArrayXi>(column_indices, nnz);
	new (&jacobian_mat_csr.values) Eigen::Map<Eigen::ArrayXd>(values, nnz);

    MatCreateSeqAIJWithArrays(
        PETSC_COMM_WORLD,
		n_rows,
		n_cols,
		jacobian_mat_csr.row_pointers.data(),
		jacobian_mat_csr.column_indices.data(),
		jacobian_mat_csr.values.data(),
        &jacobian
    );

    MatAssemblyBegin(jacobian, MAT_FINAL_ASSEMBLY);
    MatAssemblyEnd(jacobian, MAT_FINAL_ASSEMBLY);
}

} // namespace


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

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-trigonometric
    Function object implementing rem_pio2_cephes capabilities

    Computes the remainder modulo \f$\pi/2\f$ with cephes algorithm,
    and the angle quadrant between 0 and 3.
    This is a quick version accurate if the input is in \f$[-20\pi,20\pi]\f$.

    @par Semantic:

    For every parameters of floating type T:

    @code
    T r;
    as_integer<T> n;
    std::tie(n, r) = rem_pio2_cephes(x);
    @endcode

    is similar to:

    @code
    as_integer<T> n = div(inearbyint, x, Pio_2<T>());
    T r =  remainder(x, Pio_2<T>());
    @endcode

    The reduction of the argument modulo \f$\pi/2\f$ is generally
    the most difficult part of trigonometric evaluations.
    The accurate algorithm over the whole floating point range
    is over costly and implies the knowledge
    of a few hundred \f$\pi\f$ decimals
    some simpler algorithms as this one
    can be used, but the precision is only insured on specific intervals.

    @see rem_pio2, rem_pio2_straight,rem_2pi, rem_pio2_medium,

  **/
  std::pair<IntegerValue, Value> rem_pio2_cephes(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/rem_pio2_cephes.hpp>
#include <boost/simd/function/simd/rem_pio2_cephes.hpp>

#endif


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Polyhedron_3.h>
#include <CGAL/point_generators_3.h>

#include <CGAL/Side_of_triangle_mesh.h>

#include <vector>
#include <fstream>
#include <limits>
#include <boost/foreach.hpp>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_3 Point;
typedef CGAL::Polyhedron_3<K> Polyhedron;

double max_coordinate(const Polyhedron& poly)
{
  double max_coord = (std::numeric_limits<double>::min)();
  BOOST_FOREACH(Polyhedron::Vertex_handle v, vertices(poly))
  {
    Point p = v->point();
    max_coord = (std::max)(max_coord, p.x());
    max_coord = (std::max)(max_coord, p.y());
    max_coord = (std::max)(max_coord, p.z());
  }
  return max_coord;
}

int main(int argc, char* argv[])
{
  const char* filename = (argc > 1) ? argv[1] : "data/eight.off";
  std::ifstream input(filename);

  Polyhedron poly;
  if (!input || !(input >> poly) || poly.empty()
             || !CGAL::is_triangle_mesh(poly))
  {
    std::cerr << "Not a valid input file." << std::endl;
    return 1;
  }

  CGAL::Side_of_triangle_mesh<Polyhedron, K> inside(poly);

  double size = max_coordinate(poly);

  unsigned int nb_points = 100;
  std::vector<Point> points;
  points.reserve(nb_points);
  CGAL::Random_points_in_cube_3<Point> gen(size);
  for (unsigned int i = 0; i < nb_points; ++i)
    points.push_back(*gen++);

  std::cout << "Test " << nb_points << " random points in cube "
    << "[-" << size << "; " << size <<"]" << std::endl;

  int nb_inside = 0;
  int nb_boundary = 0;
  for (std::size_t i = 0; i < nb_points; ++i)
  {
    CGAL::Bounded_side res = inside(points[i]);

    if (res == CGAL::ON_BOUNDED_SIDE) { ++nb_inside; }
    if (res == CGAL::ON_BOUNDARY) { ++nb_boundary; }
  }

  std::cerr << "Total query size: " << points.size() << std::endl;
  std::cerr << "  " << nb_inside << " points inside " << std::endl;
  std::cerr << "  " << nb_boundary << " points on boundary " << std::endl;
  std::cerr << "  " << points.size() - nb_inside - nb_boundary << " points outside " << std::endl;

  return 0;
}


/**
 * \file se_2_3_localization.cpp
 *
 *  Created on: Aug 03, 2020
 *     \author: prashanthr05
 *
 *  ---------------------------------------------------------
 *  This file is:
 *  (c) 2020 Prashanth Ramadoss @ DIC-IIT, Genova, Italy
 *
 *  adapted from the file se3_localization.cpp:
 *  (c) 2018 Joan Sola @ IRI-CSIC, Barcelona, Catalonia
 *
 *  This file is part of `manif`, a C++ template-only library
 *  for Lie theory targeted at estimation for robotics.
 *  Manif is:
 *  (c) 2018 Jeremie Deray @ IRI-UPC, Barcelona
 *  ---------------------------------------------------------
 *
 *  ---------------------------------------------------------
 *  Demonstration example:
 *
 *  3D Robot localization and linear velocity estimation based on strap-down IMU model and fixed beacons.
 *
 *  ---------------------------------------------------------
 *
 *  We consider a robot in 3D space surrounded by a small
 *  number of punctual landmarks or _beacons_.
 *  The robot is assumed to be mounted with an IMU whose
 *  measurements are fed as exogeneous inputs to the system.
 *  The robot is able to measure the location
 *  of the beacons w.r.t its own reference frame.
 *  We assume in this example that the IMU frame coincides with the robot frame.
 *
 *  The robot extended pose X is in SE_2(3) and the beacon positions b_k in R^3,
 *
 *      X = |    R   p  v|              // position, orientation and linear velocity
 *          |        1   |
 *          |           1|
 *
 *      b_k = (bx_k, by_k, bz_k)    // lmk coordinates in world frame
 *
 *      alpha_k = (alphax_k, alphay_k, alphaz_k) // linear accelerometer measurements in IMU frame
 *
 *      omega_k = (omegax_k, omegay_k, omegaz_k) // gyroscope measurements in IMU frame
 *
 *      g = (0, 0, -9.80665)  // acceleration due to gravity in world frame
 *
 * Consider robot coordinate frame B and world coordinate frame A.
 * - p is the position of the origin of the robot frame B with respect to the world frame A
 * - R is the orientation of the robot frame B with respect to the world frame A
 * - v is the velocity of the robot frame with respect to the world frame,
 *            expressed in a frame whose origin coincides with the robot frame, oriented similar to the world frame
 *            (it is equivalent to p_dot in continuous time. This is usually called mixed-frame representation
 *            and is denoted as (B[A] v_AB), where B[A] is the mixed frame as described above.
 *            For reference, please see "Multibody Dynamics Notation" by Silvio Traversaro and Alessandro Saccon.
 *            Link: https://research.tue.nl/en/publications/multibody-dynamics-notation-version-2)
 * - a is the frame acceleration in mixed-representation (equivalent to p_doubledot in continuous time).
 * - omega_b as the angular velocity of the robot expressed in the robot frame
 *
 * The kinematic equations (1) can be written as,
 * p <-- p + v dt + 0.5 a dt^2
 * R <-- R Exp_SO3(omega_b)
 * v <-- v + a dt
 *
 * However, we would like to express the kinematics equations in the form,
 * X <-- X * Exp(u)
 * where, X \in SE_2(3), u \in R^9 and u_hat \in se_2(3)
 * Note that here input vector u is expressed in the local frame (robot frame).
 * This can be seen as a motion integration on a manifold defined by the group SE_2(3).
 *
 * The exponential mapping of SE_2(3) is defined as,
 * for u = [u_p, u_w, u_v]
 * Exp(u) = | Exp_SO3(u_w)   JlSO3(u_w) u_p   JlSO3(u_w) u_v |
 *          | 0    0    0                 1                0 |
 *          | 0    0    0                 0                1 |
 * where, JlSO3 is the left Jacobian of the SO(3) group.
 *
 * Please see the Appendix C of the paper "A micro Lie theory for state estimation in robotics",
 * for the definition of the left Jacobian of SO(3).
 * Please see the Appendix D of the paper, for the definition of Exp map for SE(3).
 * The Exp map of SE_2(3) is a simple extension from the Exp map of SE(3).
 * Also, please refer to Example 7 of the paper to understand when and how the left Jacobian of SO(3)
 * appears in the definitions of Exp maps. The Example 7 illustrates the scenario for SE(3).
 * We use a direct extension here for SE_2(3).
 * One can arrive to such a definition by following the convergent Taylor's series expansion
 * for the matrix exponential of the Lie algebra element (Equation 16 of the paper).
 *
 * As a result of X <-- X * Exp(u), we get (2)
 * p <-- p + R JlSO3(u_w) u_p
 * R <-- R Exp_SO3(u_w)
 * v <-- v + R JlSO3(u_w) u_v
 *
 * It is important to notice the subtle difference between (1) and (2) here,
 * which is specifically the influence of the left Jacobian of SO(3) in (2).
 * The approach in (1) considers the motion integration is done by defining
 * the exponential map in R3xSO(3)xR3 instead of SE_2(3),
 * in the sense explored in Example 7 of the Micro Lie theory paper. It must be noted that
 * as dt tends to 0, both sets of equations (1) and (2) tend to be the same, since JlSO3 tends to identity.
 *
 * Since, (2) exploits the algebra of the SE_2(3) group properly,
 * we would like to draw a relationship between the sets of equations (2)
 * and the IMU measurements which will constitute the exogeneous input vector u \in se_2(3).
 *
 * Considering R.T as the transpose of R, the IMU measurements are modeled as,
 *    - linear accelerometer measurements alpha = R.T (a - g) + w_acc
 *    - gyroscope measurements omega = omega_b + w_omega
 * Note that the IMU measurements are expressed in the IMU frame (coincides with the robot frame - assumption).
 * The IMU measurements are corrupted by noise,
 *    - w_omega is the additive white noise affecting the gyroscope measurements
 *    - w_acc is the additive white noise affecting the linear accelerometer measurements
 * It must be noted that we do not consider IMU biases in the IMU measurement model in this example.
 *
 * Taking into account all of the above considerations, the exogenous input vector u (3) becomes,
 *   u = (u_p, u_w, u_v) where,
 *   u_w = omega dt
 *   u_p = (R.T v dt + 0.5 dt^2 (alpha + R.T g)
 *   u_v = (alpha + R.T g) dt
 *
 * This choice of input vector allows us to directly use measurements from the IMU
 * for an unified motion integration involving position, orientation and linear velocity of the robot using SE_2(3).
 * Equations (2) and (3) lead us to the following evolution equations,
 *
 * p <-- p + JlSO3 R.T v dt + 0.5 JlSO3 (alpha + R.T g) dt^2
 * R <-- R Exp_SO3(omega dt)
 * v <-- v + JlSO3 (alpha + R.T g) dt
 *
 * The system propagation noise covariance matrix becomes,
 *    U = diagonal(0, 0, 0, sigma_omegax^2, sigma_omegay^2, sigma_omegaz^2, sigma_accx^2, sigma_accy^2, sigma_accz^2).
 *
 *  At the arrival of a exogeneous input u, the robot pose is updated
 *  with X <-- X * Exp(u) = X + u.
 *
 *  Landmark measurements are of the range and bearing type,
 *  though they are put in Cartesian form for simplicity.
 *  Their noise n is zero mean Gaussian, and is specified
 *  with a covariances matrix R.
 *  We notice that the SE_2(3) action is the same as a
 *  rigid motion action of SE(3).
 *  This is the action of X \in SE_2(3) on a 3-d point b \in R^3 defined as,
 *  X b = R b + p
 *
 *  Thus, the landmark measurements can be expressed as a group action on 3d points,
 *  y = h(X,b) = X^-1 * b
 *
 *      y_k = (brx_k, bry_k, brz_k)    // lmk coordinates in robot frame
 *
 *  We consider the beacons b_k situated at known positions.
 *  We define the extended pose to estimate as X in SE_2(3).
 *  The estimation error dx and its covariance P are expressed
 *  in the tangent space at X.
 *
 *  All these variables are summarized again as follows
 *
 *    X   : robot's extended pose, SE_2(3)
 *    u   : robot control input, u = u(X, y_imu) \in se_2(3) with X as state and y_imu = [alpha, omega] as IMU readings, see Eq. (3)
 *    U   : control perturbation covariance
 *    b_k : k-th landmark position, R^3
 *    y   : Cartesian landmark measurement in robot frame, R^3
 *    R   : covariance of the measurement noise
 *
 *  The motion and measurement models are
 *
 *    X_(t+1) = f(X_t, u) = X_t * Exp ( u )     // motion equation
 *    y_k     = h(X, b_k) = X^-1 * b_k          // measurement equation
 *
 *  The algorithm below comprises first a simulator to
 *  produce measurements, then uses these measurements
 *  to estimate the state, using a Lie-based error-state Kalman filter.
 *
 *  This file has plain code with only one main() function.
 *  There are no function calls other than those involving `manif`.
 *
 *  Printing simulated state and estimated state together
 *  with an unfiltered state (i.e. without Kalman corrections)
 *  allows for evaluating the quality of the estimates.
 *
 * A side note: Besides the approach described here in this illustration example,
 * there are other interesting works like the paper,
 * The Invariant Extended Kalman filter as a stable observer (https://arxiv.org/pdf/1410.1465.pdf)
 * which assume a specific structure for the system propagation dynamics "f(X_t, u)" (group affine dynamics)
 * that simplifies the covariance propagation and enables error dynamics with stronger convergence properties.
 *
 */

#include "manif/SE_2_3.h"

#include <Eigen/Dense>

#include <vector>
#include <iostream>
#include <iomanip>

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

using namespace Eigen;

typedef Array<double, 3, 1> Array3d;
typedef Array<double, 6, 1> Array6d;
typedef Matrix<double, 6, 1> Vector6d;
typedef Matrix<double, 6, 6> Matrix6d;
typedef Array<double, 9, 1> Array9d;
typedef Matrix<double, 9, 1> Vector9d;
typedef Matrix<double, 9, 9> Matrix9d;

int main()
{
    std::srand((unsigned int) time(0));

    // START CONFIGURATION
    //
    //
    const int NUMBER_OF_LMKS_TO_MEASURE = 5;

    // Define the robot extended pose element and its covariance
    manif::SE_2_3d X, X_simulation, X_unfiltered;
    Matrix9d    P;

    X_simulation.setIdentity();
    X.setIdentity();
    X_unfiltered.setIdentity();
    P.setZero();
    P.block<3, 3>(0, 0) = 0.001*Eigen::Matrix3d::Identity();
    P.block<3, 3>(3, 3) = 0.01*Eigen::Matrix3d::Identity();
    P.block<3, 3>(6, 6) = 0.001*Eigen::Matrix3d::Identity();

    // acceleration due to gravity in world frame
    Vector3d g;
    g << 0, 0, -9.80665;
    const double dt = 0.01;

    // IMU measurements in IMU frame
    Vector3d alpha, alpha_const, omega, alpha_prev, omega_prev;
    alpha_const << 0.1, 0.01, 0.1; // constant acceleration in IMU frame without gravity compensation
    omega << 0.01, 0.1, 0; // constant angular velocity about x- and y-direction in IMU frame

    // Previous IMU measurements in IMU frame initialized to values expected when stationary
    alpha_prev = alpha = alpha_const - (X_simulation.rotation()).transpose()*g;
    omega_prev << 0, 0, 0;

    // Define a control vector and its noise and covariance
    manif::SE_2_3Tangentd  u_simu, u_est, u_unfilt;
    Vector9d            u_nom, u_noisy, u_noise;
    Array9d             u_sigmas;
    Matrix9d            U;

    u_sigmas << 0.0, 0.0, 0.0, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01;
    U        = (u_sigmas * u_sigmas).matrix().asDiagonal();

    // Declare the Jacobians of the motion wrt robot and control
    manif::SE_2_3d::Jacobian F;     // F = J_x_x + (J_x_u * J_u_x)
    manif::SE_2_3d::Jacobian J_x_x; // d(X * exp(u)) / dX
    manif::SE_2_3d::Jacobian J_x_u; // d(X * exp(u)) / du
    manif::SE_2_3d::Jacobian J_u_x; // du / dX, since u is a state-dependent vector

    // Define five landmarks in R^3
    Vector3d b0, b1, b2, b3, b4, b;
    b0 << 2.0,  0.0,  0.0;
    b1 << 3.0, -1.0, -1.0;
    b2 << 2.0, -1.0,  1.0;
    b3 << 2.0,  1.0,  1.0;
    b4 << 2.0,  1.0, -1.0;
    std::vector<Vector3d> landmarks;
    landmarks.push_back(b0);
    landmarks.push_back(b1);
    landmarks.push_back(b2);
    landmarks.push_back(b3);
    landmarks.push_back(b4);

    // Define the beacon's measurements
    Vector3d                y, y_noise;
    Array3d                 y_sigmas;
    Matrix3d                R;   /**< Beacon measurement noise covariance matrix **/
    std::vector<Vector3d>   measurements(landmarks.size());

    y_sigmas << 0.01, 0.01, 0.01;
    R        = (y_sigmas * y_sigmas).matrix().asDiagonal();

    // Declare the Jacobian of the measurements wrt the robot pose
    Matrix<double, 3, 9>    H;      // H = J_e_x

    // Declare some temporaries
    Vector3d                e, z;   // expectation, innovation
    Matrix3d                E, Z;   // covariances of the above
    Matrix<double, 9, 3>    K;      // Kalman gain
    manif::SE_2_3Tangentd      dx;     // optimal update step, or error-state
    manif::SE_2_3d::Jacobian   J_xi_x; // Jacobian is typedef Matrix
    Matrix<double, 3, 9>       J_e_xi; // Jacobian

    //
    //
    // CONFIGURATION DONE

    // DEBUG
    cout << std::fixed   << std::setprecision(3) << std::showpos << endl;
    cout << "X STATE     :    X      Y      Z    TH_x   TH_y   TH_z     V_x   V_y    V_z" << endl;
    cout << "---------------------------------------------------------------------------" << endl;
    cout << "X simulated : Pos: " << X_simulation.translation().transpose() <<
                             " RPY: " << X_simulation.rotation().eulerAngles(2, 1, 0).reverse().transpose() <<
                             " Vel: " << X_simulation.linearVelocity().transpose() << endl;
    cout << "X estimated : Pos: " << X.translation().transpose() <<
                             " RPY: " << X.rotation().eulerAngles(2, 1, 0).reverse().transpose() <<
                             " Vel: " << X.linearVelocity().transpose() << endl;
    cout << "X unfilterd : Pos: " << X_unfiltered.translation().transpose() <<
                             " RPY: " << X_unfiltered.rotation().eulerAngles(2, 1, 0).reverse().transpose() <<
                             " Vel: " << X_unfiltered.linearVelocity().transpose() << endl;
    cout << "---------------------------------------------------------------------------" << endl;
    // END DEBUG

    // START TEMPORAL LOOP
    //
    //

    // Make 10 steps. Measure up to three landmarks each time.
    for (double t = 0; t < 10; t+=dt)
    {
        //// I. Simulation ###############################################################################

        /// get current simulated state and measurements from previous step
        auto R_k = X_simulation.rotation();
        auto v_k = X_simulation.linearVelocity();
        auto acc_k = alpha_prev + R_k.transpose()*g;

        /// input vector
        u_nom << (dt*(R_k.transpose())*v_k + 0.5*dt*dt*acc_k),  dt*omega_prev, dt*acc_k;

        /// simulate noise
        u_noise = u_sigmas * Array9d::Random();             // control noise
        u_noisy = u_nom + u_noise;                          // noisy control

        u_simu   = u_nom;
        u_unfilt = u_noisy;

        /// first we move - - - - - - - - - - - - - - - - - - - - - - - - - - - -
        X_simulation = X_simulation + u_simu;               // overloaded X.rplus(u) = X * exp(u)
        /// update expected IMU measurements
        alpha = alpha_const - X_simulation.rotation().transpose()*g; // update expected IMU measurement after moving

        /// then we measure all landmarks - - - - - - - - - - - - - - - - - - - -
        for (int i = 0; i < landmarks.size(); i++)
        {
            b = landmarks[i];                               // lmk coordinates in world frame

            /// simulate noise
            y_noise = y_sigmas * Array3d::Random();         // measurement noise

            y = X_simulation.inverse().act(b);              // landmark measurement, before adding noise
            y = y + y_noise;                                // landmark measurement, noisy
            measurements[i] << y;                     // store for the estimator just below
        }

        //// II. Estimation ###############################################################################

        /// get current state estimate to build the state-dependent control vector
        auto R_k_est = X.rotation();
        auto v_k_est = X.linearVelocity();
        auto acc_k_est = alpha_prev + R_k_est.transpose()*g;

        Eigen::Vector3d accLin = dt*(R_k_est.transpose())*v_k_est + 0.5*dt*dt*acc_k_est;
        Eigen::Vector3d gLin = R_k_est.transpose()*g*dt;
        Eigen::Matrix3d accLinCross = manif::skew(accLin);
        Eigen::Matrix3d gCross = manif::skew(gLin);

        u_est << accLin,  dt*omega_prev, dt*acc_k_est;
        u_est += u_noise;

        /// First we move - - - - - - - - - - - - - - - - - - - - - - - - - - - -

        X = X.plus(u_est, J_x_x, J_x_u);                        // X * exp(u), with Jacobians

        // Prepare Jacobian of state-dependent control vector
        J_u_x.setZero();
        J_u_x.block<3, 3>(0, 3) = accLinCross;
        J_u_x.block<3, 3>(0, 6) = Eigen::Matrix3d::Identity()*dt;
        J_u_x.block<3, 3>(6, 3) = gCross;
        F = J_x_x + J_x_u*J_u_x;                                // chain rule for system model Jacobian

        P = F * P * F.transpose() + J_x_u * U * J_x_u.transpose();

        /// Then we correct using the measurements of each lmk - - - - - - - - -
        for (int i = 0; i < NUMBER_OF_LMKS_TO_MEASURE; i++)
        {
            // landmark
            b = landmarks[i];                               // lmk coordinates in world frame

           // measurement
            y = measurements[i];                            // lmk measurement, noisy

            // expectation
            e = X.inverse(J_xi_x).act(b, J_e_xi);           // note: e = R.tr * ( b - t ), for X = (R,t).
            H = J_e_xi * J_xi_x;                            // note: H = J_e_x = J_e_xi * J_xi_x
            E = H * P * H.transpose();

            // innovation
            z = y - e;
            Z = E + R;

            // Kalman gain
            K = P * H.transpose() * Z.inverse();            // K = P * H.tr * ( H * P * H.tr + R).inv

            // Correction step
            dx = K * z;                                     // dx is in the tangent space at X

            // Update
            X = X + dx;                                     // overloaded X.rplus(dx) = X * exp(dx)
            P = P - K * Z * K.transpose();
        }

        //// III. Unfiltered ##############################################################################

        // move also an unfiltered version for comparison purposes
        X_unfiltered = X_unfiltered + u_unfilt;

        alpha_prev = alpha;
        omega_prev = omega;

        //// IV. Results ##############################################################################

        // DEBUG
        cout << "X simulated : Pos: " << X_simulation.translation().transpose() <<
                             " RPY: " << X_simulation.rotation().eulerAngles(2, 1, 0).reverse().transpose() <<
                             " Vel: " << X_simulation.linearVelocity().transpose() << endl;
        cout << "X estimated : Pos: " << X.translation().transpose() <<
                             " RPY: " << X.rotation().eulerAngles(2, 1, 0).reverse().transpose() <<
                             " Vel: " << X.linearVelocity().transpose() << endl;
        cout << "X unfilterd : Pos: " << X_unfiltered.translation().transpose() <<
                             " RPY: " << X_unfiltered.rotation().eulerAngles(2, 1, 0).reverse().transpose() <<
                             " Vel: " << X_unfiltered.linearVelocity().transpose() << endl;
        cout << "---------------------------------------------------------------------------" << endl;

        cout << "X simulated : log: " << X_simulation.log() << endl;
        cout << "X estimated : log: " << X.log() << endl;
        cout << "X unfilterd : log: " << X_unfiltered.log() << endl;
        cout << "---------------------------------------------------------------------------" << endl;
        cout << "---------------------------------------------------------------------------" << endl;
        // END DEBUG

    }

    //
    //
    // END OF TEMPORAL LOOP. DONE.

    return 0;
}


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

int main(void)
{
    int const N = 5;
    Eigen::MatrixXi A(N, N);
    A.setRandom();

    std::cout << "A =\n" << A << '\n' <<std::endl;
    std::cout << "A(2..3,:) =\n" << A.middleRows(2, 2) << std::endl;

    return 0;
}


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

#include <Eigen/Dense>

#include <openbr/plugins/openbr_internal.h>
#include <openbr/core/opencvutils.h>
#include <openbr/core/eigenutils.h>

using namespace cv;

namespace br
{

/*!
 * \ingroup transforms
 * \brief Procrustes alignment of points
 * \author Scott Klum \cite sklum
 */
class ProcrustesTransform : public MetadataTransform
{
    Q_OBJECT

    Q_PROPERTY(bool warp READ get_warp WRITE set_warp RESET reset_warp STORED false)
    BR_PROPERTY(bool, warp, true)

    Eigen::MatrixXf meanShape;

    void train(const TemplateList &data)
    {
        QList< QList<QPointF> > normalizedPoints;

        // Normalize all sets of points
        foreach (br::Template datum, data) {
            QList<QPointF> points = datum.file.points();
            QList<QRectF> rects = datum.file.rects();

            if (points.empty() || rects.empty()) continue;

            // Assume rect appended last was bounding box
            points.append(rects.last().topLeft());
            points.append(rects.last().topRight());
            points.append(rects.last().bottomLeft());
            points.append(rects.last().bottomRight());

            // Center shape at origin
            Scalar mean = cv::mean(OpenCVUtils::toPoints(points).toVector().toStdVector());
            for (int i = 0; i < points.size(); i++) points[i] -= QPointF(mean[0],mean[1]);

            // Remove scale component
            float norm = cv::norm(OpenCVUtils::toPoints(points).toVector().toStdVector());
            for (int i = 0; i < points.size(); i++) points[i] /= norm;

            normalizedPoints.append(points);
        }

        if (normalizedPoints.empty()) qFatal("Unable to calculate normalized points");

        // Determine mean shape, assuming all shapes contain the same number of points
        meanShape = Eigen::MatrixXf(normalizedPoints[0].size(), 2);

        for (int i = 0; i < normalizedPoints[0].size(); i++) {
            double x = 0;
            double y = 0;

            for (int j = 0; j < normalizedPoints.size(); j++) {
                x += normalizedPoints[j][i].x();
                y += normalizedPoints[j][i].y();
            }

            x /= (double)normalizedPoints.size();
            y /= (double)normalizedPoints.size();

            meanShape(i,0) = x;
            meanShape(i,1) = y;
        }
    }

    void projectMetadata(const File &src, File &dst) const
    {
        QList<QPointF> points = src.points();
        QList<QRectF> rects = src.rects();

        if (points.empty() || rects.empty()) {
            dst = src;
            if (Globals->verbose) qWarning("Procrustes alignment failed because points or rects are empty.");
            return;
        }

        // Assume rect appended last was bounding box
        points.append(rects.last().topLeft());
        points.append(rects.last().topRight());
        points.append(rects.last().bottomLeft());
        points.append(rects.last().bottomRight());

        Scalar mean = cv::mean(OpenCVUtils::toPoints(points).toVector().toStdVector());
        for (int i = 0; i < points.size(); i++) points[i] -= QPointF(mean[0],mean[1]);

        Eigen::MatrixXf srcMat(points.size(), 2);
        float norm = cv::norm(OpenCVUtils::toPoints(points).toVector().toStdVector());
        for (int i = 0; i < points.size(); i++) {
            points[i] /= norm;
            srcMat(i,0) = points[i].x();
            srcMat(i,1) = points[i].y();
        }

        Eigen::JacobiSVD<Eigen::MatrixXf> svd(srcMat.transpose()*meanShape, Eigen::ComputeThinU | Eigen::ComputeThinV);
        Eigen::MatrixXf R = svd.matrixU()*svd.matrixV().transpose();

        dst = src;

        // Store procrustes stats in the order:
        // R(0,0), R(1,0), R(1,1), R(0,1), mean_x, mean_y, norm
        QList<float> procrustesStats;
        procrustesStats << R(0,0) << R(1,0) << R(1,1) << R(0,1) << mean[0] << mean[1] << norm;
        dst.setList<float>("ProcrustesStats",procrustesStats);

        if (warp) {
            Eigen::MatrixXf dstMat = srcMat*R;
            for (int i = 0; i < dstMat.rows(); i++) {
                dst.appendPoint(QPointF(dstMat(i,0),dstMat(i,1)));
            }
        }
    }

    void store(QDataStream &stream) const
    {
        stream << meanShape;
    }

    void load(QDataStream &stream)
    {
        stream >> meanShape;
    }

};

BR_REGISTER(Transform, ProcrustesTransform)

} // namespace br

#include "metadata/procrustes.moc"


#include <stan/math/rev/scal.hpp>
#include <gtest/gtest.h>
#include <test/unit/math/rev/scal/fun/nan_util.hpp>
#include <test/unit/math/rev/scal/util.hpp>
#include <boost/math/special_functions/beta.hpp>

TEST(AgradRev,ibeta_vvv) {
  using stan::math::var;
  using stan::math::ibeta;
  using stan::math::ibeta;

  using boost::math::ibeta_derivative;

  AVAR a = 0.6;
  AVAR b = 0.3;
  AVAR c = 0.5;
  AVAR f = ibeta(a,b,c);
  EXPECT_FLOAT_EQ(0.3121373, f.val());

  AVEC x = createAVEC(a,b,c);
  VEC grad_f;
  f.grad(x,grad_f);
  EXPECT_FLOAT_EQ(-0.436993,grad_f[0]);
  EXPECT_FLOAT_EQ(0.7779751,grad_f[1]);
  EXPECT_FLOAT_EQ(ibeta_derivative(a.val(), b.val(), c.val()),grad_f[2]);

  a = 3;
  b = 2;
  c = 0.2;
  f = ibeta(a,b,c);
  EXPECT_FLOAT_EQ(0.0272, f.val());
  x = createAVEC(a,b,c);
  f.grad(x,grad_f);
  EXPECT_FLOAT_EQ(-0.03737671,grad_f[0]);
  EXPECT_FLOAT_EQ(0.02507405,grad_f[1]);
  EXPECT_FLOAT_EQ(ibeta_derivative(a.val(), b.val(), c.val()),grad_f[2]);
}
TEST(AgradRev,ibeta_vvd) {
  using stan::math::var;
  using stan::math::ibeta;
  using stan::math::ibeta;

  using boost::math::ibeta_derivative;

  AVAR a = 0.6;
  AVAR b = 0.3;
  double c = 0.5;
  AVAR f = ibeta(a,b,c);
  EXPECT_FLOAT_EQ(0.3121373, f.val());

  AVEC x = createAVEC(a,b);
  VEC grad_f;
  f.grad(x,grad_f);
  EXPECT_FLOAT_EQ(-0.436993,grad_f[0]);
  EXPECT_FLOAT_EQ(0.7779751,grad_f[1]);

  a = 3;
  b = 2;
  c = 0.2;
  f = ibeta(a,b,c);
  EXPECT_FLOAT_EQ(0.0272, f.val());
  x = createAVEC(a,b);
  f.grad(x,grad_f);
  EXPECT_FLOAT_EQ(-0.03737671,grad_f[0]);
  EXPECT_FLOAT_EQ(0.02507405,grad_f[1]);
}
TEST(AgradRev,ibeta_vdv) {
  using stan::math::var;
  using stan::math::ibeta;
  using stan::math::ibeta;

  using boost::math::ibeta_derivative;

  AVAR a = 0.6;
  double b = 0.3;
  AVAR c = 0.5;
  AVAR f = ibeta(a,b,c);
  EXPECT_FLOAT_EQ(0.3121373, f.val());

  AVEC x = createAVEC(a,c);
  VEC grad_f;
  f.grad(x,grad_f);
  EXPECT_FLOAT_EQ(-0.436993,grad_f[0]);
  EXPECT_FLOAT_EQ(ibeta_derivative(a.val(), b, c.val()),grad_f[1]);

  a = 3;
  b = 2;
  c = 0.2;
  f = ibeta(a,b,c);
  EXPECT_FLOAT_EQ(0.0272, f.val());
  x = createAVEC(a,c);
  f.grad(x,grad_f);
  EXPECT_FLOAT_EQ(-0.03737671,grad_f[0]);
  EXPECT_FLOAT_EQ(ibeta_derivative(a.val(), b, c.val()),grad_f[1]);
}
TEST(AgradRev,ibeta_vdd) {
  using stan::math::var;
  using stan::math::ibeta;
  using stan::math::ibeta;

  using boost::math::ibeta_derivative;

  AVAR a = 0.6;
  double b = 0.3;
  double c = 0.5;
  AVAR f = ibeta(a,b,c);
  EXPECT_FLOAT_EQ(0.3121373, f.val());

  AVEC x = createAVEC(a);
  VEC grad_f;
  f.grad(x,grad_f);
  EXPECT_FLOAT_EQ(-0.436993,grad_f[0]);

  a = 3;
  b = 2;
  c = 0.2;
  f = ibeta(a,b,c);
  EXPECT_FLOAT_EQ(0.0272, f.val());
  x = createAVEC(a);
  f.grad(x,grad_f);
  EXPECT_FLOAT_EQ(-0.03737671,grad_f[0]);
}
TEST(AgradRev,ibeta_dvv) {
  using stan::math::var;
  using stan::math::ibeta;
  using stan::math::ibeta;

  using boost::math::ibeta_derivative;

  double a = 0.6;
  AVAR b = 0.3;
  AVAR c = 0.5;
  AVAR f = ibeta(a,b,c);
  EXPECT_FLOAT_EQ(0.3121373, f.val());

  AVEC x = createAVEC(b,c);
  VEC grad_f;
  f.grad(x,grad_f);
  EXPECT_FLOAT_EQ(0.7779751,grad_f[0]);
  EXPECT_FLOAT_EQ(ibeta_derivative(a, b.val(), c.val()),grad_f[1]);

  a = 3;
  b = 2;
  c = 0.2;
  f = ibeta(a,b,c);
  EXPECT_FLOAT_EQ(0.0272, f.val());
  x = createAVEC(b,c);
  f.grad(x,grad_f);
  EXPECT_FLOAT_EQ(0.02507405,grad_f[0]);
  EXPECT_FLOAT_EQ(ibeta_derivative(a, b.val(), c.val()),grad_f[1]);
}
TEST(AgradRev,ibeta_dvd) {
  using stan::math::var;
  using stan::math::ibeta;
  using stan::math::ibeta;

  using boost::math::ibeta_derivative;

  double a = 0.6;
  AVAR b = 0.3;
  double c = 0.5;
  AVAR f = ibeta(a,b,c);
  EXPECT_FLOAT_EQ(0.3121373, f.val());

  AVEC x = createAVEC(b);
  VEC grad_f;
  f.grad(x,grad_f);
  EXPECT_FLOAT_EQ(0.7779751,grad_f[0]);

  a = 3;
  b = 2;
  c = 0.2;
  f = ibeta(a,b,c);
  EXPECT_FLOAT_EQ(0.0272, f.val());
  x = createAVEC(b);
  f.grad(x,grad_f);
  EXPECT_FLOAT_EQ(0.02507405,grad_f[0]);
}
TEST(AgradRev,ibeta_ddv) {
  using stan::math::var;
  using stan::math::ibeta;
  using stan::math::ibeta;

  using boost::math::ibeta_derivative;

  double a = 0.6;
  double b = 0.3;
  AVAR c = 0.5;
  AVAR f = ibeta(a,b,c);
  EXPECT_FLOAT_EQ(0.3121373, f.val());

  AVEC x = createAVEC(c);
  VEC grad_f;
  f.grad(x,grad_f);
  EXPECT_FLOAT_EQ(ibeta_derivative(a, b, c.val()),grad_f[0]);

  a = 3;
  b = 2;
  c = 0.2;
  f = ibeta(a,b,c);
  EXPECT_FLOAT_EQ(0.0272, f.val());
  x = createAVEC(c);
  f.grad(x,grad_f);
  EXPECT_FLOAT_EQ(ibeta_derivative(a, b, c.val()),grad_f[0]);
}

struct ibeta_fun {
  template <typename T0, typename T1, typename T2>
  inline
  typename stan::return_type<T0,T1,T2>::type
  operator()(const T0& arg1,
             const T1& arg2,
             const T2& arg3) const {
    return ibeta(arg1,arg2,arg3);
  }
};

TEST(AgradRev,ibeta_NaN) {
  ibeta_fun ibeta_;
  test_nan(ibeta_,0.6,0.3,0.5,true,false);
}

TEST(AgradRev, check_varis_on_stack) {
  AVAR a = 0.6;
  AVAR b = 0.3;
  AVAR c = 0.5;
  test::check_varis_on_stack(stan::math::ibeta(a, b, c));
  test::check_varis_on_stack(stan::math::ibeta(a, b, 0.5));
  test::check_varis_on_stack(stan::math::ibeta(a, 0.3, c));
  test::check_varis_on_stack(stan::math::ibeta(a, 0.3, 0.5));
  test::check_varis_on_stack(stan::math::ibeta(0.6, b, c));
  test::check_varis_on_stack(stan::math::ibeta(0.6, b, 0.5));
  test::check_varis_on_stack(stan::math::ibeta(0.6, 0.3, c));
}


//
// Created by Xinyu Zhang on 3/26/21.
//
#include <iostream>
// #include <Eigen/Dense>
// #include <cosan/io/utils.h>
#include <cosan/selection/randomgridsearch.h>
#include <cosan/selection/gridsearch.h>
#include <cosan/selection/kfold.h>
#include <cosan/selection/timeseriessplit.h>
#include <cosan/selection/randomkfold.h>
#include <cosan/utils/utils.h>
#include <cosan/data/CosanData.h>
#include <cosan/model/CosanPCRRidge.h>
#include <cosan/model/CosanPrincipalComponentRegression.h>
#include <cosan/model/CosanRidgeRegression.h>
#include <cosan/model/CosanLinearRegression.h>
#include <cosan/preprocessing/customtransform.h>
#include <cosan/preprocessing/encoder.h>
#include <cosan/preprocessing/minmaxscaler.h>
#include <cosan/preprocessing/missingvalues.h>
#include <cosan/preprocessing/normalizer.h>
#include <cosan/preprocessing/onehotEncoder.h>
#include <cosan/preprocessing/ordinalEncoder.h>
#include <cosan/preprocessing/overunderflow.h>
#include <cosan/preprocessing/polynomialfeatures.h>
#include <cosan/preprocessing/preprocessor.h>
#include <cosan/preprocessing/principalcomponentanalysis.h>
#include <cosan/preprocessing/standardScaler.h>
// #include <cosan/model/CosanLinearRegression.h>
// #include <cosan/model/CosanRidgeRegression.h>
//using namespace Eigen;
//using namespace std;
// gcc -I. ./test/Implementation.cpp
typedef double db;
int main() {
	fmt::print("fmt library can be used", 42);
//  Data Reading
	constexpr gsl::index nrows = 3;
	constexpr gsl::index ncols = 3;

    Cosan::CosanMatrix<db> CM;
    CM.resize(nrows,ncols);
    CM<< 1,2,3,
        4,5,6,
        7,8,9;
    Cosan::CosanData<db>  CD0(CM);
    std::vector<db> inputX({1,2,3,4,5,6});
    Cosan::CosanData<db>  CD1(inputX,inputX,6,"rowfirst");
    std::cout<<CD1.GetInput()<<std::endl;
    db lb=0,ub=1;
    Cosan::CosanData<db> CD2(3,4,lb,ub);
    std::cout<<CD2.GetInput()<<std::endl;

//  Data Preprocessing
    Cosan::CosanRawData<db> CRD("./example_data/toy2/X_.csv","./example_data/toy2/Y_.csv");
    std::cout<<CRD.GetSummaryMessageX()<<std::endl;
    std::cout<<CRD.GetSummaryMessageY()<<std::endl;
	Cosan::OverUnderFlow  OUF(CRD);
	Cosan::MissingValues  MSV(CRD);
	Cosan::StandardScaler SS(CRD);
	Cosan::Normalizer NM(CRD,2);
    Cosan::Encoder ED(CRD,true);
    lb = 2;
    ub=6;
	Cosan::MinmaxScaler MMS(CRD,lb,ub);
	Cosan::PolynomialFeatures PF(CRD,{{1,0},{1,1}});
	Cosan::PrincipalComponentAnalysis PCA(CRD);
	std::cout<<PCA.GetPC()<<std::endl;
//  Model fitting
	Cosan::CosanRawData<db> CD("./example_data/toy/X.csv","./example_data/toy/y.csv");
	Cosan::CosanLinearRegression<db> CLRwbias(true);
	CLRwbias.fit(CD.GetInput(),CD.GetTarget());
	std::cout<<CLRwbias.GetBeta()<<std::endl;
	std::cout<<(CLRwbias.predict(CD.GetInput())-CD.GetTarget()).norm()<<std::endl;
	Cosan::CosanLinearRegression<db> CLRwobias(false);
	CLRwobias.fit(CD.GetInput(),CD.GetTarget());
	std::cout<<CLRwobias.GetBeta()<<std::endl;
	std::cout<<(CLRwobias.predict(CD.GetInput())-CD.GetTarget()).norm()<<std::endl;

	db RegularizationTerm = 1;
	Cosan::CosanRidgeRegression<db> CRRwBias(RegularizationTerm,true);
	CRRwBias.fit(CD.GetInput(),CD.GetTarget());
	std::cout<<CRRwBias.GetBeta()<<std::endl;
	std::cout<<(CRRwBias.predict(CD.GetInput())-CD.GetTarget()).norm()<<std::endl;

	gsl::index ncomp = 4;
	Cosan::CosanPrincipalComponentRegression<db> CPCR(ncomp);
	CPCR.fit(CD.GetInput(),CD.GetTarget());
	std::cout<<CPCR.GetBeta()<<std::endl;
	std::cout<<(CPCR.predict(CD.GetInput())-CD.GetTarget()).norm()<<std::endl;

	db regularier = 0.01;
	Cosan::CosanPCRRidge<db> CPCRR({ncomp,regularier});
	CPCRR.fit(CD.GetInput(),CD.GetTarget());
	std::cout<<CPCRR.GetBeta()<<std::endl;
	std::cout<<(CPCRR.predict(CD.GetInput())-CD.GetTarget()).norm()<<std::endl;
	std::cout<<CD.GetcolsX()<<std::endl;

	constexpr gsl::index nrows1 = 10000;
	constexpr gsl::index ncols1 = 10;

    Cosan::CosanMatrix<db> X_input;
    Cosan::CosanMatrix<db> Y_input;
	X_input.resize(nrows1,ncols1);
	Y_input.resize(nrows1,1);
    X_input = Eigen::Matrix<decltype(X_input)::Scalar,nrows1,ncols1>::Random();
    Y_input = Eigen::Matrix<decltype(X_input)::Scalar,nrows1,1>::Random();
    Cosan::CosanData CD_search(X_input,Y_input);
// 	Cosan::CosanRidgeRegression<db> CRRwBias(RegularizationTerm,true);
    Cosan::MeanSquareError<decltype(X_input)::Scalar> mse;
    db a = 0.05;
    std::vector<db> v(10);
    std::generate(v.begin(), v.end(), [n = 1, &a]() mutable { return n++ * a; });
    Cosan::KFold kf(5);
    Cosan::GridSearch GDS(CD_search,CRRwBias,mse,kf,v);
    fmt::print("The best params selected is\n ");
    std::cout<<GDS.GetBestParams()<<std::endl;
//  Grid Search

    return 0;
}


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, Indiana University,
// Bloomington, IN 47405.
//
// Permission to modify the code and to distribute the code is
// granted, provided the text of this NOTICE is retained, a notice if
// the code was modified is included with the above COPYRIGHT NOTICE
// and with the COPYRIGHT NOTICE in the LICENSE file, and that the
// LICENSE file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
#include <vector>
#include <string>
#include <boost/graph/topological_sort.hpp>
#include <boost/graph/leda_graph.hpp>
// Undefine macros from LEDA that conflict with the C++ Standard Library.
#undef string
#undef vector

int
main()
{
  using namespace boost;
  typedef GRAPH < std::string, char >graph_t;
  graph_t leda_g;
  typedef graph_traits < graph_t >::vertex_descriptor vertex_t;
  std::vector < vertex_t > vert(7);
  vert[0] = add_vertex(std::string("pick up kids from school"), leda_g);
  vert[1] = add_vertex(std::string("buy groceries (and snacks)"), leda_g);
  vert[2] = add_vertex(std::string("get cash at ATM"), leda_g);
  vert[3] =
    add_vertex(std::string("drop off kids at soccer practice"), leda_g);
  vert[4] = add_vertex(std::string("cook dinner"), leda_g);
  vert[5] = add_vertex(std::string("pick up kids from soccer"), leda_g);
  vert[6] = add_vertex(std::string("eat dinner"), leda_g);

  add_edge(vert[0], vert[3], leda_g);
  add_edge(vert[1], vert[3], leda_g);
  add_edge(vert[1], vert[4], leda_g);
  add_edge(vert[2], vert[1], leda_g);
  add_edge(vert[3], vert[5], leda_g);
  add_edge(vert[4], vert[6], leda_g);
  add_edge(vert[5], vert[6], leda_g);

  std::vector < vertex_t > topo_order;
  node_array < default_color_type > color_array(leda_g);

  topological_sort(leda_g, std::back_inserter(topo_order),
                   color_map(make_leda_node_property_map(color_array)));

  std::reverse(topo_order.begin(), topo_order.end());
  int n = 1;
  for (std::vector < vertex_t >::iterator i = topo_order.begin();
       i != topo_order.end(); ++i, ++n)
    std::cout << n << ": " << leda_g[*i] << std::endl;

  return EXIT_SUCCESS;
}


#pragma once

#include <Eigen/Dense>

struct Triangle
{
public:
    Triangle(Eigen::Vector3d a_vertex0, Eigen::Vector3d a_vertex1,
             Eigen::Vector3d a_vertex2)
        : vertex0(a_vertex0), vertex1(a_vertex1), vertex2(a_vertex2)
    {
    }

    Eigen::Vector3d vertex0;
    Eigen::Vector3d vertex1;
    Eigen::Vector3d vertex2;
};

class RayCaster
{
public:
    RayCaster(Eigen::Vector3d ray_origin, Eigen::Vector3d direction_vector);
    ~RayCaster();

    /* Tomas Moeller algorithm */
    bool CheckTriangleIntersection(const Triangle& triangle);

private:
    Eigen::Vector3d ray_origin_;
    Eigen::Vector3d direction_vector_;
};


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

namespace ublas = boost::numeric::ublas;

ublas::matrix<double> get_K(int N, double a0 = 2, double a1 = 2)
{
	ublas::identity_matrix<double> I(N, N);
	ublas::matrix<double> m1 = 2.0 * I;
	m1(0, 0) = a0;
	m1(N-1, N-1) = a1;

	ublas::symmetric_matrix<double, ublas::lower> m2(N, N);
	for (unsigned i = 0; i < m2.size1(); ++ i)
		for (unsigned j = 0; j <= i; ++ j)
			m2(i, j) = (j == i-1)? -1:0;
	return m1 + m2;
}

int main()
{
	int N = 3;
	double h = 1.0 / (N - 1);
	ublas::matrix<double> K = get_K(N);
	std::cout << K << std::endl;

	ublas::matrix<double> A = get_K(N, 1, 1);
	std::cout << A << std::endl;

	ublas::matrix<double> A1 = get_K(N, 2, 1);
	std::cout << A1 << std::endl;

	ublas::matrix<double> A2 = get_K(N, 1, 2);
	std::cout << A2 << std::endl;

	ublas::matrix<double> Kinv(N, N);
	Kinv.assign(ublas::identity_matrix<double> (K.size1()));
	ublas::permutation_matrix<size_t> pm(K.size1());
	ublas::lu_factorize(K, pm);
	ublas::lu_substitute(K, pm, Kinv);

	std::cout << Kinv << std::endl;

	double* tmp = (double*) Kinv.data().begin();
	int size = Kinv.data().size();

	for (int i = 0; i < size; ++i)
		std::cout << tmp[i] << std::endl;
	return 0;
}


#pragma once
#ifndef _RAY_SPHERE_
#define _RAY_SPHERE_

#include <Eigen/Core>

using namespace Eigen;

namespace raytracer
{

class Sphere : public Surface
{
public:
  Sphere(Vector3f center, float radius, std::string material_name = "") :
    Surface(center, material_name),
    radius_(radius),
    radius2_(radius * radius)
  {
  }

  bool Intersect(const Ray& ray, HitData& hit) override
  {
    // Calculate ray-sphere intersection using geometric approach
    Vector3f posray = position_ - ray.position();
    // Cos theta of hit point and ray
    float s = posray.dot(ray.direction());
    float length2 = posray.squaredNorm();

    // If the angle between the ray and the direction is less than 90
    if (s > 0.0f)
    {
      float m2 = length2 - s * s;

      if (m2 < radius2_)
      {
        float q = std::sqrt(radius2_ - m2);

        hit.t = (length2 > radius2_) ? s - q : s + q;
        hit.hit_point = ray.evaluate(hit.t);

        return (hit.t > 0.0f && hit.t < hit.tMax);
      }
    }

    return false;
  }

  Vector3f normal(const Vector3f& point) const
  {
    return (point - position_).normalized();
  }
private:
  float radius_, radius2_;
};

}

#endif /* end of include guard: _RAY_SPHERE_ */


#ifndef FAST_GICP_SO3_HPP
#define FAST_GICP_SO3_HPP

#include <Eigen/Core>

namespace fast_gicp {

inline Eigen::Matrix3f skew(const Eigen::Vector3f& x) {
  Eigen::Matrix3f skew = Eigen::Matrix3f::Zero();
  skew(0, 1) = -x[2];
  skew(0, 2) = x[1];
  skew(1, 0) = x[2];
  skew(1, 2) = -x[0];
  skew(2, 0) = -x[1];
  skew(2, 1) = x[0];

  return skew;
}

}

#endif


/**
   \file powell_method.hpp
   \brief powerll optimization method
   \author Junhua Gu
 */

#ifndef POWELL_METHOD
#define POWELL_METHOD
#define OPT_HEADER
#include <core/optimizer.hpp>
//#include <blitz/array.h>
#include <limits>
#include <cassert>
#include <cmath>
#include "../linmin/linmin.hpp"
#include <algorithm>
#include <iostream>

namespace opt_utilities
{
  /**
     \brief Impliment of an optimization method
     \tparam rT return type of the object function
     \tparam pT parameter type of the object function
   */
  template <typename rT,typename pT>
  class powell_method
    :public opt_method<rT,pT>
  {
  public:
    typedef pT array1d_type;
    typedef rT T;
  private:
    func_obj<rT,pT>* p_fo;
    optimizer<rT,pT>* p_optimizer;
    volatile bool bstop;
    //typedef blitz::Array<rT,2> array2d_type;

    const char* do_get_type_name()const
    {
      return "powell method";
    }
  private:
    array1d_type start_point;
    array1d_type end_point;

  private:
    int ncom;
    array1d_type pcom_p;
    array1d_type xicom_p;
    rT threshold;
    T** xi;
    T* xi_1d;
  private:
    rT func(const pT& x)
    {
      assert(p_fo!=NULL_PTR);
      return p_fo->eval(x);
    }

  private:
    void clear_xi()
    {
      if(xi_1d!=NULL_PTR)
	{
	  delete[] xi_1d;
	}
      if(xi!=NULL_PTR)
	{
	  delete[] xi;
	}
    }

    void init_xi(int n)
    {
      clear_xi();
      xi_1d=new T[n*n];
      xi=new T*[n];
      for(int i=0;i!=n;++i)
	{
	  xi[i]=xi_1d+i*n;
	}
      for(int i=0;i!=n;++i)
	{
	  for(int j=0;j!=n;++j)
	    {
	      xi[i][j]=(j==i?1:0);
	    }
	}
    }

    void powell(array1d_type& p,const T ftol,
	   int& iter,T& fret)
    {
      const int ITMAX=200;
      const T TINY=std::numeric_limits<T>::epsilon();
      int i,j,ibig;
      T del,fp,fptt,t;
      int n=(int)get_size(p);
      array1d_type pt(n);
      array1d_type ptt(n);
      array1d_type xit(n);
      fret=p_fo->eval(p);

      for(j=0;j<n;++j)
	{
	  //get_element(pt,j)=get_element(p,j);
	  set_element(pt,j,get_element(p,j));
	}
      for(iter=0;!bstop;++iter)
	{
	  fp=fret;
	  ibig=0;
	  del=0.0;
	  for(i=0;i<n;++i)
	    {
#ifdef _OPENMP
#pragma omp parallel for
#endif
	      for(j=0;j<n;++j)
		{
		  //get_element(xit,j)=xi[j][i];
		  set_element(xit,j,xi[j][i]);
		}
	      fptt=fret;
	      linmin(p,xit,fret,(*p_fo));
	      if((fptt-fret)>del)
		{
		  del=fptt-fret;
		  ibig=i+1;
		}
	    }
	  if(T(2.)*(fp-fret)<=ftol*(tabs(fp)+tabs(fret))+TINY)
	    {
	      return;
	    }
	  if(iter==ITMAX)
	    {
	      std::cerr<<"powell exceeding maximun iterations."<<std::endl;
	      return;
	    }
#ifdef _OPENMP
#pragma omp parallel for
#endif
	  for(j=0;j<n;++j)
	    {
	      //get_element(ptt,j)=T(2.)*get_element(p,j)-get_element(pt,j);
	      set_element(ptt,j,T(2.)*get_element(p,j)-get_element(pt,j));
	      //get_element(xit,j)=
	      //get_element(p,j)-get_element(pt,j);
	      set_element(xit,j,get_element(p,j)-get_element(pt,j));
	      //get_element(pt,j)=get_element(p,j);
	      set_element(pt,j,get_element(p,j));
	    }
	  fptt=func(ptt);
	  if(fptt<fp)
	    {
	      t=T(2.)*(fp-T(2.)*fret+fptt)*sqr(T(fp-fret-del))-del*sqr(T(fp-fptt));
	      if(t<T(0.))
		{
		  linmin(p,xit,fret,*p_fo);
#ifdef _OPENMP
#pragma omp parallel for
#endif
		  for(j=0;j<n;++j)
		    {
		      xi[j][ibig-1]=xi[j][n-1];
		      xi[j][n-1]=get_element(xit,j);

		    }
		}
	    }
	}
    }

  public:

    powell_method()
      :threshold(1e-4),xi(NULL_PTR),xi_1d(NULL_PTR)
    {}

    virtual ~powell_method()
    {
      clear_xi();
    };

    powell_method(const powell_method<rT,pT>& rhs)
      :opt_method<rT,pT>(rhs),p_fo(rhs.p_fo),p_optimizer(rhs.p_optimizer),
       start_point(rhs.start_point),
       end_point(rhs.end_point),
       ncom(rhs.ncom),
       threshold(rhs.threshold),xi(NULL_PTR),xi_1d(NULL_PTR)
    {
    }

    powell_method<rT,pT>& operator=(const powell_method<rT,pT>& rhs)
    {
      threshold=rhs.threshold;
      xi=0;
      xi_1d=0;
      p_fo=rhs.p_fo;
      p_optimizer=rhs.p_optimizer;
      start_point=rhs.start_point;
      end_point=rhs.end_point;
      ncom=rhs.ncom;
      threshold=rhs.threshold;
    }

    opt_method<rT,pT>* do_clone()const
    {
      return new powell_method<rT,pT>(*this);
    }

    void do_set_start_point(const array1d_type& p)
    {
      resize(start_point,get_size(p));
      opt_eq(start_point,p);
    }

    array1d_type do_get_start_point()const
    {
      return start_point;
    }

    void do_set_lower_limit(const array1d_type&)
    {}

    void do_set_upper_limit(const array1d_type&)
    {}

    void do_set_precision(rT t)
    {
      threshold=t;
    }

    rT do_get_precision()const
    {
      return threshold;
    }

    void do_set_optimizer(optimizer<rT,pT>& o)
    {
      p_optimizer=&o;
      p_fo=p_optimizer->ptr_func_obj();
    }

    pT do_optimize()
    {
      bstop=false;
      init_xi((int)get_size(start_point));

      for(int i=0;i<(int)get_size(start_point);++i)
	{
	  for(int j=0;j<(int)get_size(start_point);++j)
	    {
	      xi[i][j]=(i==j)?1:0;
	    }
	}

      int iter=100;
      opt_eq(end_point,start_point);
      rT fret;
      powell(end_point,threshold,iter,fret);
      return end_point;
    }

    void do_stop()
    {
      bstop=true;
    }

  };

}

#endif
//EOF


#ifndef PRECONDITIONED_CONJUGATE_GRADIENT_H
#define PRECONDITIONED_CONJUGATE_GRADIENT_H
#include <Eigen/Dense>
#include "mtao/solvers/linear/linear.hpp"
#include "mtao/solvers/cholesky/ldlt.hpp"
#include <iostream>

namespace mtao::solvers::linear {
template <typename MatrixType, typename VectorType, typename Preconditioner>
    struct PCGSolver;

template <typename MatrixType, typename VectorType, typename Preconditioner>
struct solver_traits<PCGSolver< MatrixType, VectorType, Preconditioner> > {
    using Scalar = typename VectorType::Scalar;
    using Matrix = MatrixType;
    using Vector = VectorType;
};

template <typename MatrixType, typename VectorType, typename Preconditioner>
struct PCGSolver: public IterativeLinearSolver<PCGSolver< MatrixType, VectorType, Preconditioner> >
{
    typedef MatrixType Matrix;
    typedef VectorType Vector;
    typedef typename Vector::Scalar Scalar;
    using Base = IterativeLinearSolver<PCGSolver< MatrixType, VectorType, Preconditioner> >;
    using Base::A ;
    using Base::b ;
    using Base::x ;
    using Base::Base;
    void compute()
    {
        precond = Preconditioner(A());
        r = b()-A()*x();
        precond->solve(r,z);
        p = z;
        Ap = A()*p;
        rdz = r.dot(z);
    }
    Scalar error()
    {
        return r.template lpNorm<Eigen::Infinity>();
    }

    void step()
    {
        alpha = (rdz)/(p.dot(Ap));
        x()+=alpha * p;
        r-=alpha * Ap;
        precond->solve(r,z);
        beta=1/rdz;
        rdz = r.dot(z);
        beta*=rdz;
        p=z+beta*p;
        Ap=A()*p;
    }
private:
    Vector r;
    Vector z;
    Vector p;
    Vector Ap;
    Scalar rdz;
    Scalar alpha, beta;
    std::optional<Preconditioner> precond;

};

template <typename Preconditioner, typename Matrix, typename Vector>
void PCGSolve(const Matrix & A, const Vector & b, Vector & x)
{
    auto residual = (b-A*x).template lpNorm<Eigen::Infinity>();
    auto solver = PCGSolver<Matrix,Vector, Preconditioner>(5*A.rows(), 1e-5*residual);
    //auto solver = IterativeLinearSolver<PreconditionedConjugateGradientCapsule<Matrix,Vector, Preconditioner> >(A.rows(), 1e-5);
    solver.solve(A,b,x);
    x = solver.x();
}

template <typename Matrix, typename Vector>
void DenseCholeskyPCGSolve(const Matrix & A, const Vector & b, Vector & x)
{
    PCGSolve<cholesky::DenseLDLT_MIC0<std::decay_t<decltype(A)>> >(A,b,x);
}

template <typename Matrix, typename Vector>
void SparseCholeskyPCGSolve(const Matrix & A, const Vector & b, Vector & x)
{
    PCGSolve<cholesky::SparseLDLT_MIC0<Matrix,Vector> >(A,b,x);

}
template <typename Matrix, typename Vector>
void CholeskyPCGSolve(const Matrix & A, const Vector & b, Vector & x)
{
    PCGSolve<cholesky::LDLT_MIC0<Matrix,Vector> >(A,b,x);

}

}

#endif


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

#include <boost/numeric/odeint.hpp>
#include <smooth/bundle.hpp>
#include <smooth/compat/odeint.hpp>
#include <smooth/feedback/pid.hpp>
#include <smooth/se2.hpp>

#include <chrono>

#ifdef ENABLE_PLOTTING
#include <matplot/matplot.h>
#endif

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

using Time = std::chrono::duration<double>;

int main()
{
  smooth::feedback::PIDParams prm{};
  smooth::feedback::PID<Time, smooth::SE2d> pid(prm);

  // set desired trajectory
  Eigen::Vector3d vdes(1, 0, 0.4);
  auto xdes = [&vdes](Time t) -> decltype(pid)::TrajectoryReturnT {
    return std::make_tuple(
      smooth::SE2d(smooth::SO2d(M_PI_2), Eigen::Vector2d(2.5, 0)) + (t.count() * vdes),
      vdes,
      Eigen::Vector3d::Zero());
  };

  pid.set_xdes(xdes);

  // input variable
  Eigen::Vector2d u;

  // prepare for integrating the closed-loop system
  using State = smooth::Bundle<smooth::SE2d, Eigen::Vector3d>;
  using Deriv = typename State::Tangent;
  runge_kutta4<State, double, Deriv, double, vector_space_algebra> stepper{};
  const auto ode = [&u](const State & x, Deriv & d, double) {
    d.template head<3>() = x.part<1>();
    d.template tail<3>() << u(0), 0, u(1);
  };

  State x(smooth::SE2d::Identity(), Eigen::Vector3d::Zero());
  std::vector<double> tvec, xvec, yvec, u1vec, u2vec;

  // integrate closed-loop system
  for (std::chrono::milliseconds t = 0s; t < 30s; t += 50ms) {
    // compute input
    Eigen::Vector3d a = pid(t, x.part<0>(), x.part<1>());

    // input allocation from desired acceleration
    u(0) = std::clamp<double>(a(0), -1, 1);               // throtte <- a_x
    u(1) = std::clamp<double>(a(2) + 0.3 * a(1), -1, 1);  // steering <- a_Yaw + 0.3 a_y

    // store data
    tvec.push_back(duration_cast<Time>(t).count());
    xvec.push_back(x.part<0>().r2().x());
    yvec.push_back(x.part<0>().r2().y());

    u1vec.push_back(u(0));
    u2vec.push_back(u(1));

    // step dynamics
    stepper.do_step(ode, x, 0, 0.05);
  }

#ifdef ENABLE_PLOTTING
  matplot::figure();
  matplot::hold(matplot::on);
  matplot::title("Path");

  matplot::plot(xvec, yvec)->line_width(2);
  matplot::plot(
    matplot::transform(tvec, [&](auto t) { return std::get<0>(xdes(Time(t))).r2().x(); }),
    matplot::transform(tvec, [&](auto t) { return std::get<0>(xdes(Time(t))).r2().y(); }),
    "k--")
    ->line_width(2);
  matplot::legend({"actual", "desired"});

  matplot::figure();
  matplot::hold(matplot::on);
  matplot::title("Inputs");
  matplot::plot(tvec, u1vec)->line_width(2);
  matplot::plot(tvec, u2vec)->line_width(2);
  matplot::legend({"u1", "u2"});

  matplot::show();
#else
  std::cout << "TRAJECTORY:" << std::endl;
  for (auto i = 0u; i != tvec.size(); ++i) {
    std::cout << "t=" << tvec[i] << ": x=" << xvec[i] << ", y=" << yvec[i] << std::endl;
  }
#endif
}


#include "parameter.hpp"

#include <iostream>
#include <cassert>

#include <Eigen/Dense>

int main()
{
    // Scalar
    op::Parameter scalar_param_1(1.0);
    op::Parameter scalar_param_2(2.0);
    op::Parameter scalar_param_3(3.0);
    op::Parameter scalar_param_4(4.0);
    double scalar = 3.;
    op::Parameter scalar_param_ptr(&scalar);

    op::Parameter result_scalar = scalar_param_ptr + scalar_param_2 + scalar_param_ptr + scalar_param_2;
    assert(result_scalar.get_value() == 10.);

    result_scalar = scalar_param_ptr + scalar_param_2 - scalar_param_ptr - scalar_param_2;
    assert(result_scalar.get_value() == 0.);

    result_scalar = scalar_param_ptr * scalar_param_2 - scalar_param_ptr * scalar_param_2;
    assert(result_scalar.get_value() == 0.);

    result_scalar = scalar_param_ptr * scalar_param_2 * scalar_param_ptr * scalar_param_2;
    assert(result_scalar.get_value() == 36.);

    scalar = 1.;
    result_scalar = scalar_param_2 + scalar_param_ptr;
    assert(result_scalar.get_value() == 3.);
    result_scalar = scalar_param_2 - scalar_param_ptr;
    assert(result_scalar.get_value() == 1.);
    result_scalar = scalar_param_2 * scalar_param_ptr;
    assert(result_scalar.get_value() == 2.);
    result_scalar = scalar_param_2 / scalar_param_ptr;
    assert(result_scalar.get_value() == 2.);

    // Matrix
    // multiply 2x2/2x2
    Eigen::Matrix2d dyn_matrix;
    dyn_matrix << 1., 2., 3., 4.;
    op::Parameter matrix_parameter(&dyn_matrix);
    op::Parameter result_matrix = matrix_parameter * matrix_parameter;

    Eigen::Matrix2d reference_matrix;

    reference_matrix << 7., 10., 15., 22.;
    assert(result_matrix.get_values() == reference_matrix);

    dyn_matrix(1, 0) = 1.;
    reference_matrix << 3., 10., 5., 18.;
    assert(result_matrix.get_values() == reference_matrix);

    dyn_matrix(1, 0) = 3.;

    Eigen::Vector2d dyn_vector(1., 2.);

    // multiply 2x2/2x1
    op::Parameter vector_parameter(&dyn_vector);
    result_matrix = matrix_parameter * vector_parameter;
    assert(result_matrix.get_values() == Eigen::Vector2d(5., 11));

    // multiply 1x2/2x2
    result_matrix = vector_parameter.transpose() * matrix_parameter;
    assert(result_matrix.get_values() == Eigen::Vector2d(7., 10.).transpose());

    // multiply 1x1/2x2
    result_matrix = scalar_param_2 * matrix_parameter;
    reference_matrix << 2., 4., 6., 8.;
    assert(result_matrix.get_values() == reference_matrix);

    // multiply 2x2/1x1
    result_matrix = matrix_parameter * scalar_param_2;
    assert(result_matrix.get_values() == reference_matrix);

    Eigen::Matrix3d m1, m2;
    m1.setRandom();
    m2.setRandom();
    op::Parameter eigen1(&m1);
    op::Parameter eigen2(m2);

    scalar = 3.;
    Eigen::MatrixXd m = scalar * m1 * m2;
    op::Parameter result = scalar_param_ptr * eigen1 * eigen2;
    assert((result.get_values() - m).cwiseAbs().sum() < 1e-10);

    scalar = 10.;
    m = scalar * m1 * m2;
    assert((result.get_values() - m).cwiseAbs().sum() < 1e-10);

    Eigen::MatrixXd m3x2(3, 2);
    Eigen::MatrixXd m2x5(2, 5);
    m3x2.setRandom();
    m2x5.setRandom();

    m = m3x2 * m2x5;
    result = op::Parameter(m3x2) * op::Parameter(m2x5);

    assert((result.get_values() - m).cwiseAbs().sum() < 1e-10);

    std::cout << "All tests were successful."
              << "\n";
}


#include <terrain_server/feature/CurvatureFeature.h>
#include <Eigen/Dense>

namespace terrain_server
{

namespace feature
{

CurvatureFeature::CurvatureFeature() :
		positive_threshold_(6.0), negative_threshold_(-6.0)
{
	name_ = "Curvature";
}

CurvatureFeature::~CurvatureFeature()
{

}

void CurvatureFeature::computeCost(double& cost_value,
								   const dwl::Terrain& terrain_info)
{
	double curvature = terrain_info.curvature;

	// The worse condition
	if (curvature * 10000 > 9) {
		cost_value = max_cost_;
		return;
	}

	if (curvature > positive_threshold_)
		cost_value = 0.;
	else if (curvature < negative_threshold_)
		cost_value = max_cost_;
	else
		cost_value = max_cost_
				- log((curvature - negative_threshold_)
								/ (positive_threshold_ - negative_threshold_));
}

} //@namespace feature
} //@namespace terrain


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "kkt_inverse.h"

#include <Eigen/Core>
#include <Eigen/LU>
#include "EPS.h"
#include <cstdio>

template <typename T>
IGL_INLINE void igl::kkt_inverse(
  const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& A,
  const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& Aeq,
  const bool use_lu_decomposition,
  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& S)
{
  typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Mat;
        // This threshold seems to matter a lot but I'm not sure how to
        // set it
  const T treshold = igl::FLOAT_EPS;
  //const T treshold = igl::DOUBLE_EPS;

  const int n = A.rows();
  assert(A.cols() == n);
  const int m = Aeq.rows();
  assert(Aeq.cols() == n);

  // Lagrange multipliers method:
  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> LM(n + m, n + m);
  LM.block(0, 0, n, n) = A;
  LM.block(0, n, n, m) = Aeq.transpose();
  LM.block(n, 0, m, n) = Aeq;
  LM.block(n, n, m, m).setZero();

  Mat LMpinv;
  if(use_lu_decomposition)
  {
    // if LM is close to singular, use at your own risk :)
    LMpinv = LM.inverse();
  }else
  {
    // use SVD
    typedef Eigen::Matrix<T, Eigen::Dynamic, 1> Vec;
    Vec singValues;
    Eigen::JacobiSVD<Mat> svd;
    svd.compute(LM, Eigen::ComputeFullU | Eigen::ComputeFullV );
    const Mat& u = svd.matrixU();
    const Mat& v = svd.matrixV();
    const Vec& singVals = svd.singularValues();

    Vec pi_singVals(n + m);
    int zeroed = 0;
    for (int i=0; i<n + m; i++)
    {
      T sv = singVals(i, 0);
      assert(sv >= 0);
                 // printf("sv: %lg ? %lg\n",(double) sv,(double)treshold);
      if (sv > treshold) pi_singVals(i, 0) = T(1) / sv;
      else
      {
        pi_singVals(i, 0) = T(0);
        zeroed++;
      }
    }

    printf("kkt_inverse : %i singular values zeroed (threshold = %e)\n", zeroed, treshold);
    Eigen::DiagonalMatrix<T, Eigen::Dynamic> pi_diag(pi_singVals);

    LMpinv = v * pi_diag * u.transpose();
  }
  S = LMpinv.block(0, 0, n, n + m);

  //// debug:
  //mlinit(&g_pEngine);
  //
  //mlsetmatrix(&g_pEngine, "A", A);
  //mlsetmatrix(&g_pEngine, "Aeq", Aeq);
  //mlsetmatrix(&g_pEngine, "LM", LM);
  //mlsetmatrix(&g_pEngine, "u", u);
  //mlsetmatrix(&g_pEngine, "v", v);
  //MatrixXd svMat = singVals;
  //mlsetmatrix(&g_pEngine, "singVals", svMat);
  //mlsetmatrix(&g_pEngine, "LMpinv", LMpinv);
  //mlsetmatrix(&g_pEngine, "S", S);

  //int hu = 1;
}

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


#include "FEAnalyzer.h"

#include <math.h>
#include <algorithm>
#include <array>
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <QQuickItem>
#include <QVariant>
#include <QVariantList>
#include <QQmlProperty>

struct InputNode
{
    Vec2f pos; // a vector2 with the (x , y) postion of the node in world space in m
    bool isSupport; // true if input node cannot move
};

struct InputMember
{
    float area; // cross sectional area of the member in m^2
    float youngsModulus; // the youngs modulus of the member in (N / m^2) or Pa
    unsigned nodeI; // the starting node index of the member
    unsigned nodeJ; // the end node index of the member
};

struct Output {
    std::vector<Vec2f> nodeOffsets;
    std::vector<float> memberStressIndicator;
    Eigen::VectorXf stressForces;
    Eigen::VectorXf error;
};

struct Node
{
    int id; // the nodes id#
    Vec2f pos; // a vector2 with the (x , y) postion of the node in world space in m
    bool isSupport; // true if input node cannot move
};

struct Member
{
    float area; // cross sectioal area of the memmber in m^2
    float youngsModulus; // the youngs modulus of the member in (N/m^2) or Pa
    Node nodeI; // the starting node of the member
    Node nodeJ; // the end node of the member
    float length; // the length of the member in m
    float theta; // the angle of the beam
    std::array<unsigned, 4> dofs; // global degrees of freedom
    // values above this line should be supplied when passing into ComputeDisplacements
    float stress; // will be the stress in the member in (N/m^2)
    Eigen::MatrixXf k_global; // the local stiffness matrix
    Eigen::MatrixXf L; // transformation Matrix
};

const float m_per_px = 1.0f;

float pixelToWorldX(float x) {
    return x * m_per_px;
}

float worldToPixelOffsetX(float x) {
    return x / m_per_px;
}

float pixelToWorldY(float y) {
    return (900 - y) * m_per_px;
}

float worldToPixelOffsetY(float y) {
    return -y / m_per_px;
}

Output computeDisplacements(std::vector<InputNode> inNodes,
                            std::vector<InputMember> inMembers,
                            Eigen::VectorXf externalForces) {

    std::vector<Node> nodes;
    for (unsigned i = 0; i < inNodes.size(); ++i) {
        InputNode in = inNodes[i];
        Node node;
        node.id = i;
        node.pos = in.pos;
        node.isSupport = in.isSupport;
        nodes.push_back(node);
    }

    std::vector<Member> members;
    for (unsigned i = 0; i < inMembers.size(); ++i) {
        InputMember in = inMembers[i];
        Member m;
        m.area = in.area;
        m.youngsModulus = in.youngsModulus;
        m.nodeI = nodes[in.nodeI];
        m.nodeJ = nodes[in.nodeJ];
        Vec2f memberVector = m.nodeJ.pos - m.nodeI.pos;
        m.length = magnitude(memberVector);
        m.theta = atan2f(memberVector.y, memberVector.x);
        m.dofs = {
            in.nodeI * 2,
            in.nodeI * 2 + 1,
            in.nodeJ * 2,
            in.nodeJ * 2 + 1
        };

        float C = cos(m.theta);
        float S = sin(m.theta);
        Eigen::MatrixXf matC(2,4);
        matC << C,S,0,0,
                0,0,C,S;
        // m.L = matC;
        //construct the member stiffness matrixes 'k' for each member
        Eigen::MatrixXf matA(2, 2);
        matA << C*C, C*S, C*S, S*S;
        Eigen::MatrixXf k(4, 4);
        k << matA, -matA, -matA, matA;
        // m.L.transpose();
        m.k_global = (m.area * m.youngsModulus / m.length) * k;
        members.push_back(m);
    }

    Eigen::MatrixXf K(2*nodes.size(), 2*nodes.size());
    K.setZero();  // new matrix size dof, dof, of floats is the struture stiffness matrix
    for (const Member& member : members) {
        //distribute the member stiffnesses to the structure stiffness matrix
        for (unsigned i = 0; i < 4; i++) {
            for (unsigned j = 0; j < 4; j++) {
                int A = member.dofs[i];
                int B = member.dofs[j];
                K(A, B) += member.k_global(i, j);
            }
        }
    }

    Eigen::VectorXf& F = externalForces;
    for (const Node& node : nodes) {
        if (node.isSupport) {
            K.row(node.id * 2).setZero();
            K.col(node.id * 2).setZero();
            K.row(node.id * 2 + 1).setZero();
            K.col(node.id * 2 + 1).setZero();

            F.row(node.id * 2).setZero();
            F.row(node.id * 2 + 1).setZero();
        }
    }

    // solve [K]{u}={F}
    Eigen::VectorXf u = K.fullPivLu().solve(F);

    for (unsigned i = 0; i < members.size(); ++i) {
        Member& m = members[i];
        Vec2f displacementI = { u(m.nodeI.id * 2), u(m.nodeI.id * 2 + 1) };
        Vec2f displacementJ = { u(m.nodeJ.id * 2), u(m.nodeJ.id * 2 + 1) };
        Vec2f totalDisplacement = displacementJ - displacementI;
        Vec2f memberVector = m.nodeJ.pos - m.nodeI.pos;
        Vec2f memberDirection = memberVector / magnitude(memberVector); // todo: ensure members are not 0-length
        float projectedDisplacement = dot(totalDisplacement, memberDirection);
        m.stress = projectedDisplacement * m.youngsModulus / m.length;
    }

    Output o;
    o.nodeOffsets.resize(nodes.size());
    for (unsigned i = 0; i < o.nodeOffsets.size(); ++i) {
        o.nodeOffsets[i] = { u(i * 2), u(i * 2 + 1) };
    }
    o.memberStressIndicator.resize(members.size());
    for (unsigned i = 0; i < o.memberStressIndicator.size(); ++i) {
        o.memberStressIndicator[i] = members[i].stress;
    }
    Eigen::VectorXf stressForces(2*nodes.size());
    stressForces.setZero();
    for (unsigned i = 0; i < members.size(); ++i) {
        const Member& member = members[i];
        float alongBeamForce = member.area * member.stress;
        float xForce = cosf(member.theta) * alongBeamForce;
        float yForce = sinf(member.theta) * alongBeamForce;
        stressForces(2*member.nodeI.id) += xForce;
        stressForces(2*member.nodeI.id+1) += yForce;
        stressForces(2*member.nodeJ.id) += -xForce;
        stressForces(2*member.nodeJ.id+1) += -yForce;
    }
    o.stressForces = stressForces;
    o.error = F - (K*u);
    return o;
}

template <class T>
int index_of(const std::vector<T>& v, const T& value) {
    auto it = std::find(v.cbegin(), v.cend(), value);
    if (it == v.cend()) {
        return -1;
    } else {
        return std::distance(v.cbegin(), it);
    }
}

void addInitialBeamWeightToNodes(Eigen::VectorXf& forces,
  const std::vector<InputNode>& inNodes,
  const std::vector<InputMember>& inMembers) {
  const float density = 7850.0f; // kg/m^3
  const float g = -9.81f; //gravity m/s^2 in the y axis
  for (unsigned i = 0; i < inMembers.size(); ++i) {
    Vec2f v = inNodes[inMembers[i].nodeJ].pos - inNodes[inMembers[i].nodeI].pos;
    float length = magnitude(v);
    float mass = density * length * inMembers[i].area;
    float force = mass * g;
    float force_per_node = force / 2;
    forces(2*inMembers[i].nodeI+1) += force_per_node;
    forces(2*inMembers[i].nodeJ+1) += force_per_node;
  }
}

struct Input {
    std::vector<InputNode> nodes;
    std::vector<InputMember> members;
};

Input extractInput(const QVariantList& nodes,
                   const QVariantList& beams) {
    std::vector<InputNode> inNodes;
    std::vector<QQuickItem*> items;
    foreach(QVariant v, nodes) {
        QQuickItem* node = qobject_cast<QQuickItem*>(v.value<QObject*>());
        items.push_back(node);
        InputNode n;
        QVariant support = QQmlProperty::read(node, QStringLiteral("structural"));
        n.isSupport = support.toBool();
        QVariant x = QQmlProperty::read(node, QStringLiteral("x"));
        n.pos.x = pixelToWorldX(x.toReal());
        QVariant y = QQmlProperty::read(node, QStringLiteral("y"));
        n.pos.y = pixelToWorldY(y.toReal());
        inNodes.push_back(n);
    }

    std::vector<InputMember> inMembers;
    foreach(QVariant v, beams) {
        QQuickItem* beam = qobject_cast<QQuickItem*>(v.value<QObject*>());

        QVariant lav = QQmlProperty::read(beam, QStringLiteral("leftAnchor"));
        QQuickItem* la = qobject_cast<QQuickItem*>(lav.value<QObject*>());
        int left_index = index_of(items, la);
        QVariant rav = QQmlProperty::read(beam, QStringLiteral("rightAnchor"));
        QQuickItem* ra = qobject_cast<QQuickItem*>(rav.value<QObject*>());
        int right_index = index_of(items, ra);
        InputMember m;
        m.area = 10100.f / 1e6f;
        m.youngsModulus = 200.f * 1e9f;
        m.nodeI = (unsigned)left_index;
        m.nodeJ = (unsigned)right_index;
        inMembers.push_back(m);
    }

    return { inNodes, inMembers };
}

FEAnalyzer::FEAnalyzer() {
    in_ = new Input;
    relaxation_ = 1.f;
}

FEAnalyzer::~FEAnalyzer() {
    delete in_;
}

void FEAnalyzer::applyOutputToInput(const Output& o) {
    for (unsigned i = 0; i < o.nodeOffsets.size(); ++i) {
        in_->nodes[i].pos += (o.nodeOffsets[i] * relaxation_);
    }
}

void FEAnalyzer::processBridge(const QVariantList& nodes,
                               const QVariantList& beams) {
    *in_ = extractInput(nodes, beams);
    Eigen::VectorXf forces(2 * nodes.size());
    forces.setZero();
    addInitialBeamWeightToNodes(forces, in_->nodes, in_->members);
    gravityForces_ = forces;
    Output o = computeDisplacements(in_->nodes, in_->members, forces);
    stressForces_ = o.stressForces;
    applyOutputToInput(o);
    emitCompleted(o);
}

void FEAnalyzer::emitCompleted(const Output& o) {
    QVariantList nodeOffsets;
    for (unsigned i = 0; i < o.nodeOffsets.size(); ++i) {
        QVariantMap offset;
        offset.insert("x", worldToPixelOffsetX(o.nodeOffsets[i].x));
        offset.insert("y", worldToPixelOffsetY(o.nodeOffsets[i].y));
        nodeOffsets.append(QVariant::fromValue(offset));
    }
    QVariantList beamStress;
    for (unsigned i = 0; i < o.memberStressIndicator.size(); ++i) {
        beamStress.append(QVariant::fromValue(o.memberStressIndicator[i]));
    }
    emit processingComplete(nodeOffsets, beamStress);

    // check for success or failure
    bool onlyLowError = std::all_of(
        o.error.data(),
        o.error.data() + o.error.rows(), [](float forceError){
        return fabsf(forceError) < 10.f;
    });
    bool onlyLowStress = std::all_of(
        o.memberStressIndicator.cbegin(),
        o.memberStressIndicator.cend(), [](float stress){
        return fabsf(stress) < 3.5e8;
    });
    bool onlySmallOffsets = std::all_of(
        o.nodeOffsets.cbegin(),
        o.nodeOffsets.cend(), [](Vec2f offset){
        return fabsf(offset.x) < 100.f && fabsf(offset.y) < 100.f;
    });

    if (!onlyLowError || !onlyLowStress || !onlySmallOffsets) {
        emit failed();
    } else {
        emit converged();
    }
}

void FEAnalyzer::step() {
    Eigen::VectorXf forces = gravityForces_ + stressForces_;
    Output o = computeDisplacements(in_->nodes, in_->members, forces);
    Eigen::VectorXf stressDifferences = stressForces_ - o.stressForces;
    stressForces_ = o.stressForces;
    applyOutputToInput(o);
    emitCompleted(o);
}

/*
int main()
{
	std::vector<InputNode> nodes = {
		{{0,40}, true},
	  {{20,40}, false},
		{{20,0}, true},
		{{50,0}, true},
	};
	std::vector<InputMember> members = {
		{ 1.5, 1e7, 0, 1 },
		{ 1.0, 1e7, 2, 1 },
		{ 3.0, 1e7, 3, 1 },
	};

	Eigen::VectorXf F(2 * nodes.size());
	F.setZero();
	F(2) = -4000;
	F(3) = -8000;
	Output o = computeDisplacements(nodes, members, F);
	std::cout << "displacements:" << std::endl;
	for (unsigned i = 0; i < o.nodeOffsets.size(); ++i) {
		Vec2f offset = o.nodeOffsets[i];
		std::cout << offset.x << "," << offset.y << std::endl;
	}
	return 0;
}
*/


#pragma once

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

#include <ipc/utils/eigen_ext.hpp>

namespace ipc {

/// @brief Compute the distance between a point and a plane.
/// @note The distance is actually squared distance.
/// @param p The point.
/// @param origin The origin of the plane.
/// @param normal The normal of the plane.
/// @return The distance between the point and plane.
template <typename DerivedP, typename DerivedOrigin, typename DerivedNormal>
auto point_plane_distance(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedOrigin>& origin,
    const Eigen::MatrixBase<DerivedNormal>& normal)
{
    auto point_to_plane = (p - origin).dot(normal);
    return point_to_plane * point_to_plane / normal.squaredNorm();
}

/// @brief Compute the distance between a point and a plane.
/// @note The distance is actually squared distance.
/// @param p The point.
/// @param t0 The first vertex of the triangle.
/// @param t1 The second vertex of the triangle.
/// @param t2 The third vertex of the triangle.
/// @return The distance between the point and plane.
template <
    typename DerivedP,
    typename DerivedT0,
    typename DerivedT1,
    typename DerivedT2>
auto point_plane_distance(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedT0>& t0,
    const Eigen::MatrixBase<DerivedT1>& t1,
    const Eigen::MatrixBase<DerivedT2>& t2)
{
    assert(p.size() == 3);
    assert(t0.size() == 3);
    assert(t1.size() == 3);
    assert(t2.size() == 3);

    auto normal = cross(t1 - t0, t2 - t0);
    return point_plane_distance(p, t0, normal);
}

// Symbolically generated derivatives;
namespace autogen {
    void point_plane_distance_gradient(
        double v01,
        double v02,
        double v03,
        double v11,
        double v12,
        double v13,
        double v21,
        double v22,
        double v23,
        double v31,
        double v32,
        double v33,
        double g[12]);

    void point_plane_distance_hessian(
        double v01,
        double v02,
        double v03,
        double v11,
        double v12,
        double v13,
        double v21,
        double v22,
        double v23,
        double v31,
        double v32,
        double v33,
        double H[144]);
} // namespace autogen

/// @brief Compute the gradient of the distance between a point and a plane.
/// @note The distance is actually squared distance.
/// @param[in] p The point.
/// @param[in] origin The origin of the plane.
/// @param[in] normal The normal of the plane.
/// @param[out] grad The gradient of the distance wrt p.
template <
    typename DerivedP,
    typename DerivedOrigin,
    typename DerivedNormal,
    typename DerivedGrad>
void point_plane_distance_gradient(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedOrigin>& origin,
    const Eigen::MatrixBase<DerivedNormal>& normal,
    Eigen::PlainObjectBase<DerivedGrad>& grad)
{
    grad = (2 * (p - origin).dot(normal)) / normal.squaredNorm() * normal;
}

/// @brief Compute the gradient of the distance between a point and a plane.
/// @note The distance is actually squared distance.
/// @param[in] p The point.
/// @param[in] t0 The first vertex of the triangle.
/// @param[in] t1 The second vertex of the triangle.
/// @param[in] t2 The third vertex of the triangle.
/// @param[out] grad The gradient of the distance wrt p, t0, t1, and t2.
template <
    typename DerivedP,
    typename DerivedT0,
    typename DerivedT1,
    typename DerivedT2,
    typename DerivedGrad>
void point_plane_distance_gradient(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedT0>& t0,
    const Eigen::MatrixBase<DerivedT1>& t1,
    const Eigen::MatrixBase<DerivedT2>& t2,
    Eigen::PlainObjectBase<DerivedGrad>& grad)
{
    assert(p.size() == 3);
    assert(t0.size() == 3);
    assert(t1.size() == 3);
    assert(t2.size() == 3);

    grad.resize(p.size() + t0.size() + t1.size() + t2.size());
    autogen::point_plane_distance_gradient(
        p[0], p[1], p[2], t0[0], t0[1], t0[2], t1[0], t1[1], t1[2], t2[0],
        t2[1], t2[2], grad.data());
}

/// @brief Compute the hessian of the distance between a point and a plane.
/// @note The distance is actually squared distance.
/// @param[in] p The point.
/// @param[in] origin The origin of the plane.
/// @param[in] normal The normal of the plane.
/// @param[out] hess The hessian of the distance wrt p.
template <
    typename DerivedP,
    typename DerivedOrigin,
    typename DerivedNormal,
    typename DerivedHess>
void point_plane_distance_hessian(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedOrigin>& origin,
    const Eigen::MatrixBase<DerivedNormal>& normal,
    Eigen::PlainObjectBase<DerivedHess>& hess)
{
    if (normal.cols() == 1) {
        // (n×1)(n×1)ᵀ = (n×n)
        hess = 2 / normal.squaredNorm() * normal * normal.transpose();
    } else {
        assert(normal.rows() == 1);
        // (1×n)ᵀ(1×n) = (n×n)
        hess = 2 / normal.squaredNorm() * normal.transpose() * normal;
    }
}

/// @brief Compute the hessian of the distance between a point and a plane.
/// @note The distance is actually squared distance.
/// @param[in] p The point.
/// @param[in] t0 The first vertex of the triangle.
/// @param[in] t1 The second vertex of the triangle.
/// @param[in] t2 The third vertex of the triangle.
/// @param[out] hess The hessian of the distance wrt p, t0, t1, and t2.
template <
    typename DerivedP,
    typename DerivedT0,
    typename DerivedT1,
    typename DerivedT2,
    typename DerivedHess>
void point_plane_distance_hessian(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedT0>& t0,
    const Eigen::MatrixBase<DerivedT1>& t1,
    const Eigen::MatrixBase<DerivedT2>& t2,
    Eigen::PlainObjectBase<DerivedHess>& hess)
{
    assert(p.size() == 3);
    assert(t0.size() == 3);
    assert(t1.size() == 3);
    assert(t2.size() == 3);

    hess.resize(
        p.size() + t0.size() + t1.size() + t2.size(),
        p.size() + t0.size() + t1.size() + t2.size());
    autogen::point_plane_distance_hessian(
        p[0], p[1], p[2], t0[0], t0[1], t0[2], t1[0], t1[1], t1[2], t2[0],
        t2[1], t2[2], hess.data());
}

} // namespace ipc