/*
ID: neo-white
LANG: C++
TASK: Kmeans_PlusPlus.cpp
*/

#include "Kmeans_PlusPlus.h"
#include "../Core/Core.h"
#include <boost/serialization/vector.hpp>
#include <boost/serialization/queue.hpp>
#include <boost/serialization/export.hpp>
#include <vector>
#include <queue>
#include <fstream>
#include <time.h>

//BOOST_CLASS_EXPORT(kmeans_plusplus::Kmeans_PlusPlus)
namespace kmeans_plusplus{

    //Flexible_vector *kmeans_plusplus(Flexible_vector *problem,int k)
    std::vector<int> Kmeans_PlusPlus::kmeans_plusplus(Flexible_vector *problem,int k)
    {
    //	fprintf(stderr,"In kmeans_Lloyd \n");
        int l = problem->prob.l;
        int n_clusters = k;
        int max_index = problem->prob.max_feature;

        //choose initial center uniformly at random
        std::vector<int> queue;
        queue.resize(l);
        std::fill(queue.begin(),queue.end(), 0);
        srand((unsigned)time(NULL));
        int index = (rand()%l);
        queue[index] = 1;
        int iter_counter = 0;
        std::cout<<"kmeans++:iteration "<< iter_counter<<". candidated center "<<index<<std::endl;

        //choose remaining centers
        int remainingInstances = l - 1;
        std::vector<double> distances;
        distances.resize(l);
        std::vector<double> cumProbs;
        cumProbs.resize(l);
        std::vector<double> weights;
        weights.resize(l);
        int ii,jj,kk;
        if(n_clusters > 1)
        {
            //proceed with selecting the rest
            //distances to the initial randomly chose center
            for(ii=0;ii<l;ii++)
                distances[ii] = euclidean_distance(problem->get_value_without_label(ii),problem->get_value_without_label(index));

            int center_candidate;
            //now choose the remaining cluster centers
            for(jj = 1; jj < n_clusters; jj++)
            {
                bool find = false;
                //distances converted to probabilities
                double sum_distances = 0;
                for(kk=0;kk<l;kk++)
                    sum_distances += distances[kk];
                for(kk=0;kk<l;kk++)
                    weights[kk] = distances[kk]/sum_distances;
                double sum_probs = 0;
                for(kk=0;kk<l;kk++)
                {
                    sum_probs += weights[kk];
                    cumProbs[kk] = sum_probs;
                }
                cumProbs[l-1] = 1.0;  //make sure there are no rounding issues
                //choose a random instance
                double prob = (double) rand()/RAND_MAX;  //random between 0 and 1
                for(ii=0;ii<l;ii++)
                {
                    if(prob < cumProbs[ii])
                    {
                        if(queue[ii] != 1)
                        {
                            queue[ii] = 1;
                            center_candidate = ii;
                            std::cout<<"kmeans++:iteration "<<iter_counter+1<<". candidated center "<<ii<<".prob "<<prob<<std::endl;
                            remainingInstances--;
                            find = true;
                            break;
                        }
                    }
                }
                iter_counter++;

                if(find == false)
                {
                    std::cout<<"kmeans++:iteration "<<iter_counter+1<<". No find center. prob "<<prob<<std::endl;
                    break;
                }

                if(remainingInstances == 0)
                    break;

                //prepares to choose the next cluster center.
                //check distances against the new cluster center to see if it is closer
                double newDist;
                for(ii = 0;ii < l;ii++)
                {
                    if(distances[ii] > 0)
                    {
                        newDist = euclidean_distance(problem->get_value_without_label(ii),problem->get_value_without_label(center_candidate));
                        if(newDist < distances[ii])
                        {
                            distances[ii] = newDist;
                        }
                    }
                }
            }
        }
        return queue;
    }

/*
    std::vector<int> Kmeans_Lloyd::init_random(size_t length, size_t random_seed)
    {
        int tmp;
        std::vector<int> queue;
        for(size_t i=0;i<length;i++)
            queue.push_back(0);

        srand((unsigned)time(NULL));
        for(size_t j=0;j<random_seed;j++)
        {
            while(1)
            {
                tmp = (rand()%length);
                if(queue[tmp] == 0)
                {
                    queue[tmp] = 1;
                    break;
                }
            }
        }
        return queue;
    }

    void Kmeans_Lloyd::finish(Flexible_vector *records, size_t feat_dim)
    {
        std::ofstream file;
        file.open("./result.txt");
        size_t n_records = records->prob.l;
        for(size_t i = 0; i < n_records; ++i)
        {
            std::vector<node> record;
            record.clear();
            record = records->get_value_without_label(i);
            double dis = (1<<16)-1;
            int who = -1;
            for(size_t k = 0; k < this->fvkl.prob.l; k++)
            {
                std::vector<node> centroid = this->fvkl.get_value_without_label(k);
                double tmp = euclidean_distance(record,centroid);

                if (tmp < dis){
                    who = k;
                    dis = tmp;
                }
            }
            file << who << std::endl;
        }
    }
*/
    double Kmeans_PlusPlus::euclidean_distance(std::vector<node> px,std::vector<node> py)
    {
    	double sum = 0;
    	double distances = 0;
        size_t ipx = 0;
        size_t ipy = 0;
        while((ipx <= (px.size()-1))||(ipy <= (py.size()-1)))
    	{
            if((ipx <= (px.size()-1))&&(ipy <= (py.size()-1)))
            {
    		    if(px[ipx].index == py[ipy].index)
    		    {
    		    	sum += (px[ipx].value-py[ipy].value)*(px[ipx].value-py[ipy].value);
    		    	++ipx;
    		    	++ipy;
    		    }
    		    else
    		    {
    		    	if(px[ipx].index > py[ipy].index)
                    {
                        sum += py[ipy].value*py[ipy].value;
    		    		++ipy;
                    }
    		    	else
                    {
                        sum += px[ipx].value*px[ipx].value;
    		    		++ipx;
                    }
    		    }
            }else if((ipx == px.size())&&(ipy <= (py.size()-1)))
            {
                sum += py[ipy].value*py[ipy].value;
    		    ++ipy;
            }else if((ipy == py.size())&&(ipx <= (px.size()-1)))
            {
                sum += px[ipx].value*px[ipx].value;
    	   		++ipx;
            }
    	}
        distances = sqrt(sum);
    	return distances;
    }
}


/**
 * @file covariances.cpp
 * @brief How to recover covariances.
 * @author Michael Kaess
 * @version $Id: covariances.cpp 6377 2012-03-30 20:06:44Z kaess $
 *
 * Copyright (C) 2009-2013 Massachusetts Institute of Technology.
 * Michael Kaess, Hordur Johannsson, David Rosen,
 * Nicholas Carlevaris-Bianco and John. J. Leonard
 *
 * This file is part of iSAM.
 *
 * iSAM 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 2.1 of the License, or (at
 * your option) any later version.
 *
 * iSAM 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 iSAM.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include <iostream>
#include <stdio.h>

#include <Eigen/LU>

#include <isam/isam.h>

using namespace std;
using namespace isam;
using namespace Eigen;

Slam slam;

int main(int argc, const char* argv[]) {
  Pose2d origin;
  Noise noise = SqrtInformation(10. * eye(3));
  Pose2d_Node* pose_node_1 = new Pose2d_Node(); // create node
  slam.add_node(pose_node_1); // add it to the Slam graph
  Pose2d_Factor* prior = new Pose2d_Factor(pose_node_1, origin, noise); // create prior measurement, an factor
  slam.add_factor(prior); // add it to the Slam graph

  Pose2d_Node* pose_node_2 = new Pose2d_Node(); // create node
  slam.add_node(pose_node_2); // add it to the Slam graph

  Pose2d delta(1., 0., 0.);
  Pose2d_Pose2d_Factor* odo = new Pose2d_Pose2d_Factor(pose_node_1, pose_node_2, delta, noise);
  slam.add_factor(odo);

  slam.batch_optimization();

#if 0
  const Covariances& covariances = slam.covariances();
#else
  // operate on a copy (just an example, cloning is useful for running
  // covariance recovery in a separate thread)
  const Covariances& covariances = slam.covariances().clone();
#endif

  // recovering the full covariance matrix
  cout << "Full covariance matrix:" << endl;
  MatrixXd cov_full = covariances.marginal(slam.get_nodes());
  cout << cov_full << endl << endl;

  // sanity checking by inverting the information matrix, not using R
  SparseSystem Js = slam.jacobian();
  MatrixXd J(Js.num_cols(), Js.num_cols());
  for (int r=0; r<Js.num_cols(); r++) {
    for (int c=0; c<Js.num_cols(); c++) {
      J(r,c) = Js(r,c);
    }
  }
  MatrixXd H = J.transpose() * J;
  MatrixXd cov_full2 = H.inverse();
  cout << cov_full2 << endl;

  // recovering the block-diagonals only of the full covariance matrix
  cout << "Block-diagonals only:" << endl;
  Covariances::node_lists_t node_lists;
  list<Node*> nodes;
  nodes.push_back(pose_node_1);
  node_lists.push_back(nodes);
  nodes.clear();
  nodes.push_back(pose_node_2);
  node_lists.push_back(nodes);
  list<MatrixXd> cov_blocks = covariances.marginal(node_lists);
  int i = 1;
  for (list<MatrixXd>::iterator it = cov_blocks.begin(); it!=cov_blocks.end(); it++, i++) {
    cout << "block " << i << endl;
    cout << *it << endl;
  }

  // recovering individual entries, here the right block column
  cout << "Right block column:" << endl;
  Covariances::node_pair_list_t node_pair_list;
  node_pair_list.push_back(make_pair(pose_node_1, pose_node_2));
  node_pair_list.push_back(make_pair(pose_node_2, pose_node_2));
  list<MatrixXd> cov_entries = covariances.access(node_pair_list);
  for (list<MatrixXd>::iterator it = cov_entries.begin(); it!=cov_entries.end(); it++) {
    cout << *it << endl;
  }
}


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver 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.
 *
 *     PCMSolver 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 PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#ifndef ONELAYERERF_HPP
#define ONELAYERERF_HPP

#include <cmath>
#include <iosfwd>

#include "Config.hpp"

#include <boost/math/special_functions/erf.hpp>

/*! \file OneLayerErf.hpp
 *  \class OneLayerErf
 *  \brief A erf dielectric profile
 *  \author Roberto Di Remigio
 *  \date 2015
 *  \note The parameter given from user input for width_ is divided by 6.0 in
 *  the constructor to keep consistency with \cite Frediani2004a
 */

class OneLayerErf
{
private:
    /// Dielectric constant on the left of the interface
    double epsilon1_;
    /// Dielectric constant one the right of the interface
    double epsilon2_;
    /// Width of the transition layer
    double width_;
    /// Center of the transition layer
    double center_;
    /*! Returns value of dielectric profile at given point
     *  \param[in] point where to evaluate the profile
     */
    double value(double point) const {
        double epsPlus = (epsilon1_ + epsilon2_) / 2.0;
        double epsMinus = (epsilon2_ - epsilon1_) / 2.0;
        double val = boost::math::erf((point - center_) / width_);
        return (epsPlus + epsMinus * val); //epsilon(r)
    }
    /*! Returns value of derivative of dielectric profile at given point
     *  \param[in] point where to evaluate the derivative
     */
    double derivative(double point) const {
        double factor = (epsilon2_ - epsilon1_) / (width_ * std::sqrt(M_PI));
        double t = (point - center_) / width_;
        double val = std::exp(-std::pow(t, 2));
        return (factor * val); //first derivative of epsilon(r)
    }
    std::ostream & printObject(std::ostream & os)
    {
        os << "Profile functional form: erf" << std::endl;
        os << "Permittivity inside  = " << epsilon1_ << std::endl;
        os << "Permittivity outside = " << epsilon2_ << std::endl;
        os << "Profile width        = " << width_    << " AU" << std::endl;
        os << "Profile center       = " << center_   << " AU";
        return os;
    }
public:
    OneLayerErf() {}
    OneLayerErf(double e1, double e2, double w, double c) :
        epsilon1_(e1), epsilon2_(e2), width_(w/6.0), center_(c) {}
    /*! Returns a tuple holding the permittivity and its derivative
     *  \param[in]   r evaluation point
     */
    pcm::tuple<double, double> operator()(const double r) const
    {
        return pcm::make_tuple(value(r), derivative(r));
    }
    double epsilon1() const { return epsilon1_; }
    double epsilon2() const { return epsilon2_; }
    double width() const { return width_; }
    double center() const { return center_; }
    friend std::ostream & operator<<(std::ostream & os, OneLayerErf & th) {
        return th.printObject(os);
    }
};

#endif // ONELAYERERF_HPP


