#include "convex_hull.hpp"
#include <Eigen/Geometry>
#include "ear/helpers/assert.hpp"

namespace ear {

  std::vector<Facet> convex_hull(const std::vector<Eigen::Vector3d> &positions,
                                 double tolerance) {
    using Vec = Eigen::Vector3d;
    using Tri = std::array<size_t, 3>;

    // the mean point, guaranteed to be inside the convex hull
    Vec inside_point = Vec::Zero();
    for (auto &pos : positions) inside_point += pos;
    inside_point /= (double)positions.size();

    // find all triangles on the convex hull:
    // - iterate through all possible triangles
    // - find the triangle normal, checking for collinearity
    // - it's on the convex hull if:
    //    - the inside point is not on the plane of the triangle
    //    - all points are on the same side of the plane of the triangle as the
    //    inside point
    std::vector<Tri> hull_tris;
    std::vector<Vec> hull_tri_normals;

    for (size_t i = 0; i < positions.size(); i++)
      for (size_t j = i + 1; j < positions.size(); j++)
        for (size_t k = j + 1; k < positions.size(); k++) {
          Vec normal =
              (positions[j] - positions[i]).cross(positions[k] - positions[i]);
          ear_assert(normal.squaredNorm() > tolerance,
                     "collinear points in convex hull");
          normal.normalize();  // XXX: not really required, but helps make
                               // tolerances consistent

          double dot_inside = normal.dot(inside_point - positions[i]);

          if (std::abs(dot_inside) < tolerance)
            continue;  // tri coplanar with inside point

          bool points_on_same_side_as_inside = true;
          for (auto &pos : positions) {
            double dot_point = normal.dot(pos - positions[i]);

            // check if signs are equal, with tolerance
            if (!(dot_inside > 0 ? dot_point > -tolerance
                                 : dot_point < tolerance)) {
              points_on_same_side_as_inside = false;
              break;
            }
          }

          if (points_on_same_side_as_inside) {
            hull_tris.push_back({i, j, k});
            hull_tri_normals.push_back(normal);
          }
        }

    // merge coplanar triangles into facets
    std::vector<Facet> hull_facets;
    std::vector<Vec> hull_facet_normals;

    for (size_t tri_i = 0; tri_i < hull_tris.size(); tri_i++) {
      const Vec &tri_point = positions[hull_tris[tri_i][0]];
      const Vec &tri_norm = hull_tri_normals[tri_i];

      // check each facet to see if this triangle is on the same plane.
      // if it is, add the points to the facet. if no facet is found, make a
      // new facet.
      bool found_facet = false;
      for (size_t facet_i = 0; facet_i < hull_facets.size(); facet_i++) {
        const Vec &facet_norm = hull_facet_normals[facet_i];
        const Vec &facet_point = positions[*hull_facets[facet_i].begin()];

        // they are on the same plane if the line between the test points is
        // not along the normal, and if the normals point in the same or
        // opposite direction. alternatively, we could just test all tri
        // vertices.
        if (std::abs((tri_point - facet_point).dot(facet_norm)) < tolerance &&
            facet_norm.cross(tri_norm).squaredNorm() < tolerance) {
          for (size_t corner_idx : hull_tris[tri_i]) {
            hull_facets[facet_i].insert((Facet::value_type)corner_idx);
          }

          found_facet = true;
          break;
        }
      }

      if (!found_facet) {
        Facet f;
        for (size_t corner_idx : hull_tris[tri_i])
          f.insert((Facet::value_type)corner_idx);
        hull_facets.push_back(std::move(f));
        hull_facet_normals.push_back(tri_norm);
      }
    }

    return hull_facets;
  }
}  // namespace ear


//==================================================================================================
/*!
  @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_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_REM_PIO2_HPP_INCLUDED

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

 /*!

    @ingroup group-trigonometric
    Function object implementing rem_pio2 capabilities

    Computes the remainder modulo \f$\pi/2\f$.

    @par Semantic:

    For every parameter of floating type T

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

    is similar to:

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

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

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

#endif


#include <vector>
#include <boost/math/distributions/lognormal.hpp>
#include "lognormal_dist.h"

stochastic::LognormalDistribution::LognormalDistribution(double mean, double std_dev)
  : Distribution(),
    mean_{mean},
    std_dev_{std_dev},
    distribution_{mean, std_dev_}
{}

std::vector<double> stochastic::LognormalDistribution::cumulative_dist_func(
    const std::vector<double>& locations) const {
  std::vector<double> evaluations(locations.size());

  for (unsigned int i = 0; i < locations.size(); ++i) {
    evaluations[i] = cdf(distribution_, locations[i]);
  }

  return evaluations;
}

std::vector<double> stochastic::LognormalDistribution::inv_cumulative_dist_func(
    const std::vector<double>& probabilities) const {
  std::vector<double> evaluations(probabilities.size());

  for (unsigned int i = 0; i < probabilities.size(); ++i) {
    evaluations[i] = quantile(distribution_, probabilities[i]);
  }

  return evaluations;
}


#include "ExperimentAmbientCube.h"

#include <Eigen/Eigen>
#include <Eigen/nnls.h>

namespace Probulator {

ExperimentAmbientCube::AmbientCube ExperimentAmbientCube::solveAmbientCubeLeastSquares(const ImageBase<vec3>& directions, const Image& irradiance)
{
	using namespace Eigen;

	AmbientCube ambientCube;

	const u64 sampleCount = directions.getPixelCount();

	MatrixXf A;
	A.resize(sampleCount, 6);

	for (u64 sampleIt = 0; sampleIt < sampleCount; ++sampleIt)
	{
		const vec3& direction = directions.at(sampleIt);
		vec3 dirSquared = direction * direction;

		if (direction.x < 0)
		{
			A(sampleIt, 0) = dirSquared.x;
			A(sampleIt, 1) = 0.0f;
		}
		else
		{
			A(sampleIt, 0) = 0.0f;
			A(sampleIt, 1) = dirSquared.x;
		}

		if (direction.y < 0)
		{
			A(sampleIt, 2) = dirSquared.y;
			A(sampleIt, 3) = 0.0f;
		}
		else
		{
			A(sampleIt, 2) = 0.0f;
			A(sampleIt, 3) = dirSquared.y;
		}

		if (direction.z < 0)
		{
			A(sampleIt, 4) = dirSquared.z;
			A(sampleIt, 5) = 0.0f;
		}
		else
		{
			A(sampleIt, 4) = 0.0f;
			A(sampleIt, 5) = dirSquared.z;
		}
	}

	NNLS<MatrixXf> solver(A);

	VectorXf b;
	b.resize(sampleCount);

	for (u32 channelIt = 0; channelIt < 3; ++channelIt)
	{
		for (u64 sampleIt = 0; sampleIt < sampleCount; ++sampleIt)
		{
			b[sampleIt] = irradiance.at(sampleIt)[channelIt];
		}

		solver.solve(b);
		VectorXf x = solver.x();

		for (u64 basisIt = 0; basisIt < 6; ++basisIt)
		{
			ambientCube.irradiance[basisIt][channelIt] = x[basisIt];
		}
	}

	return ambientCube;
}

ExperimentAmbientCube::AmbientCube ExperimentAmbientCube::solveAmbientCubeProjection(const Image& irradiance)
{
	AmbientCube ambientCube;

	vec3 cubeDirections[6] =
	{
		vec3(-1.0f, 0.0f, 0.0f),
		vec3(+1.0f, 0.0f, 0.0f),
		vec3(0.0f, -1.0f, 0.0f),
		vec3(0.0f, +1.0f, 0.0f),
		vec3(0.0f, 0.0f, -1.0f),
		vec3(0.0f, 0.0f, +1.0f),
	};

	for(u32 i=0; i<6; ++i)
	{
		vec2 texcoord = cartesianToLatLongTexcoord(cubeDirections[i]);
		ambientCube.irradiance[i] = (vec3)irradiance.sampleNearest(texcoord);
	}

	return ambientCube;
}

void ExperimentAmbientCube::run(SharedData& data)
{
	AmbientCube ambientCube;

	if (m_projectionEnabled)
	{
		ambientCube = solveAmbientCubeProjection(m_input->m_irradianceImage);
	}
	else
	{
		ambientCube = solveAmbientCubeLeastSquares(data.m_directionImage, m_input->m_irradianceImage);
	}

	m_radianceImage = Image(data.m_outputSize);
	m_irradianceImage = Image(data.m_outputSize);

	data.m_directionImage.forPixels2D([&](const vec3& direction, ivec2 pixelPos)
	{
		vec3 sampleIrradianceH = ambientCube.evaluate(direction);
		m_irradianceImage.at(pixelPos) = vec4(sampleIrradianceH, 1.0f);
		m_radianceImage.at(pixelPos) = vec4(0.0f);
	});
}

}


#include "math_helper.h"

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

namespace recon
{

namespace MathHelper
{

void getTopViewHomography(const double* cparams, int src_w, int src_h, int dst_w, int dst_h,
                          const Eigen::Vector4d& plane, Eigen::Matrix3d& H)
{
  double fx = cparams[0];
  double fy = cparams[1];
  double cx = cparams[2];
  double cy = cparams[3];

  Eigen::Vector3d n;
  n << plane[0], plane[1], plane[2];
  double d = plane[3];
  if(d < 0.0) {
    throw "Error!\n";
  }
  std::cout << "n = \n" << n << "\n\n";
  std::cout << "d = " << d << "\n\n";

  Eigen::Matrix3d K1 = Eigen::Matrix3d::Identity();
  K1(0,0) = fx;
  K1(1,1) = fy;
  K1(0,2) = cx;
  K1(1,2) = cy;
  Eigen::Matrix3d K1_inv = K1.inverse();

  Eigen::Vector3d q1a;
  Eigen::Vector3d q1b;
  q1a << (double) 0.0, (double) src_h - 1.0, 1.0;
  q1b << (double) src_w - 1.0, (double) src_h - 1.0, 1.0;
  q1a = K1_inv * q1a;
  q1b = K1_inv * q1b;

  Eigen::Vector3d Q1a, Q1b;
  Q1a = (-d * q1a) / n.dot(q1a);
  Q1b = (-d * q1b) / n.dot(q1b);
  std::cout << "Q1a:\n" << Q1a << "\nQ1b:\n" << Q1b << "\n\n";

  // determine R matrix
  Eigen::Matrix3d R;
  //Eigen::Vector3d Q1diff = Q1a - Q1b;
  Eigen::Vector3d Q1diff = Q1b - Q1a;
  std::cout << Q1diff << "\n";
  R.row(0) = Q1diff / Q1diff.norm();
  R.row(2) = -n;
  R.row(1) = -(R.row(0).cross(R.row(2)));

  std::cout << "0 == " << Q1diff.dot(n) << "\n";

  //double Ty = -10.0 - (R.row(1) * Q1a);
  double Ty = 10.0 - (R.row(1) * Q1a);
  std::cout << "Ty = " << Ty << "\n";
  Eigen::Vector3d t;
  t << 0.0, Ty, 0.0;

  Eigen::Vector3d Q2a = R * Q1a + t;
  Eigen::Vector3d Q2b = R * Q1b + t;
  std::cout << "Q2a:\n" << Q2a << "\nQ2b:\n" << Q2b << "\n\n";;
  double dst_cx = dst_w / 2.0;
  double dst_cy = dst_h / 2.0;
  double f2 = (dst_cy - 1.0) * (Q2a[2] / Q2a[1]);

  Eigen::Matrix3d K2;
  K2 << f2,   0.0,  dst_cx,
        0.0,  f2,   dst_cy,
        0.0,  0.0,  1.0;
  std::cout << "K2:\n" << K2 << "\n";
  std::cout << "q2a:\n" << (K2 * Q2a) / Q2a[2] << "\nq2b:\n" << (K2 * Q2b) / Q2b[2] << "\n\n";;

  //std::cout << "d = " << d << "\n";
  //std::cout << "<n,Q1a> = " << n.dot(Q1a) << "\n\n";
  Eigen::Matrix3d H_n = R - ((t * n.transpose()) / d);
  std::cout << "Q2a_H:\n" << H_n * Q1a << "\n\n";
  H = K2 * H_n * K1_inv;
  Eigen::Vector3d q2a_H = H * q1a;
  q2a_H = q2a_H / q2a_H[2];
  std::cout << "q2a_H:\n" << q2a_H << "\n\n";
}

void debugTopViewHomography(const double* cparams, int src_w, int src_h, int dst_w, int dst_h,
                            const Eigen::Vector4d& plane, pcl::visualization::PCLVisualizer& viewer)
{
  double fx = cparams[0];
  double fy = cparams[1];
  double cx = cparams[2];
  double cy = cparams[3];

  Eigen::Vector3d n;
  n << plane[0], plane[1], plane[2];
  double d = plane[3];
  if(d < 0.0) {
    throw "Error!\n";
  }
  std::cout << "n = \n" << n << "\n\n";
  std::cout << "d = " << d << "\n\n";

  Eigen::Matrix3d K1 = Eigen::Matrix3d::Identity();
  K1(0,0) = fx;
  K1(1,1) = fy;
  K1(0,2) = cx;
  K1(1,2) = cy;
  Eigen::Matrix3d K1_inv = K1.inverse();

  Eigen::Vector3d q1a;
  Eigen::Vector3d q1b;
  q1a << (double) 0.0, (double) src_h - 1.0, 1.0;
  q1b << (double) src_w - 1.0, (double) src_h - 1.0, 1.0;
  q1a = K1_inv * q1a;
  q1b = K1_inv * q1b;

  Eigen::Vector3d Q1a, Q1b;
  Q1a = (-d * q1a) / n.dot(q1a);
  Q1b = (-d * q1b) / n.dot(q1b);
  std::cout << "Q1a:\n" << Q1a << "\nQ1b:\n" << Q1b << "\n\n";

  pcl::PointXYZ pt1, pt2;
  for(int i = 0; i < 3; i++) {
    pt1.data[i] = Q1a[i];
    pt2.data[i] = Q1b[i];
  }
  viewer.addLine<pcl::PointXYZ, pcl::PointXYZ>(pt1, pt2, 0, 200, 0, "line1");
}

void projectPoint(const double* cam_params, Eigen::Vector3d& point, Eigen::Vector2d& proj)
{
  double fx = cam_params[0];
  double fy = cam_params[1];
  double cx = cam_params[2];
  double cy = cam_params[3];

  proj[0] = point[0] / point[2];
  proj[1] = point[1] / point[2];
  proj[0] = fx * proj[0] + cx;
  proj[1] = fy * proj[1] + cy;
}

void setTransform2DEM(Eigen::Vector4d& plane_model, Eigen::Matrix4d& transform)
{
  Eigen::Vector3d n1, n2;
  for(int i = 0; i < 3; i++) {
    n1[i] = plane_model[i];
    n2[i] = 0.0;
  }
  n2[1] = 1.0;
  // find axis with cross product

  // find angle with dot product

  // move Y with d after rotation
}

void createPolyFromPlane(const Eigen::Vector4d& plane, pcl::PointCloud<pcl::PointXYZ>::Ptr poly_cloud)
{
  double a = plane[0];
  double b = plane[1];
  double c = plane[2];
  double d = plane[3];

  double x[4] = {-20, 20, 20, -20};
  double z[4] = {  0,  0, 100, 100};
  for(int i = 0; i < 4; i++) {
    double y = (a*x[i] + c*z[i] + d) / (-b);
    pcl::PointXYZ pt(x[i], y, z[i]);
    poly_cloud->points.push_back(pt);
  }
}

void triangulate(const double (&cam_params)[5], double x, double y, double disp, cv::Mat& pt3d)
{
  double f = cam_params[0];
  double cx = cam_params[2];
  double cy = cam_params[3];
  double b = cam_params[4];

  pt3d.at<double>(0) = (x - cx) * b / disp;
  pt3d.at<double>(1) = (y - cy) * b / disp;
  pt3d.at<double>(2) = f * b / disp;
  pt3d.at<double>(3) = 1.0;
}

void getSignedDistancesToModel(const Eigen::Vector4d& plane, const pcl::PointCloud<pcl::PointXYZ>::Ptr pc,
                                           std::vector<double>& distances)
{
  distances.assign(pc->points.size(), 0.0);
  double a = plane[0];
  double b = plane[1];
  double c = plane[2];
  double d = plane[3];
  double norm = std::sqrt(a*a + b*b + c*c);
  for(size_t i = 0; i < pc->points.size(); i++) {
    pcl::PointXYZ pt = pc->points[i];
    double p = a*pt.x + b*pt.y + c*pt.z + d;
    distances[i] = p / norm;
  }
}

void getDistancesToModel(const Eigen::Vector4d& plane, const pcl::PointCloud<pcl::PointXYZ>::Ptr pc,
                         std::vector<double>& distances)
{
  distances.assign(pc->points.size(), 0.0);
  double a = plane[0];
  double b = plane[1];
  double c = plane[2];
  double d = plane[3];
  double norm = std::sqrt(a*a + b*b + c*c);
  for(size_t i = 0; i < pc->points.size(); i++) {
    pcl::PointXYZ pt = pc->points[i];
    double p = a*pt.x + b*pt.y + c*pt.z + d;
    distances[i] = std::abs(p / norm);
  }
}

}

}


/*
2019.03.29 by Aurora. Contact:fanyi@mail.ustc.edu.cn

Utilities functions.

*/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
/*#include <complex.h>*/
#include <algorithm>
#include <boost/math/special_functions/spherical_harmonic.hpp>
#include "struct.h"

