#ifndef KIDONO_FEATURE_EXTRACTOR_HPP
#define KIDONO_FEATURE_EXTRACTOR_HPP

#include <vector>
#include <Eigen/Dense>
#include <Eigen/StdVector>
#include <boost/range/algorithm.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/centroid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/impl/plane_clipper3D.hpp>

namespace hdl_people_detection {

/**
 * @brief A class to extract Kidono's features
 * @see Kiyosumi Kidono et al., "Pedestrian Recognition Using High-definition LIDAR"
 * @see http://www.aisl.cs.tut.ac.jp/~jun/pdffiles/kidono-iv2011.pdf
 */
class KidonoFeatureExtractor {
private:
  double distance_scale;
public:
  /**
   * @brief constructor
   * @param distance_scale
   */
  KidonoFeatureExtractor(double distance_scale = 1.0)
    : distance_scale(distance_scale)
  {}

  /**
   * @brief extract features
   * @param cloud  src cloud
   * @return extracted feature vector
   */
  std::vector<float> extract(pcl::PointCloud<pcl::PointXYZI>::ConstPtr cloud) const {
    std::vector<float> feature;		feature.reserve(256);
    feature.push_back(cloud->points.size());
    feature.push_back(minimumDistance(cloud) * distance_scale);

    Eigen::Vector3f mean;
    Eigen::Matrix3f covariance;

    boost::copy(covariance3d(cloud, mean, covariance), std::back_inserter(feature));

    auto centered_cloud = centeredCloud(cloud, mean);

    boost::copy(inertiaMoment3d(cloud), std::back_inserter(feature));

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver(covariance);

    Eigen::Vector3f e1 = solver.eigenvectors().col(2);
    Eigen::Vector3f e2 = solver.eigenvectors().col(1);
    Eigen::Vector3f e3 = solver.eigenvectors().col(0);

    if (e1.z() < 0.0f) {
      e1 = -e1;
    }

    boost::copy(covarianceIn3zones(centered_cloud, e1, e2, e3), std::back_inserter(feature));
    boost::copy(histogram2d(centered_cloud, e1, e2, 14, 7), std::back_inserter(feature));
    boost::copy(histogram2d(centered_cloud, e1, e3, 9, 5), std::back_inserter(feature));
    boost::copy(sliceFeature(centered_cloud, e1, e2, e3, 10), std::back_inserter(feature));
    boost::copy(intensityDistribution(centered_cloud, 25), std::back_inserter(feature));

    return feature;
  }

private:
  template<typename T>
  T square(T v) const { return v * v; }

  pcl::PointCloud<pcl::PointXYZI>::Ptr centeredCloud(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr& cloud, const Eigen::Vector3f& mean) const {
    pcl::PointCloud<pcl::PointXYZI>::Ptr centered_cloud(new pcl::PointCloud<pcl::PointXYZI>());
    centered_cloud->resize(cloud->size());
    for (int i = 0; i < cloud->size(); i++) {
      centered_cloud->at(i).getVector3fMap() = cloud->at(i).getVector3fMap() - mean;
      centered_cloud->at(i).intensity = cloud->at(i).intensity;
    }
    centered_cloud->width = centered_cloud->size();
    centered_cloud->height = 1;
    centered_cloud->is_dense = false;

    return centered_cloud;
  }

  float minimumDistance(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr& cloud) const {
    float dist = cloud->front().getVector3fMap().squaredNorm();
    for (int i = 1; i < cloud->size(); i++) {
      dist = std::min(dist, cloud->at(i).getVector3fMap().squaredNorm());
    }
    return sqrtf(dist);
  }

  std::vector<float> covariance3d(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr& cloud, Eigen::Vector3f& mean, Eigen::Matrix3f& covariance) const {
    Eigen::Vector4f centroid;
    pcl::compute3DCentroid(*cloud, centroid);
    pcl::computeCovarianceMatrix(*cloud, centroid, covariance);

    mean = centroid.topLeftCorner(3, 1);

    std::vector<float> feature(6);
    feature[0] = covariance(0, 0);		feature[1] = covariance(0, 1);		feature[2] = covariance(0, 2);
    feature[3] = covariance(1, 1);		feature[4] = covariance(1, 2);
    feature[5] = covariance(2, 2);
    return feature;
  }

  std::vector<float> inertiaMoment3d(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr& centered_cloud) const {
    std::vector<float> feature(6, 0.0f);

    for (int i = 0; i < centered_cloud->size(); i++) {
      const auto& pt = centered_cloud->at(i);
      feature[0] += square(pt.y) + square(pt.z);
      feature[1] += -pt.x * pt.y;
      feature[2] += -pt.x * pt.z;
      feature[3] += square(pt.x) + square(pt.z);
      feature[4] += -pt.y * pt.z;
      feature[5] += square(pt.x) + square(pt.y);
    }

    return feature;
  }

  std::vector<float> covariance2d(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr& cloud, const Eigen::Vector3f& e1, const Eigen::Vector3f& e2) const {
    if (cloud->empty()){
      return std::vector<float>(3, 0.0f);
    }

    Eigen::MatrixXf ptdata(cloud->size(), 2);
    for (int i = 0; i < cloud->size(); i++) {
      ptdata(i, 0) = cloud->at(i).getVector3fMap().dot(e1);
      ptdata(i, 1) = cloud->at(i).getVector3fMap().dot(e2);
    }

    Eigen::Vector2f mean = ptdata.colwise().mean().transpose();
    Eigen::MatrixXf centered = ptdata.rowwise() - mean.transpose();
    Eigen::Matrix2f covariance = (centered.transpose() * centered) / cloud->size();

    std::vector<float> feature(3);
    feature[0] = covariance(0, 0);	feature[1] = covariance(0, 1);
    feature[2] = covariance(1, 1);

    return feature;
  }

  std::vector<float> covarianceIn3zones(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr& centered_cloud, const Eigen::Vector3f& e1, const Eigen::Vector3f& e2, const Eigen::Vector3f& e3) const {
    pcl::PointCloud<pcl::PointXYZI>::Ptr upper_cloud(new pcl::PointCloud<pcl::PointXYZI>());
    pcl::PointCloud<pcl::PointXYZI>::Ptr lower_cloud(new pcl::PointCloud<pcl::PointXYZI>());
    pcl::PointCloud<pcl::PointXYZI>::Ptr right_cloud(new pcl::PointCloud<pcl::PointXYZI>());
    pcl::PointCloud<pcl::PointXYZI>::Ptr left_cloud(new pcl::PointCloud<pcl::PointXYZI>());

    pcl::PointIndices::Ptr indices(new pcl::PointIndices());

    pcl::PlaneClipper3D<pcl::PointXYZI> clipper(Eigen::Vector4f(e1.x(), e1.y(), e1.z(), 0.0f));
    clipper.clipPointCloud3D(*centered_cloud, indices->indices);

    pcl::ExtractIndices<pcl::PointXYZI> extract;
    extract.setInputCloud(centered_cloud);
    extract.setIndices(indices);
    extract.filter(*upper_cloud);

    extract.setNegative(true);
    extract.filter(*lower_cloud);

    indices->indices.clear();
    clipper.setPlaneParameters(Eigen::Vector4f(e2.x(), e2.y(), e2.z(), 0.0f));
    clipper.clipPointCloud3D(*lower_cloud, indices->indices);

    extract.setInputCloud(lower_cloud);
    extract.setIndices(indices);
    extract.setNegative(false);
    extract.filter(*left_cloud);

    extract.setNegative(true);
    extract.filter(*right_cloud);

    std::vector<float> feature;		feature.reserve(9);
    boost::copy(covariance2d(upper_cloud, e1, e2), std::back_inserter(feature));
    boost::copy(covariance2d(left_cloud, e1, e2), std::back_inserter(feature));
    boost::copy(covariance2d(right_cloud, e1, e2), std::back_inserter(feature));

    return feature;
  }

  std::vector<float> histogram2d(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr& cloud, const Eigen::Vector3f& axis1, const Eigen::Vector3f& axis2, int bin1, int bin2) const {
    std::vector<Eigen::Vector2f> pts2d(cloud->size());
    for (int i = 0; i < cloud->size(); i++) {
      pts2d[i][0] = cloud->at(i).getVector3fMap().dot(axis1);
      pts2d[i][1] = cloud->at(i).getVector3fMap().dot(axis2);
    }

    Eigen::Array2f min_pt = pts2d.front();
    Eigen::Array2f max_pt = pts2d.front();
    for (int i = 1; i < pts2d.size(); i++) {
      min_pt = pts2d[i].array().min(min_pt);
      max_pt = pts2d[i].array().max(max_pt);
    }

    Eigen::Array2f size = max_pt - min_pt;
    Eigen::Array2f inv_size = Eigen::Array2f(bin1 - 0.1f, bin2 - 0.1f) / size;

    float weight = 1.0f / pts2d.size();
    std::vector<float> hist(bin1 * bin2, 0.0f);
    for (int i = 0; i < pts2d.size(); i++) {
      Eigen::Array2i index = ((pts2d[i].array() - min_pt) * inv_size).cast<int>();
      hist[index[0] + index[1] * bin1] += weight;
    }

    return hist;
  }

  std::vector<float> sliceFeature(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr& centered_cloud, const Eigen::Vector3f& e1, const Eigen::Vector3f& e2, const Eigen::Vector3f& e3, int slice_n = 10) const {
    Eigen::Matrix3f cvt2eigenspace;
    cvt2eigenspace << e1.transpose(), e2.transpose(), e3.transpose();

    float e1_min = 9999.0f;
    float e1_max = -9999.0f;

    std::vector<Eigen::Vector3f> aligned_cloud(centered_cloud->size());
    for (int i = 0; i < centered_cloud->size(); i++) {
      aligned_cloud[i] = cvt2eigenspace * centered_cloud->at(i).getVector3fMap();
      e1_min = std::min(e1_min, aligned_cloud[i][0]);
      e1_max = std::max(e1_max, aligned_cloud[i][0]);
    }

    float height = e1_max - e1_min;
    float scale = (slice_n - 0.1f) / height;

    std::vector<Eigen::Array2f> min_pts(slice_n);
    std::vector<Eigen::Array2f> max_pts(slice_n);
    for (int i = 0; i < slice_n; i++) {
      min_pts[i] = Eigen::Array2f::Ones() * 9999.0f;
      max_pts[i] = Eigen::Array2f::Ones() * -9999.0f;
    }

    for (int i = 0; i < aligned_cloud.size(); i++) {
      int n = static_cast<int>((aligned_cloud[i][0] - e1_min) * scale);
      min_pts[n] = min_pts[n].min(aligned_cloud[i].bottomLeftCorner(2, 1).array());
      max_pts[n] = max_pts[n].max(aligned_cloud[i].bottomLeftCorner(2, 1).array());
    }

    std::vector<float> feature;
    feature.reserve(slice_n * 2);

    for (int i = 0; i < slice_n; i++){
      Eigen::Array2f w = max_pts[i] - min_pts[i];
      if (w[0] > 0.0f && w[1] > 0.0f) {
        feature.push_back(w[0]);
        feature.push_back(w[1]);
      }
      else {
        feature.push_back(0.0f);
        feature.push_back(0.0f);
      }
    }

    return feature;
  }