/* =========================================================================
   Copyright (c) 2010-2016, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/** \example blas3.cpp
*
*  In this tutorial it is shown how BLAS level 3 functionality in ViennaCL can be used.
*
*  We begin with defining preprocessor constants and including the necessary headers.
**/

//disable debug mechanisms to have a fair comparison with ublas:
#ifndef NDEBUG
 #define NDEBUG
#endif

// System headers
#include <iostream>

// ublas headers
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

// Must be set if you want to use ViennaCL algorithms on ublas objects
#define VIENNACL_WITH_UBLAS 1

// ViennaCL headers
#include "viennacl/scalar.hpp"
#include "viennacl/vector.hpp"
#include "viennacl/matrix.hpp"
#include "viennacl/linalg/prod.hpp"
#include "viennacl/tools/random.hpp"
#include "viennacl/tools/timer.hpp"

#define BLAS3_MATRIX_SIZE   400

using namespace boost::numeric;

/**
*  Later in this tutorial we will iterate over all available OpenCL devices.
*  To ensure that this tutorial also works if no OpenCL backend is activated, we need this dummy-struct.
**/
#ifndef VIENNACL_WITH_OPENCL
  struct dummy
  {
    std::size_t size() const { return 1; }
  };
#endif

/**
* We don't need additional auxiliary routines, so let us start straight away with main():
*/
int main()
{
  typedef float     ScalarType;

  viennacl::tools::timer timer;
  double exec_time;

  viennacl::tools::uniform_random_numbers<ScalarType> randomNumber;

  /**
  * Set up some ublas objects and initialize with data:
  **/
  ublas::matrix<ScalarType> ublas_A(BLAS3_MATRIX_SIZE, BLAS3_MATRIX_SIZE);
  ublas::matrix<ScalarType, ublas::column_major> ublas_B(BLAS3_MATRIX_SIZE, BLAS3_MATRIX_SIZE);
  ublas::matrix<ScalarType> ublas_C(BLAS3_MATRIX_SIZE, BLAS3_MATRIX_SIZE);
  ublas::matrix<ScalarType> ublas_C1(BLAS3_MATRIX_SIZE, BLAS3_MATRIX_SIZE);

  for (unsigned int i = 0; i < ublas_A.size1(); ++i)
    for (unsigned int j = 0; j < ublas_A.size2(); ++j)
      ublas_A(i,j) = ScalarType(10) * randomNumber();

  for (unsigned int i = 0; i < ublas_B.size1(); ++i)
    for (unsigned int j = 0; j < ublas_B.size2(); ++j)
      ublas_B(i,j) = ScalarType(10) * randomNumber();

  /**
  * Set up some ViennaCL objects. Data initialization will happen later.
  **/
  //viennacl::ocl::set_context_device_type(0, viennacl::ocl::gpu_tag());  //uncomment this is you wish to use GPUs only
  viennacl::matrix<ScalarType> vcl_A(BLAS3_MATRIX_SIZE, BLAS3_MATRIX_SIZE);
  viennacl::matrix<ScalarType, viennacl::column_major> vcl_B(BLAS3_MATRIX_SIZE, BLAS3_MATRIX_SIZE);
  viennacl::matrix<ScalarType> vcl_C(BLAS3_MATRIX_SIZE, BLAS3_MATRIX_SIZE);

  /**
  *
Matrix-matrix Products

  *
  * First compute the reference product using uBLAS:
  **/
  std::cout << "--- Computing matrix-matrix product using ublas ---" << std::endl;
  timer.start();
  ublas_C = ublas::prod(ublas_A, ublas_B);
  exec_time = timer.get();
  std::cout << " - Execution time: " << exec_time << std::endl;

  /**
  * Now iterate over all OpenCL devices in the context and compute the matrix-matrix product.
  * If the OpenCL backend is disabled, we use the dummy-struct defined above.
  **/
  std::cout << std::endl << "--- Computing matrix-matrix product on each available compute device using ViennaCL ---" << std::endl;
#ifdef VIENNACL_WITH_OPENCL
  std::vector<viennacl::ocl::device> devices = viennacl::ocl::current_context().devices();
#else
  dummy devices;
#endif

  for (std::size_t device_id=0; device_id<devices.size(); ++device_id)
  {
#ifdef VIENNACL_WITH_OPENCL
    viennacl::ocl::current_context().switch_device(devices[device_id]);
    std::cout << " - Device Name: " << viennacl::ocl::current_device().name() << std::endl;
#endif

    /**
    * Copy the data from the uBLAS objects, compute one matrix-matrix-product as a 'warm up', then take timings:
    **/
    viennacl::copy(ublas_A, vcl_A);
    viennacl::copy(ublas_B, vcl_B);
    vcl_C = viennacl::linalg::prod(vcl_A, vcl_B);
    viennacl::backend::finish();
    timer.start();
    vcl_C = viennacl::linalg::prod(vcl_A, vcl_B);
    viennacl::backend::finish();
    exec_time = timer.get();
    std::cout << " - Execution time on device (no setup time included): " << exec_time << std::endl;

    /**
    * Verify the result
    **/
    viennacl::copy(vcl_C, ublas_C1);

    std::cout << " - Checking result... ";
    bool check_ok = true;
    for (std::size_t i = 0; i < ublas_C.size1(); ++i)
    {
      for (std::size_t j = 0; j < ublas_C.size2(); ++j)
      {
        if ( std::fabs(ublas_C1(i,j) - ublas_C(i,j)) / ublas_C(i,j) > 1e-4 )
        {
          check_ok = false;
          break;
        }
      }
      if (!check_ok)
        break;
    }
    if (check_ok)
      std::cout << "[OK]" << std::endl << std::endl;
    else
      std::cout << "[FAILED]" << std::endl << std::endl;

  }

  /**
  *  That's it. A more extensive benchmark for dense BLAS routines is also available.
  **/
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;
  return EXIT_SUCCESS;
}


////////////////////////////////////////////////////////////////////////////////
#include "intersect_ray.h"
#include "aabb.h"
#include "aabb_binary.h"
#include <Eigen/Dense>
#include <geogram/mesh/mesh_geometry.h>
#include <limits>
#include <numeric>
#undef IGL_STATIC_LIBRARY
#include <igl/ray_mesh_intersect.h>
////////////////////////////////////////////////////////////////////////////////

bool triangle_intersect_ray(const Eigen::Vector3d &a,
                            const Eigen::Vector3d &b,
                            const Eigen::Vector3d &c,
                            const Eigen::Vector3d &ray_origin,
                            const Eigen::Vector3d &ray_direction,
                            Eigen::Vector3d &hit_position,
                            Eigen::Vector3d &hit_normal,
                            double &hit_param)
{
    igl::Hit hit;
    Eigen::Matrix<double, 3, 3> V;
    V.row(0) = a.transpose();
    V.row(1) = b.transpose();
    V.row(2) = c.transpose();
    Eigen::RowVector3i F(0, 1, 2);
    auto ret = igl::ray_mesh_intersect(ray_origin, ray_direction, V, F, hit);
    if (ret) {
        hit_param = hit.t;
        hit_position = ray_origin + hit_param * ray_direction;
        hit_normal = (b - a).cross(c - a).normalized();
    }
    return ret;
}

////////////////////////////////////////////////////////////////////////////////

bool box_intersect_ray(const Eigen::AlignedBox3d &box,
                       const Eigen::Vector3d &ray_origin,
                       const Eigen::Vector3d &ray_direction)
{
    double tmin = 0;
    double tmax = std::numeric_limits<double>::infinity();
    for (int dim = 0; dim < 3; ++dim) {
        if (std::abs(ray_direction(dim)) < 1e-5) {
            if (ray_origin[dim] < box.min()[dim] || ray_origin[dim] > box.max()[dim]) {
                return false;
            }
        }
        else {
            double t1 = (box.min()[dim] - ray_origin[dim]) / ray_direction[dim];
            double t2 = (box.max()[dim] - ray_origin[dim]) / ray_direction[dim];
            tmin = std::max(std::min(t1, t2), tmin);
            tmax = std::min(std::max(t1, t2), tmax);
        }
    }
    return (tmin <= tmax);
}

////////////////////////////////////////////////////////////////////////////////

template <>
bool aabb_intersect_ray<AABBTree>(const Eigen::MatrixXd &V,
                                  const Eigen::MatrixXi &F,
                                  const AABBTree &tree,
                                  const Eigen::Vector3d &ray_origin,
                                  const Eigen::Vector3d &ray_direction,
                                  Eigen::Vector3d &hit_position,
                                  Eigen::Vector3d &hit_normal,
                                  double &hit_param)
{
    return tree.shoot_ray(V, F, ray_origin, ray_direction, hit_position, hit_normal, hit_param);
}

////////////////////////////////////////////////////////////////////////////////

template <>
bool aabb_intersect_ray<AABBTreeBinary>(const Eigen::MatrixXd &V,
                                        const Eigen::MatrixXi &F,
                                        const AABBTreeBinary &tree,
                                        const Eigen::Vector3d &ray_origin,
                                        const Eigen::Vector3d &ray_direction,
                                        Eigen::Vector3d &hit_position,
                                        Eigen::Vector3d &hit_normal,
                                        double &hit_param)
{
    return tree.shoot_ray(V, F, ray_origin, ray_direction, hit_position, hit_normal, hit_param);
}

////////////////////////////////////////////////////////////////////////////////

template <>
bool aabb_intersect_ray<igl::AABB<Eigen::MatrixXd, 3>>(const Eigen::MatrixXd &V,
                                                       const Eigen::MatrixXi &F,
                                                       const igl::AABB<Eigen::MatrixXd, 3> &tree,
                                                       const Eigen::Vector3d &ray_origin,
                                                       const Eigen::Vector3d &ray_direction,
                                                       Eigen::Vector3d &hit_position,
                                                       Eigen::Vector3d &hit_normal,
                                                       double &hit_param)
{
    igl::Hit hit;
    bool ret = tree.intersect_ray(V, F, ray_origin.transpose(), ray_direction.transpose(), hit);
    if (ret) {
        hit_param = hit.t;
        hit_position = ray_origin + hit.t * ray_direction;
        Eigen::Vector3d a = V.row(F.row(hit.id)[0]).transpose();
        Eigen::Vector3d b = V.row(F.row(hit.id)[1]).transpose();
        Eigen::Vector3d c = V.row(F.row(hit.id)[2]).transpose();
        hit_normal = (b - a).cross(c - a).normalized();
    }
    return ret;
}

////////////////////////////////////////////////////////////////////////////////

template <>
bool aabb_intersect_ray<igl::embree::EmbreeIntersector>(const Eigen::MatrixXd &V,
                                                        const Eigen::MatrixXi &F,
                                                        const igl::embree::EmbreeIntersector &tree,
                                                        const Eigen::Vector3d &ray_origin,
                                                        const Eigen::Vector3d &ray_direction,
                                                        Eigen::Vector3d &hit_position,
                                                        Eigen::Vector3d &hit_normal,
                                                        double &hit_param)
{
    igl::Hit hit;
    bool ret = tree.intersectRay(ray_origin.transpose().cast<float>(),
                                 ray_direction.transpose().cast<float>(), hit);
    if (ret) {
        hit_param = hit.t;
        hit_position = ray_origin + hit.t * ray_direction;
        Eigen::Vector3d a = V.row(F.row(hit.id)[0]).transpose();
        Eigen::Vector3d b = V.row(F.row(hit.id)[1]).transpose();
        Eigen::Vector3d c = V.row(F.row(hit.id)[2]).transpose();
        hit_normal = (b - a).cross(c - a).normalized();
    }
    return ret;
}

////////////////////////////////////////////////////////////////////////////////

template <>
bool aabb_intersect_ray<GEO::MeshFacetsAABB>(const Eigen::MatrixXd &V,
                                             const Eigen::MatrixXi &F,
                                             const GEO::MeshFacetsAABB &tree,
                                             const Eigen::Vector3d &ray_origin,
                                             const Eigen::Vector3d &ray_direction,
                                             Eigen::Vector3d &hit_position,
                                             Eigen::Vector3d &hit_normal,
                                             double &hit_param)
{
    // Multiply by big constant because AABB has a segment isect routine
    // (not ray isect routine), so it would ignore intersections further
    // away than (R.origin + R.direction), and we want them!
    GEO::vec3 origin(ray_origin.data());
    GEO::vec3 direction(ray_direction.data());
    GEO::vec3 p2 = origin + 10000.0 * direction;
    double t;
    GEO::index_t f;
    if (tree.segment_nearest_intersection(origin, p2, t, f)) {
        t *= 10000.0;
        if (t > 0) {
            hit_param = t;
            hit_position = ray_origin + t * ray_direction;
            GEO::vec3 normal = GEO::normalize(GEO::Geom::mesh_facet_normal(*tree.mesh(), f));
            hit_normal << normal[0], normal[1], normal[2];
            return true;
        }
    }
    return false;
}


// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @file           coordinate.cpp
*   @brief          飞行器用到的各种坐标系变换。
*   @details        现有：地球坐标系，导航坐标系，机体坐标系。
                    地球坐标系采用WGS-84坐标系各项参数。
					地球空间直角坐标系原点为参考椭球的中心，X轴和Y轴位于赤道平面，X轴通过零子午线，Z轴与椭球极轴一致。
					导航坐标系原点位于参考点，X轴指向北边，Y轴指向东边，Z轴指向地下。
					机体坐标系原点位于质心点，X轴指向机头前方，Y轴指向右侧，Z轴指向下方。
					所有坐标系均为右手系。
*   @author         LiDaiwei
*   @date           20191203
*   @version        1.0.0.1
*   @par Copyright
*                   LiDaiwei
*   @par History
*                   1.0.0.1: LiDaiwei, 20191203, 首次创建
*

*/

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @name           头文件。
*   @{
*/

#include "coordinate.h"
#include <algorithm>
//#include <Eigen/Dense>

#include<iostream>
using namespace std;
using namespace Eigen;
/** @}  */

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          计算重力加速度。
*   @details        计算重力加速度。
*   @param[out]     gravity         精确重力
*   @param[in]      latitude        纬度(角度)
*   @retval         0               正常
*   @retval         1               错误
*/
int accuraty_gravity (
	double* gravity,
	const double latitude)
{
	double L=0;
	deg2rad(&L,latitude);
	*gravity=978.03267714*(1+0.00193185138639*sin(L)*sin(L))/
		sqrt(1-0.00669437999013*sin(L)*sin(L));
	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          计算地球主曲率半径。
*   @details        参考椭球子午圈上各点曲率半径RM和卯酉圈（它所在的平面与子午面垂直）上各点的曲率半径RN称为主曲率半径。
*   @param[out]     RM              子午圈曲率半径
*   @param[out]     RN              卯酉圈曲率半径
*   @param[in]      latitude        纬度(角度)
*   @retval         0               正常
*   @retval         1               错误
*/
int earth_curvature_radius (
	double* RM,
	double* RN,
	const double latitude)
{
	double L=0;
	deg2rad(&L,latitude);
	*RM=earth_radius*(1-2*earth_ellipticity+3*earth_ellipticity*sin(L)*sin(L));
	*RN=earth_radius*(1+earth_ellipticity*sin(L)*sin(L));

	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          地球空间直角坐标系转换到经纬度高度坐标系。
*   @details        地球空间直角坐标系原点为参考椭球的中心，X轴和Y轴位于赤道平面，X轴通过零子午线，Z轴与椭球极轴一致。
*   @param[out]     longitude       经度（角度）
*   @param[out]     latitude        纬度（角度）
*   @param[out]     height          高度
*   @param[in]      x               地球空间直角坐标系X坐标
*   @param[in]      y               地球空间直角坐标系Y坐标
*   @param[in]      z               地球空间直角坐标系Z坐标
*   @retval         0               正常
*   @retval         1               错误
*/
int xyz_to_llh(
	double* longitude,
	double* latitude,
	double* height,
	const double x,
	const double y,
	const double z)
{
	double lon=0,lat=0;
	lon=atan2(y,x);
	//lon = lon < 0 ? (M_PI + lon) : lon;
	rad2deg(longitude,lon);
	//迭代法求纬度和高度
	double HpRN_ip1=0,RN_ip1=0;
	double lat_i=0,lat_ip1=0;
	lat_i=atan(z/(pow((1-earth_ellipticity),2)*sqrt(x*x+y*y)));
	int num = 0;
	do{
		HpRN_ip1=x/(cos(lat_i)*cos(lon));
		RN_ip1=earth_radius/sqrt(pow(cos(lat_i),2)+(1-pow(earth_e1,2))*pow(sin(lat_i),2));
		lat_ip1=atan(HpRN_ip1*z/((HpRN_ip1-RN_ip1*pow(earth_e1,2))*sqrt(x*x+y*y)));
		num++;
	}while (abs( lat_ip1-lat_i)>1e-6 && num<1e3);

	lat=lat_ip1;
	rad2deg(latitude,lat);
	*height=HpRN_ip1-RN_ip1;
	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          经纬度高度坐标系转换到地球空间直角坐标系。
*   @details        地球空间直角坐标系原点为参考椭球的中心，X轴和Y轴位于赤道平面，X轴通过零子午线，Z轴与椭球极轴一致。
*   @param[out]      x               地球空间直角坐标系X坐标
*   @param[out]      y               地球空间直角坐标系Y坐标
*   @param[out]      z               地球空间直角坐标系Z坐标
*   @param[in]       longitude       经度（角度）
*   @param[in]       latitude        纬度（角度）
*   @param[in]       height          高度
*   @retval          0               正常
*   @retval          1               错误
*/
int llh_to_xyz(
	double* x,
	double* y,
	double* z,
	const double longitude,
	const double latitude,
	const double height)
{
	double lon=0,lat=0,RN=0,RM;
	deg2rad(&lon,longitude);
	deg2rad(&lat,latitude);
	earth_curvature_radius(&RM,&RN,latitude);

	*x=(height+RN)*cos(lat)*cos(lon);
	*y=(height+RN)*cos(lat)*sin(lon);
	*z=(RN*pow((1-earth_ellipticity),2)+height)*sin(lat);
	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          地球经纬度坐标系转换到导航坐标系的旋转矩阵。
*   @details        导航坐标系原点位于参考点，X轴指向北边，Y轴指向东边，Z轴指向地下。
*   @param[out]     R_en            旋转矩阵:3x3,正交
*   @param[in]      longitude0      导航坐标系参考点经度(角度)
*   @param[in]      latitude0       导航坐标系参考点纬度(角度)
*   @retval         0               正常
*   @retval         1               错误
*/
int rotation_earth_to_navigation(
	Matrix3d* R_en,
	const double longitude0,
	const double latitude0)
{
	double lon = 0, L = 0;
	deg2rad(&lon, longitude0); deg2rad(&L, latitude0);
	//先转到天东北系
	Matrix3d R_en2;
	R_en2(0, 0) = cos(L) * cos(lon);
	R_en2(0, 1) = cos(L) * sin(lon);
	R_en2(0, 2) = sin(L);
	R_en2(1, 0) = -sin(lon);
	R_en2(1, 1) = cos(lon);
	R_en2(1, 2) = 0;
	R_en2(2, 0) = -sin(L) * cos(lon);
	R_en2(2, 1) = -sin(L) * sin(lon);
	R_en2(2, 2) = cos(L);
	//再从天东北转到北东地
	Matrix3d R_n2n;
	R_n2n << 0, 0, 1,
		0, 1, 0,
		-1, 0, 0;
	*R_en = R_n2n * R_en2;
	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          导航坐标系转换到地球经纬度坐标系的旋转矩阵。
*   @details        导航坐标系原点位于参考点，X轴指向北边，Y轴指向东边，Z轴指向地下。
*   @param[out]     R_ne            旋转矩阵:3x3,正交
*   @param[in]      longitude0      导航坐标系参考点经度(角度)
*   @param[in]      latitude0       导航坐标系参考点纬度(角度)
*   @retval         0               正常
*   @retval         1               错误
*/
int rotation_navigation_to_earth(
	Matrix3d* R_ne,
	const double longitude0,
	const double latitude0)
{
	Matrix3d R_en;
	rotation_earth_to_navigation(&R_en, longitude0, latitude0);
	*R_ne = R_en.transpose();
	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          地球经纬度坐标系转换到导航坐标系。
*   @details        导航坐标系原点位于参考点，X轴指向北边，Y轴指向东边，Z轴指向地面。
*   @param[out]     north            导航坐标系内东向坐标
*   @param[out]     east           导航坐标系内北向坐标
*   @param[out]     downward          导航坐标系内天向坐标
*   @param[in]      longitude       机体经度（角度）
*   @param[in]      latitude        机体纬度（角度）
*   @param[in]      height          机体高度
*   @param[in]      longitude0      导航坐标系参考点经度（角度）
*   @param[in]      latitude0       导航坐标系参考点纬度（角度）
*   @param[in]      height0         导航坐标系参考点高度
*   @retval         0               正常
*   @retval         1               错误
*/
int earth_to_navigation(
	double* north,
	double* east,
	double* downward,
	const double longitude,
	const double latitude,
	const double height,
	const double longitude0,
	const double latitude0,
	const double height0)
{

	Matrix3d R_en;
	rotation_earth_to_navigation(&R_en, longitude0, latitude0);
	Vector3d Pe, Pn, t;
	llh_to_xyz(&Pe(0), &Pe(1), &Pe(2), longitude, latitude, height);
	llh_to_xyz(&t(0), &t(1), &t(2), longitude0, latitude0, height0);

	Pn = R_en * (Pe - t);

	*north = Pn(0);
	*east = Pn(1);
	*downward = Pn(2);
	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          导航坐标系转换到地球经纬度坐标系。
*   @details        导航坐标系原点位于参考点，X轴指向北边，Y轴指向东边，Z轴指向地面。
*   @param[out]     longitude       机体经度（角度）
*   @param[out]     latitude        机体纬度（角度）
*   @param[out]     height          机体高度
*   @param[in]      north            机体导航坐标系内东向坐标
*   @param[in]      east           机体导航坐标系内北向坐标
*   @param[in]      downward          机体导航坐标系内天向坐标
*   @param[in]      longitude0      导航坐标系参考点经度（角度）
*   @param[in]      latitude0       导航坐标系参考点纬度（角度）
*   @param[in]      height0         导航坐标系参考点高度
*   @retval         0               正常
*   @retval         1               错误
*/
int navigation_to_earth(
	double* longitude,
	double* latitude,
	double* height,
	const double north,
	const double east,
	const double downward,
	const double longitude0,
	const double latitude0,
	const double height0)
{
	Matrix3d R_ne;
	rotation_navigation_to_earth(&R_ne, longitude0, latitude0);
	Vector3d Pe, Pn, t;
	Pn << north, east, downward;//在导航系内的坐标
	llh_to_xyz(&t(0), &t(1), &t(2), longitude0, latitude0, height0);
	Pe = R_ne * Pn + t;
	xyz_to_llh(longitude, latitude, height, Pe(0), Pe(1), Pe(2));

	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          由欧拉角得到导航坐标系转换到机体坐标系的旋转矩阵。
*   @details        导航坐标系原点位于参考点，X轴指向北边，Y轴指向东边，Z轴指向地下。
                    机体坐标系原点位于质心点，X轴指向机头，Y轴指向右侧，Z轴指向下方。
*   @param[out]     R_nb            旋转矩阵:3x3,正交
*   @param[in]      roll            机体导航坐标系内横滚角（角度）
*   @param[in]      pitch           机体导航坐标系内俯仰角（角度）
*   @param[in]      yaw             机体导航坐标系内航向角（角度）
*   @retval         0               正常
*   @retval         1               错误
*/
int rotation_navigation_to_body(
	Matrix3d* R_nb,
	const double roll,
	const double pitch,
	const double yaw)
{
	////////先将东-北-天的导航坐标系转到北-东-地的方向
	//////Matrix3d R_nn2;
	//////R_nn2 << 0, 1, 0,
	//////		1, 0, 0,
	//////		0, 0, -1;

	//从北-东-地的导航坐标系以Z（航向）-Y（俯仰）-X（滚转）的顺序旋转到载体系的旋转矩阵：
	double r, p, y;
	deg2rad(&r, roll); deg2rad(&p, pitch); deg2rad(&y, yaw);
	double c1 = cos(y), s1 = sin(y);
	double c2 = cos(p), s2 = sin(p);
	double c3 = cos(r), s3 = sin(r);
	//可查Z1Y2X3顺规旋转矩阵如下：
	Matrix3d R_n2b;
	R_n2b(0, 0) = c1 * c2;
	R_n2b(0, 1) = c1 * s2 * s3 - c3 * s1;
	R_n2b(0, 2) = s1 * s3 + c1 * c3 * s2;
	R_n2b(1, 0) = c2 * s1;
	R_n2b(1, 1) = c1 * c3 + s1 * s2 * s3;
	R_n2b(1, 2) = c3 * s1 * s2 - c1 * s3;
	R_n2b(2, 0) = -s2;
	R_n2b(2, 1) = c2 * s3;
	R_n2b(2, 2) = c2 * c3;

	*R_nb = R_n2b.transpose();// *R_nn2;

	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          机体坐标系转换到导航坐标系的旋转矩阵。
*   @details        导航坐标系原点位于参考点，X轴指向北边，Y轴指向东边，Z轴指向地下。
					机体坐标系原点位于质心点，X轴指向机头，Y轴指向右侧，Z轴指向下方。
*   @param[out]     R_bn            旋转矩阵:3x3,正交
*   @param[in]      roll            机体导航坐标系内横滚角（角度）
*   @param[in]      pitch           机体导航坐标系内俯仰角（角度）
*   @param[in]      yaw             机体导航坐标系内航向角（角度）
*   @retval         0               正常
*   @retval         1               错误
*/
int rotation_body_to_navigation(
	Matrix3d* R_bn,
	const double roll,
	const double pitch,
	const double yaw)
{
	Matrix3d R_nb;
	rotation_navigation_to_body(&R_nb, roll, pitch, yaw);
	*R_bn = R_nb.transpose();

	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          由Rnb旋转矩阵得到欧拉角。
*   @details        Rnb是导航坐标系转换到机体坐标系
                    导航坐标系原点位于参考点，X轴指向北边，Y轴指向东边，Z轴指向地下。
					机体坐标系原点位于质心点，X轴指向机头，Y轴指向右侧，Z轴指向下方。
*   @param[out]     roll            机体导航坐标系内横滚角（角度）
*   @param[out]     pitch           机体导航坐标系内俯仰角（角度）
*   @param[out]     yaw             机体导航坐标系内航向角（角度）
*   @param[in]      R_nb            旋转矩阵:3x3,正交
*   @retval         0               正常
*   @retval         1               错误
*/
int rotation_nb_to_euler(
	double* roll,
	double* pitch,
	double* yaw,
	const Matrix3d R_nb)
{
	double r, p, y;
	//////Matrix3d R_nn2_inv;
	//////R_nn2_inv << 0, 1, 0,
	//////		1, 0, 0,
	//////		0, 0, -1;
	Matrix3d R_n2b = R_nb.transpose();// *R_nn2_inv;
	r = atan2(R_n2b(2,1), R_n2b(2,2));
	p = atan(-R_n2b(2, 0) / sqrt(1 - R_n2b(2, 0) * R_n2b(2, 0)));
	y = atan2(R_n2b(1, 0), R_n2b(0, 0));

	rad2deg(roll, r);
	rad2deg(pitch, p);
	rad2deg(yaw, y);

	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          由方向余弦旋转矩阵求四元数
*   @details        由方向余弦旋转矩阵求四元数
*   @param[out]     q               四元数:4维向量
*   @param[in]      R               方向余弦旋转矩阵:3x3
*   @retval         0               正常
*   @retval         1               错误
*/
int rotation_to_quaternion(
	Vector4d* q,
	const Matrix3d R)
{
	Vector4d __q;
	__q(0) = 0.5 * sqrt(1 + R(0, 0) + R(1, 1) + R(2, 2));
	if (fabs(__q(0)) < 1e-4) {
		if (R(0, 0) > R(1, 1) && R(0, 0) > R(2, 2)) {
			double t = sqrt(1 + R(0, 0) - R(1, 1) - R(2, 2));
			__q(0) = (R(2, 1) - R(1, 2)) / t;
			__q(1) = t / 4;
			__q(2) = (R(0, 2) + R(2, 0)) / t;
			__q(3) = (R(0, 1) + R(1, 0)) / t;
		}
		else if (R(1, 1) > R(0, 0) && R(1, 1) > R(2, 2)) {
			double t = sqrt(1 - R(0, 0) + R(1, 1) - R(2, 2));
			__q(0) = (R(0, 2) - R(2, 0)) / t;
			__q(1) = (R(0, 1) + R(1, 0)) / t;
			__q(2) =  t / 4;
			__q(3) = (R(2, 1) + R(1, 2)) / t;
		}
		else {
			double t = sqrt(1 - R(0, 0) - R(1, 1) + R(2, 2));
			__q(0) = (R(1, 0) - R(0, 1)) / t;
			__q(1) = (R(0, 2) + R(2, 0)) / t;
			__q(2) = (R(1, 2) - R(2, 1)) / t;
			__q(3) =  t / 4;
		}
	}
	else {
		__q(1) = (R(2, 1) - R(1, 2)) / (4 * __q(0));
		__q(2) = (R(0, 2) - R(2, 0)) / (4 * __q(0));
		__q(3) = (R(1, 0) - R(0, 1)) / (4 * __q(0));
	}
	quaterntion_normalized(q, __q);
	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          由四元数求方向余弦旋转矩阵
*   @details        由四元数求方向余弦旋转矩阵
*   @param[out]     R               方向余弦旋转矩阵:3x3
*   @param[in]      q               四元数:4维向量
*   @retval         0               正常
*   @retval         1               错误
*/
int quaternion_to_rotation(
	Matrix3d* R,
	const Vector4d & q)//不加'&'会报错：具有 __declspec(align('16')) 的形参将不被对齐
{
	(*R)(0, 0) = q(0) * q(0) + q(1) * q(1) - q(2) * q(2) - q(3) * q(3);
	(*R)(0, 1) = 2 * (q(1) * q(2) - q(0) * q(3));
	(*R)(0, 2) = 2 * (q(1) * q(3) + q(0) * q(2));
	(*R)(1, 0) = 2 * (q(1) * q(2) + q(0) * q(3));
	(*R)(1, 1) = q(0) * q(0) - q(1) * q(1) + q(2) * q(2) - q(3) * q(3);
	(*R)(1, 2) = 2 * (q(2) * q(3) - q(0) * q(1));
	(*R)(2, 0) = 2 * (q(1) * q(3) - q(0) * q(2));
	(*R)(2, 1) = 2 * (q(2) * q(3) + q(0) * q(1));
	(*R)(2, 2) = q(0) * q(0) - q(1) * q(1) - q(2) * q(2) + q(3) * q(3);

	//Matrix3d R_nn2_inv;
	//R_nn2_inv << 0, 1, 0,
	//	1, 0, 0,
	//	0, 0, -1;
	//(*R) = (*R) * R_nn2_inv;

	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          由欧拉角求从载体系到导航系的转动四元数
*   @details        由欧拉角求从载体系到导航系的转动四元数
*   @param[out]     q               从载体系到导航系的转动四元数:4维向量
*   @param[in]      roll            机体导航坐标系内横滚角（角度）
*   @param[in]      pitch           机体导航坐标系内俯仰角（角度）
*   @param[in]      yaw             机体导航坐标系内航向角（角度）
*   @retval         0               正常
*   @retval         1               错误
*/
int euler_to_quaternion_bn(
	Vector4d* q,
	const double roll,
	const double pitch,
	const double yaw)
{
	Matrix3d Rnb;
	rotation_navigation_to_body(&Rnb, roll, pitch, yaw);
	//cout << R << endl;
	//cin.get();
	Matrix3d Rbn;
	Rbn = Rnb.transpose();
	rotation_to_quaternion(q, Rbn);
	if (fabs(roll) < 1e-4 && fabs(pitch) < 1e-4 && fabs(yaw) < 1e-4) {
		(*q) << 0, 0, 0, 0;
		(*q)(0) = 1;
	}

	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          由从载体系到导航系的转动四元数求欧拉角
*   @details        由从载体系到导航系的转动四元数求欧拉角
*   @param[out]     roll            机体导航坐标系内横滚角（角度）
*   @param[out]     pitch           机体导航坐标系内俯仰角（角度）
*   @param[out]     yaw             机体导航坐标系内航向角（角度）
*   @param[in]      q               四元数:4维向量
*   @retval         0               正常
*   @retval         1               错误
*/
int quaternion_bn_to_euler(
	double* roll,
	double* pitch,
	double* yaw,
	const Vector4d& q)//不加'&'会报错：具有 __declspec(align('16')) 的形参将不被对齐
{
	Matrix3d Rbn;
	quaternion_to_rotation(&Rbn, q);

	rotation_nb_to_euler(roll, pitch, yaw, Rbn.transpose());

	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          四元数归一化
*   @details        四元数归一化
*   @param[out]     qout            归一化后的四元数
*   @param[in]      qin             四元数:4维向量
*   @retval         0               正常
*   @retval         1               错误
*/

int quaterntion_normalized(
	Vector4d* qout,
	const Vector4d& qin)
{

	double q0, q1, q2, q3;

	q0 = qin(0) / qin.norm();
	q1 = qin(1) / qin.norm();
	q2 = qin(2) / qin.norm();
	q3 = qin(3) / qin.norm();
	(*qout)<< q0, q1, q2, q3;

	return 0;
}


#include <vector>
#include <string>
#include <cstdarg>
#include <cstring>
#include <cstddef>
#include <iterator>

#include <boost/lexical_cast.hpp>

#include <pybind11/pybind11.h>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Constrained_Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_conformer_2.h>
#include <CGAL/Delaunay_mesher_2.h>
#include <CGAL/Delaunay_mesh_face_base_2.h>
#include <CGAL/Delaunay_mesh_vertex_base_2.h>
#include <CGAL/Delaunay_mesh_size_criteria_2.h>
#include <CGAL/lloyd_optimize_mesh_2.h>

namespace py = pybind11;

using K = CGAL::Exact_predicates_inexact_constructions_kernel;
using Vb = CGAL::Delaunay_mesh_vertex_base_2<K>;
using Fb = CGAL::Delaunay_mesh_face_base_2<K>;
using Tds = CGAL::Triangulation_data_structure_2<Vb, Fb>;
using CDT = CGAL::Constrained_Delaunay_triangulation_2<K, Tds>;
using Criteria = CGAL::Delaunay_mesh_size_criteria_2<CDT>;
using Mesher = CGAL::Delaunay_mesher_2<CDT, Criteria>;

using Point = CDT::Point;
using Vertex_handle = CDT::Vertex_handle;

template <typename T>
class TypedInputIterator
{
public:
    using iterator_category = std::input_iterator_tag;
    using difference_type = std::ptrdiff_t;
    using value_type = T;
    using pointer = T*;
    using reference = T&;

    explicit TypedInputIterator(py::iterator& py_iter) :
        py_iter_(py_iter)
    {
    }

    explicit TypedInputIterator(py::iterator&& py_iter) :
        py_iter_(py_iter)
    {
    }

    value_type operator*()
    {
        return (*py_iter_).template cast<value_type>();
    }

    TypedInputIterator operator++(int)
    {
        auto copy = *this;
        ++py_iter_;
        return copy;
    }

    TypedInputIterator& operator++()
    {
        ++py_iter_;
        return *this;
    }

    bool operator!=(TypedInputIterator &rhs)
    {
        return py_iter_ != rhs.py_iter_;
    }

    bool operator==(TypedInputIterator &rhs)
    {
        return py_iter_ == rhs.py_iter_;
    }

private:
    py::iterator py_iter_;
};

PYBIND11_MODULE(cgal_mesher, m)
{
    py::class_<Point>(m, "Point")
            .def(py::init<int, int>(),  py::arg("x"), py::arg("y"))
            .def(py::init<double, double>(), py::arg("x"), py::arg("y"))
            .def_property_readonly("x", &Point::x)
            .def_property_readonly("y", &Point::y)
            .def("__repr__",
            [](const Point &p) {
                std::string r("Point(");
                r += boost::lexical_cast<std::string>(p.x());
                r += ", ";
                r += boost::lexical_cast<std::string>(p.y());
                r += ")";
                return r;
            })
            ;

    py::class_<Vertex_handle>(m, "VertexHandle");

    py::class_<CDT>(m, "ConstrainedDelaunayTriangulation")
            .def(py::init())
            .def("insert", [](CDT & cdt, const Point & p) { return cdt.insert(p); })
            .def("insert_constraint",
                 [](CDT & cdt, Vertex_handle a, Vertex_handle b)
                 {
                     cdt.insert_constraint(a, b);
                 })
            .def("remove", &CDT::remove)
            .def("number_of_vertices", &CDT::number_of_vertices)
            .def("number_of_faces", &CDT::number_of_faces)
            .def("vertices", [](CDT & cdt) -> py::iterator
                 {
                     return py::make_iterator(cdt.vertices_begin(), cdt.vertices_end());
                 })
            ;

    m.def("make_conforming_delaunay", &CGAL::make_conforming_Delaunay_2<CDT>,
          py::arg("cdt")
    );

    m.def("make_conforming_gabriel", &CGAL::make_conforming_Gabriel_2<CDT>,
        py::arg("cdt")
    );

    py::class_<Criteria>(m, "Criteria")
            .def(py::init<double, double>(),
                 py::arg("aspect_bound") = 0.125,
                 py::arg("size_bound") = 0.0)
            .def_property("size_bound", &Criteria::size_bound, &Criteria::set_size_bound)
            .def_property("aspect_bound",
                          [](const Criteria & c) { c.bound(); },
                          [](Criteria & c, double bound) { c.set_bound(bound); })
            ;

    py::class_<Mesher>(m, "Mesher")
            .def(py::init<CDT&>())
            .def("refine_mesh", &Mesher::refine_mesh)
            .def_property(
                "criteria",
                &Mesher::get_criteria,
                [](Mesher& mesher, const Criteria & criteria)
                {
                    mesher.set_criteria(criteria);
                }
            )
            .def_property_readonly("seeds", [](Mesher& mesher) {
                return py::make_iterator(mesher.seeds_begin(), mesher.seeds_end());
            })
            .def("seeds_from", [](Mesher & mesher, py::iterable iterable, bool mark)
            {
                py::iterator iterator = py::iter(iterable);
                TypedInputIterator<Point> points_begin(iterator);
                TypedInputIterator<Point> points_end(py::iterator::sentinel());
                mesher.set_seeds(points_begin, points_end, mark);
            })
            .def("clear_seeds", &Mesher::clear_seeds)
            ;

    m.def("lloyd_optimize", [](
        CDT& cdt,
        int max_iteration_number,
        double time_limit,
        double convergence,
        double freeze_bound)
        {
            CGAL::lloyd_optimize_mesh_2(cdt,
                CGAL::parameters::max_iteration_number = max_iteration_number,
                CGAL::parameters::time_limit = time_limit,
                CGAL::parameters::convergence = convergence,
                CGAL::parameters::freeze_bound = freeze_bound
            );
        },
          py::arg("cdt"),
          py::arg("max_iteration_number") = 0,
          py::arg("time_limit") = 0.0,
          py::arg("convergence") = 0.001,
          py::arg("freeze_bound") = 0.001
    );
}


/*
 * Copyright (c) 2016 Abhishek Agrawal (abhishek.agrawal@protonmail.com)
 * Distributed under the MIT License.
 * See accompanying file LICENSE.md or copy at http://opensource.org/licenses/MIT
 */

// This program will make use of the ATOM solver to construct a transfer trajectory
// between two points in space. Only a single transfer segment is simulated here, that is, no multitargeting.
// There is just one departure and one arrival object considered in the simulation. The object TLEs are
// taken from a catalog file. The user can specify which object will be departure and which will be arrival
// along with the time of flight.

#include <iostream>
#include <sstream>
#include <stdexcept>
#include <string>
#include <vector>
#include <fstream>
#include <exception>
#include <cstdlib>
#include <iterator>

#include <libsgp4/Globals.h>
#include <libsgp4/SGP4.h>
#include <libsgp4/Tle.h>

#include <Atom/atom.hpp>
#include <Atom/convertCartesianStateToTwoLineElements.hpp>

#include <Astro/orbitalElementConversions.hpp>

#include <SML/sml.hpp>
#include <SML/constants.hpp>
#include <SML/basicFunctions.hpp>
#include <SML/linearAlgebra.hpp>

#include <boost/array.hpp>

#include </usr/local/abhi/pykep/src/lambert_problem.cpp>
#include </usr/local/abhi/pykep/src/lambert_problem.h>
#include </usr/local/abhi/pykep/src/keplerian_toolbox.h>

typedef double Real;
typedef std::vector < Real > Vector6;
typedef std::vector < Real > Vector3;
typedef std::vector < Real > Vector2;
typedef std::vector < std::vector < Real > > Vector2D;
typedef boost::array < Real, 3 > array3;

//! Remove newline characters from string.
void removeNewline( std::string& string )
{
    string.erase( std::remove( string.begin( ), string.end( ), '\r' ), string.end( ) );
    string.erase( std::remove( string.begin( ), string.end( ), '\n' ), string.end( ) );
}

//! Convert SGP4 ECI object to state vector.
Vector6 getStateVector( const Eci state )
{
    Vector6 result( 6 );
    result[ 0 ] = state.Position( ).x;
    result[ 1 ] = state.Position( ).y;
    result[ 2 ] = state.Position( ).z;
    result[ 3 ] = state.Velocity( ).x;
    result[ 4 ] = state.Velocity( ).y;
    result[ 5 ] = state.Velocity( ).z;
    return result;
}

Vector6 getStateVectorInMetre( const Eci state )
{
    Vector6 result( 6 );
    result[ 0 ] = state.Position( ).x * 1000.0;
    result[ 1 ] = state.Position( ).y * 1000.0;
    result[ 2 ] = state.Position( ).z * 1000.0;
    result[ 3 ] = state.Velocity( ).x * 1000.0;
    result[ 4 ] = state.Velocity( ).y * 1000.0;
    result[ 5 ] = state.Velocity( ).z * 1000.0;
    return result;
}

Vector6 standardKeplerian( const Vector6 elements )
{
    Vector6 result( 6 );
    result[ 0 ] = elements[ 0 ] / 1000.0;
    result[ 1 ] = elements[ 1 ];
    result[ 2 ] = sml::convertRadiansToDegrees( elements[ 2 ] );
    result[ 3 ] = sml::convertRadiansToDegrees( elements[ 3 ] );
    result[ 4 ] = sml::convertRadiansToDegrees( elements[ 4 ] );
    result[ 5 ] = sml::convertRadiansToDegrees( elements[ 5 ] );
    return result;
}

int main( void )
{
    // conversion from km to m
    // const double km2m = 1000;
    const double Rearth = kXKMPER; // earth radius in km
    // earth radius and diameter
    // const double EarthRadius = kXKMPER * km2m; // unit m
    // const double EarthDiam = 2 * EarthRadius;

    // grav. parameter 'mu' of earth
    const double muEarth = kMU*( pow( 10, 9 ) ); // unit m^3/s^2

    // vectors to store arrival and departure velocities for the transfer trajectory
    Vector3 DepartureVelocity( 3 );
    Vector3 ArrivalVelocity( 3 );

    // read the TLE file. Line based parsing, using string streams
    std::string line;

    // std::ifstream tlefile( "../../src/napa_prograde_catalog.txt" );
    // const bool is_retro = false;

    std::ifstream tlefile( "../../src/napa_retrograde_catalog.txt" );
    const bool is_retro = true;

    if( !tlefile.is_open() )
        perror("error while opening file");

    std::vector < Tle > tleObjects; // vector of TLE objects

    while( !tlefile.eof( ) )
    {
        std::vector < std::string > tleStrings;

        std::getline( tlefile, line ); // read line from the catalog file
        removeNewline( line ); // remove new line characters such as '/n'
        tleStrings.push_back( line );
        // std::cout << line << std::endl;

        std::getline( tlefile, line );
        removeNewline( line );
        tleStrings.push_back( line );
        // std::cout << line << std::endl;

        std::getline( tlefile, line );
        removeNewline( line );
        tleStrings.push_back( line );
        // std::cout << line << std::endl << std::endl;

        tleObjects.push_back( Tle( tleStrings[ 0 ], tleStrings[ 1 ], tleStrings[ 2 ] ) );
    }
    tlefile.close( );

    const int DebrisObjects = tleObjects.size( );
    std::cout << "Total debris objects = " << DebrisObjects << std::endl;

    // some variables for the "catch" segment
    int failCount = 0;
    int catchDepartureID;
    int catchArrivalID;

    std::cout.precision( 15 );

    //*********************** Inputs *********************************************************************************************************//
    Tle departureObject = tleObjects[ 2 ];
    Tle arrivalObject = tleObjects[ 3 ];
    DateTime departureEpoch;
    // departureEpoch.Initialise( 2016, 1, 11, 4, 51, 21, 455936 );
    departureEpoch.Initialise( 2016, 1, 11, 4, 50, 21, 0 );
    const double TOF = 15130.0; // time of flight
    //***************************************************************************************************************************************//

    SGP4 sgp4Departure( departureObject );
    const Eci tleDepartureState = sgp4Departure.FindPosition( departureEpoch );
    const Vector6 departureState = getStateVector( tleDepartureState );

    array3 departurePosition;
    array3 departureVelocity;
    for( int j = 0; j < 3; j++ )
    {
        departurePosition[ j ] = departureState[ j ];
        departureVelocity[ j ] = departureState[ j + 3 ];
    }

    const int departureObjectId = static_cast< int >( departureObject.NoradNumber( ) );
    catchDepartureID = departureObjectId; // for the catch segment of the program

    SGP4 sgp4Arrival( arrivalObject );
    const int arrivalObjectId = static_cast< int >( arrivalObject.NoradNumber( ) );
    catchArrivalID = arrivalObjectId;

    const DateTime arrivalEpoch = departureEpoch.AddSeconds( TOF );
    const Eci tleArrivalState = sgp4Arrival.FindPosition( arrivalEpoch );
    // const Eci tleArrivalState = sgp4Arrival.FindPosition( 0.0 );
    const Vector6 arrivalState = getStateVector( tleArrivalState );

    array3 arrivalPosition;
    array3 arrivalVelocity;
    for( int j = 0; j < 3; j++ )
    {
        arrivalPosition[ j ] = arrivalState[ j ];
        arrivalVelocity[ j ] = arrivalState[ j + 3 ];
    }

    kep_toolbox::lambert_problem targeter( departurePosition, arrivalPosition, TOF, kMU, is_retro, 5 );
    const int numberOfSolutions = targeter.get_v1( ).size( );
    std::vector< array3 > departureDeltaVs( numberOfSolutions ); // delta-V components at the departure point
    std::vector< array3 > arrivalDeltaVs( numberOfSolutions );
    std::vector< Real > transferDeltaVs( numberOfSolutions ); // magnitude of the total delta-V of one transfer between two points

    for ( int j = 0; j < numberOfSolutions; j++ )
    {
        array3 transferDepartureVelocity = targeter.get_v1( )[ j ]; // velocity of the s/c at the departure point in the transfer orbit
        array3 transferArrivalVelocity = targeter.get_v2( )[ j ];

        departureDeltaVs[ j ] = sml::add( transferDepartureVelocity, sml::multiply( departureVelocity, -1.0 ) );
        arrivalDeltaVs[ j ] = sml::add( transferArrivalVelocity, sml::multiply( arrivalVelocity, -1.0 ) );

        transferDeltaVs[ j ] = sml::norm< Real >( departureDeltaVs[ j ] ) + sml::norm< Real >( arrivalDeltaVs[ j ] );
    }

    const std::vector< Real >::iterator minDeltaVIterator = std::min_element( transferDeltaVs.begin( ), transferDeltaVs.end( ) );
    const int minimumDeltaVIndex = std::distance( transferDeltaVs.begin( ), minDeltaVIterator );

    Real lambertDepartureBurn = sml::norm< Real >( departureDeltaVs[ minimumDeltaVIndex ] );
    Real lambertArrivalBurn = sml::norm< Real >( arrivalDeltaVs[ minimumDeltaVIndex ] );
    Real lambertDV = transferDeltaVs[ minimumDeltaVIndex ];

    array3 minIndexDepartureVelocity = targeter.get_v1( )[ minimumDeltaVIndex ]; // best guess for velocity in transfer orbit at the departure point

    Vector6 lambertDepState( 6 );
    for ( int i = 0; i < 3; i++ )
    {
        lambertDepState[ i ] = departurePosition [ i ] * 1000.0;
        lambertDepState[ i + 3 ] = minIndexDepartureVelocity[ i ] * 1000.0;
    }
    const Real tolerance = 10.0 * std::numeric_limits< Real >::epsilon( );
    Vector6 LambertKep( 6 );
    LambertKep = astro::convertCartesianToKeplerianElements( lambertDepState, muEarth, tolerance );
    Vector6 LambertKepStd = standardKeplerian( LambertKep );
    std::cout << std::endl << "LambertKep = " << LambertKepStd << std::endl << std::endl;

    Vector3 departureVelocityGuess( 3 );
    Vector3 atomDeparturePosition( 3 );
    Vector3 atomArrivalPosition( 3 );

    for( int j = 0; j < 3; j++ )
    {
        departureVelocityGuess[ j ] = minIndexDepartureVelocity[ j ];
        atomDeparturePosition[ j ] = departurePosition[ j ];
        atomArrivalPosition[ j ] = arrivalPosition[ j ];
    }

    array3 atomDepartureVelocity;
    array3 atomArrivalVelocity;

    std::string SolverStatusSummary;
    int numberOfIterations;
    const int maxIterations = 100;
    const Tle referenceTle = Tle( );

    try
    {
        Vector3 outputDepartureVelocity( 3 );
        Vector3 outputArrivalVelocity( 3 );
        atom::executeAtomSolver< Real, Vector3 >( atomDeparturePosition,
                                                  departureEpoch,
                                                  atomArrivalPosition,
                                                  TOF,
                                                  departureVelocityGuess,
                                                  outputDepartureVelocity,
                                                  outputArrivalVelocity,
                                                  SolverStatusSummary,
                                                  numberOfIterations,
                                                  referenceTle,
                                                  kMU,
                                                  kXKMPER,
                                                  1.0e-10,
                                                  1.0e-5,
                                                  maxIterations );

        Vector6 atomDepState( 6 );
        for ( int i = 0; i < 3; i++ )
        {
            atomDepState[ i ] = departurePosition[ i ] * 1000.0;
            atomDepState[ i + 3 ] = outputDepartureVelocity[ i ] * 1000.0;
        }
        Vector6 atomDepKep = astro::convertCartesianToKeplerianElements( atomDepState, muEarth, tolerance );
        Vector6 atomDepKepStd( 6 );
        atomDepKepStd = standardKeplerian( atomDepKep );
        std::cout << std::endl << "atom dep kep = " << atomDepKepStd << std::endl << std::endl;

        for( int k = 0; k < 3; k++)
        {
            atomDepartureVelocity[ k ] = outputDepartureVelocity[ k ];
            atomArrivalVelocity[ k ] = outputArrivalVelocity[ k ];
        }

        array3 atomDepartureDeltaV;
        array3 atomArrivalDeltaV;
        Real AtomDeltaV;

        atomDepartureDeltaV = sml::add( atomDepartureVelocity, sml::multiply( departureVelocity, -1.0 ) );
        atomArrivalDeltaV = sml::add( atomArrivalVelocity, sml::multiply( arrivalVelocity, -1.0 ) );

        double AtomDepartureBurn = sml::norm< Real >( atomDepartureDeltaV );
        double AtomArrivalBurn = sml::norm< Real >( atomArrivalDeltaV );
        AtomDeltaV = sml::norm< Real >( atomDepartureDeltaV ) + sml::norm< Real >( atomArrivalDeltaV );
        std::cout << "Atom DeltaV = " << AtomDeltaV << std::endl;
    }

    catch( const std::exception& err )
    {
        ++failCount;
        std::cout << "Exception Caught = " << err.what( ) << std::endl;
        std::cout << "For departure ID = " << catchDepartureID << " " << "For Arrival ID = " << catchArrivalID << std::endl << std::endl;
        // std::cout << "Fail count = " << failCount << std::endl;
    }

   return EXIT_SUCCESS;
}


#include <iostream>
#include <vector>
#include <algorithm>
#include <math.h>
#include <map>
#include <unordered_map>
#include <numeric>
#include <Eigen>
#include <time.h>
using namespace std;

// 平面
enum Plane
{
	XOY,
	XOZ,
	YOZ
};

// 空间直方图计算
// 法向量的视点坐标
struct Normal
{
	double x, y, z;
};
struct Point3D
{
	double x, y, z;
	Normal normal;
	double angle;
	// 重点：重载“<”，作为map的键
	bool operator<(const Point3D & p)const {
		return (x < p.x) || (x == p.x && y < p.y);
	}

};

struct Point2D
{
	float x, y;
	bool operator<(const Point2D & p)const {
		return (x < p.x) || (x == p.x && y < p.y);
	}

};

struct Boundary
{
	double xMin, xMax;
	double yMin, yMax;
	double zMin, zMax;
};

ostream & operator<<(ostream & out, Point2D A) {

	out << "[" << A.x << "," << A.y << "]";
	return out;
}

ostream & operator<<(ostream & out, Point3D A) {

	out << "[" << A.x << "," << A.y << "," << A.z << "]";
	return out;
}
ostream & operator<<(ostream & out, Normal A) {

	out << "[" << A.x << "," << A.y << "," << A.z << "]";
	return out;
}
// 读取文件点云数据
vector<Point2D> getData(const char* filename, Plane intestingPlane, vector<float> &boundaryValue) {

	FILE *fp_txt1;

	fp_txt1 = fopen(filename, "r");

	Point3D TxtPoint1;
	Point2D tempData;
	vector<Point2D> pointCloudData;

	vector<float> x_data, y_data;

	if (fp_txt1)
	{
		// 获取要统计的平面的点云
		switch (intestingPlane) {
		case XOY:
			while (fscanf(fp_txt1, "%lf %lf %lf", &TxtPoint1.x, &TxtPoint1.y, &TxtPoint1.z) != EOF)
			{

				tempData.x = TxtPoint1.x;
				x_data.push_back(TxtPoint1.x);
				y_data.push_back(TxtPoint1.y);
				tempData.y = TxtPoint1.y;
				pointCloudData.push_back(tempData);
			}
			break;
		case XOZ:
			while (fscanf(fp_txt1, "%lf %lf %lf", &TxtPoint1.x, &TxtPoint1.y, &TxtPoint1.z) != EOF)
			{

				tempData.x = TxtPoint1.x;
				tempData.y = TxtPoint1.z;
				x_data.push_back(TxtPoint1.x);
				y_data.push_back(TxtPoint1.z);
				pointCloudData.push_back(tempData);
			}
			break;
		case YOZ:
			while (fscanf(fp_txt1, "%lf %lf %lf", &TxtPoint1.x, &TxtPoint1.y, &TxtPoint1.z) != EOF)
			{

				tempData.x = TxtPoint1.y;
				tempData.y = TxtPoint1.z;
				x_data.push_back(TxtPoint1.y);
				y_data.push_back(TxtPoint1.z);
				pointCloudData.push_back(tempData);
			}
			break;
		default:
			break;
		}

	}
	else{

		cout << "txt数据加载失败！" << endl;
	}
	sort(x_data.begin(), x_data.end());
	sort(y_data.begin(), y_data.end());
	boundaryValue.push_back(x_data[0]);
	boundaryValue.push_back(x_data[x_data.size() - 1]);
	boundaryValue.push_back(y_data[0]);
	boundaryValue.push_back(y_data[y_data.size() - 1]);
	return pointCloudData;
}

// 统计各个单位点云数目,
map<Point2D, int> countPointCloud(vector<Point2D> pointCloudDatas, vector<float> boudaryData, int segNum) {
	// 区间数10*10

	float xSegValue = (boudaryData[1] - boudaryData[0]) / segNum;
	float ySegValue = (boudaryData[3] - boudaryData[2]) / segNum;
	//cout << "xSegValue: " << xSegValue << " ySegValue: " << ySegValue << endl;
	map<Point2D, int> mpPointData;
	int tenSum = 0;
	// 判断属于哪个区间，并统计该区间点云的个数
	for (Point2D & pointCloudData : pointCloudDatas) {
		Point2D tempData;

		// 值减去最小值再除以区间间隔值，判断属于哪个区间
		float xTemp = (pointCloudData.x - boudaryData[0]) / xSegValue;
		float yTemp = (pointCloudData.y - boudaryData[2]) / ySegValue;
		tempData.x = floorf(xTemp);
		tempData.y = floorf(yTemp);
		if (tempData.x == segNum || tempData.y == segNum) {
			tenSum += 1;

		}
		else {
			mpPointData[tempData] += 1;
		}

	}
	int sum = 0;
	for (auto it = mpPointData.begin(); it != mpPointData.end(); it++) {
		sum += it->second;
	}
	return mpPointData;
}

// 计算相似度
float cacular2Similarity(map<Point2D,int> data1, map<Point2D,int> data2, int segNum) {
	// 存储100个单元的点云数
	vector<int> unitPointNum1(segNum * segNum, 0);
	vector<int> unitPointNum2(segNum * segNum, 0);
	// 临时存储当前遍历的位置
	Point2D curLoc;
	int k = 0;

	// 提取出100个单元中每个单元的点云数
	for (int i = 0; i < segNum; ++i) {
		for (int j = 0; j < segNum; ++j) {
			curLoc.x = i;
			curLoc.y = j;
			if (data1.find(curLoc) != data1.end()) {
				unitPointNum1[k] = data1[curLoc];
			}
			if (data2.find(curLoc) != data2.end()) {
				unitPointNum2[k] = data2[curLoc];
			}
			k++;
		}
	}

	int sumNum1 = accumulate(unitPointNum1.begin(), unitPointNum1.end(), 0);
	int sumNum2 = accumulate(unitPointNum2.begin(), unitPointNum2.end(), 0);
	double similarity = 0;
	for (int i = 0; i < segNum * segNum; ++i) {
		similarity += sqrt(((double)unitPointNum1[i] / sumNum1) * ((double)unitPointNum2[i] / sumNum2));
	}
	return similarity;
}

//二维平面栅格化后，相似度比较
void similarity2D(const char *billboardFile, const char *geometricFile) {

	vector<Plane> Planes{ XOY, XOZ, YOZ };
	vector<float> similarities(3);
	int segNum = 20;
	for (int i = 0; i < 3; i++)
	{
		vector<float> b_boudaryData;
		vector<float> g_boudaryData;
		vector<Point2D> billboardData = getData(billboardFile, Planes[i], b_boudaryData);
		vector<Point2D> geometricData = getData(geometricFile, Planes[i], g_boudaryData);

		// 2D网格点云数目统计信息
		auto billboardDataCount = countPointCloud(billboardData, b_boudaryData, segNum);
		auto geometricDataCount = countPointCloud(geometricData, g_boudaryData, segNum);

		// 计算相似度
		similarities[i] = cacular2Similarity(billboardDataCount, geometricDataCount, segNum);
		cout << similarities[i] << endl;
	}

	cout.setf(ios::fixed);
	cout.setf(ios::showpoint);
	cout.precision(4);

	float average = (float)accumulate(similarities.begin(), similarities.end(), 0.0f) / 3;
	cout << "========================相似度信息=========================" << endl;
	for (float i : similarities) {
		cout << i << " ";
	}
	cout << average << endl;
}

// 获取三维点云的数据，遍历点云获得边界X、Y、Z的最大，最小值
 vector<Point3D> get3Data(const char* filename, Boundary &boundary) {

	 FILE *fp_txt1;

	 fp_txt1 = fopen(filename, "r");

	 Point3D TxtPoint;
	 vector<Point3D>pointData;
	 vector<double> x_data, y_data, z_data;
	 while (fscanf(fp_txt1, "%lf %lf %lf %lf %lf %lf", &TxtPoint.x, &TxtPoint.y, &TxtPoint.z,
													   &TxtPoint.normal.x, &TxtPoint.normal.y, &TxtPoint.normal.z) != EOF)
	 {
		 x_data.push_back(TxtPoint.x);
		 y_data.push_back(TxtPoint.y);
		 z_data.push_back(TxtPoint.z);
		 pointData.push_back(TxtPoint);
	 }
	 cout << "=========================统计数据======================" << endl;
	 cout << "3D点云数据数目: " << pointData.size() << endl;

	 // 统计边界
	 sort(x_data.begin(), x_data.end());
	 sort(y_data.begin(), y_data.end());
	 sort(z_data.begin(), z_data.end());
	 boundary.xMin = x_data[0]; boundary.xMax = x_data[x_data.size() - 1];
	 boundary.yMin = y_data[0]; boundary.yMax = y_data[y_data.size() - 1];
	 boundary.zMin = z_data[0]; boundary.zMax = z_data[z_data.size() - 1];

	 return pointData;
}

 // 统计各个网格内的数据，网格化
map<Point3D, vector<Point3D>> countMeshData(vector<Point3D> &pointDatas, Boundary boundary, int meshNum) {

	double xMeshValue = (boundary.xMax - boundary.xMin) / meshNum;
	double yMeshValue = (boundary.yMax - boundary.yMin) / meshNum;
	double zMeshValue = (boundary.zMax - boundary.zMin) / meshNum;
	// 键为网格坐标，键值为该网格内的点云
	map<Point3D, vector<Point3D>> meshData;
	Point3D tempData;
	for (auto & pointData : pointDatas) {
		// 值减去最小值再除以区间间隔值，判断属于哪个区间
		double xTemp = (pointData.x - boundary.xMin) / xMeshValue;
		double yTemp = (pointData.y - boundary.yMin) / yMeshValue;
		double zTemp = (pointData.z - boundary.zMin) / zMeshValue;
		tempData.x = floor(xTemp);
		tempData.y = floor(yTemp);
		tempData.z = floor(zTemp);
		if (tempData.x == meshNum || tempData.y == meshNum || tempData.z == meshNum) {
			// 边界处理
			auto xtemp = tempData.x == meshNum ? tempData.x - 1 : tempData.x;
			tempData.x = xtemp;
			auto ytemp = tempData.y == meshNum ? tempData.y - 1 : tempData.y;
			tempData.y = ytemp;
			auto ztemp = tempData.z == meshNum ? tempData.z - 1 : tempData.z;
			tempData.z = ztemp;
		}
		// 数据存储
		meshData[tempData].push_back(pointData);
	}
	int pointSum = 0;
	cout << "网格数量：" << meshData.size() << endl;
	return meshData;
}

int main() {

	const char* billboardFile = "billboard.txt";
	const char* geometricFile = "geometric.txt";
	similarity2D(billboardFile, geometricFile);
	system("pause");
	return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TOOLBOX_ELLIPTIC_FUNCTIONS_SCALAR_ELLPE_HPP_INCLUDED
#define NT2_TOOLBOX_ELLIPTIC_FUNCTIONS_SCALAR_ELLPE_HPP_INCLUDED
#include <nt2/toolbox/elliptic/functions/ellpe.hpp>
#include <boost/math/special_functions.hpp>
#include <nt2/include/constants/digits.hpp>
#include <nt2/include/constants/real.hpp>
#include <nt2/toolbox/trigonometric/constants.hpp>
#include <nt2/toolbox/polynomials/functions/scalar/impl/horner.hpp>
#include <nt2/include/functions/scalar/is_ltz.hpp>
#include <nt2/include/functions/scalar/is_eqz.hpp>
#include <nt2/include/functions/scalar/oneminus.hpp>
#include <nt2/include/functions/scalar/sqrt.hpp>
#include <nt2/include/functions/scalar/log.hpp>
#include <nt2/sdk/error/policies.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::ellpe_, tag::cpu_
                            , (A0)
                            , (scalar_< arithmetic_<A0> >)
                            )
  {

    typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;

    NT2_FUNCTOR_CALL(1)
    {
      return nt2::ellpe(result_type(a0));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is double
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::ellpe_, tag::cpu_
                            , (A0)
                            , (scalar_< double_<A0> >)
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL(1)
    {
      typedef result_type type;
      if (a0>One<A0>()||(is_ltz(a0))) return Nan<type>();
      if (is_eqz(a0))  return One<type>();
      if (a0 == One<A0>()) return Pio_2<type>();
      return boost::math::ellint_2(sqrt(oneminus(type(a0))), nt2_policy());
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is float
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::ellpe_, tag::cpu_
                            , (A0)
                            , (scalar_< single_<A0> >)
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
    {
      if (a0>One<A0>()||(is_ltz(a0))) return Nan<A0>();
      if (is_eqz(a0))  return One<A0>();
      if (a0 == One<A0>()) return Pio_2<A0>();
      A0 tmp1 = horner< NT2_HORNER_COEFF(float, 11,
                              (0x392102f5,
                               0x3b246c1b,
                               0x3c0e578f,
                               0x3c2fe240,
                               0x3bfebca9,
                               0x3bf882cf,
                               0x3c3d8b3f,
                               0x3cb2d89a,
                               0x3d68ac90,
                               0x3ee2e430,
                               0x3f800000) ) > (a0);
      A0 tmp2 = horner< NT2_HORNER_COEFF(float, 10,
                                    (0x38098de4,
                                     0x3a84557e,
                                     0x3bd53114,
                                     0x3c8a54f6,
                                     0x3cd67118,
                                     0x3d0925e1,
                                     0x3d2ef92b,
                                     0x3d6fffe9,
                                     0x3dc00000,
                                     0x3e800000
                                     ) ) > (a0);
      return tmp1-nt2::log(a0)*a0*tmp2;
    }
  };
} }

#endif


#include <algorithm>
#include <boost/optional.hpp>
#include <climits>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <functional>
#include <iostream>
#include <limits>
#include <list>
#include <map>
#include <numeric>
#include <queue>
#include <sstream>
#include <string>
#include <tuple>
#include <type_traits>
#include <unordered_map>
#include <utility>
#include <vector>

#define REP(i, n) for(int i = 0, i##_MACRO = (n); i < i##_MACRO; i++)
#define RANGE(i, a, b) for(int i = (a), i##_MACRO = (b); i < i##_MACRO; i++)
#define EACH(e, a) for(auto&& e : a)
#define ALL(a) (a).begin(), (a).end()
#define AALL(a, n) (a), ((a) + (n))
#define FILL(a, n) memset((a), n, sizeof(a))
#define FILLZ(a) FILL(a, 0)
#define INT(x) (static_cast<int>(x))
#define MODNUM (INT(1e9 + 7))
#define MOD(x) ((x) % MODNUM)

using namespace std;

using ll = long long;
using VI = vector<int>;
using VI2D = vector<vector<int>>;

const int INF = 2e9;
const double EPS = 1e-10;
const double PI = acos(-1.0);

const int dx[] = {-1, 0, 1, 0};
const int dy[] = {0, -1, 0, 1};

template <typename T>
int sign(T x) {
	return x < 0 ? -1 : x > 0 ? 1 : 0;
}

template <>
int sign(double x) {
	return x < -EPS ? -1 : x > EPS ? 1 : 0;
}

template <typename T>
void chmax(T& m, T x) {
	m = max(m, x);
}

template <typename T>
void chmin(T& m, T x) {
	m = min(m, x);
}

template <typename T>
T square(T x) {
	return x * x;
}

inline int toInt(string s) {
	int v;
	istringstream sin(s);
	sin >> v;
	return v;
}

inline long long mod_pow(long long a, long long n, long long mod) {
	long long ret = 1;
	while(n != 0) {
		if(n % 2) {
			ret = (ret * a) % mod;
		}
		a = (a * a) % mod, n /= 2;
	}
	return ret;
}

// modの逆元
inline long long mod_inv(long long n, long long mod) {
	return mod_pow(n, mod - 2, mod);
}

// エラトステネスの篩
template <typename T>
inline std::vector<T> sieve_of_eratosthenes(T n) {
	std::vector<T> sieve(n, 0);
	for(int i = 2; i < n; i++) {
		sieve[i] = i;
	}
	T i = 2;
	while(i * i < n) {
		if(sieve[i]) {
			for(T j = i * i; j < n; j += i) {
				sieve[j] = 0;
			}
		}
		i++;
	}
	return sieve;
}

// 素数リスト
template <typename T>
inline std::vector<T> prime_list(T n) {
	std::vector<T> primes = sieve_of_eratosthenes(n);
	primes.erase(std::remove(primes.begin(), primes.end(), 0), primes.end());
	return primes;
}

// 素因数分解(素数表を用いる)
inline std::unordered_map<long long, int>
factor(long long n, std::vector<long long>& primes) {
	std::unordered_map<long long, int> factors;
	for(int i = 0; primes[i] * primes[i] <= n; i++) {
		int j = 0;
		while(n % primes[i] == 0) {
			n /= primes[i];
			j++;
		}
		if(j != 0) {
			factors[primes[i]] = j;
		}
	}
	if(n != 1) {
		factors[n] = 1;
	}
	return factors;
}

// vectorの要素すべての最小公倍数をmodで割った余りを返す
template <typename T>
inline long long
mod_lcm(std::vector<T>& v, std::vector<long long>& primes, long long mod) {
	int n = v.size();
	std::unordered_map<long long, int> lcm_factors;
	for(int i = 0; i < n; i++) {
		std::unordered_map<long long, int> factors = factor(v[i], primes);
		for(const std::pair<long long, int>& factor : factors) {
			if(lcm_factors[factor.first] < factor.second) {
				lcm_factors[factor.first] = factor.second;
			}
		}
	}
	long long retval = 1;
	for(const std::pair<long long, int>& factor : lcm_factors) {
		retval = (retval * mod_pow(factor.first, factor.second, mod)) % mod;
	}
	return retval;
}

int main() {
	int n;
	scanf("%d", &n);
	VI a(n);
	REP(i, n) { scanf("%d", &a[i]); }
	vector<ll> primes = prime_list<ll>(1e6 + 100);
	ll lcmall = mod_lcm(a, primes, MODNUM);
	ll result = 0;
	REP(i, n) { result = MOD(result + MOD(lcmall * mod_inv(a[i], MODNUM))); }
	printf("%lld\n", result);
	return 0;
}


#include <iostream>
#include <vector>

#include <boost/scope_exit.hpp>

#include <amgcl/backend/builtin.hpp>
#include <amgcl/value_type/static_matrix.hpp>
#include <amgcl/adapter/crs_tuple.hpp>
#include <amgcl/mpi/distributed_matrix.hpp>
#include <amgcl/io/mm.hpp>

#include <amgcl/profiler.hpp>

namespace amgcl {
    profiler<> prof;
}

namespace math = amgcl::math;

template <class Val>
void assemble(
        int n, int beg, int end,
        std::vector<int> &ptr,
        std::vector<int> &col,
        std::vector<Val> &val
        )
{
    int chunk = end - beg;

    ptr.clear(); ptr.reserve(chunk + 1); ptr.push_back(0);
    col.clear(); col.reserve(chunk * 4);
    val.clear(); val.reserve(chunk * 4);

    for(int j = beg, i = 0; j < end; ++j, ++i) {
        if (j > 0) {
            col.push_back(j - 1);
            val.push_back(-math::identity<Val>());
        }

        col.push_back(j);
        val.push_back(2 * math::identity<Val>());

        if (j+1 < n) {
            col.push_back(j+1);
            val.push_back(-math::identity<Val>());
        }

        if (j+5 < n) {
            col.push_back(j+5);
            val.push_back(-0.1 * math::identity<Val>());
        }

        ptr.push_back(col.size());
    }
}

template <class Val>
void test() {
    typedef typename math::rhs_of<Val>::type Rhs;

    amgcl::mpi::communicator comm(MPI_COMM_WORLD);

    int n = 16;
    int chunk_len = (n + comm.size - 1) / comm.size;
    int chunk_beg = std::min(n, chunk_len * comm.rank);
    int chunk_end = std::min(n, chunk_len * (comm.rank + 1));
    int chunk = chunk_end - chunk_beg;

    std::vector<int> chunks(comm.size);
    MPI_Allgather(&chunk, 1, MPI_INT, &chunks[0], 1, MPI_INT, comm);
    std::vector<int> displ(comm.size, 0);
    for(int i = 1; i < comm.size; ++i)
        displ[i] = displ[i-1] + chunks[i-1];

    std::vector<int>    ptr;
    std::vector<int>    col;
    std::vector<Val> val;
    std::vector<Rhs> x(chunk);
    std::vector<Rhs> y(chunk);

    assemble(n, chunk_beg, chunk_end, ptr, col, val);

    for(int i = 0; i < chunk; ++i) x[i] = math::constant<Rhs>(drand48());

    typedef amgcl::backend::builtin<Val> Backend;
    typedef amgcl::mpi::distributed_matrix<Backend> Matrix;

    Matrix A(comm, std::tie(chunk, ptr, col, val), chunk);

    auto B = amgcl::mpi::product(A, A);
    B->move_to_backend();

    amgcl::backend::spmv(1, *B, x, 0, y);

    std::vector<Rhs> X(n), R(n);
    MPI_Gatherv(&x[0], chunk, amgcl::mpi::datatype<Rhs>(), &X[0], &chunks[0], &displ[0], amgcl::mpi::datatype<Rhs>(), 0, comm);
    MPI_Gatherv(&y[0], chunk, amgcl::mpi::datatype<Rhs>(), &R[0], &chunks[0], &displ[0], amgcl::mpi::datatype<Rhs>(), 0, comm);

    if (comm.rank == 0) {
        std::vector<Rhs> Y(n);
        assemble(n, 0, n, ptr, col, val);

        amgcl::backend::crs<Val> A( std::tie(n, ptr, col, val) );
        amgcl::backend::spmv(1, *amgcl::backend::product(A, A), X, 0, Y);

        double s = 0;
        for(int i = 0; i < n; ++i) {
            double d = math::norm(R[i] - Y[i]);
            s += d * d;
        }
        std::cout << "Error: " << s << std::endl;
    }
}

int main(int argc, char *argv[]) {
    MPI_Init(&argc, &argv);
    BOOST_SCOPE_EXIT(void) {
        MPI_Finalize();
    } BOOST_SCOPE_EXIT_END

    test< double >();
    test< amgcl::static_matrix<double,2,2> >();
}


/**
 * @file clipper.cpp
 * @brief Core CLIPPER algorithm: find dense clusters w.r.t constraints
 * @author Parker Lusk <plusk@mit.edu>
 * @date 3 October 2020
 */

#include <chrono>
#include <iostream>
#include <functional>
#include <limits>
#include <queue>
#include <random>
#include <utility>
#include <vector>

#include <Eigen/Sparse>

#include "clipper/find_dense_cluster.h"
#include "clipper/utils.h"

namespace clipper {

inline void homotopy(Eigen::MatrixXd& Md, const Eigen::MatrixXd& M, const Eigen::MatrixXd& Cb, double d)
{
  // if (M.cols() < 2600) {
    Md = M - d*Cb;
  //   return;
  // }

// #pragma omp parallel for default(none) shared(Md, M, d, Cb)
//   for (size_t c=0; c<M.cols(); ++c) {
//     Md.col(c) = M.col(c) - d*Cb.col(c);
//   }
}

template <typename T, IsEigenBase<T>>
Solution findDenseCluster(const T& _M, const T& C,
                          const Eigen::VectorXd& u0, const Params& params)
{
  const auto t1 = std::chrono::high_resolution_clock::now();
  //
  // Initialization
  //

  const size_t n = _M.cols();

  // Zero out any entry corresponding to an active constraint
  const Eigen::MatrixXd M = _M.cwiseProduct(C);

  // Binary complement of constraint matrix
  const Eigen::MatrixXd Cb = Eigen::MatrixXd::Ones(n,n) - C;

  // one step of power method to have a good scaling of u
  Eigen::VectorXd u = M * u0;
  u /= u.norm();

  // initial value of d
  double d = 0; // zero if there are no active constraints
  Eigen::MatrixXd Cbu = Cb * u;
  const auto idxD = ((Cbu.array()>params.eps) && (u.array()>params.eps));
  if (idxD.sum() > 0) {
    Eigen::MatrixXd Mu = M * u;
    const Eigen::VectorXd num = idxD.select(Mu, std::numeric_limits<double>::infinity());
    const Eigen::VectorXd den = idxD.select(Cbu, 1);
    d = (num.array() / den.array()).minCoeff();
  }

  Eigen::MatrixXd Md = Eigen::MatrixXd(M.rows(), M.cols());
  homotopy(Md, M, Cb, d);

  // initialize memory
  Eigen::VectorXd gradF = Eigen::VectorXd(n);
  Eigen::VectorXd unew = Eigen::VectorXd(n);
  Eigen::VectorXd Mu = Eigen::VectorXd(n);
  Eigen::VectorXd num = Eigen::VectorXd(n);
  Eigen::VectorXd den = Eigen::VectorXd(n);

  //
  // Orthogonal projected gradient ascent with homotopy
  //

  double F = 0; // objective value

  size_t i, j, k; // iteration counters
  for (i=0; i<params.maxoliters; ++i) {
    F = u.transpose() * Md * u; // current objective value

    //
    // Orthogonal projected gradient ascent
    //

    for (j=0; j<params.maxiniters; ++j) {
      gradF = Md * u;

      // if (params.orthogonal) {
      //   // orthogonal projection of gradient onto tangent plane to S^n at u
      //   gradF = gradF - (gradF.transpose() * u) * u;

      //   if (gradF.norm() < params.tol_Fop) break;
      // }

      // double alpha = params.alpha;
      // if (alpha <= 0) {
      //   const auto idxA = ((gradF.array()<-params.eps) && (u.array()>params.eps));
      //   if (idxA.sum()) {
      //     const Eigen::VectorXd num = idxA.select(u, std::numeric_limits<double>::infinity());
      //     const Eigen::VectorXd den = idxA.select(gradF, 1);
      //     alpha = (num.array() / den.array()).abs().minCoeff();
      //   } else {
      //     alpha = std::pow(1.0/params.beta, 3) / gradF.norm();
      //   }
      // }

      double alpha = 1;

      //
      // Backtracking line search on gradient ascent
      //

      double Fnew = 0, deltaF = 0;
      for (k=0; k<params.maxlsiters; ++k) {
        unew = u + alpha * gradF;                     // gradient step
        unew = unew.cwiseMax(0);                      // project onto positive orthant
        unew.normalize();                             // project onto S^n
        Fnew = unew.transpose() * Md * unew;          // new objective value after step
        deltaF = Fnew - F;                            // change in objective value

        if (deltaF < -params.eps) {
          // objective value decreased---we need to backtrack, so reduce step size
          alpha = alpha * params.beta;
        } else {
          break; // obj value increased, stop line search
        }
      }
      const double deltau = (unew - u).norm();

      // update values
      F = Fnew;
      u = unew;

      // check if desired accuracy has been reached by gradient ascent
      if (deltau < params.tol_u || std::abs(deltaF) < params.tol_F) break;
    }

    //
    // Increase d
    //

    Cbu = Cb * u;
    const auto idxD = ((Cbu.array() > params.eps) && (u.array() > params.eps));
    if (idxD.sum() > 0) {
      Mu = M * u;
      num = idxD.select(Mu, std::numeric_limits<double>::infinity());
      den = idxD.select(Cbu, 1);
      const double deltad = (num.array() / den.array()).abs().minCoeff();

      d += deltad;
      homotopy(Md, M, Cb, d);

    } else {
      break;
    }
  }

  //
  // Generate output
  //

  // estimate cluster size using largest eigenvalue
  const int omega = std::round(F);

  // extract indices of nodes in identified dense cluster
  std::vector<int> I = utils::findIndicesOfkLargest(u, omega);

  const auto t2 = std::chrono::high_resolution_clock::now();
  const auto duration = std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1);
  const double elapsed = static_cast<double>(duration.count()) / 1e9;

  Solution soln;
  soln.t = elapsed;
  soln.ifinal = i;
  std::swap(soln.nodes, I);
  soln.u.swap(u);
  soln.score = F;

  return soln;
}

template <typename T, IsEigenBase<T>>
Solution findDenseCluster(const T& M, const T& C,
                          const Params& params)
{
  return findDenseCluster(M, C, utils::randvec(M.cols()), params);
}

// template specializations for dense and sparse Eigen matrices
template Solution findDenseCluster<Eigen::MatrixXd>(const Eigen::MatrixXd&,
            const Eigen::MatrixXd&, const Eigen::VectorXd&, const Params&);
template Solution findDenseCluster<SpMat>(const SpMat&,
            const SpMat&, const Eigen::VectorXd&, const Params&);
template Solution findDenseCluster<Eigen::MatrixXd>(const Eigen::MatrixXd&,
            const Eigen::MatrixXd&, const Params&);
template Solution findDenseCluster<SpMat>(const SpMat&,
            const SpMat&, const Params&);

// methods for sparse matrices from scoreSparseConsistency

// sparse method similar to the above with the difference that M,C have zero
// elements along the diagonal
Solution findDenseClusterOfSparseGraph(const SpMat &M, const SpMat &C,
                                       const Eigen::VectorXd &u0,
                                       const Params &params) {
  // std::cout << "number of threads : " << Eigen::nbThreads() << std::endl;
  const auto t1 = std::chrono::high_resolution_clock::now();
  //
  // Initialization
  //

  const size_t n = M.cols();

  // un-necessary compute
  // Zero out any entry corresponding to an active constraint
  // const Eigen::MatrixXd M = _M.cwiseProduct(C);

  // this needs to be replaced
  // Binary complement of constraint matrix
  // const Eigen::MatrixXd Cb = Eigen::MatrixXd::Ones(n, n) - C;
  const Eigen::VectorXd ones = Eigen::VectorXd::Ones(n);

  // one step of power method to have a good scaling of u
  Eigen::VectorXd u = M * u0 + u0; // since M here is not diagonal
  // Eigen::VectorXd u = u0;
  u /= u.norm();

  // initial value of d
  double d = 0; // zero if there are no active constraints
  // Eigen::MatrixXd Cbu = Cb * u;
  Eigen::MatrixXd Cbu = ones * u.sum() - C * u - u;
  const auto idxD = ((Cbu.array() > params.eps) && (u.array() > params.eps));
  if (idxD.sum() > 0) {
    Eigen::MatrixXd Mu = M * u + u;
    const Eigen::VectorXd num =
        idxD.select(Mu, std::numeric_limits<double>::infinity());
    const Eigen::VectorXd den = idxD.select(Cbu, 1);
    d = (num.array() / den.array()).minCoeff();
  }

  // this should be replaced with an efficient representation
  // Md = M - d * Cb;
  // Eigen::MatrixXd Md = Eigen::MatrixXd(M.rows(), M.cols());
  // homotopy(Md, M, Cb, d);

  // initialize memory
  Eigen::VectorXd gradF = Eigen::VectorXd(n);
  Eigen::VectorXd gradFnew = Eigen::VectorXd(n);
  Eigen::VectorXd unew = Eigen::VectorXd(n);
  Eigen::VectorXd Mu = Eigen::VectorXd(n);
  Eigen::VectorXd num = Eigen::VectorXd(n);
  Eigen::VectorXd den = Eigen::VectorXd(n);

  //
  // Orthogonal projected gradient ascent with homotopy
  //

  double F = 0; // objective value

  size_t i, j, k; // iteration counters
  for (i = 0; i < params.maxoliters; ++i) {
    // F = u.transpose() * Md * u; // current objective value
    // gradF = (1 + d) * (M * u + u) - d * ones * u.sum();
    gradF = (1 + d) * u - d * ones * u.sum() + M * u + d * C * u;
    F = u.dot(gradF);
    // std::cout << std::endl;
    // std::cout << "obj F: " << F << " - dval: " << d << std::endl;
    //
    // Orthogonal projected gradient ascent
    //

    for (j = 0; j < params.maxiniters; ++j) {
      // gradF = Md * u;

      // if (params.orthogonal) {
      //   // orthogonal projection of gradient onto tangent plane to S^n at u
      //   gradF = gradF - (gradF.transpose() * u) * u;

      //   if (gradF.norm() < params.tol_Fop) break;
      // }

      // double alpha = params.alpha;
      // if (alpha <= 0) {
      //   const auto idxA = ((gradF.array()<-params.eps) &&
      //   (u.array()>params.eps)); if (idxA.sum()) {
      //     const Eigen::VectorXd num = idxA.select(u,
      //     std::numeric_limits<double>::infinity()); const Eigen::VectorXd den
      //     = idxA.select(gradF, 1); alpha = (num.array() /
      //     den.array()).abs().minCoeff();
      //   } else {
      //     alpha = std::pow(1.0/params.beta, 3) / gradF.norm();
      //   }
      // }

      double alpha = 1;

      //
      // Backtracking line search on gradient ascent
      //

      double Fnew = 0, deltaF = 0;
      for (k = 0; k < params.maxlsiters; ++k) {
        unew = u + alpha * gradF; // gradient step
        unew = unew.cwiseMax(0);  // project onto positive orthant
        unew.normalize();         // project onto S^n
        // Fnew = unew.transpose() * Md * unew; // new objective value after
        // step
        // gradFnew = (1 + d) * (M * unew + unew) - d * ones * unew.sum();
        gradFnew =
            (1 + d) * unew - d * ones * unew.sum() + M * unew + d * C * unew;
        Fnew = unew.dot(gradFnew);

        deltaF = Fnew - F; // change in objective value

        if (deltaF < -params.eps) {
          // objective value decreased---we need to backtrack, so reduce step
          // size
          alpha = alpha * params.beta;
        } else {
          // std::cout << "breaking at " << k << " out of ls " <<
          // params.maxlsiters
          //           << std::endl;
          break; // obj value increased, stop line search
        }
      }
      const double deltau = (unew - u).norm();

      // std::cout << "Fnew: " << Fnew << " Unew: " << unew.sum() << std::endl;
      // update values
      F = Fnew;
      u = unew;
      // gradF = gradFnew;

      // check if desired accuracy has been reached by gradient ascent
      if (deltau < params.tol_u || std::abs(deltaF) < params.tol_F) {
        // std::cout << "breaking at " << j << " out of in " <<
        // params.maxiniters
        //           << std::endl;
        break;
      }
    }

    //
    // Increase d
    //

    // Cbu = Cb * u;
    Cbu = ones * u.sum() - C * u - u;
    const auto idxD = ((Cbu.array() > params.eps) && (u.array() > params.eps));
    if (idxD.sum() > 0) {
      Mu = M * u + u;
      num = idxD.select(Mu, std::numeric_limits<double>::infinity());
      den = idxD.select(Cbu, 1);
      const double deltad = (num.array() / den.array()).abs().minCoeff();

      // std::cout << "delta d:" << deltad << std::endl;
      d += deltad;
      // homotopy(Md, M, Cb, d);

    } else {
      // std::cout << "breaking at " << i << " out of oo " << params.maxoliters
      //           << std::endl;
      break;
    }
  }
  //
  // Generate output
  //

  // estimate cluster size using largest eigenvalue
  const int omega = std::round(F);

  // extract indices of nodes in identified dense cluster
  std::vector<int> I = utils::findIndicesOfkLargest(u, omega);

  const auto t2 = std::chrono::high_resolution_clock::now();
  const auto duration =
      std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1);
  const double elapsed = static_cast<double>(duration.count()) / 1e9;

  Solution soln;
  soln.t = elapsed;
  soln.ifinal = i;
  std::swap(soln.nodes, I);
  soln.u.swap(u);
  soln.score = F;

  return soln;
}

Solution findDenseClusterOfSparseGraph(const SpMat &M, const SpMat &C,
                                       const Params &params) {
  return findDenseClusterOfSparseGraph(M, C,
                                       C * Eigen::VectorXd::Ones(C.cols()) +
                                           Eigen::VectorXd::Ones(C.cols()),
                                       params);
}

} // ns clipper