/*****************MACRO FOR DEBUG*****************/
#define DEBUG_UTIL

#ifdef DEBUG_UTIL
#define printf_d printf
#else
#define printf_d //
#endif
/***************MACRO FOR DEBUG END***************/

#define PI 3.141592653589793238462643383279

double s_r(double r_ij, parameters_info_struct * parameters_info)
{
    double result;
    double rc = parameters_info->cutoff_2;
    double rcs = parameters_info->cutoff_1;
    result = (r_ij >= rc) ? 0 : ((r_ij >= rcs) ? 1 / r_ij * (0.5 * cos((r_ij - rcs) / (rc - rcs) * PI) + 0.5) : 1 / r_ij);
    return result;
}

double fastpow2(double number, int dummy)
{
    return number * number;
}

double fastpown(double number, int power)
{
    int i;
    double result = 1;
    int N = power;
    if (power == 0)
    {
        return 1;
    }
    if (power < 0)
    {
        N *= -1;
    }
    for (i = 1; i <= N; i++)
    {
        result *= number;
    }
    if (power < 0)
    {
        return 1.0 / result;
    }
    else
    {
        return result;
    }

}

double f_c(double r_ij, double r_c)
{
    double result;
    double rc = r_c;
    result = (r_ij <= rc) ? 0.5 * tanh(1 - r_ij / rc) * tanh(1 - r_ij / rc) * tanh(1 - r_ij / rc) : 0;
    return result;
}

double d_f_c_d_r(double r_ij, double r_c)
{
    double result;
    double rc = r_c;
    result = (r_ij <= rc) ? (-3.0 / cosh(1 - r_ij / rc) / cosh(1 - r_ij / rc) * tanh(1 - r_ij / rc) * tanh(1 - r_ij / rc) / 2.0 / rc ) : (0);
    return result;
}

double R_sup_n(double r_ij, double n, double r_c)
{
    double f_c(double r_ij, double r_c);

    double result;
    return fastpown(r_ij, (int)n) * f_c(r_ij, r_c);
}

double d_R_sup_n_d_r(double r_ij, double n, double r_c)
{
    double f_c(double r_ij, double r_c);
    double d_f_c_d_r(double r_ij, double r_c);
    double result;

    result = n * fastpown(r_ij, (int)(n - 1)) * f_c(r_ij, r_c) + fastpown(r_ij, (int)n) * d_f_c_d_r(r_ij, r_c);
    return result;
}

double factorial(int n)
{
    if (n < 0)
    {
        printf("From factorial: n must be positive!\n");
        exit(999);
    }
    return ((n == 0)||(n == 1)) ? 1 : factorial(n - 1) * n;
}

/*double P_LM(double x, int l, int m)
{
    double factorial(int n);

    if (m < 0)
    {
        return pow(-1, -m) * factorial(l + m) / factorial(l - m) * P_LM(x, l, (-1) * m);
    }

    switch (l)
    {
        case 0:
        {
            return 1.0;
        }
        case 1:
        {
            switch (m)
            {
                case 0:
                {
                    return x;
                }
                case 1:
                {
                    return -1 * sqrt(1 - x * x);
                }
            }
        }
        case 2:
        {
            switch (m)
            {
                case 0:
                {
                    return 0.5 * (3 * x * x - 1);
                }
                case 1:
                {
                    return -3 * x * sqrt(1 - x * x);
                }
                case 2:
                {
                    return 3 * (1 - x * x);
                }
            }
        }
        case 3:
        {
            switch (m)
            {
                case 0:
                {
                    return 0.5 * x * (5 * x * x - 3);
                }
                case 1:
                {
                    return 1.5 * (1 - 5 * x * x) * sqrt(1 - x * x);
                }
                case 2:
                {
                    return 15 * x * (1 - x * x);
                }
                case 3:
                {
                    return -15 * sqrt(1 - x * x) * sqrt(1 - x * x) * sqrt(1 - x * x);
                }
            }
        }
        case 4:
        {
            switch (m)
            {
                case 0:
                {
                    return 0.125 * (35 * x * x * x * x - 30 * x * x + 3);
                }
                case 1:
                {
                    return 2.5 * x * (3 - 7 * x * x) * sqrt(1 - x * x);
                }
                case 2:
                {
                    return 7.5 * (7 * x * x - 1) * (1 - x * x);
                }
                case 3:
                {
                    return -105 * x * sqrt(1 - x * x) * sqrt(1 - x * x) * sqrt(1 - x * x);
                }
                case 4:
                {
                    return 105 * (1 - x * x) * (1 - x * x);
                }
            }
        }
        case 5:
        {
            switch (m)
            {
                case 0:
                {
                    return 0.125 * x * (63 * x * x * x * x - 70 * x * x + 15);
                }
            }
        }
        default:
        {
            printf("From P_LM: Invalid value of l and m!\n");
            exit(999);
        }
    }
}*/

/*double complex Y_LM(double * r_ij, int l, int m)//here r_ij is a vector
{
    double P_LM(double cos_theta, int l, int m);
    double factorial(int n);

    double complex result;
    double theta, phi;
    double r = sqrt(r_ij[0] * r_ij[0] + r_ij[1] * r_ij[1] + r_ij[2] * r_ij[2]);
    theta = acos(r_ij[2] / r);
    phi = atan(r_ij[1] / r_ij[0]);
    result = sqrt((2 * l + 1) / (4 * PI) * factorial(l - m) / factorial(l + m)) * P_LM(cos(theta), l, m) * cexp(m * phi * I);
    return result;
}*/