  std::vector<float> intensityDistribution(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr& centered_cloud, int hist_n = 25) const {
    Eigen::ArrayXf intensity(centered_cloud->size());
    for (int i = 0; i < centered_cloud->size(); i++) {
      intensity[i] = centered_cloud->at(i).intensity;
    }

    float weight = 1.0f / intensity.size();
    std::vector<float> feature(hist_n + 2, 0.0f);

    float scale = (hist_n - 0.1f) / 255.0f;
    for (int i = 0; i < intensity.size(); i++) {
      int n = static_cast<int>(intensity[i] * scale);
      feature[n] += weight;
    }

    float mean = intensity.mean();
    float stddev = sqrt((intensity - mean).square().mean());

    feature[hist_n] = mean;
    feature[hist_n + 1] = stddev;

    return feature;
  }
};

}

#endif


//
//  Copyright (c) 2007
//  Tsai, Dung-Bang
//  National Taiwan University, Department of Physics
//
//  E-Mail : dbtsai (at) gmail.com
//  Begine : 2007/11/20
//  Last modify : 2007/11/22
//  Version : v0.1
//
//  EXPGM_PAD computes the matrix exponential exp(H) for general matrixs,
//  including complex and real matrixs using the irreducible (p,p) degree
//  rational Pade approximation to the exponential
//  exp(z) = r(z)=(+/-)( I+2*(Q(z)/P(z))).
//
//  Usage :
//
//    U = expm_pad(H)
//    U = expm_pad(H, p)
//
//    where p is internally set to 6 (recommended and gererally satisfactory).
//
//  See also MATLAB supplied functions, EXPM and EXPM1.
//
//  Reference :
//  EXPOKIT, Software Package for Computing Matrix Exponentials.
//  ACM - Transactions On Mathematical Software, 24(1):130-156, 1998
//
//  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.
//

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_EXPM_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_EXPM_HPP

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <complex>
#include <stdexcept>

namespace boost { namespace numeric { namespace ublasx {

template<typename MATRIX>
MATRIX expm_pad(const MATRIX &H, const int p = 6)
{
	typedef typename MATRIX::value_type value_type;
        typedef typename MATRIX::size_type size_type;
	typedef double real_value_type;	// Correct me. Need to modify.
	assert(H.size1() == H.size2());
	const size_type n = H.size1();
	const identity_matrix<value_type> I(n);
	matrix<value_type> U(n,n),H2(n,n),P(n,n),Q(n,n);
	real_value_type norm = 0.0;

// Calcuate Pade coefficients  (1-based instead of 0-based as in the c vector)
	vector<real_value_type> c(p+2);
	c(1)=1;
	for(size_type i = 1; i <= p; ++i)
		c(i+1) = c(i) * ((p + 1.0 - i)/(i * (2.0 * p + 1 - i)));
// Calcuate the infinty norm of H, which is defined as the largest row sum of a matrix
	for(size_type i=0; i<n; ++i)
	{
		real_value_type temp = 0.0;
		for(size_type j=0;j<n;j++)
			temp += std::abs<real_value_type>(H(i,j)); // Correct me, if H is complex, can I use that abs?
		norm = std::max<real_value_type>(norm, temp);
	}
	if (norm == 0.0)
	{
		throw ::std::runtime_error("[expm_pad] Error: null input.");
	}
// Scaling, seek s such that || H*2^(-s) || < 1/2, and set scale = 2^(-s)
 	int s = 0;
	real_value_type scale = 1.0;
	if(norm > 0.5)
	{
		s = std::max<int>(0, static_cast<int>((log(norm) / log(2.0) + 2.0)));
		scale /= static_cast<real_value_type>(std::pow(2.0, s));
		U.assign(scale * H); // Here U is used as temp value due to that H is const
	}
// Horner evaluation of the irreducible fraction, see the following ref above.
// Initialise P (numerator) and Q (denominator)
	H2.assign( prod(U, U) );
	Q.assign( c(p+1)*I );
	P.assign( c(p)*I );
	size_type odd = 1;
	for( size_type k = p - 1; k > 0; --k)
	{
		if( odd == 1)
		{
			Q = ( prod(Q, H2) + c(k) * I );
		}
		else
		{
			P = ( prod(P, H2) + c(k) * I );
		}
		odd = 1 - odd;
	}
	if( odd == 1)
	{
		Q = ( prod(Q, U) );
		Q -= P ;
		//U.assign( -(I + 2*(Q\P)));
	}
	else
	{
		P = (prod(P, U));
		Q -= P;
		//U.assign( I + 2*(Q\P));
	}
// In origine expokit package, they use lapack ZGESV to obtain inverse matrix,
// and in that ZGESV routine, it uses LU decomposition for obtaing inverse matrix.
// Since in ublas, there is no matrix inversion template, I simply use the build-in
// LU decompostion package in ublas, and back substitute by myself.
//
//////////////// Implement Matrix Inversion ///////////////////////
	permutation_matrix<size_type> pm(n);
	int res = lu_factorize(Q, pm);
	if( res != 0)
	{
		throw ::std::runtime_error("[expm_pad] Error: matrix inversion in template expm_pad.");
	}
	H2 = I;  // H2 is not needed anymore, so it is temporary used as identity matrix for substituting.
	lu_substitute(Q, pm, H2);
	if( odd == 1)
		U.assign( -(I + 2.0 * prod(H2, P)));
 	else
		U.assign( I + 2.0 * prod(H2, P));
// Squaring
	for(size_t i = 0; i < s; ++i)
	{
		U = (prod(U,U));
	}
	return U;
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_EXPM_HPP


#ifndef ALM_HERMITE_QUATERNION_CURVE_H
#define ALM_HERMITE_QUATERNION_CURVE_H

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

class HermiteQuaternionCurve{
public:
	HermiteQuaternionCurve(const Eigen::Quaterniond & quat_start, const Eigen::Vector3d & angular_velocity_start,
						   const Eigen::Quaterniond & quat_end, const Eigen::Vector3d & angular_velocity_end);
	~HermiteQuaternionCurve();

	// All values are expressed in "world frame"
	void evaluate(const double & s_in, Eigen::Quaterniond & quat_out);
	void getAngularVelocity(const double & s_in, Eigen::Vector3d & ang_vel_out);
	void getAngularAcceleration(const double & s_in, Eigen::Vector3d & ang_acc_out);

private:
	Eigen::Quaterniond qa; // Starting quaternion
	Eigen::Vector3d omega_a; // Starting Angular Velocity
	Eigen::Quaterniond qb; // Ending quaternion
	Eigen::Vector3d omega_b; // Ending Angular velocity

	Eigen::AngleAxisd omega_a_aa; // axis angle representation of omega_a
	Eigen::AngleAxisd omega_b_aa; // axis angle representation of omega_b

	void initialize_data_structures();

	void computeBasis(const double & s_in); // computes the basis functions
	void computeOmegas();

	Eigen::Quaterniond q0; // quat0
	Eigen::Quaterniond q1; // quat1
	Eigen::Quaterniond q2; // quat1
	Eigen::Quaterniond q3; // quat1

	double b1; // basis 1
	double b2; // basis 2
	double b3; // basis 3

	double bdot1; // 1st derivative of basis 1
	double bdot2; // 1st derivative of basis 2
	double bdot3; // 1st derivative of basis 3

	double bddot1; // 2nd derivative of basis 1
	double bddot2; // 2nd derivative of basis 2
	double bddot3; // 2nd derivative of basis 3

	Eigen::Vector3d omega_1;
	Eigen::Vector3d omega_2;
	Eigen::Vector3d omega_3;

	Eigen::AngleAxisd omega_1aa;
	Eigen::AngleAxisd omega_2aa;
	Eigen::AngleAxisd omega_3aa;

	// Allocate memory for quaternion operations
	Eigen::Quaterniond qtmp1;
	Eigen::Quaterniond qtmp2;
	Eigen::Quaterniond qtmp3;

	// progression variable
	double s_;
	// by default clamps within 0 and 1.
	double clamp(const double & s_in, double lo = 0.0, double hi = 1.0);

	void printQuat(const Eigen::Quaterniond & quat);

};

#endif


/*
 *
 * Copyright (c) Toon Knapen, Karl Meerbergen & Kresimir Fresl 2003
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_SYEV_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_SYEV_HPP

#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/traits/detail/array.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits.hpp>
#endif

#include <cassert>

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    ///////////////////////////////////////////////////////////////////
    //
    // Eigendecomposition of a real symmetric matrix A = Q * D * Q'
    //
    ///////////////////////////////////////////////////////////////////

    /*
     * syev() computes the eigendecomposition of a N x N matrix
     * A = Q * D * Q',  where Q is a N x N orthogonal matrix and
     * D is a diagonal matrix. The diagonal elements D(i,i) is an
     * eigenvalue of A and Q(:,i) is a corresponding eigenvector.
     *
     * On return of syev, A is overwritten by Q and w contains the main
     * diagonal of D.
     *
     * int syev (char jobz, char uplo, A& a, W& w, minimal_workspace ) ;
     *    jobz : 'V' : compute eigenvectors
     *           'N' : do not compute eigenvectors
     *    uplo : 'U' : only the upper triangular part of A is used on input.
     *           'L' : only the lower triangular part of A is used on input.
     */

    namespace detail {

      inline
      void syev (char const jobz, char const uplo, integer_t const n,
                 float* a, integer_t const lda,
                 float* w, float* work, integer_t const lwork, integer_t& info)
      {
        LAPACK_SSYEV (&jobz, &uplo, &n, a, &lda, w, work, &lwork, &info);
      }

      inline
      void syev (char const jobz, char const uplo, integer_t const n,
                 double* a, integer_t const lda,
                 double* w, double* work, integer_t const lwork, integer_t& info)
      {
        LAPACK_DSYEV (&jobz, &uplo, &n, a, &lda, w, work, &lwork, &info);
      }

      template <typename A, typename W, typename Work>
      int syev (char jobz, char uplo, A& a, W& w, Work& work) {

/*#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<A>::matrix_structure,
          traits::general_t
        >::value));
#endif*/

        integer_t const n = traits::matrix_size1 (a);
        assert ( n>0 );
        assert (traits::matrix_size2 (a)==n);
        assert (traits::leading_dimension (a)>=n);
        assert (traits::vector_size (w)==n);
        assert (3*n-1 <= traits::vector_size (work));
        assert ( uplo=='U' || uplo=='L' );
        assert ( jobz=='N' || jobz=='V' );

        integer_t info;
        detail::syev (jobz, uplo, n,
                     traits::matrix_storage (a),
                     traits::leading_dimension (a),
                     traits::vector_storage (w),
                     traits::vector_storage (work),
                     traits::vector_size (work),
                     info);
        return info;
      }
    }  // namespace detail

    // Function that allocates work arrays
    template <typename A, typename W>
    int syev (char jobz, char uplo, A& a, W& w, optimal_workspace ) {
       typedef typename A::value_type value_type ;

       std::ptrdiff_t const n = traits::matrix_size1 (a);

       traits::detail::array<value_type> work( std::max<std::ptrdiff_t>(1,34*n) );
       return detail::syev(jobz, uplo, a, w, work);
    } // syev()

    // Function that allocates work arrays
    template <typename A, typename W>
    int syev (char jobz, char uplo, A& a, W& w, minimal_workspace ) {
       typedef typename A::value_type value_type ;

       std::ptrdiff_t const n = traits::matrix_size1 (a);

       traits::detail::array<value_type> work( std::max<std::ptrdiff_t>(1,3*n-1) );
       return detail::syev(jobz, uplo, a, w, work);
    } // syev()

    // Function that allocates work arrays
    template <typename A, typename W, typename Work>
    int syev (char jobz, char uplo, A& a, W& w, detail::workspace1<Work> workspace ) {
       typedef typename traits::matrix_traits<A>::value_type value_type ;

       return detail::syev(jobz, uplo, a, w, workspace.select(value_type()));
    } // syev()

    // Function without workarray as argument
    template <typename A, typename W>
    inline
    int syev (char jobz, char uplo, A& a, W& w) {
       return syev(jobz, uplo, a, w, optimal_workspace());
    } // syev()

    //
    // With UPLO integrated in matrix type (this is not possible
    // since a contains the eigenvectors afterwards and thus A cannot be symmetric)
    //
    template <typename A, typename W>
    int syev (char jobz, A& a, W& w, optimal_workspace ) {
       typedef typename A::value_type value_type ;

       std::ptrdiff_t const n = traits::matrix_size1 (a);
       char uplo = traits::matrix_uplo_tag( a ) ;
/*#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
       typedef typename traits::matrix_traits<A>::matrix_structure matrix_structure ;
       BOOST_STATIC_ASSERT( (boost::mpl::or_< boost::is_same< matrix_structure, traits::symmetric_t >
                                            , boost::is_same< matrix_structure, traits::hermitian_t >
                                            >::value)
                          ) ;
#endif*/

       traits::detail::array<value_type> work( std::max<std::ptrdiff_t>(1,34*n) );
       return detail::syev(jobz, uplo, a, w, work);
    } // syev()

    // Function that allocates work arrays
    template <typename A, typename W>
    int syev (char jobz, A& a, W& w, minimal_workspace ) {
       typedef typename A::value_type value_type ;

       std::ptrdiff_t const n = traits::matrix_size1 (a);
       char uplo = traits::matrix_uplo_tag( a ) ;
/*#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
       typedef typename traits::matrix_traits<A>::matrix_structure matrix_structure ;
       BOOST_STATIC_ASSERT( (boost::mpl::or_< boost::is_same< matrix_structure, traits::symmetric_t >
                                            , boost::is_same< matrix_structure, traits::hermitian_t >
                                            >::value)
                          ) ;
#endif*/
       traits::detail::array<value_type> work( std::max<std::ptrdiff_t>(1,3*n-1) );
       return detail::syev(jobz, uplo, a, w, work);
    } // syev()

    // Function that allocates work arrays
    template <typename A, typename W, typename Work>
    int syev (char jobz, A& a, W& w, detail::workspace1<Work> workspace ) {
       typedef typename A::value_type value_type ;
       char uplo = traits::matrix_uplo_tag( a ) ;
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
       typedef typename traits::matrix_traits<A>::matrix_structure matrix_structure ;
       BOOST_STATIC_ASSERT( (boost::mpl::or_< boost::is_same< matrix_structure, traits::symmetric_t >
                                            , boost::is_same< matrix_structure, traits::hermitian_t >
                                            >::value)
                          ) ;
#endif
       return detail::syev(jobz, uplo, a, w, workspace.select(value_type()));
    } // syev()

    // Function without workarray as argument
    template <typename A, typename W>
    inline
    int syev (char jobz, A& a, W& w) {
       char uplo = traits::matrix_uplo_tag( a ) ;
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
       typedef typename traits::matrix_traits<A>::matrix_structure matrix_structure ;
       BOOST_STATIC_ASSERT( (boost::mpl::or_< boost::is_same< matrix_structure, traits::symmetric_t >
                                            , boost::is_same< matrix_structure, traits::hermitian_t >
                                            >::value)
                          ) ;
#endif
       return syev(jobz, uplo, a, w, optimal_workspace());
    } // syev()

  }

}}}