double Y_LM_r(double * coord_ij, int L, int m)//here L is n of the boost::math::spherical_harmonics
{
    double theta, phi;
    double r = sqrt(coord_ij[0] * coord_ij[0] + coord_ij[1] * coord_ij[1] + coord_ij[2] * coord_ij[2]);
    double result_r;
    theta = acos(coord_ij[2] / r);
    phi = atan(coord_ij[1] / coord_ij[0]);
    result_r = boost::math::spherical_harmonic_r(L, m, theta, phi);
    return result_r;
}
double Y_LM_i(double * coord_ij, int L, int m)//here L is n of the boost::math::spherical_harmonics
{
    double theta, phi;
    double r = sqrt(coord_ij[0] * coord_ij[0] + coord_ij[1] * coord_ij[1] + coord_ij[2] * coord_ij[2]);
    double result_i;
    theta = acos(coord_ij[2] / r);
    phi = atan(coord_ij[1] / coord_ij[0]);
    result_i = boost::math::spherical_harmonic_i(L, m, theta, phi);
    return result_i;
}
std::complex<double> Y_LM(double * coord_ij, int L, int m)
{
    double theta, phi;
    double r = sqrt(coord_ij[0] * coord_ij[0] + coord_ij[1] * coord_ij[1] + coord_ij[2] * coord_ij[2]);
    std::complex<double> result;
    theta = acos(coord_ij[2] / r);
    phi = atan(coord_ij[1] / coord_ij[0]);
    if (phi < 0)
    {
        phi += (2 * PI);
    }
    //printf_d("L, m, theta, phi: %d, %d, %.6lf, %.6lf\n", L, m, theta, phi);
    result = boost::math::spherical_harmonic(L, m, theta, phi);
    return result;
}
std::complex<double> d_Y_LM_d_theta(double * coord_ij, int L, int m)
{
    double theta, phi;
    double r = sqrt(coord_ij[0] * coord_ij[0] + coord_ij[1] * coord_ij[1] + coord_ij[2] * coord_ij[2]);
    std::complex<double> result;
    std::complex<double> I(0.0, 1.0);
    theta = acos(coord_ij[2] / r);
    phi = atan(coord_ij[1] / coord_ij[0]);
    if (phi < 0)
    {
        phi += (2 * PI);
    }
    /*\partial Y/\partial \theta = m * cot(\theta) * Y_LM(\theta, \phi) + \sqrt((L-m) * (L + m + 1)) * exp(- I * \phi) * Y_L(M+1)(\theta, \phi)*/
    result = m * tan((double)PI / 2.0 - theta) * boost::math::spherical_harmonic(L, m, theta, phi) + sqrt((L-m) * (L + m + 1)) * exp(-I * phi) * boost::math::spherical_harmonic(L, m + 1, theta, phi);
    return result;
}
std::complex<double> d_Y_LM_d_phi(double * coord_ij, int L, int m)
{
    double theta, phi;
    double r = sqrt(coord_ij[0] * coord_ij[0] + coord_ij[1] * coord_ij[1] + coord_ij[2] * coord_ij[2]);
    std::complex<double> result;
    std::complex<double> I(0.0, 1.0);
    theta = acos(coord_ij[2] / r);
    phi = atan(coord_ij[1] / coord_ij[0]);
    if (phi < 0)
    {
        phi += (2 * PI);
    }
    /*\partial Y/\partial \phi = I * m * Y_LM(\theta, \phi)*/
    result = I * (double)m * boost::math::spherical_harmonic(L, m, theta, phi);
    return result;
}

double cos_bond_angle(double * coord_i, double * coord_j, double * coord_k)//centered at i
{
    double coord_ji[3];// In DeePMD type sym_coord, r_ji means r_(j - i). Here we keep the same.
    double coord_ki[3];
    double norm_ji, norm_ki;//norm = (x^2 + y^2 + z^2)
    double dot_jiki = 0;
    double result = 0;
    int i, j, k;
    norm_ji =0; norm_ki = 0;
    for (i = 0; i <= 2; i++)
    {
        coord_ji[i] = coord_j[i] - coord_i[i];
        coord_ki[i] = coord_k[i] - coord_i[i];
        norm_ji += (coord_ji[i] * coord_ji[i]);
        norm_ki += (coord_ki[i] * coord_ki[i]);
        dot_jiki += (coord_ji[i] * coord_ki[i]);
    }
    /*cos(\theta_ijk_centered_i) = dot_prod(coord_ji, coord_ki) / \sqrt(norm_ji * norm_ki)*/
    result = dot_jiki / sqrt(norm_ji * norm_ki);
    return result;
}
int d_cos_bond_angle_d_coord(double * coord_i, double * coord_j, double * coord_k, double * result)
{
    double fastpown(double number, int power);

    double coord_ji[3];// In DeePMD type sym_coord, r_ji means r_(j - i). Here we keep the same.
    double coord_ki[3];
    double norm_ji = 0;
    double norm_ki = 0;//norm = (x^2 + y^2 + z^2)
    double dot_jiki = 0;
    //double result = 0;
    int i, j, k;
    //norm_ji =0; norm_ki = 0;
    for (i = 0; i <= 2; i++)
    {
        coord_ji[i] = coord_j[i] - coord_i[i];
        coord_ki[i] = coord_k[i] - coord_i[i];
        norm_ji += (coord_ji[i] * coord_ji[i]);
        norm_ki += (coord_ki[i] * coord_ki[i]);
        dot_jiki += (coord_ji[i] * coord_ki[i]);
    }
    double dist_ji = sqrt(norm_ji);
    double dist_ki = sqrt(norm_ki);
    /*dxi, yi, zi, xj, yj, zj, xk, yk, zk*/
    result[0] = (- coord_ji[0] - coord_ki[0]) / (dist_ji * dist_ki) - (dot_jiki * (- 2 * coord_ki[0] * norm_ji - 2 * coord_ji[0] * norm_ki)) / ( 2 * fastpown((dist_ji * dist_ki), 3));
    result[1] = (- coord_ji[1] - coord_ki[1]) / (dist_ji * dist_ki) - (dot_jiki * (- 2 * coord_ki[1] * norm_ji - 2 * coord_ji[1] * norm_ki)) / ( 2 * fastpown((dist_ji * dist_ki), 3));
    result[2] = (- coord_ji[2] - coord_ki[2]) / (dist_ji * dist_ki) - (dot_jiki * (- 2 * coord_ki[2] * norm_ji - 2 * coord_ji[2] * norm_ki)) / ( 2 * fastpown((dist_ji * dist_ki), 3));
    result[3] = coord_ki[0] / (dist_ji * dist_ki) - coord_ji[0] * norm_ki * dot_jiki / fastpown((dist_ji * dist_ki), 3);
    result[4] = coord_ki[1] / (dist_ji * dist_ki) - coord_ji[1] * norm_ki * dot_jiki / fastpown((dist_ji * dist_ki), 3);
    result[5] = coord_ki[2] / (dist_ji * dist_ki) - coord_ji[1] * norm_ki * dot_jiki / fastpown((dist_ji * dist_ki), 3);
    result[6] = coord_ji[0] / (dist_ji * dist_ki) - coord_ki[0] * norm_ki * dot_jiki / fastpown((dist_ji * dist_ki), 3);
    result[7] = coord_ji[1] / (dist_ji * dist_ki) - coord_ki[1] * norm_ki * dot_jiki / fastpown((dist_ji * dist_ki), 3);
    result[8] = coord_ji[2] / (dist_ji * dist_ki) - coord_ki[1] * norm_ki * dot_jiki / fastpown((dist_ji * dist_ki), 3);

    return 0;
}

int cross_prod(double * vec1, double * vec2, double * vec_result)
{
    vec_result[0] = (vec1[1] * vec2[2] - vec1[2] * vec2[1]);
    vec_result[1] = (vec1[2] * vec2[0] - vec1[0] * vec2[2]);
    vec_result[2] = (vec1[0] * vec2[1] - vec1[1] * vec2[0]);
    return 0;
}
double cos_dihedral_angle(double * coord_i, double * coord_j, double * coord_k, double * coord_l)//centered at i and j, plane_ijk and plane_ijl
{
    int cross_prod(double * vec1, double * vec2, double * vec_result);

    /*Use coord_ki, ji to calculate norm_vec of plane_kij = ji \times ki; Use coord_ij, lj to calculate norm_vec of plane_ijl = ij \times lj*/
    double coord_ki[3];// In DeePMD type sym_coord, r_ji means r_(j - i). Here we keep the same.
    double coord_ji[3];
    double coord_ij[3];
    double coord_lj[3];
    double norm_vec_kij[3];
    double norm_vec_ijl[3];
    double norm_norm_vec_kij = 0;//norm = (x^2 + y^2 + z^2)
    double norm_norm_vec_ijl = 0;
    double dot_kij_ijl = 0;
    double result;
    int error_code;
    int i, j, k;
    for (i = 0; i <= 2; i++)
    {
        coord_ki[i] = coord_k[i] - coord_i[i];
        coord_ji[i] = coord_j[i] - coord_i[i];
        coord_ij[i] = coord_i[i] - coord_j[i];
        coord_lj[i] = coord_l[i] - coord_j[i];
    }
    error_code = cross_prod(coord_ji, coord_ki, norm_vec_kij);
    error_code = cross_prod(coord_ij, coord_lj, norm_vec_ijl);
    for (i = 0 ; i <= 2; i++)
    {
        norm_norm_vec_kij += (norm_vec_kij[i] * norm_vec_kij[i]);
        norm_norm_vec_ijl += (norm_vec_ijl[i] * norm_vec_ijl[i]);
        dot_kij_ijl += norm_vec_kij[i] * norm_vec_ijl[i];
    }
    if (norm_norm_vec_kij * norm_norm_vec_ijl == 0)//three points are on a line; no plane is formed.
    {
        return 999;
    }
    result = dot_kij_ijl / sqrt(norm_norm_vec_kij * norm_norm_vec_ijl) * ((double)-1.0);
    return result;
}

int d_cos_dihedral_angle_d_coord(double * coord_i, double * coord_j, double * coord_k, double * coord_l, double * result)//centered at i and j, plane_ijk and plane_ijl
{
    int cross_prod(double * vec1, double * vec2, double * vec_result);

    /*Use coord_ki, ji to calculate norm_vec of plane_kij = ji \times ki; Use coord_ij, lj to calculate norm_vec of plane_ijl = ij \times lj*/
    double coord_ki[3];// In DeePMD type sym_coord, r_ji means r_(j - i). Here we keep the same.
    double coord_ji[3];
    double coord_ij[3];
    double coord_lj[3];
    double norm_vec_kij[3];
    double norm_vec_ijl[3];
    double norm_norm_vec_kij = 0;//norm = (x^2 + y^2 + z^2)
    double norm_norm_vec_ijl = 0;
    double dot_kij_ijl = 0;
    double xi, yi, zi, xj, yj, zj, xk, yk, zk, xl, yl, zl;
    xi = coord_i[0]; yi = coord_i[1]; zi=coord_i[2];
    xj = coord_j[0]; yj = coord_j[1]; zj=coord_j[2];
    xk = coord_k[0]; yk = coord_k[1]; zk=coord_k[2];
    xl = coord_l[0]; yl = coord_l[1]; zl=coord_l[2];
    //double result;
    int error_code;
    int i, j, k;
    for (i = 0; i <= 2; i++)
    {
        coord_ki[i] = coord_k[i] - coord_i[i];
        coord_ji[i] = coord_j[i] - coord_i[i];
        coord_ij[i] = coord_i[i] - coord_j[i];
        coord_lj[i] = coord_l[i] - coord_j[i];
    }
    error_code = cross_prod(coord_ji, coord_ki, norm_vec_kij);
    error_code = cross_prod(coord_ij, coord_lj, norm_vec_ijl);
    for (i = 0 ; i <= 2; i++)
    {
        norm_norm_vec_kij += (norm_vec_kij[i] * norm_vec_kij[i]);
        norm_norm_vec_ijl += (norm_vec_ijl[i] * norm_vec_ijl[i]);
        dot_kij_ijl += norm_vec_kij[i] * norm_vec_ijl[i];
    }
    /*dxi, yi, zi, xj, yj, zj, xk, yk, zk, xl, yl, zl*/
    result[0] = (-((-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))*(-yj+yl))-(yj-yk)*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl))-((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))*(zj-zl)-(-zj+zk)*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl)))/sqrt((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)))-((-((-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl)))-((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))-(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))*((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(2*(-yj+yl)*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl))+2*(zj-zl)*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl)))+(2*(yj-yk)*(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))+2*(-zj+zk)*((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk)))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2))))/(2.*pow((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)),1.5));
    result[1] = (-((xj-xl)*(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk)))-(-xj+xk)*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl))-(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))*(-zj+zl)-(zj-zk)*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))/sqrt((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)))-((-((-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl)))-((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))-(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))*((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(2*(xj-xl)*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl))+2*(-zj+zl)*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))+(2*(-xj+xk)*(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))+2*(zj-zk)*(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk)))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2))))/(2.*pow((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)),1.5));
    result[2] = (-((-xj+xl)*((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk)))-(yj-yl)*(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))-(xj-xk)*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))-(-yj+yk)*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))/sqrt((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)))-((-((-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl)))-((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))-(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))*((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(2*(-xj+xl)*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))+2*(yj-yl)*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))+(2*(xj-xk)*((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))+2*(-yj+yk)*(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk)))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2))))/(2.*pow((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)),1.5));
    result[3] = (-((-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))*(yi-yl))-(-yi+yk)*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl))-((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))*(-zi+zl)-(zi-zk)*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl)))/sqrt((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)))-((-((-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl)))-((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))-(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))*((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(2*(yi-yl)*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl))+2*(-zi+zl)*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl)))+(2*(-yi+yk)*(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))+2*(zi-zk)*((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk)))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2))))/(2.*pow((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)),1.5));
    result[4] = (-((-xi+xl)*(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk)))-(xi-xk)*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl))-(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))*(zi-zl)-(-zi+zk)*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))/sqrt((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)))-((-((-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl)))-((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))-(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))*((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(2*(-xi+xl)*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl))+2*(zi-zl)*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))+(2*(xi-xk)*(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))+2*(-zi+zk)*(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk)))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2))))/(2.*pow((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)),1.5));
    result[5] = (-((xi-xl)*((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk)))-(-yi+yl)*(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))-(-xi+xk)*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))-(yi-yk)*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))/sqrt((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)))-((-((-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl)))-((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))-(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))*((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(2*(xi-xl)*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))+2*(-yi+yl)*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))+(2*(-xi+xk)*((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))+2*(yi-yk)*(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk)))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2))))/(2.*pow((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)),1.5));
    result[6] = -((2*(yi-yj)*(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))+2*(-zi+zj)*((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk)))*(-((-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl)))-((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))-(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)))/(2.*pow((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)),1.5))+(-((yi-yj)*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl)))-(-zi+zj)*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl)))/sqrt((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)));
    result[7] = -((2*(-xi+xj)*(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))+2*(zi-zj)*(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk)))*(-((-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl)))-((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))-(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)))/(2.*pow((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)),1.5))+(-((-xi+xj)*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl)))-(zi-zj)*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))/sqrt((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)));
    result[8] = -((2*(xi-xj)*((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))+2*(-yi+yj)*(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk)))*(-((-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl)))-((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))-(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)))/(2.*pow((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)),1.5))+(-((xi-xj)*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl)))-(-yi+yj)*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))/sqrt((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)));
    result[9] = -((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(2*(-yi+yj)*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl))+2*(zi-zj)*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl)))*(-((-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl)))-((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))-(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl))))/(2.*pow((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)),1.5))+(-((-yi+yj)*(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk)))-(zi-zj)*((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk)))/sqrt((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)));
    result[10] = -((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(2*(xi-xj)*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl))+2*(-zi+zj)*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))*(-((-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl)))-((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))-(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl))))/(2.*pow((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)),1.5))+(-((xi-xj)*(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk)))-(-zi+zj)*(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk)))/sqrt((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)));
    result[11] = -((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(2*(-xi+xj)*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))+2*(yi-yj)*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl)))*(-((-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk))*(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl)))-((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk))*((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl))-(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk))*(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl))))/(2.*pow((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)),1.5))+(-((-xi+xj)*((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk)))-(yi-yj)*(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk)))/sqrt((pow(-((-xi+xk)*(-yi+yj))+(-xi+xj)*(-yi+yk),2)+pow((-xi+xk)*(-zi+zj)+(xi-xj)*(-zi+zk),2)+pow(-((-yi+yk)*(-zi+zj))+(-yi+yj)*(-zi+zk),2))*(pow(-((-xj+xl)*(yi-yj))+(xi-xj)*(-yj+yl),2)+pow((-xj+xl)*(zi-zj)+(-xi+xj)*(-zj+zl),2)+pow(-((-yj+yl)*(zi-zj))+(yi-yj)*(-zj+zl),2)));
    //result = dot_kij_ijl / sqrt(norm_norm_vec_kij * norm_norm_vec_ijl) * ((double)-1.0);
    return 0;
}

int calc_N_neigh_inter(int K, int N)// total N types; K body interaction; MULTIPLY N to the return value to get correct answer!!!
{
    int i;
    int result = 0;
    if (K == 1) return 1;
    if (N == 1) return 1;
    for (i = 1; i <= N; i++)
    {
        result += calc_N_neigh_inter(K - 1, i);
    }
    return result;
}

int compare_Nei_type(int N_neighb_atom, int * current_type, int * params_type)//For example, current_type = {1, 8, 8}, params_type = {8, 1, 8}, then return 1
{
    int i, j, k;
    int sum = 0;
    /*std::vector<int> current_type_ (current_type, current_type + N_neighb_atom);
    std::vector<int> params_type_ (params_type, params_type + N_neighb_atom);*/
    for (i = 0; i <= N_neighb_atom - 1; i++)
    {
        if (current_type[i] == -1)
        {
            return 0;
        }
    }
    if (params_type[0] == -1)
    {
        return 1;
    }
    /*std::sort(current_type_.begin(), current_type_.end());
    std::sort(params_type_.begin(), params_type_.end());
    for (i = 0; i <= N_neighb_atom - 1; i++)
    {
        sum += ((current_type_[i] - params_type_[i]) * (current_type_[i] - params_type_[i]));
    }
    return ( sum == 0 ? 1 : 0);*/
    switch (N_neighb_atom)
    {
        case 1://two body
        {
            if (current_type[0] == params_type[0])
            {
                return 1;
            }
            break;
        }
        case 2://three body
        {
            if ((((current_type[0] == params_type[0])&&(current_type[1] == params_type[1]))) || (((current_type[0] == params_type[1])&&(current_type[1] == params_type[0]))))
            {
                return 1;
            }
            break;
        }
        case 3://four body
        {
            int mul1 = current_type[0] * current_type[1] * current_type[2];
            int mul2 = params_type[0] * params_type[1] * params_type[2];
            int sum1 = current_type[0] + current_type[1] + current_type[2];
            int sum2 = params_type[0] + params_type[1] + params_type[2];
            if ((mul1 == mul2)&&(sum1 == sum2))
            {
                return 1;
            }
            break;
        }
    }
    return 0;
}

int find_index_int(int target, int * array, int array_length)
{
    std::vector<int> array_ (array, array + array_length);
    std::vector<int>::iterator it = std::find(array_.begin(), array_.end(), target);
    int index = std::distance(array_.begin(), it);
    return index;
}

double **** calloc_params_LASP(int dim1, int dim2, int ** dim3_, int ** dim4_)
{
    int N_types_all_frame = dim1;
    int N_PTSD_types = dim2;
    int ** N_cutoff_radius = dim3_;
    int ** N_neigh_inter = dim4_;
    int i, j, k, l;
    double **** result = NULL;
    result = (double ****)calloc(dim1, sizeof(double ***));
    for (i = 0; i <= dim1 - 1; i++)
    {
        result[i] = (double ***)calloc(dim2, sizeof(double **));
        for (j = 0 ; j <= dim2 - 1; j++)
        {
            result[i][j] = (double **)calloc(N_cutoff_radius[i][j], sizeof(double *));
            for (k = 0; k <= N_cutoff_radius[i][j] - 1; k++)
            {
                result[i][j][k] = (double *)calloc(N_neigh_inter[i][j], sizeof(double));
            }
        }
    }
    return result;
}

int free_params_LASP(double **** target, int dim1, int dim2, int ** dim3_, int ** dim4_)
{
    return 1;
}

int free_sym_coord(void * sym_coord_, int sym_coord_type, parameters_info_struct * parameters_info)
{
    switch (sym_coord_type)
    {
        case 1:
        {
            int i, j;
            sym_coord_DeePMD_struct * sym_coord_DeePMD = (sym_coord_DeePMD_struct *)sym_coord_;
            for (i = 0; i <= parameters_info->Nframes_tot - 1; i++)
            {
                //free(sym_coord_DeePMD[i].type);
                for (j = 0; j <= sym_coord_DeePMD[i].N_Atoms - 1; j++)
                {
                    free(sym_coord_DeePMD[i].coord_converted[j]);
                    free(sym_coord_DeePMD[i].d_to_center_x[j]);
                    free(sym_coord_DeePMD[i].d_to_center_y[j]);
                    free(sym_coord_DeePMD[i].d_to_center_z[j]);
                }

                free(sym_coord_DeePMD[i].coord_converted);
                free(sym_coord_DeePMD[i].d_to_center_x);
                free(sym_coord_DeePMD[i].d_to_center_y);
                free(sym_coord_DeePMD[i].d_to_center_z);
            }
            free(sym_coord_DeePMD);
            return 0;
            break;
        }
        case 2:
        {
            int i, j, k;
            sym_coord_LASP_struct * sym_coord_LASP = (sym_coord_LASP_struct *)sym_coord_;
            for (i = 0; i <= parameters_info->Nframes_tot - 1; i++)
            {
                //free(sym_coord_DeePMD[i].type);
                for (j = 0; j <= sym_coord_LASP[i].N_Atoms - 1; j++)
                {
                    free(sym_coord_LASP[i].coord_converted[j]);
                    for (k = 0; k <= parameters_info->N_sym_coord - 1; k++)
                    {
                        free(sym_coord_LASP[i].idx_nei[j][k]);
                        free(sym_coord_LASP[i].d_x[j][k]);
                        free(sym_coord_LASP[i].d_y[j][k]);
                        free(sym_coord_LASP[i].d_z[j][k]);
                    }
                    free(sym_coord_LASP[i].idx_nei[j]);
                    free(sym_coord_LASP[i].d_x[j]);
                    free(sym_coord_LASP[i].d_y[j]);
                    free(sym_coord_LASP[i].d_z[j]);
                    /*Not completed
                    free(sym_coord_LASP[i].d_to_center_x[j]);
                    free(sym_coord_LASP[i].d_to_center_y[j]);
                    free(sym_coord_LASP[i].d_to_center_z[j]);*/
                }

                free(sym_coord_LASP[i].coord_converted);
                free(sym_coord_LASP[i].idx_nei);
                free(sym_coord_LASP[i].d_x);
                free(sym_coord_LASP[i].d_y);
                free(sym_coord_LASP[i].d_z);
                /*Not completed
                free(sym_coord_LASP[i].d_to_center_x);
                free(sym_coord_LASP[i].d_to_center_y);
                free(sym_coord_LASP[i].d_to_center_z);*/
            }
            free(sym_coord_LASP);
            return 0;
            break;
        }
        default:
        {
            break;
        }
    }
    return 0;

}

int cart_to_frac(double * cart, double box[3][3], double * frac)
{
    double a1 = box[0][0], a2 = box[0][1], a3 = box[0][2], b1 = box[1][0], b2 = box[1][1], b3 = box[1][2], c1 = box[2][0], c2 = box[2][1], c3 = box[2][2];
    double denominator = (a1 * b2 * c3 + b1 * c2 * a3 + c1 * a2 * b3 - c1 * b2 * a3 - b1 * a2 * c2 - a1 * c2 * b3);
    if (denominator == 0)
    {
        return 1;//Two or more box vectors are parallel.
    }
    /*Don't forget the tranpose in the reverse-matrix formula!*/
    double prefact = 1.0 / denominator;
    double rev_a1 = prefact * (b2 * c3 - c2 * b3);
    double rev_b1 = prefact * (c1 * b3 - b1 * c3);
    double rev_c1 = prefact * (b1 * c2 - c1 * b2);
    double rev_a2 = prefact * (c2 * a3 - a2 * c3);
    double rev_b2 = prefact * (a1 * c3 - c1 * a3);
    double rev_c2 = prefact * (c1 * a2 - a1 * c2);
    double rev_a3 = prefact * (a2 * b3 - b2 * a3);
    double rev_b3 = prefact * (b1 * a3 - a1 * b3);
    double rev_c3 = prefact * (a1 * b2 - b1 * a2);
    double x = cart[0], y = cart[1], z = cart[2];
    *frac = rev_a1 * x + rev_b1 * y + rev_c1 * z;
    *(frac + 1) = rev_a2 * x + rev_b2 * y + rev_c2 * z;
    *(frac + 2) = rev_a3 * x + rev_b3 * y + rev_c3 * z;
    return 0;
}
int frac_to_cart(double * cart, double box[3][3], double * frac)
{
    double a1 = box[0][0], a2 = box[0][1], a3 = box[0][2], b1 = box[1][0], b2 = box[1][1], b3 = box[1][2], c1 = box[2][0], c2 = box[2][1], c3 = box[2][2];
    double x_ = frac[0], y_ = frac[1], z_ = frac[2];
    *cart = a1 * x_ + b1 * y_ + c1 * z_;
    *(cart + 1) = a2 * x_ + b2 * y_ + c2 * z_;
    *(cart + 2) = a3 * x_ + b3 * y_ + c3 * z_;
    return 0;
}