#endif


/** @file layouts.cc
 * @author David F. Gleich
 * @date 2008-09-25
 * @copyright Stanford University, 2008
 * Graph layout wrappers
 */

/** History
 *  2008-09-25: Initial coding
 *  2008-09-27: Fixed progressive calls
 */

#include "include/matlab_bgl.h"

#include <yasmic/undir_simple_csr_matrix_as_graph.hpp>
#include <yasmic/simple_csr_matrix_as_graph.hpp>
#include <yasmic/iterator_utility.hpp>

#include <boost/graph/kamada_kawai_spring_layout.hpp>
#include <yasmic/boost_mod/fruchterman_reingold.hpp>
#include <boost/graph/gursoy_atun_layout.hpp>
#include <boost/graph/circle_layout.hpp>
#include <boost/graph/random_layout.hpp>
#include <boost/graph/simple_point.hpp>

// for constant_value_property_map
#include <yasmic/boost_mod/core_numbers.hpp>

#include <vector>
#include <iostream>
#include <algorithm>

#include <math.h>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

#include "libmbgl_util.hpp"

/** Monitor tolerance for Kamada-Kawai layout with a maximum iteration limit
 * This class fixes problems with the boost layout_tolerance code.
 */
template <typename T = double>
class layout_and_iteration_tolerance
{
public:
  layout_and_iteration_tolerance(const T& tolerance=0.001, int maxiter = 100)
  : maxiter(maxiter), iter(0), tolerance(tolerance),
    first_energy((std::numeric_limits<T>::max)()),
    last_energy((std::numeric_limits<T>::max)()),
    first_local_energy((std::numeric_limits<T>::max)()),
    last_local_energy((std::numeric_limits<T>::max)()) { }

  template<typename Graph>
  bool
  operator()(T delta_p,
              typename boost::graph_traits<Graph>::vertex_descriptor p,
              const Graph& g,
              bool global)
  {
    bool done = false;
    if (global) {
      if (first_energy == (std::numeric_limits<T>::max)()) {
        first_energy = delta_p;
        last_energy = delta_p;
        return delta_p < (std::numeric_limits<T>::epsilon)();
      }
      T diff = last_energy - delta_p;
      if (diff < T(0)) diff = -diff;
      done = (delta_p < (std::numeric_limits<T>::epsilon)()
              || diff/first_energy < tolerance);
      last_energy = delta_p;
    } else {
      if (first_local_energy == (std::numeric_limits<T>::max)()) {
        first_local_energy = delta_p;
        last_local_energy = delta_p;
        return delta_p < (std::numeric_limits<T>::epsilon)();
      }
      T diff = last_local_energy - delta_p;
      // uncommenting the following line causes the layout to cycle
      // if (diff < T(0)) diff = -diff;
      done = (delta_p < (std::numeric_limits<T>::epsilon)()
              || diff/first_local_energy < tolerance);
      last_local_energy = delta_p;
    }
    if (!done && global) {
      iter++;
      done = iter>maxiter;
    }
    return done;
  }

private:
  int maxiter, iter;
  T tolerance;
  T first_energy;
  T last_energy;
  T first_local_energy;
  T last_local_energy;
};

/** Compute a spring layout of a graph
 * @param nverts the number of vertices in the graph
 * @param ja the connectivity for each vertex
 * @param ia the row connectivity points into ja
 * @param weight the weight of each edge (can be NULL for unweighted graphs)
 * @param tol the stopping tolerance in terms of layout change
 * @param iterations the maximum number of global layout iterations
 * @param spring_constant the spring constant
 * @param progressive a binary value (0 or 1) if we should reuse the positions
 * @param positions an array of positions, length nverts*2
 * @param spring_strength a matrix of spring strengths between vertices,
 *           size nverts-by-nverts
 * @param distance a matrix of distances between vertices,
 *           size nverts-by-nverts
 */
int kamada_kawai_spring_layout(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, double *weight,
    double tol, int iterations, double spring_constant, int progressive,
    double edge_length,
    double *positions,
    double *spring_strength, double *distance)
{
  using namespace yasmic;
  using namespace boost;
  typedef undir_simple_csr_matrix<mbglIndex,double> crs_graph;
  crs_graph g(nverts, nverts, ia[nverts], ia, ja, weight);
  assert(nverts == 0 || positions);
  if (tol < 0 || spring_strength == NULL || distance == NULL) { return -1; }
  std::vector<simple_point<double> > position_vec(nverts);
  std::vector<std::pair<double,double> > partial_deriv_vec(nverts);
  if (!progressive) {
    // initial random layout
    circle_graph_layout(g,
        make_iterator_property_map(position_vec.begin(),get(vertex_index,g)),
        (double)nverts*edge_length/(2*M_PI));
  } else {
    // copy the layout from positions
    mbglIndex n = num_vertices(g);
    for (mbglIndex i = 0; i<n; i++) {
      position_vec[i].x = positions[i+0*n];
      position_vec[i].y = positions[i+1*n];
    }
  }
  bool rval = false;
  if (weight == NULL) {
    rval = kamada_kawai_spring_layout(g,
      make_iterator_property_map(position_vec.begin(),get(vertex_index,g)),
      boost::detail::constant_value_property_map<double>(1.0), // edge_weight
      boost::edge_length(edge_length), // edge_or_side_length
      layout_and_iteration_tolerance<double>(tol,iterations), // done
      spring_constant,
      get(vertex_index,g),
      row_matrix<double>(distance,nverts,nverts),
      row_matrix<double>(spring_strength,nverts,nverts),
      make_iterator_property_map(partial_deriv_vec.begin(),get(vertex_index,g)));
  } else {
    rval = kamada_kawai_spring_layout(g,
      make_iterator_property_map(position_vec.begin(),get(vertex_index,g)),
      get(edge_weight,g), // edge_weight
      boost::edge_length(edge_length), // edge_or_side_length
      layout_and_iteration_tolerance<double>(tol,iterations), // done
      spring_constant,
      get(vertex_index,g),
      row_matrix<double>(distance,nverts,nverts),
      row_matrix<double>(spring_strength,nverts,nverts),
      make_iterator_property_map(partial_deriv_vec.begin(),get(vertex_index,g)));
  }
  if (rval) {
    mbglIndex n = num_vertices(g);
    for (mbglIndex i = 0; i<n; i++) {
      positions[i+0*n] = position_vec[i].x;
      positions[i+1*n] = position_vec[i].y;
    }
    return 0;
  } else {
    return -2;
  }
}

/** Compute a force directed layout of a graph
 * @param nverts the number of vertices in the graph
 * @param ja the connectivity for each vertex
 * @param ia the row connectivity points into ja
 * @param iterations the number of iterations to run
 * @param initial_temp the initial temperature of the system
 * @param grid_force_pairs a binary value (0 or 1) if the alg should use
 *   a grid to compute the force between pairs
 * @param width the total width of the layout
 * @param height the total height of the layout
 * @param progressive a binary value (0 or 1) if we should start from
 *   the positions in the positions value
 * @param positions an array of positions, length nverts*2
 */
int fruchterman_reingold_force_directed_layout(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia,
    int iterations, double initial_temp, int grid_force_pairs,
    double width, double height, int progressive,
    double *positions)
{
  using namespace yasmic;
  using namespace boost;
  typedef simple_csr_matrix<mbglIndex,double> crs_graph;
  crs_graph g(nverts, nverts, ia[nverts], ia, ja, NULL);
  assert(nverts == 0 || positions);
  std::vector<simple_point<double> > position_vec(nverts);
  if (width <= 0 || height <= 0 || iterations <= 0) { return -1; }
  if (!progressive) {
    // initial random layout
    minstd_rand gen;
    random_graph_layout(g,
        make_iterator_property_map(position_vec.begin(),get(vertex_index,g)),
        -width/2.0, width/2.0, -height/2.0, height/2.0, gen);
  } else {
    // copy the layout from positions
    mbglIndex n = num_vertices(g);
    for (mbglIndex i = 0; i<n; i++) {
      position_vec[i].x = positions[i+0*n];
      position_vec[i].y = positions[i+1*n];
    }
  }
  if (grid_force_pairs) {
    fruchterman_reingold_force_directed_layout(g,
        make_iterator_property_map(position_vec.begin(),get(vertex_index,g)),
        width, height,
        cooling(linear_cooling<double>(iterations, initial_temp)));
  } else {
    fruchterman_reingold_force_directed_layout(g,
        make_iterator_property_map(position_vec.begin(),get(vertex_index,g)),
        width, height,
        cooling(linear_cooling<double>(iterations, initial_temp)).
          force_pairs(all_force_pairs()));
  }
  // copy the positions over
  mbglIndex n = num_vertices(g);
  for (mbglIndex i = 0; i<n; i++) {
    positions[i+0*n] = position_vec[i].x;
    positions[i+1*n] = position_vec[i].y;
  }
  return 0;
}

/** A helper function to allocate the points and convert back to positions
 */
template <typename Graph, typename Topology>
int gursoy_atun_layout_helper(const Graph& g, bool weighted,
    Topology space, int space_dim, int nsteps,
    double diameter_i, double diameter_f, double lc_i, double lc_f,
    double *positions)
{
  using namespace boost;
  std::vector<typename Topology::point_type> position_map(num_vertices(g));
  if (weighted) {
    gursoy_atun_layout(g, space,
      make_iterator_property_map(position_map.begin(),get(vertex_index,g)),
      nsteps, diameter_i, diameter_f, lc_i, lc_f, get(vertex_index,g),
      get(edge_weight,g));
  } else {
    gursoy_atun_layout(g, space,
          make_iterator_property_map(position_map.begin(),get(vertex_index,g)),
          nsteps, diameter_i, diameter_f, lc_i, lc_f, get(vertex_index,g));
  }
  // copy the positions over
  mbglIndex n = num_vertices(g);
  mbglIndex numdim = (mbglIndex)space_dim;
  for (mbglIndex i = 0; i<n; i++) {
    for (mbglIndex d = 0; d<numdim; d++) {
      positions[i+d*n] = position_map[i][d];
    }
  }
  return 0;
}

const int gursoy_atun_invalid_dim = -10;
const int gursoy_atun_dim_too_large = -11;
const int gursoy_atun_max_dim = 10;

/** Compute a topologically uniform layout
 * @param nverts the number of vertices in the graph
 * @param ja the connectivity for each vertex
 * @param ia the row connectivity points into ja
 * @param weight the weight of each edge (can be NULL for unweighted graphs)
 * @param topology the topology type
 * @param topology_dim the topology dimension
 * @param iterations the number of iterations
 * @param diameter_i the initial diameter for modifications
 * @param diameter_f the final diameter for modifications
 * @param learning_constant_i the initial learning constant
 * @param learning_constant_f the final learning constant
 * @param positions an array of positions, length nverts*topology_dim
 */