double max(double a, double b)
{
    return (a >= b ? a : b);
}


#include "matrix.h"
#include "mex.h"

#include <iostream>
#include <math.h>
#include <chrono>
#include <random>
#include <vector>

#include <Eigen\Eigen>
#include <Eigen\LU>
#include <Eigen\Dense>
#include <Eigen\SVD>

#include "SampleConsensusProblem.hpp"
#include "SampleConsensus.hpp"
#include "Ransac.hpp"
#include "Msac.hpp"
#include "Prosac.hpp"
#include "Lmeds.hpp"

using namespace std;
using namespace Eigen;
using namespace aslam;

class HomographyEstimatorProblem : public aslam::SampleConsensusProblem <MatrixXd>
{
public:
	HomographyEstimatorProblem() { }
	virtual ~HomographyEstimatorProblem() { }

	size_t numElements() const{ return points1_.rows(); }

	virtual int getSampleSize() const { return 4; }

	// Take a subset of size 4 of points and estimate homography matrix using them
	virtual bool computeModelCoefficients(const std::vector<int> & indices, MatrixXd& model) const
	{
		if (indices.size() != getSampleSize())
		{
			mexPrintf("Invalid set of sample points given (%d)!\n", indices.size());
			return false;
		}

		MatrixXd p1(indices.size(), 3);
		MatrixXd p2(indices.size(), 3);
		for (int i = 0; i < indices.size(); i++)
		{
			p1.row(i) = points1_.row(indices[i]);
			p2.row(i) = points2_.row(indices[i]);
		}
		auto H = calculateHomography(p1, p2);

		model = H;
		// end calculate homography
		return true;
	}

	virtual void optimizeModelCoefficients(const vector<int> & inliers, const MatrixXd& model, MatrixXd& optimized_model)
	{
		optimized_model = model;
	}

	// It will return a matrix H, the homography matrix, such that x1=H*x2
	// Note: points are assumed to be normalized and in homogeneous coordinate
	// Sizes of two sets of points must be equal. for performace purposes, this condition is not checked in this function
	// Note: since normalise2Dpoints works in-place on the points matrix, normalizing points inside this function will
	// harm our collection of points. But because of two observations, we are allowed to normalise points inside this function
	// and it will cause no harm:
	//	1) Points from each step of ransac will be passed to this function to calculate the homography matrix. To do so, we will
	//	   copy those points from the main point matrix to temporary matrices. So normalisation won't affect the original matrix.
	//  2) After exhausting the SAC operation, the final Homography matrix will be calculated using all of the inliers. After this
	//	   calculation, the program will return. So affecting the data points, won't do harm.
	MatrixXd calculateHomography(MatrixXd& x1, MatrixXd& x2) const
	{
		MatrixXd T1, T2;
		normalize2Dpoints(x1, T1);
		normalize2Dpoints(x2, T2);
		MatrixXd A = MatrixXd::Zero(3 * x1.rows(), 9);
		for (int i = 0; i < x1.rows(); i++)
		{
			A(3 * i, 3) = -x1(i, 0);
			A(3 * i, 4) = -x1(i, 1);
			A(3 * i, 5) = -x1(i,2);
			A(3 * i, 6) = x2(i, 1) * x1(i, 0);
			A(3 * i, 7) = x2(i, 1) * x1(i, 1);
			A(3 * i, 8) = x2(i, 1) * x1(i, 2);

			A(3 * i + 1, 0) = x1(i, 0);
			A(3 * i + 1, 1) = x1(i, 1);
			A(3 * i + 1, 2) = x1(i, 2);
			A(3 * i + 1, 6) = -x2(i, 0) * x1(i, 0);
			A(3 * i + 1, 7) = -x2(i, 0) * x1(i, 1);
			A(3 * i + 1, 8) = -x2(i, 0) * x1(i, 2);

			A(3 * i + 2, 0) = x2(i, 1) * x1(i, 0);
			A(3 * i + 2, 1) = x2(i, 1) * x1(i, 1);
			A(3 * i + 2, 2) = x2(i, 1) * x1(i, 2);
			A(3 * i + 2, 3) = x2(i, 0) * x1(i, 0);
			A(3 * i + 2, 4) = x2(i, 0) * x1(i, 1);
			A(3 * i + 2, 5) = x2(i, 0) * x1(i, 2);
		}
		JacobiSVD<MatrixXd> svd(A, ComputeThinV);
		svd.computeV();
		MatrixXd minEigenVector = MatrixXd(svd.matrixV().col(8));
		minEigenVector.resize(3, 3);
		auto H = T2.inverse() * minEigenVector * T1;
		return H;
	}

	// As the final step, estimate the homography matrix using all of the inlier correspondences
	void calculateModelUsingAllInliers(const vector<int>& inliers, MatrixXd& finalModel)
	{
		MatrixXd p1(inliers.size(), 3);
		MatrixXd p2(inliers.size(), 3);
		for (int i = 0; i < inliers.size(); i++)
		{
			p1.row(i) = points1_.row(inliers[i]);
			p2.row(i) = points2_.row(inliers[i]);
		}
		auto H = calculateHomography(p1, p2);

		finalModel = H;
	}

	/// evaluate the score for the elements at indices based on this model.
	/// low scores mean a good fit.
	virtual void getSelectedDistancesToModel(const MatrixXd& model, const vector<int> & indices, vector<double> & scores) const
	{
		scores.resize(indices.size());
		// Iterate through correspondences and calculate the projection error

		auto H = model;
		for (size_t i = 0; i < indices.size(); ++i)
		{
			auto x1 = points1_.row(indices[i]);
			auto x2 = points2_.row(indices[i]);
			auto Hx1 = H * x1;
			auto invHx2 = H.inverse() * x2;
			x1 /= x1(2);
			x2 /= x2(2);
			Hx1 /= Hx1(2);
			invHx2 /= invHx2(2);
			scores[i] = (x1 - invHx2).unaryExpr([](double a) {return a*a; }).sum() + (x2 - Hx1).unaryExpr([](double a) {return a*a; }).sum();
		}
	}

	// normalize 2d points so that they would have a zero mean and their mean distance from origin would be sqrt(2)
	void normalize2Dpoints(MatrixXd& points, MatrixXd& normalizationMat) const
	{
		double meanx = points.col(0).mean();
		double meany = points.col(1).mean();
		/*auto colx = points.col(0).unaryExpr([meanx](double x) { return x - meanx; });
		auto coly = points.col(1).unaryExpr([meany](double y) { return y - meany; });
		auto dist = (colx.unaryExpr([](double x) { return x*x; }) + coly.unaryExpr([](double y) {return y*y; })).unaryExpr([](double d) {return sqrt(d); });*/
		double meandist = (points.col(0).unaryExpr([meanx](double x) { return (x - meanx)*(x - meanx); }) + points.col(1).unaryExpr([meany](double y) { return (y - meany)*(y - meany); }))
			.unaryExpr([](double d){return sqrt(d); }).mean();
		double scale = sqrt(2) / meandist;

		MatrixXd T(3, 3);
		T << scale, 0, -scale*meanx
			, 0, scale, -scale*meany
			, 0, 0, 1;
		points = (T*points.transpose()).transpose();
		normalizationMat = T;
	}

	MatrixXd points1_;
	MatrixXd norm1_;
	MatrixXd points2_;
	MatrixXd norm2_;
};

// This is the entry point from MATLAB to this routine
// plhs and prhs represent a pointer to the left hand side (output) and a pointer to the right hand side (input) respectively
// matlab function signature: H = homography_estimator(points1, points2, 'mode')
// H -> 9-by-9 homography matrix as output
// points1 -> n-by-2 matrix, coordinates of the keypoints of the first image
// points2 -> n-by-2 matrix, coordinates of the keypoints of the second image
// 'mode' -> {'ransac', 'prosac', 'mlesac'}
void mexFunction(int nlhs, mxArray *plhs [], int nrhs, const mxArray*prhs [])
{
	if (nlhs != 1)
	{
		mexPrintf("Only one output expected\n");
		return;
	}
	if (nrhs != 3)
	{
		mexPrintf("Only three inputs expected\n");
		return;
	}

	const mwSize *dims1, *dims2;
	char* modeStr;
	double *points1, *points2;

	dims1 = mxGetDimensions(prhs[0]);
	dims2 = mxGetDimensions(prhs[1]);

	int dim1y = (int) dims1[0]; int dim1x = (int) dims1[1];
	int dim2y = (int) dims2[0]; int dim2x = (int) dims2[1];

	if (dim1x != dim2x || dim1y != dim2y)
	{
		mexPrintf("Error! two input points arrays must be of the same size\n");
		return;
	}

	if (dim1x != 2 || dim2x != 2)
	{
		mexPrintf("Error! Points must be 2-D!\n");
		return;
	}

	if (dim1y < 4)
	{
		mexPrintf("Not enough correspondences provided\n");
		return;
	}

	modeStr = mxArrayToString(prhs[2]);

	if (!modeStr || mxGetDimensions(prhs[2])[0] != 1)
	{
		mexPrintf("Error! Last argument must be an string!\n");
		return;
	}

	mexPrintf("string:\n%s\n", modeStr);
	mexPrintf("dimx = %d, dimy = %d\n", dim1x, dim1y);
	//vector<Correspondence> correspondences(dim1y);
	points1 = mxGetPr(prhs[0]);
	points2 = mxGetPr(prhs[1]);

	boost::shared_ptr<HomographyEstimatorProblem> homoproblem_ptr(new HomographyEstimatorProblem);
	HomographyEstimatorProblem& homoproblem = *homoproblem_ptr;
	homoproblem.points1_.resize(dim1y, 3);
	homoproblem.points1_.col(2).setOnes();
	homoproblem.points2_.resize(dim1y, 3);
	homoproblem.points2_.col(2).setOnes();
	homoproblem.setUniformIndices(dim1y);

	for (size_t j = 0; j < dim1y; j++)
	{
		/*Correspondence corr;
		corr.p1.x = points1[j];
		corr.p2.x = points2[j];
		corr.p1.y = points1[dim1y + j];
		corr.p2.y = points2[dim1y + j];*/
		//correspondences[j] = Correspondence(points1[j], points1[dim1y + j], points2[j], points2[dim1y + j]);
		//  mexPrintf("correspondence[%d] = (%lf, %lf; %lf, %lf)\n", j, points1[j], points1[dim1y + j], points2[j], points2[dim1y + j]);
		// mexPrintf("correspondence[%d] = (%lf, %lf; %lf, %lf)\n", j, correspondences[j].p1.x, correspondences[j].p1.y, correspondences[j].p2.x, correspondences[j].p2.y);
		homoproblem.points1_(j, 0) = points1[j];
		homoproblem.points1_(j, 1) = points1[dim1y + j];
		homoproblem.points2_(j, 0) = points2[j];
		homoproblem.points2_(j, 1) = points2[dim1y + j];
	}

	homoproblem.normalize2Dpoints(homoproblem.points1_, homoproblem.norm1_);
	homoproblem.normalize2Dpoints(homoproblem.points2_, homoproblem.norm2_);

	// HomographyEstimatorProblem *homoproblem = new HomographyEstimatorProblem();

	SampleConsensus<HomographyEstimatorProblem> *sac;
	if (!strcmp("ransac", modeStr))
	{
		sac = new Ransac<HomographyEstimatorProblem>(1000, 0.1, 0.9);
	}
	else if (!strcmp("lmeds", modeStr))
	{
		sac = new Lmeds<HomographyEstimatorProblem>(1000, 0.1, 0.9);
	}
	else if (!strcmp("msac", modeStr))
	{
		sac = new Msac<HomographyEstimatorProblem>(1000, 0.1, 0.9);
	}
	else if (!strcmp("prosac", modeStr))
	{
		sac = new Prosac<HomographyEstimatorProblem>(1000, 0.1, 0.9);
	}
	else
	{
		mexPrintf("Error! unknown estimation mode selected!\n");
		return;
	}
	// if(!strcmp("rmsac", modeStr))
	// {
	// 	sac = new Rmsac<HomographyEstimatorProblem>(1000, 0.1, 0.9);
	// }

	// Msac<HomographyEstimatorProblem> ransac(1000, 0.1, 0.9);

	sac->sac_model_ = homoproblem_ptr;
	sac->computeModel(4);
	auto homographyModel = sac->model_coefficients_;
	mexPrintf("Number of iterations: %d\n", sac->iterations_);
	mexPrintf("Inliers size: %d (out of %d input correspondences)\n", sac->inliers_.size(), homoproblem.points1_.rows());

	homoproblem.calculateModelUsingAllInliers(sac->inliers_, homographyModel);
	auto H = homoproblem.norm2_.inverse() * homographyModel * homoproblem.norm1_;

	auto outArray = mxCreateDoubleMatrix(3, 3, mxREAL);
	double *outp = mxGetPr(outArray);

	// for (size_t i = 0; i < 3; i++)
	// {
	// 	for (size_t j = 0; j < 3; j++)
	// 	{
	// 		mexPrintf("homography matrix(%d, %d) = %lf\n", i, j, ransac.model_coefficients_.Matrix[j*3+i]);
	// 	}
	// }

	// Copy the estimated homography matrix to the output matrix, note the difference between MATLAB's column-wise arrays vs. C's row-wise arrays convention
	for (size_t i = 0; i < 3; i++)
	{
		for (size_t j = 0; i < 3; j++)
		{
			outp[i + 3 * j] = H(i, j);
		}
	}

	plhs[0] = outArray;
}

PCL SAC + Boost/homography_estimator_pcl.cpp


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

#include <iostream>
#include <boost/math/tools/ulps_plot.hpp>
#include <boost/core/demangle.hpp>
#include <boost/math/special_functions/jacobi_theta.hpp>

using boost::math::tools::ulps_plot;

int main() {
    using PreciseReal = long double;
    using CoarseReal = float;

    CoarseReal q = 0.5;

    auto jacobi_theta1_coarse = [=](CoarseReal z) {
        return boost::math::jacobi_theta1<CoarseReal>(z, q);
    };
    auto jacobi_theta1_precise = [=](PreciseReal z) {
        return boost::math::jacobi_theta1<PreciseReal>(z, q);
    };
    auto jacobi_theta2_coarse = [=](CoarseReal z) {
        return boost::math::jacobi_theta2<CoarseReal>(z, q);
    };
    auto jacobi_theta2_precise = [=](PreciseReal z) {
        return boost::math::jacobi_theta2<PreciseReal>(z, q);
    };
    auto jacobi_theta3_coarse = [=](CoarseReal z) {
        return boost::math::jacobi_theta3m1<CoarseReal>(z, q);
    };
    auto jacobi_theta3_precise = [=](PreciseReal z) {
        return boost::math::jacobi_theta3m1<PreciseReal>(z, q);
    };
    auto jacobi_theta4_coarse = [=](CoarseReal z) {
        return boost::math::jacobi_theta4m1<CoarseReal>(z, q);
    };
    auto jacobi_theta4_precise = [=](PreciseReal z) {
        return boost::math::jacobi_theta4m1<PreciseReal>(z, q);
    };

    int samples = 2500;
    int width = 800;
    PreciseReal clip = 100;

    std::string filename1 = "jacobi_theta1_" + boost::core::demangle(typeid(CoarseReal).name()) + ".svg";
    auto plot1 = ulps_plot<decltype(jacobi_theta1_precise), PreciseReal, CoarseReal>(jacobi_theta1_precise, 0.0, boost::math::constants::two_pi<CoarseReal>(), samples);
    plot1.clip(clip).width(width);
    std::string title1 = "jacobi_theta1(x, 0.5) ULP plot at " + boost::core::demangle(typeid(CoarseReal).name()) + " precision";
    plot1.title(title1);
    plot1.vertical_lines(10);
    plot1.add_fn(jacobi_theta1_coarse);
    plot1.write(filename1);

    std::string filename2 = "jacobi_theta2_" + boost::core::demangle(typeid(CoarseReal).name()) + ".svg";
    auto plot2 = ulps_plot<decltype(jacobi_theta2_precise), PreciseReal, CoarseReal>(jacobi_theta2_precise, 0.0, boost::math::constants::two_pi<CoarseReal>(), samples);
    plot2.clip(clip).width(width);
    std::string title2 = "jacobi_theta2(x, 0.5) ULP plot at " + boost::core::demangle(typeid(CoarseReal).name()) + " precision";
    plot2.title(title2);
    plot2.vertical_lines(10);
    plot2.add_fn(jacobi_theta2_coarse);
    plot2.write(filename2);

    std::string filename3 = "jacobi_theta3_" + boost::core::demangle(typeid(CoarseReal).name()) + ".svg";
    auto plot3 = ulps_plot<decltype(jacobi_theta3_precise), PreciseReal, CoarseReal>(jacobi_theta3_precise, 0.0, boost::math::constants::two_pi<CoarseReal>(), samples);
    plot3.clip(clip).width(width);
    std::string title3 = "jacobi_theta3m1(x, 0.5) ULP plot at " + boost::core::demangle(typeid(CoarseReal).name()) + " precision";
    plot3.title(title3);
    plot3.vertical_lines(10);
    plot3.add_fn(jacobi_theta3_coarse);
    plot3.write(filename3);

    std::string filename4 = "jacobi_theta4_" + boost::core::demangle(typeid(CoarseReal).name()) + ".svg";
    auto plot4 = ulps_plot<decltype(jacobi_theta4_precise), PreciseReal, CoarseReal>(jacobi_theta4_precise, 0.0, boost::math::constants::two_pi<CoarseReal>(), samples);
    plot4.clip(clip).width(width);
    std::string title4 = "jacobi_theta4m1(x, 0.5) ULP plot at " + boost::core::demangle(typeid(CoarseReal).name()) + " precision";
    plot4.title(title4);
    plot4.vertical_lines(10);
    plot4.add_fn(jacobi_theta4_coarse);
    plot4.write(filename4);
}


#include <cstdio>
#include <cstdlib>
#include <iostream>
#include <fstream>
#include <vector>
#include <chrono>

#include <getopt.h>

#include "Sampling_Matrix.hpp"
#include "Alpert_Matrix.hpp"
#include "Utils.hpp"
#include "ccstomp.hpp"

#include <boost/numeric/bindings/blas/blas.h>

using namespace std;

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

  if(argc == 1 ){
    cout << argv[0] << " usage is:" << endl;
    cout << argv[0] << " --seed 'seed_value' --reduction 'reduction_value' [mesh input_file] [data input_file]" << endl;
    cout << "If the data input file is provided it should have the same number of points as the mesh, if not a default data field will be assigned" << endl;
    cout << "Please refer to README.txt in this folder for more info" << endl;
    exit (0);
  }

  /*Seed is overwritten if set as a command line option*/
    unsigned seed = chrono::system_clock::now().time_since_epoch().count();
    chrono::duration <double> duration;

    int d, N;
    double R=10.0e0;
    int c;
    double alpha=1.0e0, beta=0.0e0;
    int inc=1;

    {
          static struct option long_options[] =
        {
          /* These options set a flag. */
          /* These options don’t set a flag.
             We distinguish them by their indices. */
          {"seed",  required_argument, 0, 's'},
          {"reduction",  required_argument, 0, 'r'},
          {0, 0, 0, 0}
        };
      /* getopt_long stores the option index here. */
      int option_index = 0;

      while (c = getopt_long (argc, argv, "s",
			      long_options, &option_index) != -1){

	if(option_index == 0 && optarg){
	  seed  = atoi(optarg);
	  cout << "Seed set to " << seed << endl;
	}
	if(option_index == 1 && optarg){
	  R  = atof(optarg);
	  cout << "Reduction set to " << R << endl;
	}
      }

    }

    if(optind >= argc ) {
      cout << "Expected an input file name " << endl;
      exit(0);
    }

    cout << "Reading mesh input file" << argv[optind] << endl;
    ifstream f(argv[optind]);
    f >> N >> d;

    cout << "Number of Points " << N << endl;
    cout << "Number of dimensions " << d << endl;
    cout << "R " << R << endl;

    int M=ceil(double(N)/R);
    int NR=N-M;

    vector<vector<double> >p(N, vector<double>(d));

    // Read mesh into array
    for (int i = 0; i < N; i++)
        for (int j = 0; j < d; j++)
            f >> p[i][j];

    f.close();

    // Setting Wavelet orders
    vector<int> ki(d);
    for (int j = 0; j < d; j++)
      ki[j] = 5;

    cout << "Constructing Alpert Matrix" << endl;
    boost::numeric::ublas::compressed_matrix <double>  U(N, N);
    chrono::system_clock::time_point t1 = chrono::system_clock::now();
    boostbuild_Alpert_matrix(p, ki,  U, -1, N);
    chrono::system_clock::time_point t2 = chrono::system_clock::now();
    duration=t2-t1;
    cout << "Total wavelet matrix construction time " << duration.count() << endl;

    boost::numeric::ublas::vector<double> x(N);
    boost::numeric::ublas::vector<double> w(N);
    boost::numeric::ublas::vector<double> xw(N);
    boost::numeric::ublas::vector<double> ys(N);

    // Setting data, this step can be replaced by some commands to read an external data file
    double pi=4.0e0*atan(1.0e0);
    if (argc>6) {
      cout << "Reading data input file" << argv[optind+1] << endl;
      f.open(argv[optind+1]);
      for (int i = 0; i < N; i++)
	f >> x(i);
    }
    else
      for (int i = 0; i < N; i++) x(i)=(4.0e0*sin(8.0e0*pi*p[i][0]))*(4.0e0*sin(7.0e0*pi*p[i][1]) )*3.0e0*sin(6.0e0*pi*p[i][0]);