int gursoy_atun_layout(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, double *weight,
    gursoy_atun_layout_topology_t topology, int topology_dim,
    int iterations, double diameter_i, double diameter_f,
    double learning_constant_i, double learning_constant_f,
    double *positions)
{
  using namespace yasmic;
  using namespace boost;

  typedef simple_csr_matrix<mbglIndex,double> crs_graph;
  crs_graph g(nverts, nverts, ia[nverts], ia, ja, weight);
  assert(nverts == 0 || positions);

  switch (topology) {
  case BALL_LAYOUT_TOPOLOGY:
    if (topology_dim<2) { return gursoy_atun_invalid_dim; }
    if (topology_dim>gursoy_atun_max_dim) { return gursoy_atun_dim_too_large; }
    switch (topology_dim) {
    case 2:
      gursoy_atun_layout_helper(g, weight!=NULL,
          ball_topology<2>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 3:
      gursoy_atun_layout_helper(g, weight!=NULL,
          ball_topology<3>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 4:
      gursoy_atun_layout_helper(g, weight!=NULL,
          ball_topology<4>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 5:
      gursoy_atun_layout_helper(g, weight!=NULL,
          ball_topology<5>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 6:
      gursoy_atun_layout_helper(g, weight!=NULL,
          ball_topology<6>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 7:
      gursoy_atun_layout_helper(g, weight!=NULL,
          ball_topology<7>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 8:
      gursoy_atun_layout_helper(g, weight!=NULL,
          ball_topology<8>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 9:
      gursoy_atun_layout_helper(g, weight!=NULL,
          ball_topology<9>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 10:
      gursoy_atun_layout_helper(g, weight!=NULL,
          ball_topology<10>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    }
    break;
  case CUBE_LAYOUT_TOPOLOGY:
    if (topology_dim<2) { return gursoy_atun_invalid_dim; }
    if (topology_dim>gursoy_atun_max_dim) { return gursoy_atun_dim_too_large; }
    switch (topology_dim) {
    case 2:
      gursoy_atun_layout_helper(g, weight!=NULL,
          hypercube_topology<2>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 3:
      gursoy_atun_layout_helper(g, weight!=NULL,
          hypercube_topology<3>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 4:
      gursoy_atun_layout_helper(g, weight!=NULL,
          hypercube_topology<4>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 5:
      gursoy_atun_layout_helper(g, weight!=NULL,
          hypercube_topology<5>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 6:
      gursoy_atun_layout_helper(g, weight!=NULL,
          hypercube_topology<6>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 7:
      gursoy_atun_layout_helper(g, weight!=NULL,
          hypercube_topology<7>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 8:
      gursoy_atun_layout_helper(g, weight!=NULL,
          hypercube_topology<8>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 9:
      gursoy_atun_layout_helper(g, weight!=NULL,
          hypercube_topology<9>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    case 10:
      gursoy_atun_layout_helper(g, weight!=NULL,
          hypercube_topology<10>(),topology_dim,iterations,diameter_i,diameter_f,
          learning_constant_i, learning_constant_f, positions);
      break;
    }
    break;
  case HEART_LAYOUT_TOPOLOGY:
    if (topology_dim!=2) { return gursoy_atun_invalid_dim; }
    gursoy_atun_layout_helper(g, weight!=NULL,
      heart_topology<>(),topology_dim,iterations,diameter_i,diameter_f,
      learning_constant_i, learning_constant_f, positions);
    break;
  default:
    return -1;
  }
  return 0;
}


#ifndef STAN_MATH_PRIM_MAT_PROB_DIRICHLET_RNG_HPP
#define STAN_MATH_PRIM_MAT_PROB_DIRICHLET_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <stan/math/prim/mat/fun/log_sum_exp.hpp>
#include <cmath>

namespace stan {
namespace math {

/**
 * Return a draw from a Dirichlet distribution with specified
 * parameters and pseudo-random number generator.
 *
 * For prior counts greater than zero, the usual algorithm that
 * draws gamma variates and normalizes is used.
 *
 * For prior counts less than zero (i.e., parameters with value
 * less than one), a log-scale version of the following algorithm
 * is used to deal with underflow:
 *
 * <blockquote>
 * G. Marsaglia and W. Tsang. A simple method for generating gamma
 * variables. ACM Transactions on Mathematical Software.
 * 26(3):363--372, 2000.
 * </blockquote>
 *
 * @tparam RNG Type of pseudo-random number generator.
 * @param alpha Prior count (plus 1) parameter for Dirichlet.
 * @param rng Pseudo-random number generator.
 */
template <class RNG>
inline Eigen::VectorXd dirichlet_rng(
    const Eigen::Matrix<double, Eigen::Dynamic, 1>& alpha, RNG& rng) {
  using Eigen::VectorXd;
  using boost::gamma_distribution;
  using boost::random::uniform_real_distribution;
  using boost::variate_generator;
  using std::exp;
  using std::log;

  // separate algorithm if any parameter is less than 1
  if (alpha.minCoeff() < 1) {
    variate_generator<RNG&, uniform_real_distribution<> > uniform_rng(
        rng, uniform_real_distribution<>(0.0, 1.0));
    VectorXd log_y(alpha.size());
    for (int i = 0; i < alpha.size(); ++i) {
      variate_generator<RNG&, gamma_distribution<> > gamma_rng(
          rng, gamma_distribution<>(alpha(i) + 1, 1));
      double log_u = log(uniform_rng());
      log_y(i) = log(gamma_rng()) + log_u / alpha(i);
    }
    double log_sum_y = log_sum_exp(log_y);
    VectorXd theta(alpha.size());
    for (int i = 0; i < alpha.size(); ++i) {
      theta(i) = exp(log_y(i) - log_sum_y);
    }
    return theta;
  }

  // standard normalized gamma algorithm
  Eigen::VectorXd y(alpha.rows());
  for (int i = 0; i < alpha.rows(); i++) {
    variate_generator<RNG&, gamma_distribution<> > gamma_rng(
        rng, gamma_distribution<>(alpha(i, 0), 1e-7));
    y(i) = gamma_rng();
  }
  return y / y.sum();
}

}  // namespace math
}  // namespace stan
#endif


// Copyright (C) 2019 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

// This file was created by Steffen Urban (urbste@googlemail.com) October 2019

#include "theia/sfm/estimators/estimate_radial_dist_uncalibrated_absolute_pose.h"

#include <ceres/rotation.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <memory>
#include <vector>

#include "theia/sfm/camera/projection_matrix_utils.h"
#include "theia/sfm/create_and_initialize_ransac_variant.h"
#include "theia/sfm/estimators/feature_correspondence_2d_3d.h"
#include "theia/sfm/pose/four_point_focal_length_radial_distortion.h"
#include "theia/sfm/types.h"
#include "theia/solvers/estimator.h"
#include "theia/solvers/sample_consensus_estimator.h"
#include "theia/util/util.h"

namespace theia {
namespace {

void DistortPoint(const Eigen::Vector2d& point2d, const double& distortion,
                  Eigen::Vector2d* distorted_point) {
  const double r_u_sq = point2d[0] * point2d[0] + point2d[1] * point2d[1];

  const double denom = 2.0 * distortion * r_u_sq;
  const double inner_sqrt = 1.0 - 4.0 * distortion * r_u_sq;

  // If the denominator is nearly zero then we can evaluate the distorted
  // coordinates as k or r_u^2 goes to zero. Both evaluate to the identity.
  if (std::abs(denom) < 1e-15 || inner_sqrt < 0.0) {
    (*distorted_point)[0] = point2d[0];
    (*distorted_point)[1] = point2d[1];
  } else {
    const double scale = (1.0 - std::sqrt(inner_sqrt)) / denom;
    (*distorted_point)[0] = point2d[0] * scale;
    (*distorted_point)[1] = point2d[1] * scale;
  }
}

using Eigen::Matrix3d;
using Eigen::Vector2d;
using Eigen::Vector3d;
// An estimator for computing the uncalibrated absolute pose from 4 feature
// correspondences. The feature correspondences should be normalized such that
// the principal point is at (0, 0).
class RadialDistUncalibratedAbsolutePoseEstimator
    : public Estimator<FeatureCorrespondence2D3D,
                       RadialDistUncalibratedAbsolutePose> {
 public:
  RadialDistUncalibratedAbsolutePoseEstimator() {}

  // 4 correspondences are needed to determine the absolute pose.
  double SampleSize() const { return 4; }

  // Estimates candidate absolute poses from correspondences.
  bool EstimateModel(
      const std::vector<FeatureCorrespondence2D3D>& correspondences,
      std::vector<RadialDistUncalibratedAbsolutePose>* absolute_poses) const {
    const Vector2d features[4] = {
        correspondences[0].feature, correspondences[1].feature,
        correspondences[2].feature, correspondences[3].feature};
    const Vector3d world_points[4] = {
        correspondences[0].world_point, correspondences[1].world_point,
        correspondences[2].world_point, correspondences[3].world_point};

    std::vector<Matrix3d> rotations;
    std::vector<Vector3d> translations;
    std::vector<double> radial_distortions;
    std::vector<double> focal_lenghts;

    if (!FourPointsPoseFocalLengthRadialDistortion(
            features, world_points, meta_data_.max_focal_length,
            meta_data_.min_focal_length, meta_data_.max_radial_distortion,
            meta_data_.min_radial_distortion, &rotations, &translations,
            &radial_distortions, &focal_lenghts))
      return false;

    absolute_poses->resize(rotations.size());
    for (int i = 0; i < rotations.size(); ++i) {
      (*absolute_poses)[i].radial_distortion = radial_distortions[i];
      (*absolute_poses)[i].focal_length = focal_lenghts[i];
      (*absolute_poses)[i].rotation = rotations[i];
      (*absolute_poses)[i].translation = translations[i];
    }

    return rotations.size() > 0;
  }

  // The error for a correspondences given an absolute pose. This is the squared
  // reprojection error.
  double Error(const FeatureCorrespondence2D3D& correspondence,
               const RadialDistUncalibratedAbsolutePose& absolute_pose) const {
    // undistort the feature with the estimated radial distortion parameter
    // project der world point with the given focal length and
    // compare it to the undistorted image point
    Matrix3d K =
        Vector3d(absolute_pose.focal_length, absolute_pose.focal_length, 1.0)
            .asDiagonal();
    if (absolute_pose.translation[2] < 0.0) {
        return 1.0e10;
    }
    Vector3d reproj_pt = (absolute_pose.rotation * correspondence.world_point +
                          absolute_pose.translation);
    const Eigen::Vector2d reproj_pt_2d = (K * reproj_pt).hnormalized();
    Eigen::Vector2d distorted_point;
    DistortPoint(reproj_pt_2d, absolute_pose.radial_distortion,
                 &distorted_point);

    return (distorted_point - correspondence.feature).squaredNorm();
  }

  void SetMetadata(RadialDistUncalibratedAbsolutePoseMetaData meta_data) {
    meta_data_ = meta_data;
  }

 private:
  RadialDistUncalibratedAbsolutePoseMetaData meta_data_;

  DISALLOW_COPY_AND_ASSIGN(RadialDistUncalibratedAbsolutePoseEstimator);
};

}  // namespace

bool EstimateRadialDistUncalibratedAbsolutePose(
    const RansacParameters& ransac_params, const RansacType& ransac_type,
    const std::vector<FeatureCorrespondence2D3D>& normalized_correspondences,
    const RadialDistUncalibratedAbsolutePoseMetaData& meta_data,
    RadialDistUncalibratedAbsolutePose* absolute_pose,
    RansacSummary* ransac_summary) {
  RadialDistUncalibratedAbsolutePoseEstimator absolute_pose_estimator;
  absolute_pose_estimator.SetMetadata(meta_data);

  std::unique_ptr<
      SampleConsensusEstimator<RadialDistUncalibratedAbsolutePoseEstimator> >
      ransac = CreateAndInitializeRansacVariant(ransac_type, ransac_params,
                                                absolute_pose_estimator);
  // Estimate the absolute pose.
  const bool success = ransac->Estimate(normalized_correspondences,
                                        absolute_pose, ransac_summary);

  return success;
}

}  // namespace theia


/*
 * libcluster -- A collection of hierarchical Bayesian clustering algorithms.
 * Copyright (C) 2013 Daniel M. Steinberg (daniel.m.steinberg@gmail.com)
 *
 * This file is part of libcluster.
 *
 * libcluster is free software: you can redistribute it and/or modify it under
 * the terms of the GNU Lesser General Public License as published by the Free
 * Software Foundation, either version 3 of the License, or (at your option)
 * any later version.
 *
 * libcluster is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
 * for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with libcluster. If not, see <http://www.gnu.org/licenses/>.
 */

#include <boost/math/special_functions.hpp>
#include "distributions.h"
#include "probutils.h"

//
// Namespaces
//

using namespace std;
using namespace Eigen;
using namespace probutils;
using namespace boost::math;

//
//  File scope variables
//

// Define pi
const double pi = constants::pi<double>(); // Boost high precision pi

//
// Private Helper Functions
//

/* Compare an <int,double> double pair by the double member. Useful
 *  for sorting an array in descending order while retaining a notion of
 *  the original order of the array.
 *
 *  returns: true if i.second > j.second.
 */
bool inline obscomp (
        const std::pair<int,double>& i, // the first pair to compare.
        const std::pair<int,double>& j // the second pair to compare.
        )
{
        return i.second > j.second;
}

/* Enumerate the dimensions.
 *
 *  returns: 1:D or if D = 1, return 1.
 */
ArrayXd enumdims (const int D)
{
        ArrayXd l;

        if (D > 1)
                l = ArrayXd::LinSpaced(D, 1, D);
        else
                l.setOnes(1);

        return l;
}

//
// Stick-Breaking (Dirichlet Process) weight distribution.
//

distributions::StickBreak::StickBreak ()
        : WeightDist(),
        alpha1_p(distributions::ALPHA1PRIOR),
        alpha2_p(distributions::ALPHA2PRIOR),
        alpha1(ArrayXd::Constant(1, distributions::ALPHA1PRIOR)),
        alpha2(ArrayXd::Constant(1, distributions::ALPHA2PRIOR)),
        E_logv(ArrayXd::Zero(1)),
        E_lognv(ArrayXd::Zero(1)),
        E_logpi(ArrayXd::Zero(1)),
        ordvec(1, pair<int,double>(0,0))
{
        this->priorfcalc();
}

distributions::StickBreak::StickBreak (const double concentration)
        : WeightDist(),
        alpha2_p(distributions::ALPHA2PRIOR),
        alpha2(ArrayXd::Constant(1, distributions::ALPHA2PRIOR)),
        E_logv(ArrayXd::Zero(1)),
        E_lognv(ArrayXd::Zero(1)),
        E_logpi(ArrayXd::Zero(1)),
        ordvec(1, pair<int,double>(0,0))
{
        if (concentration <=0)
                throw invalid_argument("Concentration parameter has to be > 0!");

        this->alpha1_p = concentration;
        this->alpha1 = ArrayXd::Constant(1, concentration);
        this->priorfcalc();
}

void distributions::StickBreak::priorfcalc (void)
{
        // Prior free energy contribution
        this->F_p = lgamma(this->alpha1_p) + lgamma(this->alpha2_p)
                    - lgamma(this->alpha1_p + this->alpha2_p);
}

void distributions::StickBreak::update (const ArrayXd& Nk)
{
        const int K = Nk.size();

        // Destructively resize members to be the same size as Nk, no-op if same
        this->alpha1.resize(K);
        this->alpha2.resize(K);
        this->E_logv.resize(K);
        this->E_lognv.resize(K);
        this->E_logpi.resize(K);
        this->ordvec.resize(K, pair<int,double>(-1, -1));

        // Order independent update
        this->Nk     = Nk;
        this->alpha1 = this->alpha1_p + Nk;

        // Get at sort size order of clusters
        for (int k = 0; k < K; ++k)
        {
                this->ordvec[k].first  = k;
                this->ordvec[k].second = Nk(k);
        }
        sort(this->ordvec.begin(), this->ordvec.end(), obscomp);

        // Now do order dependent updates
        const double N = Nk.sum();
        double cumNk = 0, cumE_lognv = 0;
        for (int idx = 0, k; idx < K; ++idx)
        {
                k = this->ordvec[idx].first;

                // Alpha 2
                cumNk += Nk(k); // Accumulate cluster size sum
                this->alpha2(k) = this->alpha2_p + (N - cumNk);

                // Expected stick lengths
                double psisum    = digamma(this->alpha1(k) + this->alpha2(k));
                this->E_logv(k)  = digamma(this->alpha1(k)) - psisum;
                this->E_lognv(k) = digamma(this->alpha2(k)) - psisum;

                // Expected weights
                this->E_logpi(k) = this->E_logv(k) + cumE_lognv;
                cumE_lognv += E_lognv(k); // Accumulate log stick length left
        }
}

double distributions::StickBreak::fenergy () const
{
        const int K = this->alpha1.size();

        return K * this->F_p + (mxlgamma(this->alpha1 + this->alpha2).array()
                                - mxlgamma(this->alpha1).array() - mxlgamma(this->alpha2).array()
                                + (this->alpha1 - this->alpha1_p) * this->E_logv
                                + (this->alpha2 - this->alpha2_p) * this->E_lognv).sum();
}

//
// Generalised Dirichlet weight distribution.
//

void distributions::GDirichlet::update (const ArrayXd& Nk)
{
        // Call base class (stick breaking) update
        this->StickBreak::update(Nk);
        const int smallk = (this->ordvec.end() - 1)->first; // Get smallest cluster

        // Set last stick lengths to 1 ( log(0) = 1 ) and adjust log marginal
        this->E_logpi(smallk) = this->E_logpi(smallk) - this->E_logv(smallk);
        this->E_logv(smallk)  = 0; // exp(E[log v_K]) = 1
        this->E_lognv(smallk) = 0; // Undefined, but set to zero
}

double distributions::GDirichlet::fenergy () const
{
        const int K = this->ordvec.size();

        // GDir only has K-1 parameters, so we don't calculate the last F contrib.
        double Fpi = 0;
        for (int idx = 0, k = 0; idx < K-1; ++idx)
        {
                k = this->ordvec[idx].first;
                Fpi += lgamma(this->alpha1(k) + this->alpha2(k))
                       - lgamma(this->alpha1(k)) - lgamma(this->alpha2(k))
                       + (this->alpha1(k) - this->alpha1_p) * this->E_logv(k)
                       + (this->alpha2(k) - this->alpha2_p) * this->E_lognv(k);
        }

        return (K-1) * this->F_p + Fpi;
}

//
// Dirichlet weight distribution.
//

distributions::Dirichlet::Dirichlet ()
        : WeightDist(),
        alpha_p(distributions::ALPHA1PRIOR),
        alpha(ArrayXd::Constant(1, distributions::ALPHA1PRIOR)),
        E_logpi(ArrayXd::Zero(1))
{
}

distributions::Dirichlet::Dirichlet (const double alpha)
        : WeightDist(),
        E_logpi(ArrayXd::Zero(1))
{
        if (alpha <= 0)
                throw invalid_argument("Alpha prior must be > 0!");

        alpha_p = alpha;
        this->alpha = ArrayXd::Constant(1, alpha);
}

void distributions::Dirichlet::update (const ArrayXd& Nk)
{
        const int K = Nk.size();

        // Destructively resize members to be the same size as Nk, no-op if same
        this->alpha.resize(K);
        this->E_logpi.resize(K);

        // Hyperparameter update
        this->Nk    = Nk;
        this->alpha = this->alpha_p + Nk;

        // Expectation update
        this->E_logpi = mxdigamma(this->alpha).array() - digamma(this->alpha.sum());
}

double distributions::Dirichlet::fenergy () const
{
        const int K = this->alpha.size();

        return lgamma(this->alpha.sum()) - (this->alpha_p-1) * this->E_logpi.sum()
               + ((this->alpha-1) * this->E_logpi - mxlgamma(this->alpha).array()).sum()
               - lgamma(K * this->alpha_p) + K * lgamma(this->alpha_p);
}

//
// Gaussian Wishart cluster distribution.
//

distributions::GaussWish::GaussWish (
        const double clustwidth,
        const unsigned int D
        )
        : ClusterDist(clustwidth, D),
        nu_p(D),
        beta_p(distributions::BETAPRIOR),
        m_p(RowVectorXd::Zero(D))
{
        if (clustwidth <= 0)
                throw invalid_argument("clustwidth must be > 0!");

        // Create Prior
        this->iW_p = this->nu_p * this->prior * MatrixXd::Identity(D, D);

        try
        { this->logdW_p = -logdet(this->iW_p); }
        catch (invalid_argument e)
        { throw invalid_argument(string("Creating prior: ").append(e.what())); }

        // Calculate prior free energy contribution
        this->F_p = mxlgamma((this->nu_p + 1
                              - enumdims(this->m_p.cols())).matrix() / 2).sum();

        this->clearobs(); // Empty suff. stats. and set posteriors equal to priors
}

void distributions::GaussWish::addobs(const VectorXd& qZk, const MatrixXd& X)
{
        if (X.cols() != this->D)
                throw invalid_argument("Mismatched dims. of cluster params and obs.!");
        if (qZk.rows() != X.rows())
                throw invalid_argument("qZk and X ar not the same length!");

        MatrixXd qZkX = qZk.asDiagonal() * X;

        this->N_s += qZk.sum();
        this->x_s += qZkX.colwise().sum();       // [1xD] row vector
        this->xx_s.noalias() += qZkX.transpose() * X; // [DxD] matrix
}

void distributions::GaussWish::update ()
{
        // Prepare the Sufficient statistics
        RowVectorXd xk = RowVectorXd::Zero(this->D);
        if (this->N_s > 0)
                xk = this->x_s/this->N_s;
        MatrixXd Sk = this->xx_s - xk.transpose() * this->x_s;
        RowVectorXd xk_m = xk - this->m_p;         // for iW, (xk - m)

        // Update posterior params
        this->N    = this->N_s;
        this->nu   = this->nu_p + this->N;
        this->beta = this->beta_p + this->N;
        this->m    = (this->beta_p * this->m_p + this->x_s) / this->beta;
        this->iW   = this->iW_p + Sk
                     + (this->beta_p * this->N/this->beta) * xk_m.transpose() * xk_m;

        try
        { this->logdW = -logdet(this->iW); }
        catch (invalid_argument e)
        { throw runtime_error(string("Calc log(det(W)): ").append(e.what())); }
}

void distributions::GaussWish::clearobs ()
{
        // Reset parameters back to prior values
        this->nu    = this->nu_p;
        this->beta  = this->beta_p;
        this->m     = this->m_p;
        this->iW    = this->iW_p;
        this->logdW = this->logdW_p;

        // Empty sufficient statistics
        this->N_s  = 0;
        this->x_s  = RowVectorXd::Zero(D);
        this->xx_s = MatrixXd::Zero(D,D);
}

VectorXd distributions::GaussWish::Eloglike (const MatrixXd& X) const
{
        // Expectations of log Gaussian likelihood
        VectorXd E_logX(X.rows());
        double sumpsi = mxdigamma((this->nu+1-enumdims(this->D)).matrix()/2).sum();
        try
        {
                E_logX = 0.5 * (sumpsi + this->logdW - this->D * (1/this->beta + log(pi))
                                - this->nu * mahaldist(X, this->m, this->iW).array()).matrix();
        }
        catch (invalid_argument e)
        { throw(string("Calculating Gaussian likelihood: ").append(e.what())); }

        return E_logX;
}

distributions::ArrayXb distributions::GaussWish::splitobs (
        const MatrixXd& X
        ) const
{

        // Find the principle eigenvector using the power method if not done so
        VectorXd eigvec;
        eigpower(this->iW, eigvec);

        // 'split' the observations perpendicular to this eigenvector.
        return (((X.rowwise() - this->m)
                 * eigvec.asDiagonal()).array().rowwise().sum()) >= 0;
}

double distributions::GaussWish::fenergy () const
{
        const ArrayXd l = enumdims(this->D);
        double sumpsi = mxdigamma((this->nu + 1 - l).matrix() / 2).sum();

        return this->F_p + (this->D * (this->beta_p/this->beta - 1 - this->nu
                                       - log(this->beta_p/this->beta))
                            + this->nu * ((this->iW.ldlt().solve(this->iW_p)).trace()
                                          + this->beta_p * mahaldist(this->m, this->m_p, this->iW).coeff(0,0))
                            + this->nu_p * (this->logdW_p - this->logdW) + this->N*sumpsi)/2
               - mxlgamma((this->nu+1-l).matrix() / 2).sum();
}

//
// Normal Gamma parameter distribution.
//

distributions::NormGamma::NormGamma (
        const double clustwidth,
        const unsigned int D
        )
        : ClusterDist(clustwidth, D),
        nu_p(distributions::NUPRIOR),
        beta_p(distributions::BETAPRIOR),
        m_p(RowVectorXd::Zero(D))
{
        if (clustwidth <= 0)
                throw invalid_argument("clustwidth must be > 0!");

        // Create Prior
        this->L_p = this->nu_p * this->prior * RowVectorXd::Ones(D);
        this->logL_p = this->L_p.array().log().sum();

        this->clearobs(); // Empty suff. stats. and set posteriors equal to priors
}

void distributions::NormGamma::addobs (const VectorXd& qZk, const MatrixXd& X)
{
        if (X.cols() != this->D)
                throw invalid_argument("Mismatched dims. of cluster params and obs.!");
        if (qZk.rows() != X.rows())
                throw invalid_argument("qZk and X ar not the same length!");

        MatrixXd qZkX = qZk.asDiagonal() * X;

        this->N_s  += qZk.sum();
        this->x_s  += qZkX.colwise().sum();                           // [1xD]
        this->xx_s += (qZkX.array() * X.array()).colwise().sum().matrix(); // [1xD]
}

void distributions::NormGamma::update ()
{
        // Prepare the Sufficient statistics
        RowVectorXd xk = RowVectorXd::Zero(this->D);
        RowVectorXd Sk = RowVectorXd::Zero(this->D);
        if (this->N_s > 0)
        {
                xk = this->x_s/this->N_s;
                Sk = this->xx_s.array() - this->x_s.array().square()/this->N_s;
        }

        // Update posterior params
        this->N    = this->N_s;
        this->beta = this->beta_p + this->N;
        this->nu   = this->nu_p + this->N/2;
        this->m    = (this->beta_p * this->m_p + x_s) / this->beta;
        this->L    = this->L_p + Sk/2 + (this->beta_p * this->N / (2 * this->beta))
                     * (xk - this->m_p).array().square().matrix();

        if ((this->L.array() <= 0).any())
                throw invalid_argument(string("Calc log(L): Variance is zero or less!"));

        this->logL = this->L.array().log().sum();
}

void distributions::NormGamma::clearobs ()
{
        // Reset parameters back to prior values
        this->nu   = this->nu_p;
        this->beta = this->beta_p;
        this->m    = this->m_p;
        this->L    = this->L_p;
        this->logL = this->logL_p;

        // Empty sufficient statistics
        this->N_s  = 0;
        this->x_s  = RowVectorXd::Zero(this->D);
        this->xx_s = RowVectorXd::Zero(this->D);
}

VectorXd distributions::NormGamma::Eloglike (const MatrixXd& X) const
{
        // Distance evaluation in the exponent
        VectorXd Xmdist = (X.rowwise() - this->m).array().square().matrix()
                          * this->L.array().inverse().matrix().transpose();

        // Expectations of log Gaussian likelihood
        return 0.5 * (this->D * (digamma(this->nu) - log(2 * pi) - 1/this->beta)
                      - this->logL - this->nu * Xmdist.array());
}

distributions::ArrayXb distributions::NormGamma::splitobs (
        const MatrixXd& X
        ) const
{
        // Find location of largest element in L, this is the 'eigenvector'
        int eigvec;
        this->L.maxCoeff(&eigvec);

        // 'split' the observations perpendicular to this 'eigenvector'.
        return (X.col(eigvec).array() - this->m(eigvec)) >= 0;
}

double distributions::NormGamma::fenergy () const
{
        const VectorXd iL = this->L.array().inverse().matrix().transpose();

        return D*(lgamma(this->nu_p) - lgamma(this->nu)
                  + this->N*digamma(this->nu)/2 - this->nu)
               + D/2 * (log(this->beta) - log(this->beta_p) - 1 + this->beta_p/this->beta)
               + this->beta_p*this->nu/2*(this->m - this->m_p).array().square().matrix()*iL
               + this->nu_p*(this->logL - this->logL_p) + this->nu*this->L_p*iL;
}

//
// Exponential Gamma parameter distribution.
//

distributions::ExpGamma::ExpGamma (const double obsmag, const unsigned int D)
        : ClusterDist(obsmag, D),
        a_p(distributions::APRIOR),
        b_p(obsmag)
{
        this->clearobs(); // Empty suff. stats. and set posteriors equal to priors
}

void distributions::ExpGamma::addobs (const VectorXd& qZk, const MatrixXd& X)
{
        if (X.cols() != this->D)
                throw invalid_argument("Mismatched dims. of cluster params and obs.!");
        if (qZk.rows() != X.rows())
                throw invalid_argument("qZk and X ar not the same length!");

        this->N_s += qZk.sum();
        this->x_s += (qZk.asDiagonal() * X).colwise().sum();
}

void distributions::ExpGamma::update ()
{
        // Update posterior params
        this->N    = this->N_s;
        this->a    = this->a_p + this->N;
        this->ib   = (this->b_p + this->x_s.array()).array().inverse().matrix();
        this->logb = -this->ib.array().log().sum();
}

void distributions::ExpGamma::clearobs ()
{
        // Reset parameters back to prior values
        this->a    = this->a_p;
        this->ib   = RowVectorXd::Constant(this->D, 1/this->b_p);
        this->logb = this->D * log(this->b_p);

        // Empty sufficient statistics
        this->N_s = 0;
        this->x_s = RowVectorXd::Zero(this->D);
}

VectorXd distributions::ExpGamma::Eloglike (const MatrixXd& X) const
{
        return this->D * digamma(this->a) - this->logb
               - (this->a * X * this->ib.transpose()).array();
}

distributions::ArrayXb distributions::ExpGamma::splitobs (
        const MatrixXd& X
        ) const
{
        ArrayXd XdotL = X;// * (this->a * this->ib).transpose();
        return (XdotL > (XdotL.sum()/XdotL.size()));
}

double distributions::ExpGamma::fenergy () const
{
        return this->D * ((this->a - this->a_p) * digamma(this->a) - this->a
                          - this->a_p * log(this->b_p) - lgamma(this->a) + lgamma(this->a_p))
               + this->b_p * this->a * this->ib.sum() + this->a_p * this->logb;
}


// Author: Henrique Mendonça <henrique@apache.org>
#include "rsba/solveRSpnp.h"
#include "rsba/mat/cam.h"
#include "rsba/VideoSfmBaRs.h"
#include "rsba/struct/VideoSfM.h"

#include <ceres/rotation.h>
#include <opencv2/core/core_c.h>
#if (defined(CV_VERSION_EPOCH) && CV_VERSION_EPOCH == 2)
#include <opencv2/core/internal.hpp>
#else
#define __OPENCV_BUILD
#include <opencv2/cvconfig.h>
#include <opencv2/core/private.hpp>
#endif
#include <iostream>
//#include <boost/timer/timer.hpp>

using namespace ::cv;
using namespace ::vision;
using namespace ::vision::sfm;

template <typename TF>
struct RsBA: public ReprojectionError {
  TF point[NUM_POINT_PARAMS];
  SHUTTER rs;
  const int* const scanlines;

  RsBA(const double camera[NUM_CAM_PARAMS],
       const TF* const observed, // 2D
       const TF* const point3d,
       const SHUTTER _rs,
       const int* const _scanlines) // 3D
  : scanlines(_scanlines)
  {
    observed_x = (observed[0]); observed_y = (observed[1]);
    memcpy(camera_params, camera, sizeof(camera_params));
    memcpy(point, point3d, sizeof(point));
    rs = _rs;
  };

  template <typename T>
  bool operator()(const T* const pose0,
                  T* residuals) const {
    return operator()(pose0, pose0, residuals);
  };

  template <typename T>
  bool operator()(const T* const pose0,
                  const T* const pose1,
                  T* residuals) const {
    T pose[6];
    T obs[2] = { T(observed_x), T(observed_x) };
    interpolate_rs(pose0, pose1, rs, scanlines, obs, pose);

    T camera[NUM_CAM_PARAMS];
    for (short i = 0; i < NUM_CAM_PARAMS; i++)
      camera[i] = T(camera_params[i]);

    T proj[2]; //reprojection
    T p[NUM_POINT_PARAMS] = { T(point[0]), T(point[1]), T(point[2]) };
    if ( ! w2i(camera, pose, p, proj, false)) {
      return false;
    }

    // The error is the difference between the reprojection and observed position.
    residuals[0] = (proj[0] - T(observed_x));
    residuals[1] = (proj[1] - T(observed_y));

    T threshold(5);
    if (abs(residuals[0]) < threshold and abs(residuals[1]) < threshold) {
      return true;
    } else { //TODO check matches
      return true;
    }
  }

  // Factory to hide the construction of the CostFunction object from the client code.
  static ceres::CostFunction* Create(
      const double camera[NUM_CAM_PARAMS],
      const TF* const observed,
      const TF* const point3d,
      const SHUTTER rs,
      const int* const scanlines)
  {
    return (new ceres::AutoDiffCostFunction<RsBA,
        NUM_RESIDUALS
        ,NUM_POSE_PARAMS // Initial frame pose
        ,NUM_POSE_PARAMS
        >( // Final frame pose
                new RsBA(camera, observed, point3d, rs, scanlines)));
  }
};

bool vision::solveRsPnP(InputArray _opoints, InputArray _ipoints,
                  InputArray _cameraMatrix, InputArray _distCoeffs,
                  OutputArray _rvec, OutputArray _tvec,
                  OutputArray _rvec2, OutputArray _tvec2,
                  const SHUTTER shutter, const int scanlines[2],
                  bool useExtrinsicGuess, int flags)
{
  //boost::timer::auto_cpu_timer btimer;

  _rvec.create(3, 1, CV_64F);
  _tvec.create(3, 1, CV_64F);
  _rvec2.create(3, 1, CV_64F);
  _tvec2.create(3, 1, CV_64F);
  cv::Mat rvec = _rvec.getMat(), tvec = _tvec.getMat();
  cv::Mat rvec2 = _rvec2.getMat(), tvec2 = _tvec2.getMat();

  if (cv::norm(rvec, NORM_L1) + cv::norm(tvec, NORM_L1) +
      cv::norm(rvec2, NORM_L1) + cv::norm(tvec2, NORM_L1) == 0) {
    if (solvePnP(_opoints, _ipoints, _cameraMatrix, _distCoeffs, rvec, tvec, useExtrinsicGuess, flags))
    { // GS Init
      rvec.copyTo(rvec2);
      tvec.copyTo(tvec2);
      std::cout << "GS PnP Init: " << rvec << tvec << endl;
    }
  }

  {
    ceres::Problem problem;
    Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();

    vector<double> pose(NUM_POSE_PARAMS), pose2(NUM_POSE_PARAMS);
    { // Init poses
      double rInv[3];
      assign3(rvec.at<Vec3d>(0, 0).val, pose.data());
      invert3(pose.data(), rInv);
      ceres::AngleAxisRotatePoint(rInv, (-tvec.at<Vec3d>(0, 0)).val, pose.data()+3);

      assign3(rvec2.at<Vec3d>(0, 0).val, pose2.data());
      invert3(pose2.data(), rInv);
      ceres::AngleAxisRotatePoint(rInv, (-tvec2.at<Vec3d>(0, 0)).val, pose2.data()+3);
    }

    std::vector<double> cam = sfmCam(_cameraMatrix.getMat(), _distCoeffs.getMat());
    for (int i = 0; i < _opoints.size().width; i++) {
      ceres::CostFunction* costFunction = RsBA<float>::Create(cam.data(),
                                                              ipoints.at<Vec2f>(0, i).val,
                                                              opoints.at<Vec3f>(0, i).val,
                                                              shutter, scanlines);
      problem.AddResidualBlock(costFunction, NULL, pose.data(), pose2.data());
    }

    // Make Ceres automatically detect the bundle structure. Note that the
    // standard solver, SPARSE_NORMAL_CHOLESKY, also works fine but it is slower
    // for standard bundle adjustment problems.
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::SPARSE_SCHUR;
    //options.minimizer_progress_to_stdout = true;
    options.max_num_iterations = 10;

    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    if (summary.IsSolutionUsable())
    {
//      std::cout << summary.BriefReport() << endl;

      assign3(pose.data(), rvec.at<Vec3d>(0, 0).val);
      assign3(pose.data()+3, tvec.at<Vec3d>(0, 0).val);
      // invert translation
      ceres::AngleAxisRotatePoint(pose.data(), tvec.at<Vec3d>(0, 0).val, tvec.at<Vec3d>(0, 0).val);
      tvec *= -1;
//      std::cout << rvec << endl;
//      std::cout << tvec << endl;

      assign3(pose2.data(), rvec2.at<Vec3d>(0, 0).val);
      assign3(pose2.data()+3, tvec2.at<Vec3d>(0, 0).val);
      // invert translation
      ceres::AngleAxisRotatePoint(pose2.data(), tvec2.at<Vec3d>(0, 0).val, tvec2.at<Vec3d>(0, 0).val);
      tvec2 *= -1;
//      std::cout << rvec2 << endl;
//      std::cout << tvec2 << endl;

//      solvePnP(_opoints, _ipoints, _cameraMatrix, _distCoeffs, orvec, otvec, useExtrinsicGuess, flags);
//      std::cout << orvec << endl;
//      std::cout << otvec << endl;

      return true;
    }
  }

  return false;
}

namespace vision
{
    namespace pnpransac
    {
        struct CameraParameters
        {
            void init(Mat _intrinsics, Mat _distCoeffs, const SHUTTER _shutter, const int* _scanlines)
            {
                _intrinsics.copyTo(intrinsics);
                _distCoeffs.copyTo(distortion);
                shutter = _shutter;
                scanlines = _scanlines;
            }

            Mat intrinsics;
            Mat distortion;
            SHUTTER shutter;
            const int* scanlines;
        };

        struct Parameters
        {
            int iterationsCount;
            float reprojectionError;
            int minInliersCount;
            bool useExtrinsicGuess;
            int flags;
            CameraParameters camera;
            int min_points_count;
        };

        static vector<Point2f> project3dPoints(const Mat& opoints, const Mat& ipoints, const Parameters& params,
                                           const Mat& rvec,  const Mat& tvec,
                                           const Mat& rvec2, const Mat& tvec2)
        {
          vector<Point2f> projected_points(opoints.cols);

          for (int i = 0; i < opoints.cols; i++) {
            Point3f op(opoints.at<Vec3f>(0, i));
            Point2f ip(ipoints.at<Vec2f>(0, i));

            vector<double> pose(6, 0);
            assign3(rvec.at<Vec3d>(0, 0).val, pose.data());
            double rInv[3];
            invert3(pose.data(), rInv);
            ceres::AngleAxisRotatePoint(rInv, (-tvec.at<Vec3d>(0, 0)).val, pose.data()+3);

            vector<double> pose2(6, 0);
            assign3(rvec2.at<Vec3d>(0, 0).val, pose2.data());
            invert3(pose2.data(), rInv);
            ceres::AngleAxisRotatePoint(rInv, (-tvec2.at<Vec3d>(0, 0)).val, pose2.data()+3);

            std::vector<double> cam = sfmCam(params.camera.intrinsics, params.camera.distortion);
            double point[] = { op.x, op.y, op.z };
            double obs[] = { ip.x, ip.y };
            double poseInter[NUM_POSE_PARAMS];
            interpolate_rs(pose.data(), pose2.data(), params.camera.shutter, params.camera.scanlines, obs, poseInter);

            double proj[2]; //reprojection
            if ( ! w2i(cam.data(), poseInter, point, proj, false)) {
              std::abort();
            }
            projected_points[i].x = proj[0];
            projected_points[i].y = proj[1];
          }

          return projected_points;
        }

        static void pnpTask(const vector<char>& pointsMask, const Mat& objectPoints, const Mat& imagePoints,
                     const Parameters& params, vector<int>& inliers,
                     Mat& rvec, Mat& tvec, Mat& rvec2, Mat& tvec2,
                     const Mat& rvecInit, const Mat& tvecInit, const Mat& rvecInit2, const Mat& tvecInit2,
                     Mutex& resultsMutex)
        {
            Mat modelObjectPoints(1, params.min_points_count, CV_32FC3), modelImagePoints(1, params.min_points_count, CV_32FC2);
            for (int i = 0, colIndex = 0; i < (int)pointsMask.size(); i++)
            {
                if (pointsMask[i])
                {
                    Mat colModelImagePoints = modelImagePoints(Rect(colIndex, 0, 1, 1));
                    imagePoints.col(i).copyTo(colModelImagePoints);
                    Mat colModelObjectPoints = modelObjectPoints(Rect(colIndex, 0, 1, 1));
                    objectPoints.col(i).copyTo(colModelObjectPoints);
                    colIndex = colIndex+1;
                }
            }

            //filter same 3d points, hang in solveRsPnP
            double eps = 1e-10;
            int num_same_points = 0;
            for (int i = 0; i < params.min_points_count; i++)
                for (int j = i + 1; j < params.min_points_count; j++)
                {
                    if (norm(modelObjectPoints.at<Vec3f>(0, i) - modelObjectPoints.at<Vec3f>(0, j)) < eps)
                        num_same_points++;
                }
            if (num_same_points > 0)
                return;

            Mat localRvec, localTvec;
            Mat localRvec2, localTvec2;
            rvecInit.copyTo(localRvec);
            tvecInit.copyTo(localTvec);
            rvecInit2.copyTo(localRvec2);
            tvecInit2.copyTo(localTvec2);

            vector<int> localInliers;
            vision::solveRsPnP(modelObjectPoints, modelImagePoints,
                params.camera.intrinsics, params.camera.distortion,
                localRvec, localTvec,
                localRvec2, localTvec2,
                params.camera.shutter, params.camera.scanlines,
                params.useExtrinsicGuess, params.flags);

            vector<Point2f> projected_points = project3dPoints(objectPoints, imagePoints, params, localRvec, localTvec, localRvec2, localTvec2);
            for (int i = 0; i < objectPoints.cols; i++) {
                Point2f p(imagePoints.at<Vec2f>(0, i));
                if ((norm(p - projected_points[i]) < params.reprojectionError)) {
                    localInliers.push_back(i);
                }
            }

            if (localInliers.size() > inliers.size())
            {
              cout << localInliers.size() << "/" << objectPoints.cols << endl;
              resultsMutex.lock();

              inliers.clear();
              inliers.resize(localInliers.size());
              memcpy(&inliers[0], &localInliers[0], sizeof(int) * localInliers.size());
              localRvec.copyTo(rvec);
              localTvec.copyTo(tvec);
              localRvec2.copyTo(rvec2);
              localTvec2.copyTo(tvec2);

              resultsMutex.unlock();
            }
        }

        class PnPSolver
        {
        public:
            void operator()( const BlockedRange& r ) const
            {
                vector<char> pointsMask(objectPoints.cols, 0);
                memset(&pointsMask[0], 1, parameters.min_points_count );
                for( int i=r.begin(); i!=r.end(); ++i )
                {
                    generateVar(pointsMask);
                    pnpTask(pointsMask, objectPoints, imagePoints, parameters,
                            inliers, rvec, tvec, rvec2, tvec2,
                            initRvec, initTvec, initRvec2, initTvec2, syncMutex);

                    if ((int)inliers.size() >= parameters.minInliersCount)
                    {
#ifdef HAVE_TBB
                        tbb::task::self().cancel_group_execution();
#else
                        break;
#endif
                    }
                }
            }

            PnPSolver(const Mat& _objectPoints, const Mat& _imagePoints, const Parameters& _parameters,
                Mat& _rvec, Mat& _tvec, Mat& _rvec2,
                Mat& _tvec2, vector<int>& _inliers):
            objectPoints(_objectPoints), imagePoints(_imagePoints), parameters(_parameters),
            rvec(_rvec), tvec(_tvec), rvec2(_rvec2), tvec2(_tvec2), inliers(_inliers)
            {
              rvec.copyTo(initRvec);
              tvec.copyTo(initTvec);
              rvec2.copyTo(initRvec2);
              tvec2.copyTo(initTvec2);

              generator.state = theRNG().state; //to control it somehow...
            }

        private:
            PnPSolver& operator=(const PnPSolver&);

            const Mat& objectPoints;
            const Mat& imagePoints;
            const Parameters& parameters;
            Mat &rvec, &tvec;
            Mat &rvec2, &tvec2;
            vector<int>& inliers;
            Mat initRvec, initTvec;
            Mat initRvec2, initTvec2;

            static RNG generator;
            static Mutex syncMutex;

            void generateVar(vector<char>& mask) const
            {
                int size = (int)mask.size();
                for (int i = 0; i < size; i++)
                {
                    int i1 = generator.uniform(0, size);
                    int i2 = generator.uniform(0, size);
                    char curr = mask[i1];
                    mask[i1] = mask[i2];
                    mask[i2] = curr;
                }
            }
        };

        Mutex PnPSolver::syncMutex;
        RNG PnPSolver::generator;

    }
}

void vision::solveRsPnPRansac(InputArray _opoints, InputArray _ipoints,
                        InputArray _cameraMatrix, InputArray _distCoeffs,
                        OutputArray _rvec, OutputArray _tvec,
                        OutputArray _rvec2, OutputArray _tvec2,
                        const SHUTTER shutter, const int scanlines[2],
                        bool useExtrinsicGuess, int iterationsCount,
                        float reprojectionError, int minInliersCount,
                        OutputArray _inliers, int flags, int min_points_count)
{
  //boost::timer::auto_cpu_timer btimer;

    Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
    Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();

    CV_Assert(opoints.isContinuous());
    CV_Assert(opoints.depth() == CV_32F);
    CV_Assert((opoints.rows == 1 && opoints.channels() == 3) || opoints.cols*opoints.channels() == 3);
    CV_Assert(ipoints.isContinuous());
    CV_Assert(ipoints.depth() == CV_32F);
    CV_Assert((ipoints.rows == 1 && ipoints.channels() == 2) || ipoints.cols*ipoints.channels() == 2);

    _rvec.create(3, 1, CV_64FC1);
    _tvec.create(3, 1, CV_64FC1);
    _rvec2.create(3, 1, CV_64FC1);
    _tvec2.create(3, 1, CV_64FC1);
    Mat rvec = _rvec.getMat();
    Mat tvec = _tvec.getMat();
    Mat rvec2 = _rvec2.getMat();
    Mat tvec2 = _tvec2.getMat();

    if (cv::norm(rvec, NORM_L1) + cv::norm(tvec, NORM_L1) +
        cv::norm(rvec2, NORM_L1) + cv::norm(tvec2, NORM_L1) == 0) {
      cv::Mat gs_inliers;
      solvePnPRansac(opoints, ipoints, cameraMatrix, distCoeffs, rvec, tvec,
                     useExtrinsicGuess, iterationsCount, reprojectionError*2,
                     minInliersCount, gs_inliers, flags);

      if (gs_inliers.rows > 4)
      { // GS Init
        rvec.copyTo(rvec2);
        tvec.copyTo(tvec2);
        std::cout << "GS PnP Init: " << rvec << tvec << endl;
      }
    }

    Mat objectPoints = opoints.reshape(3, 1), imagePoints = ipoints.reshape(2, 1);

    if (minInliersCount <= 0)
        minInliersCount = objectPoints.cols;
    pnpransac::Parameters params;
    params.iterationsCount = iterationsCount;
    params.minInliersCount = minInliersCount;
    params.reprojectionError = reprojectionError;
    params.useExtrinsicGuess = useExtrinsicGuess;
    params.camera.init(cameraMatrix, distCoeffs, shutter, scanlines);
    params.flags = flags;
    params.min_points_count = min_points_count;

    vector<int> localInliers;
    Mat localRvec, localTvec;
    Mat localRvec2, localTvec2;
    rvec.copyTo(localRvec);
    tvec.copyTo(localTvec);
    rvec2.copyTo(localRvec2);
    tvec2.copyTo(localTvec2);

    if (objectPoints.cols >= params.min_points_count)
    {
        cv::parallel_for(BlockedRange(0,iterationsCount),
            pnpransac::PnPSolver(objectPoints, imagePoints, params,
                localRvec, localTvec, localRvec2, localTvec2, localInliers));
    }

    if (localInliers.size() >= (size_t)params.min_points_count)
    {
        if (flags != CV_P3P)
        {
            int i, pointsCount = (int)localInliers.size();
            Mat inlierObjectPoints(1, pointsCount, CV_32FC3), inlierImagePoints(1, pointsCount, CV_32FC2);
            for (i = 0; i < pointsCount; i++)
            {
                int index = localInliers[i];
                Mat colInlierImagePoints = inlierImagePoints(Rect(i, 0, 1, 1));
                imagePoints.col(index).copyTo(colInlierImagePoints);
                Mat colInlierObjectPoints = inlierObjectPoints(Rect(i, 0, 1, 1));
                objectPoints.col(index).copyTo(colInlierObjectPoints);
            }
            vision::solveRsPnP(inlierObjectPoints, inlierImagePoints,
                params.camera.intrinsics, params.camera.distortion,
                localRvec, localTvec,
                localRvec2, localTvec2,
                shutter, scanlines,
                true, flags);
        }
        localRvec.copyTo(rvec);
        localTvec.copyTo(tvec);
        localRvec2.copyTo(rvec2);
        localTvec2.copyTo(tvec2);
        if (_inliers.needed())
            Mat(localInliers).copyTo(_inliers);
    }
    else
    {
      tvec.setTo(Scalar(0));
      tvec2.setTo(Scalar(0));
      Mat R = Mat::eye(3, 3, CV_64F);
      Rodrigues(R, rvec);
      Rodrigues(R, rvec2);
      if ( _inliers.needed() ) _inliers.release();
    }
    return;
}