//     for (int i = 0; i < N; i++) x[i]=(4.0*sin(2.0*pi*p[i][0]) - 4.0*sin(2.0*pi*p[i][1]))*3.0*sin(2.0*pi*p[i][0]);

    // Forward wavelet transform
    t1 = chrono::system_clock::now();
    boost::numeric::ublas::axpy_prod(U, x, w, true);

    // Sorting, wavelet compression and inverse wavelet transform
    vector<double> yv(N);
    vector<size_t> iyv(N);
    for (int i = 0; i < N; i++) {yv[i]=fabs(w(i));iyv[i]=i;}
    sortandindices(yv, iyv);
    for (int i = NR; i < N; i++) ys(iyv[i])=w(iyv[i]);
    t2 = chrono::system_clock::now();
    duration=t2-t1;
    cout << "Total wavelet compression time " << duration.count() << endl;

    // xw is the reconstruction by wavelets
    boost::numeric::ublas::axpy_prod(ys, U, xw, true);

    cout << "Constructing Bernoulli Sampling Matrix" << endl;
    boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major> P(N,M,0.0e0);
    t1 = chrono::system_clock::now();
    ConstructBernoulliMatrix(M, N, seed, P);
    t2 = chrono::system_clock::now();
    duration=t2-t1;
    cout << "Total compression matrix construction time " << duration.count() << endl;

    cout << "Performing CS compression" << endl;
    boost::numeric::ublas::vector<double> y(M);
    t1 = chrono::system_clock::now();
    BLAS_DGEMV("T", &N, &M, &alpha, &P(0,0), &N, &x(0), &inc, &beta, &y(0), &inc);
    t2 = chrono::system_clock::now();
    duration=t2-t1;
    cout << "Total CS compression time " << duration.count() << endl;

    cout << "Constructing Alpert-Bernoulli Matrix product" << endl;
    t1 = chrono::system_clock::now();
    build_Alpert_Sampling_product(U,P,P);
    t2 = chrono::system_clock::now();
    duration=t2-t1;
    cout << "Alpert-Compression matrix product time " << duration.count() << endl;

    // Computing incoherence between sampling and wavelet matrices
    double maxT=0.0;
    for (int j = 0; j < M; j++)
      for (int i = 0; i < N; i++)
	if (fabs(P(i,j))>maxT)
	  maxT=fabs(P(i,j));

    cout << "Performing StOMP" << endl;
    t1 = chrono::system_clock::now();
    boost::numeric::ublas::vector<double> ywp(N);
    int * asize= new int[2]; asize[0]=M; asize[1]=N;
    int * rsize= new int[2]; rsize[0]=M; rsize[1]=1;
    int niter=0, nywp=0;
    double *activeSet=NULL;
    double *aresult=NULL;
    // activeSet and aresult are the initial guess of the solution, here we assume the coefficient
    // vector is empty
    cc_solve_stomp(&P(0,0), &y(0), asize, rsize, 1.0e-8, 50, &niter, &nywp, &ywp(0), activeSet, aresult);

    boost::numeric::ublas::vector<double> xs(N);
    boost::numeric::ublas::axpy_prod(ywp, U, xs, true);
    t2 = chrono::system_clock::now();
    duration=t2-t1;
    cout << "StOMP time " << duration.count() << endl;

    // Computing NRMSE
    double es=0.0e0, ew=0.0e0;
    for (int i = 0; i < N; i++) {
      ew+=pow(x(i)-xw(i),2.0e0);
      es+=pow(x(i)-xs(i),2.0e0);
    }

    // Computing min and max values in the data x
    double mx=x(0);
    double mn=x(0);
    for (int i = 0; i < N; i++) {
      if (x(i)>mx)
	mx=x(i);
      if (x(i)<mn)
	mn=x(i);
    }

    es=sqrt(es/double(N))/(mx-mn);
    ew=sqrt(ew/double(N))/(mx-mn);

    cout << "Alpert Wavelets Compression NRMSE = " << ew << endl;
    cout << "CS Compression NRMSE = " << es << endl;
}


//
// Created by jcfei on 18-9-29.
//

#include <iostream>
#include <vector>
#include <complex>
#include <math.h>
#include "time.h"
using namespace std;
//**********************************//
#include <fftw3.h>
#include "mkl.h"
#include <omp.h>
#include <armadillo>
//**********************************//
#include "tensor.h"
#include "tensor.cpp"
#include "cp_als.cpp"
#include "tucker_hosvd.cpp"
//**********************************//

double gettime(){
    struct timeval tv;
    gettimeofday(&tv,NULL);
    return tv.tv_sec*1000+tv.tv_usec/1000.0; //time:s
};

using namespace std;
using namespace arma;

int main() {
    double t0,t1;
    int I=3;
    int rank=0.2*I; rank=1;

    Tensor<float> a(I,I,I);
    //initialization
    for (int i = 0; i < a.n1; ++i) {
        for (int j = 0; j < a.n2; ++j) {
            for(int k=0; k< a.n3; ++k) {
                a(i,j,k) = randu<float>();
            }
        }
    }

    t0=gettime();
    tucker_core<float> result_tucker;
    result_tucker = hosvd(a,rank,rank,rank);
    t1=gettime();
    cout << "time:" <<t1-t0 <<endl;

    t0=gettime();
    cp_mats<float> result;
    result = cp_als(a,rank);
    t1=gettime();
    cout << "time:" <<t1-t0 <<endl;

    return 0;

}


// gnuplot-c++ includes
#include <gnuplot-iostream.h>

// MLearn includes
#include <MLearn/Core>
#include <MLearn/Sampling/GaussianSampling.h>

// STL includes
#include <vector>
#include <tuple>
#include <string>
#include <cmath>

// Eigen includes
#include <Eigen/Core>
#include <Eigen/SVD>

int main(int argc, char** argv){
	std::srand((unsigned int) time(0));

	Gnuplot gp;

	using namespace MLearn;
	using namespace Sampling::Gaussian;

	typedef double float_type;
	typedef MLMatrix<float_type> Matrix;
	typedef MLVector<float_type> Vector;

	uint DIM = 2;
	uint N = 5000;

	// compute a random covariance matrix
	Matrix random = Matrix::Random(DIM, DIM);
	Eigen::JacobiSVD<Matrix> svd(random,
		Eigen::ComputeFullU | Eigen::ComputeFullV );
	Matrix covariance = svd.matrixU();
	Matrix eig_val = (svd.singularValues().cwiseAbs().cwiseSqrt()).asDiagonal();
	covariance = covariance*eig_val*eig_val;
	covariance = covariance*(svd.matrixU().transpose());
	// compute mean
	Vector mean = Vector::Random(DIM);
	mean.array() -= float_type(0.5);

	// sample
	Matrix samples = MultivariateGaussian<float_type>::sample(
		mean, covariance, N);

	// draw the samples
	std::vector<std::pair<double, double> > xy_pts;
	for(uint i = 0; i < N; ++i) {
		xy_pts.push_back(std::make_pair(samples(0, i), samples(1, i)));
	}
	gp << "set multiplot\n";
	gp << "set xrange [-5:5]\nset yrange [-5:5]\n";
	gp << "plot '-' with points notitle\n";
	gp.send1d(xy_pts);

	gp << "set parametric\n";
	gp << "set trange [0:2*pi]\n";
	gp << "fx(t)=" +
	      std::to_string(svd.matrixU()(0,0)*eig_val(0,0)*1.96) +
	      "*cos(t)+" +
	      std::to_string(svd.matrixU()(0,1)*eig_val(1,1)*1.96) +
	      "*sin(t)+" + std::to_string(mean(0)) + "\n";
	gp << "fy(t) = " +
	      std::to_string(svd.matrixU()(1,0)*eig_val(0,0)*1.96) +
	      "*cos(t) + " +
	      std::to_string(svd.matrixU()(1,1)*eig_val(1,1)*1.96) +
	      "*sin(t) + " + std::to_string(mean(1)) + "\n";
	gp << "plot fx(t), fy(t) linecolor rgb \"blue\" notitle\n";

	return 0;
}


/*
 *  Copyright (C) 2003,2005 by Jarno Elonen
 *
 *  TPSDemo is Free Software / Open Source with a very permissive
 *  license:
 *
 *  Permission to use, copy, modify, distribute and sell this software
 *  and its documentation for any purpose is hereby granted without fee,
 *  provided that the above copyright notice appear in all copies and
 *  that both that copyright notice and this permission notice appear
 *  in supporting documentation.  The authors make no representations
 *  about the suitability of this software for any purpose.
 *  It is provided "as is" without express or implied warranty.
 */

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

#include "spline.h"
#include "ludecomposition.h"

#include <vector>
#include <cmath>

using namespace boost::numeric::ublas;

//-----------------------------------------------------------------------------

static double tps_base_func(double r)
{
  if ( r == 0.0 )
    return 0.0;
  else
    return r*r * log(r);
}

//-----------------------------------------------------------------------------

/*
 *  Calculate Thin Plate Spline (TPS) weights from
 *  control points.
 */
tpsdemo::Spline::Spline(const std::vector<Vec> & control_pts, double regularization)
  : p(control_pts.size()),
    control_points(control_pts),
    mtx_v(p+3, 1),
    mtx_orig_k(p, p)
{
  // You We need at least 3 points to define a plane
  if ( control_points.size() < 3 )
    throw std::runtime_error("need at least 3 points for thin plate spline");

  int id_number = rand() % 1000;

  //unsigned p = control_points.size();

  // Allocate the matrix and vector
  matrix<double> mtx_l(p+3, p+3);
  //matrix<double> mtx_v(p+3, 1);
  //matrix<double> mtx_orig_k(p, p);

  // Fill K (p x p, upper left of L) and calculate
  // mean edge length from control points
  //
  // K is symmetrical so we really have to
  // calculate only about half of the coefficients.
  double a = 0.0;
  for ( unsigned i=0; i<p; ++i )
  {
    for ( unsigned j=i+1; j<p; ++j )
    {
      Vec pt_i = control_points[i];
      Vec pt_j = control_points[j];
      pt_i.y = pt_j.y = 0;
      double elen = (pt_i - pt_j).len();
      mtx_l(i,j) = mtx_l(j,i) =
        mtx_orig_k(i,j) = mtx_orig_k(j,i) =
          tps_base_func(elen);
      a += elen * 2; // same for upper & lower tri
    }
  }
  a /= (double)(p*p);

  // Fill the rest of L
  for ( unsigned i=0; i<p; ++i )
  {
    // diagonal: reqularization parameters (lambda * a^2)
    mtx_l(i,i) = mtx_orig_k(i,i) =
      regularization * (a*a);

    // P (p x 3, upper right)
    mtx_l(i, p+0) = 1.0;
    mtx_l(i, p+1) = control_points[i].x;
    mtx_l(i, p+2) = control_points[i].z;

    // P transposed (3 x p, bottom left)
    mtx_l(p+0, i) = 1.0;
    mtx_l(p+1, i) = control_points[i].x;
    mtx_l(p+2, i) = control_points[i].z;
  }
  // O (3 x 3, lower right)
  for ( unsigned i=p; i<p+3; ++i )
    for ( unsigned j=p; j<p+3; ++j )
      mtx_l(i,j) = 0.0;

  // Fill the right hand vector V
  for ( unsigned i=0; i<p; ++i )
    mtx_v(i,0) = control_points[i].y;
  mtx_v(p+0, 0) = mtx_v(p+1, 0) = mtx_v(p+2, 0) = 0.0;

  // Solve the linear system "inplace"
  if (0 != LU_Solve(mtx_l, mtx_v))
  {
    throw SingularMatrixError();
  }
}

//-----------------------------------------------------------------------------

double tpsdemo::Spline::interpolate_height(double x, double z) const
{
  double h = mtx_v(p+0, 0) + mtx_v(p+1, 0)*x + mtx_v(p+2, 0)*z;

  Vec pt_i, pt_cur(x,0,z);
  for ( unsigned i=0; i<p; ++i )
  {
    pt_i = control_points[i];
    pt_i.y = 0;
    h += mtx_v(i,0) * tps_base_func( ( pt_i - pt_cur ).len());
  }
  return h;
}

//-----------------------------------------------------------------------------

double tpsdemo::Spline::compute_bending_energy() const
{
  matrix<double> w( p, 1 );
  for ( unsigned i=0; i<p; ++i )
    w(i,0) = mtx_v(i,0);
  matrix<double> be = prod( prod<matrix<double> >( trans(w), mtx_orig_k ), w );
  return be(0,0);
}


/********************************************************************
	created:	2012/08/27
	created:	27:8:2012   8:40
	filename: 	E:\Sync\Dropbox\Codes\glycan_pipeline\GAG\src\GAGPL\SPECTRUM\IsotopicDistribution.cpp
	file path:	E:\Sync\Dropbox\Codes\glycan_pipeline\GAG\src\GAGPL\SPECTRUM
	file base:	IsotopicDistribution
	file ext:	cpp
	author:		Han Hu

	purpose:
*********************************************************************/
#include "GAGPL/SPECTRUM/IsotopicDistribution.h"
#include "GAGPL/MISC/Param.h"

#include <time.h>
#include <algorithm>
#include <sstream>
#include <fstream>

#include <boost/make_shared.hpp>
#include <boost/xpressive/xpressive.hpp>
#include <boost/lexical_cast.hpp>

namespace gag
{
	void IsotopicDistribution::createMonoPeak()
	{
		// Get the monoisotopic isotope for each element of the composition.
		monopeak.mz = _compo.getMass();

		const std::map<std::string, int> ele_count = _compo.get();

		std::map<std::string, int>::const_iterator it = ele_count.begin();

		for(; it != ele_count.end(); it++)
		{
			// For each element, get the value of prob^coef.
			PeriodicTable& ptable = PeriodicTable::Instance();
			Isotope iso = ptable.getIsotopeByRelativeShift(it->first);
			monopeak.intensity += it->second * log(iso.abundance);
		}
		monopeak.intensity = exp(monopeak.intensity);

	}

	double IsotopicDistribution::calculatePhiValue(size_t order)
	{
		double phi_value = 0.0;

		// For each element, get the phi factor.
		const std::map<std::string, int> ele_count = _compo.get();

		std::map<std::string, int>::const_iterator it = ele_count.begin();

		// Iterate over all elements.
		for(; it != ele_count.end(); it++)
		{
			phi_value += _iso_const.getNthElementPowerSum(it->first, order) * it->second;
		}

		return phi_value;
	}

	//PowerSumVec IsotopicDistribution::calculatePhiValueVec()
	//{
	//	PowerSumVec phi_value_vec;
	//	// phi(0) = 0.
	//	phi_value_vec.push_back(0);

	//	for(size_t i=1; i<=_var_num; i++)
	//	{
	//		phi_value_vec.push_back(this->calculatePhiValue(i));
	//	}
	//
	//	return phi_value_vec;
	//}

	EleSymPolyVec IsotopicDistribution::calculateProbabilityVec()
	{
		//clock_t t = clock(); clock_t last_t = t;
		PowerSumVec phi_vec = this->calculatePhiValueVec();
		//t = clock()-last_t; last_t = clock();
		//std::cout << "Step 1: calculating phi value vec: " << (double)t/CLOCKS_PER_SEC << "s\n";

		NewtonGirardFormulae newton(_compo.getMaxNumVariants());
		EleSymPolyVec prob_vec;
		newton.updateParameters(phi_vec, prob_vec);
		//t = clock()-last_t; last_t = clock();
		//std::cout << "Step 2: newton: " << (double)t/CLOCKS_PER_SEC << "s\n";

		for(size_t i=0; i<prob_vec.size(); i++)
		{
			int sign = (i%2 == 0 ? 1 : -1);
			// q(j) = q(0) * e(j) * (-1)^j.
			prob_vec[i] = prob_vec[i] * monopeak.intensity * sign;
		}

		return prob_vec;
	}

	double IsotopicDistribution::calculateModifiedPhiValue(const std::string& symbol, size_t order)
	{

		double phi_value = phi_value_vec.at(order) - _iso_const.getNthElementPowerSum(symbol, order) + _iso_const.getNthModifiedElementPowerSum(symbol, order);

		return phi_value;
	}

	PowerSumVec IsotopicDistribution::calculateModifiedPhiValueVec(const std::string& symbol)
	{
		PowerSumVec modified_phi_value_vec;
		// phi(0) = 1.
		modified_phi_value_vec.push_back(0);

		for(size_t i=1; i<=_var_num; i++)
		{
			modified_phi_value_vec.push_back(this->calculateModifiedPhiValue(symbol, i));
		}

		return modified_phi_value_vec;
	}

	std::vector<double> IsotopicDistribution::calculateCenterMassVec(const EleSymPolyVec& prob_vec)
	{
		std::vector<double> mass_vec;

		std::map<std::string, int> ele_count = _compo.get();

		PeriodicTable& ptable = PeriodicTable::Instance();

		NewtonGirardFormulae newton(_compo.getMaxNumVariants());

		std::map<std::string, EleSymPolyVec> esp_map;
		//clock_t t = clock();
		for(std::map<std::string, int>::const_iterator it = ele_count.begin();
			it != ele_count.end(); it++)
		{
			const Isotope& mono = ptable.getIsotopeByRelativeShift(it->first);

			PowerSumVec& ps_vec = this->calculateModifiedPhiValueVec(it->first);
			EleSymPolyVec esp_vec;
			newton.updateParameters(ps_vec, esp_vec);

			esp_map.insert(std::make_pair(it->first, esp_vec));
		}
		//t = clock()-t;
		//std::cout << "Modified Phi Vec: " << (double)t/CLOCKS_PER_SEC << "s\n";

		for(size_t i = 0; i <= _var_num; i++)
		{
			std::map<std::string, EleSymPolyVec>::iterator it = esp_map.begin();
			int sign = (i%2 == 0 ? 1 : -1);
			double center_mass = 0.0;
			for(; it != esp_map.end(); it++)
			{
				const Isotope& mono = ptable.getIsotopeByRelativeShift(it->first);

				center_mass += ele_count[it->first] * sign * it->second.at(i) * monopeak.intensity * mono.mass;
			}
			// m(j) = sum(m(jk) * p(jk))/sum(p(jk))
			mass_vec.push_back(center_mass/prob_vec.at(i));
		}

		return mass_vec;
	}

	void IsotopicDistribution::setOrder(int order)
	{
		// Controlling the number of variants, and prevent unnecessary calculation.
		size_t max_num = _compo.getMaxNumVariants();
		if(order == 0) {
			// Default value calculated by formula 2.
			int heu = std::max((int)ceil(abs(2 * (_compo.getMass() - _compo.getAverageMass()))), 50);
			_var_num = (size_t)heu > max_num ? max_num : (size_t)heu;
		} else if(order > 0)
			_var_num = (size_t)order > max_num ? max_num : (size_t)order;
		else
			throw std::runtime_error("Error: invalid order number.");

		_var_num--;

		// Adding elements from the composition into _iso_const.
		updateIsotopicConstants();
	}

	AggregatedIsotopicVariants IsotopicDistribution::getAggregatedIsotopicVariants(int charge /* = 0 */)
	{
		//clock_t t = clock(); clock_t last_t = t;
		EleSymPolyVec prob_vec = this->calculateProbabilityVec();

		//t = clock()-last_t; last_t = clock();
		//std::cout << "Prob Vec: " << (double)t/CLOCKS_PER_SEC << "s\n";

		std::vector<double> center_mass_vec = this->calculateCenterMassVec(prob_vec);
		//t = clock()-last_t; last_t = clock();
		//std::cout << "Mass Vec: " << (double)t/CLOCKS_PER_SEC << "s\n";

		AggregatedIsotopicVariants peakset;

		avg_mass = 0.0;
		double sum(0.0);

		for(size_t i=0; i<= _var_num; i++)
		{
			double adjusted_mz = (charge == 0 ? center_mass_vec.at(i): calculateMZ(center_mass_vec.at(i), charge));

			PeakPtr peak = boost::make_shared<Peak>(adjusted_mz, prob_vec.at(i));
			peakset.addPeak(peak);

			// Code for calculating avg_mass
			avg_mass += adjusted_mz * prob_vec.at(i);
			sum += prob_vec.at(i);
		}

		avg_mass /= sum;

		return peakset;

	}

	void IsotopicDistribution::updateIsotopicConstants()
	{
		const std::map<std::string, int> ele_count = _compo.get();

		std::map<std::string, int>::const_iterator it = ele_count.begin();

		// Build the constants for all elements.
		for(; it != ele_count.end(); it++)
			_iso_const.addElement(it->first);

		_iso_const.updateOrder(_var_num);
	}

	void IsotopicDistribution::updatePhiValueVec()
	{
		phi_value_vec.clear();
		phi_value_vec.push_back(0);
		for(size_t i=1; i<=_var_num; i++)
		{
			phi_value_vec.push_back(this->calculatePhiValue(i));
		}
	}

	double calculateMass( double mz, int charge)
	{
		// Be careful of the ion mode.
		param::Param& param = param::Param::Instance();
		double electron_mass = param.getParameter<double>("electron_mass").first;

		return abs(charge) * mz - charge * (Composition("H").getMass() -electron_mass);
	}

	double calculateMZ(double mass, int charge, int pre_charge /* = 0 */)
	{
		param::Param& param = param::Param::Instance();
		double electron_mass = param.getParameter<double>("electron_mass").first;

		int coef_h = (pre_charge == 0 ? charge : pre_charge);

		// TBD: this should be controlled by parameter.
		// int coef_e = (pre_charge == 0 ? 1 : 0);
		return (mass + coef_h * (Composition("H").getMass() - electron_mass))/abs(charge);
	}
}


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

/*
 Copyright (C) 2010 Klaus Spanderen

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

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

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file extendedornsteinuhlenbeckprocess.hpp
    \brief extended Ornstein-Uhlenbeck process
*/

#ifndef quantlib_extended_ornstein_uhlenbeck_process_hpp
#define quantlib_extended_ornstein_uhlenbeck_process_hpp

#include <ql/stochasticprocess.hpp>

#include <boost/function.hpp>

namespace QuantLib {

    class OrnsteinUhlenbeckProcess;

    //! Extended Ornstein-Uhlenbeck process class
    /*! This class describes the Ornstein-Uhlenbeck process governed by
        \f[
            dx = a (b(t) - x_t) dt + \sigma dW_t.
        \f]

        \ingroup processes
    */
    class ExtendedOrnsteinUhlenbeckProcess : public StochasticProcess1D {
      public:
        enum Discretization { MidPoint, Trapezodial, GaussLobatto };

        ExtendedOrnsteinUhlenbeckProcess(
                                Real speed, Volatility sigma, Real x0,
                                const boost::function<Real (Real)>& b,
                                Discretization discretization = MidPoint,
                                Real intEps = 1e-4);

        //! \name StochasticProcess interface
        //@{
        Real x0() const;
        Real speed() const;
        Real volatility() const;
        Real drift(Time t, Real x) const;
        Real diffusion(Time t, Real x) const;
        Real expectation(Time t0, Real x0, Time dt) const;
        Real stdDeviation(Time t0, Real x0, Time dt) const;
        Real variance(Time t0, Real x0, Time dt) const;
        //@}
      private:
        const Real speed_;
        const Volatility vol_;
        const boost::function<Real (Real)> b_;
        const Real intEps_;
        const boost::shared_ptr<OrnsteinUhlenbeckProcess> ouProcess_;
        const Discretization discretization_;
    };
}

#endif


#pragma once

#include <Eigen/Dense>
#include <Eigen/Core>
#include <autodiff/forward.hpp>
#include <autodiff/forward/eigen.hpp>
#include <cmath>

namespace ast
{
typedef autodiff::dual Real;
typedef autodiff::VectorXdual Vec;

typedef double RealV;
typedef Eigen::MatrixXd Mat;
typedef Eigen::VectorXd VecV;

constexpr RealV epsilon = 0.001;

using namespace Eigen;
using namespace autodiff;

//struct QBody : public Vec3
//{
//  QBody() : Vec3()
//  {}

//  QBody(Real th, Real x, Real y) : Vec3(th, x, y)
//  {}

//  template<typename OtherDerived>
//  QBody(const Eigen::MatrixBase<OtherDerived>& other) : Vec3(other)
//  { }

//  template<typename OtherDerived>
//  QBody& operator=(const Eigen::MatrixBase<OtherDerived>& other)
//  {
//     this->Vec3::operator=(other);
//     return *this;
//  }

//  Real& th() { return this->data()[0]; }

//  const Real& th() const { return this->data()[0]; }

//  Real& theta() { return this->data()[0]; }

//  const Real& theta() const { return this->data()[0]; }

//  Real& x() { return this->data()[1]; }

//  const Real& x() const { return this->data()[1]; }

//  Real& y() { return this->data()[2]; }

//  const Real& y() const { return this->data()[2]; }
//};

inline Real sat(Real x, Real lim)
{
  if(x > lim)
    return lim;
  else if(x < -lim)
    return - lim;
  else
    return x;
}

inline Real clipToRange(Real x, Real rMin, Real rMax)
{
  if(x < rMin)
    return  rMin;
  else if (x > rMax)
    return rMax;
  else return x;
}

inline Real sq(Real x)
{
  return x*x;
}

inline Real cube(Real x)
{
  return x*x*x;
}

inline int sign(Real val)
{
  return (0 < val) - (val < 0);
}

template <typename T>
inline Real distance(const T& a , const T& b)
{
  return (b-a).norm();
}

inline RealV continuousAngle(RealV angle, RealV lastAngle)
{
    auto dAngle = fmod(angle, 2*M_PI) - fmod(lastAngle, 2*M_PI);

    if (dAngle > M_PI)
        return lastAngle + dAngle - 2.0 * M_PI;
    else if (dAngle < -M_PI)
        return lastAngle + dAngle + 2.0 * M_PI;
    else
        return lastAngle + dAngle;
}

inline Real discontinuousAngle(Real angle)
{
    return atan2(sin(angle), cos(angle));
}

}