#include <Eigen/Core>
#include <list>
#include <map>
#include <random>
#include "rose499/world.hpp"

namespace geom = boost::geometry;

using Point = World::Point;
using TaggedPoint = std::pair< Point, int >;

namespace
{
    std::mt19937_64 engine(0x1EA5ECED);
}

std::list< Point > planRRT(     World::RTree knownWorld,
                                double xMax,
                                double yMax,
                                Point initialPoint,
                                double direction,
                                Point goal,
                                double radius )
{
    std::uniform_real_distribution<> samplerX(0, xMax), samplerY(0, yMax);

    TaggedPoint lastPoint(initialPoint, 0);
    double closestDist = std::max(xMax, yMax);

    geom::index::rtree< TaggedPoint, geom::index::linear<5, 1> > setPoints;
    std::map< uint32_t, uint32_t > backEdges;
    std::vector< TaggedPoint > listPoints;
    std::vector< TaggedPoint > setNearest;
    std::vector< std::pair<World::Box, int> > setCollisions;

    setPoints.insert( lastPoint );
    listPoints.push_back( lastPoint );
    int indTaggedPoint = 1;
    while( Eigen::Vector2d( lastPoint.first.get<0>() - goal.get<0>(),
                            lastPoint.first.get<1>() - goal.get<1>() ).norm() >= radius )
    {
        setNearest.clear();
        setCollisions.clear();

        std::normal_distribution<> importanceSamplerX(goal.get<0>(), closestDist),
                                   importanceSamplerY(goal.get<1>(), closestDist);
        auto x = importanceSamplerX(engine);
        auto y = importanceSamplerY(engine);

      if( x > xMax || x < 0 || y > yMax || y < 0 ) { continue; }

        auto randomPoint = TaggedPoint(Point(x, y), indTaggedPoint);
        Eigen::Vector2d eigRandomPoint(randomPoint.first.get<0>(), randomPoint.first.get<1>());

        // Just make sure this isn't a crap point that is inside an obstacle.
        knownWorld.query( geom::index::intersects(randomPoint.first), std::back_inserter(setCollisions));

      if( !setCollisions.empty() ) { continue; }

        // find nearest point in our set
        setPoints.query( geom::index::nearest(randomPoint.first, 10), std::back_inserter(setNearest) );

        // Filter out points that are either too close OR do not fan out from the source point.
        auto endSetNearest =
            std::remove_if(
                setNearest.begin(),
                setNearest.end(),
                [direction, eigRandomPoint, &backEdges, &listPoints]( TaggedPoint nearest ) -> bool {
                    Eigen::Vector2d eigNearest(nearest.first.get<0>(), nearest.first.get<1>());
                    Eigen::Vector2d eigFrom(std::cos(direction), std::sin(direction));
                    Eigen::Vector2d vecLeaving = eigRandomPoint - eigNearest;
                    Eigen::Vector2d vecEntering = eigFrom;

                    if( nearest.second != 0 )
                    {
                        auto fromPoint = listPoints[backEdges[nearest.second]];
                        eigFrom = Eigen::Vector2d(fromPoint.first.get<0>(), fromPoint.first.get<1>());
                        vecEntering = eigNearest - eigFrom;
                    }

                    double proj = vecLeaving.dot(vecEntering) / (vecLeaving.norm() * vecEntering.norm());

                    return (vecLeaving.dot(vecEntering) < 0)
                        || (std::acos(proj) > std::atan2(1, 1))
                        ;//|| (vecLeaving.norm() < 1);
                }
            );
        setNearest.erase(endSetNearest, setNearest.end());

      if( setNearest.empty() ) { continue; }

        std::sort(  setNearest.begin(),
                    setNearest.end(),
                    [eigRandomPoint](TaggedPoint a, TaggedPoint b) -> bool {
                        return  (Eigen::Vector2d(a.first.get<0>(), a.first.get<1>()) - eigRandomPoint).squaredNorm()
                                <
                                (Eigen::Vector2d(b.first.get<0>(), b.first.get<1>()) - eigRandomPoint).squaredNorm();
                    } );
        TaggedPoint nearestPoint = setNearest.front();

        // gotta make sure the path is obstacle free
        geom::model::segment< World::Point > lineTo(nearestPoint.first, randomPoint.first);
        knownWorld.query( geom::index::intersects(lineTo), std::back_inserter(setCollisions));

      if( !setCollisions.empty() ) { continue; }

        closestDist = std::min( closestDist,
                        std::max( std::abs(nearestPoint.first.get<0>() - goal.get<0>()),
                                  std::abs(nearestPoint.first.get<1>() - goal.get<1>()) )
                      );

        setPoints.insert(randomPoint);
        listPoints.push_back(randomPoint);
        backEdges[randomPoint.second] = nearestPoint.second;
        indTaggedPoint++;
        lastPoint = randomPoint;
    }

    // Build path from back edges
    std::list< Point > path;
    int tag = 0;
    while( (tag = lastPoint.second) != 0 )
    {
        path.push_front(lastPoint.first);
        lastPoint = listPoints[backEdges[tag]];
    }
    path.push_front(lastPoint.first);
    return path;
}


#pragma once
#include <random>
#include <boost/range/algorithm_ext/erase.hpp>

std::mt19937_64 mt_grid(265278465287);

#define PATH_TO_GRID_POINTS_SOURCE "GRID_POINTS_SOURCE"
std::vector<std::tuple<theta_<double>,phi_<double> > > generate_random_theta_phi();
std::vector<std::list<size_t> > get_network(const std::vector<std::tuple<theta_<double>,phi_<double> > >& ps);

inline Vector3D S2R (const std::tuple<theta_<double>,phi_<double> >& p)
{
   return Vector3D
      (
         std::sin(std::get<theta_<double>>(p).value())*std::cos(std::get<phi_<double> >(p).value()),
         std::sin(std::get<theta_<double>>(p).value())*std::sin(std::get<phi_<double> >(p).value()),
         std::cos(std::get<theta_<double>>(p).value())
      );
};

std::vector<std::tuple<theta_<double>,phi_<double> > > generate_random_theta_phi()
{
   {//load it
      std::vector< std::tuple<theta_<double>,phi_<double> > > loaded_random_points;
      try
      {
         Getline gl(PATH_TO_GRID_POINTS_SOURCE);
         std::string tmp;
         while(gl.is_open())
         {
            tmp=gl.get();
            std::vector<std::string> vs;
            boost::algorithm::split(vs,tmp,boost::is_any_of(" "));
            loaded_random_points.push_back
               (
                  {
                     theta_<double>(boost::lexical_cast<double> (vs.at(0))),
                     phi_<double>(  boost::lexical_cast<double> (vs.at(1)))
                  }
               );
         }
      }catch(...)
      {
      }
      if(loaded_random_points.size()==N_GRID_POINTS)
      {
         return loaded_random_points;
      }
   }

   std::vector< std::tuple<theta_<double>,phi_<double> > > random_points(N_GRID_POINTS);
   std::uniform_real_distribution<double> dist_theta(0.0,  M_PI);
   std::uniform_real_distribution<double> dist_phi(  0.0,2*M_PI);
   for(size_t i=0;i<N_GRID_POINTS;++i)
   {
      std::get<theta_<double> >(random_points.at(i)).value() = dist_theta(mt_grid);
      std::get<phi_<double>   >(random_points.at(i)).value() = dist_phi(mt_grid);
   }

   const auto LJ = [](const double& distance)->double
   {
      const auto res = std::pow(1.0/distance,6);
      return res;
   };

   const auto E_system = [&]
   (
      bool is_virtual, size_t pos=0,
      std::tuple<theta_<double>,phi_<double> > p = std::tuple<theta_<double>,phi_<double> >(0.0,0.0)
   )->double
   {
      double E=0.0;
      for(size_t i=0,size=random_points.size();i<size;++i)
      for(size_t j=i+1;j<size;++j)
      {
         const std::tuple<theta_<double>,phi_<double> >& p_i = (!is_virtual)?(random_points.at(i)):((pos==i)?p:random_points.at(i));
         const std::tuple<theta_<double>,phi_<double> >& p_j = (!is_virtual)?(random_points.at(j)):((pos==j)?p:random_points.at(j));
         E+=LJ((S2R(p_i)-S2R(p_j)).norm());
      }
      return E;
   };

   constexpr int N_STEP    = 10000*N_GRID_POINTS;
   constexpr int DOWN_STEP = N_STEP/10;
   double kT = 1000.0;
   double kT_=kT/10;
   double E_current = E_system(false);
   constexpr double DELTA_THETA =   M_PI/100;
   constexpr double DELTA_PHI   = 2*M_PI/100;
   std::uniform_int_distribution<int>     dist_pos(0,N_GRID_POINTS-1);
   std::uniform_real_distribution<double> dist_delta_theta(-DELTA_THETA,+DELTA_THETA);
   std::uniform_real_distribution<double> dist_delta_phi(  -DELTA_PHI,  +DELTA_PHI);
   std::uniform_real_distribution<double> dist_p(0,1.0);
   int s_=0;
   for(int s=0;s<N_STEP;++s)
   {
      if((s_%DOWN_STEP)==0){s_=0;kT-=kT_;}
      const int pos = dist_pos(mt_grid);
      const auto stock = random_points.at(pos);
      const std::tuple<theta_<double>,phi_<double> > pnew
         (
            std::get<theta_<double> >(stock).value()+dist_delta_theta(mt_grid),
            std::get<phi_<double>   >(stock).value()+dist_delta_phi(mt_grid)
         );
      const double E_new = E_system(true,pos,pnew);
      const double DE    = E_new-E_current;
      if(DE<=0.0)
      {
         random_points.at(pos)=pnew;
         E_current=E_new;
      }
      else
      {
         if((std::abs(kT)>0.1)&&(std::exp(-DE/kT)<dist_p(mt_grid)))
         {
            random_points.at(pos)=pnew;
            E_current=E_new;
         }
      }
   }
   std::ofstream ofs(PATH_TO_GRID_POINTS_SOURCE,std::ios::trunc);
   boost::format fmt("%1.15e %1.15e\n");
   for(auto p : random_points)
   {
      ofs<<fmt %(std::get<theta_<double> >(p).value()) %(std::get<phi_<double> >(p).value());
   }

   return random_points;
}

std::vector<std::list<size_t> > get_network
(
   const std::vector<std::tuple<theta_<double>,phi_<double> > >& ps
)
{
   const std::vector<Vector3D> P = [ps]()
   {
      std::vector<Vector3D> res;
      for(size_t i=0,size=ps.size();i<size;++i)
      {
         res.push_back(S2R(ps.at(i)));
      }
      return res;
   }();
   std::list<size_t> marvericks = [&P]()
   {
      std::list<size_t> res;
      for(size_t i=1,size=P.size();i<size;++i)
      {
         res.push_back(i);
      }
      return res;
   }();
   std::list<std::tuple<size_t,size_t> > graph;
   const auto find_nearest_neighbor = [&P](size_t pos)
   {
      double min = DBL_MAX;
      size_t min_pos;
      for(size_t i=0,size=P.size();i<size;++i)
      {
         if(pos!=i)
         {
            const double distance = (P.at(pos)-P.at(i)).norm2();
            if(distance<min){min=distance;min_pos=i;}
         }
      }
      return min_pos;
   };
   const size_t first_nbr=find_nearest_neighbor(0);
   graph.push_back(std::tuple<size_t,size_t>(0,first_nbr));
   graph.push_back(std::tuple<size_t,size_t>(first_nbr,0));
   boost::remove_erase_if(marvericks, [first_nbr](const auto& x) { return x==first_nbr; });

   const auto find_next_pair = [&graph,&P,&marvericks]()->std::tuple<size_t,size_t>
   {
      double min_rad = DBL_MAX;
      size_t min_pos_g; size_t min_pos_m;
      for(auto it=graph.begin();it!=graph.end();++it)
      {
         const Vector3D& pg = P.at(std::get<0>(*it));
         for(auto it2=marvericks.begin();it2!=marvericks.end();++it2)
         {
            const Vector3D& pm = P.at(*it2);
            const double d2 = (pg-pm).norm2();
            const double rad =
            std::acos(1.0-d2*0.5)+((std::sqrt(d2)>std::sqrt(2.0))?M_PI:0);
            if(min_rad>rad)
            {
               min_rad=rad;
               min_pos_g=std::get<0>(*it);
               min_pos_m=*it2;
            }
         }
      }
      return {min_pos_g,min_pos_m};
   };

   while(!marvericks.empty())
   {
      const auto [i,j] = find_next_pair();
      graph.push_back(std::tuple<size_t,size_t>(i,j));
      graph.push_back(std::tuple<size_t,size_t>(j,i));
      boost::remove_erase_if(marvericks, [j=j](const auto& x) { return x==j; });
   }

   std::vector<std::list<size_t> > result;
   for(size_t i=0,size=P.size();i<size;++i)
   {
      std::list<size_t> nbrs;
      for(auto it=graph.begin();it!=graph.end();++it)
      {
         if(i==std::get<0>(*it)){nbrs.push_back(std::get<1>(*it));}
      }
      result.push_back(nbrs);
   }

   return result;
}


/* Copyright (c) 2017, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 *
 * All rights reserved.
 *
 * The Astrobee platform is licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with the
 * License. You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations
 * under the License.
 */

#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include <Eigen/Eigenvalues>

#include <vector>
#include <iostream>
//#include <ros/ros.h>

using namespace Eigen;

int compute_delta_state_and_cov(float* residual_in, int error_in, float* H_in,
        int H_rows, int H_cols, unsigned int augs_bitmask, float* R_mat_in, float* P_in,
        float* delta_state_out_out, float* P_out_out) {
  //struct timeval secs1, secs2;
  //gettimeofday(&secs1, 0);
  Map<MatrixXf> H_full(H_in, H_rows, H_cols);
  Map<VectorXf> residual_full(residual_in, H_rows);
  Map<MatrixXf> R_mat_full(R_mat_in, H_rows, H_rows);
  Map<MatrixXf> P(P_in, H_cols, H_cols);
  Map<VectorXf> delta_state_out(delta_state_out_out, H_cols);
  Map<MatrixXf> P_out(P_out_out, H_cols, H_cols);

  if (error_in) {
    delta_state_out.setZero();
    P_out = P;
    //ROS_INFO("compute_delta_state stop 1");
    return 1;
  }

  std::vector<int> used_augs;
  for (int i = 0; i < (H_cols - 15) / 6; i++)
    if (augs_bitmask & (1 << i))
      used_augs.push_back(i);
  VectorBlock<Map<VectorXf> > residual = residual_full.segment(0, used_augs.size() * 6);
  Block<Map<MatrixXf> > R_mat = R_mat_full.block(0, 0, used_augs.size() * 6, used_augs.size() * 6);
  Block<Map<MatrixXf> > reduced_H = H_full.block(0, 0, used_augs.size() * 6, used_augs.size() * 6);
  MatrixXf reduced_P(used_augs.size() * 6, used_augs.size() * 6);
  for (int i = 0; i < used_augs.size(); i++)
    for (int j = 0; j < used_augs.size(); j++)
      reduced_P.block<6, 6>(6 * i, 6 * j) = P.block<6, 6>(15 + 6 * used_augs[i], 15 + 6 * used_augs[j]);
  MatrixXf reduced_cols_P(H_cols, used_augs.size() * 6);
  for (int i = 0; i < used_augs.size(); i++)
    reduced_cols_P.block(0, 6 * i, H_cols, 6) = P.block(0, 15 + 6 * used_augs[i], H_cols, 6);
  //gettimeofday(&secs2, 0);
  //ROS_INFO("compute_delta_state %d create Ps", (((secs2.tv_sec - secs1.tv_sec) * 1000) + (secs2.tv_usec - secs1.tv_usec)));
  //secs1 = secs2;

  MatrixXf T(reduced_cols_P.rows(), reduced_H.rows()), S(reduced_H.rows(), reduced_H.rows());
  T.noalias() = reduced_cols_P * reduced_H.transpose();
  //gettimeofday(&secs2, 0);
  //ROS_INFO("compute_delta_state %d multiplies0", (((secs2.tv_sec - secs1.tv_sec) * 1000) + (secs2.tv_usec - secs1.tv_usec)));
  //secs1 = secs2;
  S.triangularView<Lower>() = reduced_H * reduced_P * reduced_H.transpose();
  S.triangularView<Lower>() += R_mat;
  //gettimeofday(&secs2, 0);
  //ROS_INFO("compute_delta_state %d multiplies1", (((secs2.tv_sec - secs1.tv_sec) * 1000) + (secs2.tv_usec - secs1.tv_usec)));
  //secs1 = secs2;

  LDLT<MatrixXf,Lower> chol(S);
  if (chol.info() != Success) {
    fprintf(stderr, "S not positive definite.\n");
    delta_state_out.setZero();
    P_out = P;
    //ROS_INFO("compute_delta_state stop 2");
    return 1;
  }
  //gettimeofday(&secs2, 0);
  //ROS_INFO("compute_delta_state %d cholesky", (((secs2.tv_sec - secs1.tv_sec) * 1000) + (secs2.tv_usec - secs1.tv_usec)));
  //secs1 = secs2;
  MatrixXf sinv = MatrixXf::Identity(S.cols(), S.cols());
  chol.solveInPlace(sinv);
  //gettimeofday(&secs2, 0);
  //ROS_INFO("compute_delta_state %d inverse", (((secs2.tv_sec - secs1.tv_sec) * 1000) + (secs2.tv_usec - secs1.tv_usec)));
  //secs1 = secs2;

  MatrixXf K;
  K.noalias() = T * sinv.selfadjointView<Lower>();
  delta_state_out.noalias() = K * residual;
  //gettimeofday(&secs2, 0);
  //ROS_INFO("compute_delta_state %d multiplies2", (((secs2.tv_sec - secs1.tv_sec) * 1000) + (secs2.tv_usec - secs1.tv_usec)));
  //secs1 = secs2;

  // F = I - KH; P = FPF' + KRK';
  // P = P - (KHP)' - KHP + K(HPH' + R)K'
  // P = P - KHP
  P_out.triangularView<Lower>() = P - K * T.transpose(); // P - KHP
  P_out.triangularView<StrictlyUpper>() = P_out.transpose();
  //P_out.noalias() = 0.5 * (P_out + P_out.transpose());

  //gettimeofday(&secs2, 0);
  //ROS_INFO("compute_delta_state %d %d stop 0", (((secs2.tv_sec - secs1.tv_sec) * 1000) + (secs2.tv_usec - secs1.tv_usec)), H_rows);
  return 0;
}


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

/*
 Copyright (C) 2014 Peter Caspers

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

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

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

#include <ql/experimental/volatility/noarbsabr.hpp>

#include <ql/math/solvers1d/brent.hpp>
#include <ql/math/modifiedbessel.hpp>

#include <boost/math/special_functions/gamma.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/functional/hash.hpp>

namespace QuantLib {

class NoArbSabrModel::integrand {
    const NoArbSabrModel* model;
    Real strike;
  public:
    integrand(const NoArbSabrModel* model, Real strike)
    : model(model), strike(strike) {}
    Real operator()(Real f) const {
        return std::max(f - strike, 0.0) * model->p(f);
    }
};

class NoArbSabrModel::p_integrand {
    const NoArbSabrModel* model;
  public:
    explicit p_integrand(const NoArbSabrModel* model)
    : model(model) {}
    Real operator()(Real f) const {
        return model->p(f);
    }
};

NoArbSabrModel::NoArbSabrModel(const Real expiryTime, const Real forward,
                               const Real alpha, const Real beta, const Real nu,
                               const Real rho)
    : expiryTime_(expiryTime), externalForward_(forward), alpha_(alpha),
      beta_(beta), nu_(nu), rho_(rho), forward_(forward),
      numericalForward_(forward) {

    QL_REQUIRE(expiryTime > 0.0 && expiryTime <= detail::NoArbSabrModel::expiryTime_max,
               "expiryTime (" << expiryTime << ") out of bounds");
    QL_REQUIRE(forward > 0.0, "forward (" << forward << ") must be positive");
    QL_REQUIRE(beta >= detail::NoArbSabrModel::beta_min && beta <= detail::NoArbSabrModel::beta_max,
               "beta (" << beta << ") out of bounds");
    Real sigmaI = alpha * std::pow(forward, beta - 1.0);
    QL_REQUIRE(sigmaI >= detail::NoArbSabrModel::sigmaI_min &&
                   sigmaI <= detail::NoArbSabrModel::sigmaI_max,
               "sigmaI = alpha*forward^(beta-1.0) ("
                   << sigmaI << ") out of bounds, alpha=" << alpha
                   << " beta=" << beta << " forward=" << forward);
    QL_REQUIRE(nu >= detail::NoArbSabrModel::nu_min && nu <= detail::NoArbSabrModel::nu_max,
               "nu (" << nu << ") out of bounds");
    QL_REQUIRE(rho >= detail::NoArbSabrModel::rho_min && rho <= detail::NoArbSabrModel::rho_max,
               "rho (" << rho << ") out of bounds");

    // determine a region sufficient for integration in the normal case

    fmin_ = fmax_ = forward_;
    for (Real tmp = p(fmax_);
         tmp > std::max(detail::NoArbSabrModel::i_accuracy / std::max(1.0, fmax_ - fmin_),
                        detail::NoArbSabrModel::density_threshold);
         tmp = p(fmax_)) {
        fmax_ *= 2.0;
    }
    for (Real tmp = p(fmin_);
         tmp > std::max(detail::NoArbSabrModel::i_accuracy / std::max(1.0, fmax_ - fmin_),
                        detail::NoArbSabrModel::density_threshold);
         tmp = p(fmin_)) {
        fmin_ *= 0.5;
    }
    fmin_ = std::max(detail::NoArbSabrModel::strike_min, fmin_);

    QL_REQUIRE(fmax_ > fmin_, "could not find a reasonable integration domain");

    integrator_ =
        ext::make_shared<GaussLobattoIntegral>(
            detail::NoArbSabrModel::i_max_iterations, detail::NoArbSabrModel::i_accuracy);

    detail::D0Interpolator d0(forward_, expiryTime_, alpha_, beta_, nu_, rho_);
    absProb_ = d0();

    try {
        Brent b;
        Real start = std::sqrt(externalForward_ - detail::NoArbSabrModel::strike_min);
        Real tmp =
            b.solve(boost::lambda::bind(&NoArbSabrModel::forwardError, this,
                                        boost::lambda::_1),
                    detail::NoArbSabrModel::forward_accuracy, start,
                    std::min(detail::NoArbSabrModel::forward_search_step, start / 2.0));
        forward_ = tmp * tmp + detail::NoArbSabrModel::strike_min;
    } catch (Error&) {
        // fall back to unadjusted forward
        forward_ = externalForward_;
    }

    Real d = forwardError(std::sqrt(forward_ - detail::NoArbSabrModel::strike_min));
    numericalForward_ = d + externalForward_;
}

Real NoArbSabrModel::optionPrice(const Real strike) const {
    if (p(std::max(forward_, strike)) < detail::NoArbSabrModel::density_threshold)
        return 0.0;
    return (1.0 - absProb_) *
        ((*integrator_)(integrand(this, strike),
                        strike, std::max(fmax_, 2.0 * strike)) /
            numericalIntegralOverP_);
}

Real NoArbSabrModel::digitalOptionPrice(const Real strike) const {
    if (strike < QL_MIN_POSITIVE_REAL)
        return 1.0;
    if (p(std::max(forward_, strike)) < detail::NoArbSabrModel::density_threshold)
        return 0.0;
    return (1.0 - absProb_)
        * ((*integrator_)(p_integrand(this),
                          strike, std::max(fmax_, 2.0 * strike)) /
           numericalIntegralOverP_);
}

Real NoArbSabrModel::forwardError(const Real forward) const {
    forward_ = forward * forward + detail::NoArbSabrModel::strike_min;
    numericalIntegralOverP_ = (*integrator_)(p_integrand(this),
                                             fmin_, fmax_);
    return optionPrice(0.0) - externalForward_;
}

Real NoArbSabrModel::p(const Real f) const {

    if (f < detail::NoArbSabrModel::density_lower_bound ||
        forward_ < detail::NoArbSabrModel::density_lower_bound)
        return 0.0;

    Real fOmB = std::pow(f, 1.0 - beta_);
    Real FOmB = std::pow(forward_, 1.0 - beta_);

    Real zf = fOmB / (alpha_ * (1.0 - beta_));
    Real zF = FOmB / (alpha_ * (1.0 - beta_));
    Real z = zF - zf;

    // Real JzF = std::sqrt(1.0 - 2.0 * rho_ * nu_ * zF + nu_ * nu_ * zF * zF);
    Real Jmzf = std::sqrt(1.0 + 2.0 * rho_ * nu_ * zf + nu_ * nu_ * zf * zf);
    Real Jz = std::sqrt(1.0 - 2.0 * rho_ * nu_ * z + nu_ * nu_ * z * z);

    Real xz = std::log((Jz - rho_ + nu_ * z) / (1.0 - rho_)) / nu_;
    Real Bp_B = beta_ / FOmB;
    // Real Bpp_B = beta_ * (2.0 * beta_ - 1.0) / (FOmB * FOmB);
    Real kappa1 = 0.125 * nu_ * nu_ * (2.0 - 3.0 * rho_ * rho_) -
                  0.25 * rho_ * nu_ * alpha_ * Bp_B;
    // Real kappa2 = alpha_ * alpha_ * (0.25 * Bpp_B - 0.375 * Bp_B * Bp_B);
    Real gamma = 1.0 / (2.0 * (1.0 - beta_));
    Real sqrtOmR = std::sqrt(1.0 - rho_ * rho_);
    Real h = 0.5 * beta_ * rho_ / ((1.0 - beta_) * Jmzf * Jmzf) *
             (nu_ * zf * std::log(zf * Jz / zF) +
              (1 + rho_ * nu_ * zf) / sqrtOmR *
                  (std::atan((nu_ * z - rho_) / sqrtOmR) +
                   std::atan(rho_ / sqrtOmR)));

    Real res =
        std::pow(Jz, -1.5) / (alpha_ * std::pow(f, beta_) * expiryTime_) *
        std::pow(zf, 1.0 - gamma) * std::pow(zF, gamma) *
        std::exp(-(xz * xz) / (2.0 * expiryTime_) +
                 (h + kappa1 * expiryTime_)) *
        modifiedBesselFunction_i_exponentiallyWeighted(gamma,
                                                       zF * zf / expiryTime_);
    return res;
}

namespace detail {

using namespace boost::assign;

D0Interpolator::D0Interpolator(const Real forward, const Real expiryTime,
                               const Real alpha, const Real beta, const Real nu,
                               const Real rho)
    : forward_(forward), expiryTime_(expiryTime), alpha_(alpha), beta_(beta),
      nu_(nu), rho_(rho), gamma_(1.0 / (2.0 * (1.0 - beta_))) {

    sigmaI_ = alpha_ * std::pow(forward_, beta_ - 1.0);

    tauG_ += 0.25, 0.5, 0.75, 1.0, 1.25, 1.5, 1.75, 2.0, 2.25, 2.5, 2.75, 3.0,
        3.25, 3.5, 3.75, 4.0, 4.25, 4.5, 4.75, 5.0, 5.25, 5.5, 5.75, 6.0, 6.25,
        6.5, 6.75, 7.0, 7.25, 7.5, 7.75, 8.0, 8.25, 8.5, 8.75, 9.0, 9.25, 9.5,
        9.75, 10.0, 10.25, 10.5, 10.75, 11.0, 11.25, 11.5, 11.75, 12.0, 12.25,
        12.5, 12.75, 13.0, 13.25, 13.5, 13.75, 14.0, 14.25, 14.5, 14.75, 15.0,
        15.25, 15.5, 15.75, 16.0, 16.25, 16.5, 16.75, 17.0, 17.25, 17.5, 17.75,
        18.0, 18.25, 18.5, 18.75, 19.0, 19.25, 19.5, 19.75, 20.0, 20.25, 20.5,
        20.75, 21.0, 21.25, 21.5, 21.75, 22.0, 22.25, 22.5, 22.75, 23.0, 23.25,
        23.5, 23.75, 24.0, 24.25, 24.5, 24.75, 25.0, 25.25, 25.5, 25.75, 26.0,
        26.25, 26.5, 26.75, 27.0, 27.25, 27.5, 27.75, 28.0, 28.25, 28.5, 28.75,
        29.0, 29.25, 29.5, 29.75, 30.0;

    sigmaIG_ += 1.0, 0.8, 0.7, 0.6, 0.5, 0.45, 0.4, 0.35, 0.3, 0.27, 0.24, 0.21,
        0.18, 0.15, 0.125, 0.1, 0.075, 0.05;

    rhoG_ += 0.75, 0.50, 0.25, 0.00, -0.25, -0.50, -0.75;

    nuG_ += 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8;

    betaG_ += 0.01, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9;
}

Real D0Interpolator::operator()() const {

    // we do not need to check the indices here, because this is already
    // done in the NoArbSabr constructor

    Size tauInd = std::upper_bound(tauG_.begin(), tauG_.end(), expiryTime_) -
                                   tauG_.begin();
    if (tauInd == tauG_.size())
        --tauInd; // tau at upper bound
    Real expiryTimeTmp = expiryTime_;
    if (tauInd == 0) {
        ++tauInd;
        expiryTimeTmp = tauG_.front();
    }
    Real tauL = (expiryTimeTmp - tauG_[tauInd - 1]) /
                (tauG_[tauInd] - tauG_[tauInd - 1]);

    int sigmaIInd =
        sigmaIG_.size() -
        (std::upper_bound(sigmaIG_.rbegin(), sigmaIG_.rend(), sigmaI_) -
         sigmaIG_.rbegin());
    if (sigmaIInd == 0)
        ++sigmaIInd; // sigmaI at upper bound
    Real sigmaIL = (sigmaI_ - sigmaIG_[sigmaIInd - 1]) /
                   (sigmaIG_[sigmaIInd] - sigmaIG_[sigmaIInd - 1]);

    int rhoInd =
        rhoG_.size() -
        (std::upper_bound(rhoG_.rbegin(), rhoG_.rend(), rho_) - rhoG_.rbegin());
    if (rhoInd == 0) {
        rhoInd++;
    }
    if (rhoInd == static_cast<int>(rhoG_.size())) {
        rhoInd--;
    }
    Real rhoL =
        (rho_ - rhoG_[rhoInd - 1]) / (rhoG_[rhoInd] - rhoG_[rhoInd - 1]);

    // for nu = 0 we know phi = 0.5*z_F^2
    Size nuInd = std::upper_bound(nuG_.begin(), nuG_.end(), nu_) - nuG_.begin();
    if (nuInd == nuG_.size())
        --nuInd; // nu at upper bound
    Real tmpNuG = nuInd > 0 ? nuG_[nuInd - 1] : 0.0;
    Real nuL = (nu_ - tmpNuG) / (nuG_[nuInd] - tmpNuG);

    // for beta = 1 we know phi = 0.0
    Size betaInd =
        std::upper_bound(betaG_.begin(), betaG_.end(), beta_) - betaG_.begin();
    Real tmpBetaG;
    if (betaInd == betaG_.size())
        tmpBetaG = 1.0;
    else
        tmpBetaG = betaG_[betaInd];
    Real betaL =
        (beta_ - betaG_[betaInd - 1]) / (tmpBetaG - betaG_[betaInd - 1]);

    Real phiRes = 0.0;
    for (int iTau = -1; iTau <= 0; ++iTau) {
        for (int iSigma = -1; iSigma <= 0; ++iSigma) {
            for (int iRho = -1; iRho <= 0; ++iRho) {
                for (int iNu = -1; iNu <= 0; ++iNu) {
                    for (int iBeta = -1; iBeta <= 0; ++iBeta) {
                        Real phiTmp;
                        if (iNu == -1 && nuInd == 0) {
                            phiTmp =
                                0.5 /
                                (sigmaI_ * sigmaI_ * (1.0 - beta_) *
                                 (1.0 - beta_)); // this is 0.5*z_F^2, see above
                        } else {
                            if (iBeta == 0 && betaInd == betaG_.size()) {
                                phiTmp =
                                    phi(detail::NoArbSabrModel::tiny_prob);
                            } else {
                                int ind = (tauInd + iTau +
                                           (sigmaIInd + iSigma +
                                            (rhoInd + iRho +
                                             (nuInd + iNu + ((betaInd + iBeta) *
                                                             nuG_.size())) *
                                                 rhoG_.size()) *
                                                sigmaIG_.size()) *
                                               tauG_.size());
                                QL_REQUIRE(ind >= 0 && ind < 1209600,
                                           "absorption matrix index ("
                                               << ind << ") invalid");
                                phiTmp = phi((Real)sabrabsprob[ind] /
                                             detail::NoArbSabrModel::nsim);
                            }
                        }
                        phiRes += phiTmp * (iTau == -1 ? (1.0 - tauL) : tauL) *
                                  (iSigma == -1 ? (1.0 - sigmaIL) : sigmaIL) *
                                  (iRho == -1 ? (1.0 - rhoL) : rhoL) *
                                  (iNu == -1 ? (1.0 - nuL) : nuL) *
                                  (iBeta == -1 ? (1.0 - betaL) : betaL);
                    }
                }
            }
        }
    }
    return d0(phiRes);
}

Real D0Interpolator::phi(const Real d0) const {
    if (d0 < 1e-14)
        return detail::NoArbSabrModel::phiByTau_cutoff * expiryTime_;
    return boost::math::gamma_q_inv(gamma_, d0) * expiryTime_;
}

Real D0Interpolator::d0(const Real phi) const {
    return boost::math::gamma_q(gamma_, std::max(0.0, phi / expiryTime_));
}

} // namespace detail

} // namespace QuantLib


// Copyright (c) Dietmar Wolz.
//
// This source code is licensed under the MIT license found in the
// LICENSE file in the root directory.

// Eigen based implementation of differential evolution using onl the DE/best/1 strategy.
// Uses two deviations from the standard DE algorithm:
// a) temporal locality introduced in
// https://www.researchgate.net/publication/309179699_Differential_evolution_for_protein_folding_optimization_based_on_a_three-dimensional_AB_off-lattice_model
// b) reinitialization of individuals based on their age.
// requires https://github.com/imneme/pcg-cpp

#include <Eigen/Core>
#include <iostream>
#include <float.h>
#include <ctime>
#include <random>
#include "pcg_random.hpp"

using namespace std;

typedef Eigen::Matrix<double, Eigen::Dynamic, 1> vec;
typedef Eigen::Matrix<int, Eigen::Dynamic, 1> ivec;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> mat;

typedef double (*callback_type)(int, double[]);

namespace differential_evolution {

static uniform_real_distribution<> distr_01 = std::uniform_real_distribution<>(0, 1);

static vec zeros(int n) {
	return  Eigen::MatrixXd::Zero(n, 1);
}

static Eigen::MatrixXd uniform(int dx, int dy, pcg64& rs) {
	return Eigen::MatrixXd::NullaryExpr( dx, dy, [&](){return distr_01(rs);});
}

static Eigen::MatrixXd uniformVec(int dim, pcg64& rs) {
	return Eigen::MatrixXd::NullaryExpr( dim, 1, [&](){return distr_01(rs);});
}

int index_max(vec& v) {
	double maxv = DBL_MIN;
	int mi = -1;
	for (int i = 0; i < v.size(); i++) {
		if (v[i] > maxv) {
			mi = i;
			maxv = v[i];
		}
	}
	return mi;
}

int index_min(vec& v) {
	double minv = DBL_MAX;
	int mi = -1;
	for (int i = 0; i < v.size(); i++) {
		if (v[i] < minv) {
			mi = i;
			minv = v[i];
		}
	}
	return mi;
}

struct IndexVal {
    int index;
    double val;
};

bool compareIndexVal(IndexVal i1, IndexVal i2) {
    return (i1.val < i2.val);
}

ivec sort_index(const vec& x) {
	int size = x.size();
	IndexVal ivals[size];
	for (int i = 0; i < size; i++) {
		ivals[i].index = i;
		ivals[i].val = x[i];
	}
	std::sort(ivals, ivals+size, compareIndexVal);
	return Eigen::MatrixXi::NullaryExpr( size, 1, [&ivals](int i){return ivals[i].index;});
}

// wrapper around the fittness function, scales according to boundaries

class Fittness {

public:

    Fittness(callback_type pfunc, const vec& lower_limit,
            const vec& upper_limit) {
        func = pfunc;
        lower = lower_limit;
        upper = upper_limit;
        evaluationCounter = 0;
        if (lower.size() > 0) // bounds defined
            scale = (upper - lower);
    }

    vec getClosestFeasible(const vec& X) const {
        if (lower.size() > 0) {
        	return X.cwiseMin(1.0).cwiseMax(0.0);
        }
        return X;
    }

    double eval(const vec& X) {
        int n = X.size();
        double parg[n];
        for (int i = 0; i < n; i++)
            parg[i] = X(i);
        double res = func(n, parg);
        evaluationCounter++;
        return res;
    }

    double value(const vec& X) {
        if (lower.size() > 0)
            return eval(decode(X));
        else
            return eval(X);
    }

	vec decode(const vec& X) const {
		if (lower.size() > 0)
			return (X.array() * scale.array()).matrix() + lower;
		else
			return X;
	}

    vec encode(const vec& X) const {
        if (lower.size() > 0)
        	return (X - lower).array() / scale.array();
        else
            return X;
    }

    int getEvaluations() {
        return evaluationCounter;
    }

private:
   callback_type func;
   vec lower;
   vec upper;
   long evaluationCounter;
   vec scale;
};

class DeOptimizer {

public:

    DeOptimizer(long runid_, Fittness* fitfun_, int dim_, int seed_, int popsize_,
            int maxEvaluations_, double keep_,
            double stopfitness_, double F_, double CR_) {
        // runid used to identify a specific run
        runid = runid_;
        // fitness function to minimize
        fitfun = fitfun_;
        // Number of objective variables/problem dimension
        dim = dim_;
        // Population size
        if (popsize_ > 0)
            popsize = popsize_;
        else
            popsize = 15*dim;
        // termination criteria
        // maximal number of evaluations allowed.
        maxEvaluations = maxEvaluations_;
        // keep best young after each iteration.
        keep = keep_;
        // Limit for fitness value.
        stopfitness = stopfitness_;
        F = F_;
        CR = CR_;
        // Number of iterations already performed.
        iterations = 0;
        bestValue = DBL_MAX;
        // stop criteria
        stop = 0;
        //std::random_device rd;
        rs = new pcg64(seed_);
        init();
    }

    ~DeOptimizer() {
    	delete rs;
    }

    double rnd01() {
        return distr_01(*rs);
    }

    double rnd02() {
        double rnd = distr_01(*rs);
        return rnd*rnd;
    }

    int rndInt(int max) {
        return (int) (max*distr_01(*rs));
    }

    void doOptimize() {

        // -------------------- Generation Loop --------------------------------

        for (iterations = 1; fitfun->getEvaluations() < maxEvaluations; iterations++) {
            for (int k = 0; k < popsize; k++) {
                vec xi = popX.col(k);
                vec xb = popX.col(bestI);
                int r1, r2;
                do { r1 = rndInt(popsize); } while (r1 == k);
                do { r2 = rndInt(popsize); } while (r2 == k || r2 == r1);
                int jr = rndInt(dim);
                vec ui = vec(xi);
                for (int j = 0; j < dim; j++) {
                    if (j == jr || rnd01() < CR)
                        //ui[j] = base[j] + F*(popX(j,r1) - popX(j,r2));
                        ui[j] = xb[j] + F*(popX(j,r1) - popX(j,r2));
                }
                ui = fitfun->getClosestFeasible(ui);

                double eu = fitfun->value(ui);
                if (!isfinite(eu)) {
                    stop = -1;
                    return;
                }
                if (eu < popY[k]) {
                    // temporal locality
                    vec uis = xb + ((ui - xi)*0.5);
                    uis = fitfun->getClosestFeasible(uis);
                    double eus = fitfun->value(uis);
                    if (!isfinite(eus)) {
                        stop = -1;
                        return;
                    }
                    if (eus < eu) {
                        popX.col(k) = uis;
                        popY(k) = eus;
                    } else {
                        popX.col(k) = ui;
                        popY(k) = eu;
                    }
                    popIter[k] = iterations;
                    if (popY[k] < popY[bestI]) {
                        bestI = k;
                        if (popY(bestI) < bestValue) {
                            bestValue = popY[bestI];
                            bestX = popX.col(bestI);
                            if (isfinite(stopfitness) && bestValue < stopfitness) {
                                stop = 1;
                                return;
                            }
                        }
                    }
                 }
                 else {
                    // reinitialize individual
                    if (keep * rnd02() + 3 < iterations - popIter[k]) {
                        popX.col(k) = uniformVec(dim, *rs);
                        popY[k] = fitfun->value(popX.col(k)); // compute fitness
                    }
                }
            }
        }
    }

    void init() {
        popX = uniform(dim, popsize, *rs);
        popY = vec(popsize);
        for (int i = 0; i < popsize; i++)
            popY[i] = fitfun->value(popX.col(i)); // compute fitness
        bestI = index_min(popY);
        bestX = popX.col(bestI);
        popIter = zeros(popsize);
    }

    vec getBestX() {
        return bestX;
    }

    double getBestValue() {
        return bestValue;
    }

    double getIterations() {
        return iterations;
    }

    double getStop() {
        return stop;
    }

private:
      long runid;
      Fittness* fitfun;
      int popsize; // population size
      int dim;
      int maxEvaluations;
      double keep;
      double stopfitness;
      int iterations;
      double guessValue;
      vec guess;
      double bestValue;
      vec bestX;
      int bestI;
      int stop;
      double F;
      double CR;
      pcg64* rs;
      mat popX;
      vec popY;
      vec popIter;
};

// see https://cvstuff.wordpress.com/2014/11/27/wraping-c-code-with-python-ctypes-memory-and-pointers/

}

using namespace differential_evolution;

extern "C" {
    double* optimizeDE_C(long runid, callback_type func, int dim, int seed,
            double *lower, double *upper, int maxEvals, double keep,
            double stopfitness, int popsize, double F, double CR) {
        int n = dim;
        double *res = new double[n + 4];
        vec lower_limit(n), upper_limit(n);
        bool useLimit = false;
        for (int i = 0; i < n; i++) {
            lower_limit[i] = lower[i];
            upper_limit[i] = upper[i];
            useLimit |= (lower[i] != 0);
            useLimit |= (upper[i] != 0);
        }
        if (useLimit == false) {
            lower_limit.resize(0);
            upper_limit.resize(0);
        }
        Fittness fitfun(func, lower_limit, upper_limit);
        DeOptimizer opt(
            runid,
            &fitfun,
            dim,
            seed,
            popsize,
            maxEvals,
            keep,
            stopfitness,
            F,
            CR);
        try {
            opt.doOptimize();
            vec bestX = fitfun.decode(opt.getBestX());
            double bestY = opt.getBestValue();
            for (int i = 0; i < n; i++)
                res[i] = bestX[i];
            res[n] = bestY;
            res[n+1] = fitfun.getEvaluations();
            res[n+2] = opt.getIterations();
            res[n+3] = opt.getStop();
            return res;
        } catch (std::exception& e) {
            cout << e.what() << endl;
            return res;
        }
    }
}


#include <iostream>

#include <Eigen/Geometry>

#include "pnp_solver.h"
#include "params_config.h"

namespace fast_p3p
{

P3PSolver::P3PSolver(const std::vector<Eigen::Vector3d> &v_bearings,
                     const std::vector<cv::Point2d> &v_keypoints,
                     const std::vector<Eigen::Vector3d> &v_map_points,
                     const Eigen::Matrix3d &intrinsicMatrix,
                     const int img_width,
                     const int img_height) : best_inlier_nb_(0),
                                             best_reprojection_error_(std::numeric_limits<double>::max()),
                                             img_width_(img_width),
                                             img_height_(img_height)
{
    // assert(v_bearing_points.size() == v_map_points.size() && v_bearing_points.size() > 3);

    assert(v_keypoints.size() == v_bearings.size() && v_bearings.size() > 3);

    total_correspondeces_nb_ = v_bearings.size();

    // std::cout << "2D keypoints size : " << v_keypoints.size() << "\n";
    // std::cout << "Map points size : " << v_map_points.size() << "\n";
    // std::cout << "image width : " << img_width_ << "\n";
    // std::cout << "image height : " << img_height_ << "\n";

    v_bearing_set_ = v_bearings;
    v_keypoint_set_ = v_keypoints;
    v_map_point_set_ = v_map_points;

    if (intrinsicMatrix.hasNaN())
    {
        ERROR_STREAM("[P3PSolver] Invalid intrinsic Matrix!");
    }
    else
    {
        cam_intrinsic_ = intrinsicMatrix;
    }

    // debug the projected keypoints
    // std::string img_path = "/home/ziqianbai/Projects/vlab/3d_marker_localizer/build/20200402/undist_low/undist_20200402-224626-551d194e_i0_0.jpg";
    // debug_img_mat_ = cv::imread(img_path);
}

P3PSolver::~P3PSolver()
{
    // std::string out_img_path = "/home/ziqianbai/Projects/vlab/3d_marker_localizer/build/20200402/undist_low/undist_20200402-224626-551d194e_i0_0_projected.jpg";
    // cv::imwrite(out_img_path, debug_img_mat_);
}

void P3PSolver::solveQuarticPolynomial(const std::array<double, 5> &coeffs,
                                       std::array<double, 4> &real_roots)
{
    const double a = coeffs[0];
    const double b = coeffs[1] / a;
    const double c = coeffs[2] / a;
    const double d = coeffs[3] / a;
    const double e = coeffs[4] / a;

    const std::complex<double> Q1 = c * c - 3. * b * d + 12. * e;
    const std::complex<double> Q2 = 2. * c * c * c - 9. * b * c * d + 27. * d * d + 27. * b * b * e - 72. * c * e;
    const std::complex<double> Q3 = 8. * b * c - 16. * d - 2. * b * b * b;
    const std::complex<double> Q4 = 3. * b * b - 8. * c;

    auto complex_cbrt = [](const std::complex<double> &z) -> std::complex<double> { return pow(z, 1. / 3.); };
    const std::complex<double> Q5 = complex_cbrt(Q2 / 2. + sqrt(Q2 * Q2 / 4. - Q1 * Q1 * Q1));
    const std::complex<double> Q6 = (Q1 / Q5 + Q5) / 3.;
    const std::complex<double> Q7 = 2. * sqrt(Q4 / 12. + Q6);

    real_roots = {{(-b - Q7 - sqrt(4. * Q4 / 6. - 4. * Q6 - Q3 / Q7)).real() / 4.,
                   (-b - Q7 + sqrt(4. * Q4 / 6. - 4. * Q6 - Q3 / Q7)).real() / 4.,
                   (-b + Q7 - sqrt(4. * Q4 / 6. - 4. * Q6 + Q3 / Q7)).real() / 4.,
                   (-b + Q7 + sqrt(4. * Q4 / 6. - 4. * Q6 + Q3 / Q7)).real() / 4.}};
}

void P3PSolver::polishQuarticPolynomialRoots(const std::array<double, 5> &coeffs,
                                             std::array<double, 4> &roots,
                                             const int iterations)
{
    for (int i = 0; i < iterations; ++i)
    {
        for (auto &root : roots)
        {
            const double error =
                coeffs[4] + root * (coeffs[3] + root * (coeffs[2] + root * (coeffs[1] + root * coeffs[0])));

            const double derivative =
                coeffs[3] + root * (2 * coeffs[2] + root * ((4 * coeffs[0] * root + 3 * coeffs[1])));

            root -= error / derivative;
        }
    }
}

void P3PSolver::solveAP3P(const Eigen::MatrixXd &bearing_vectors,
                          const Eigen::MatrixXd &world_points,
                          std::vector<Eigen::Matrix4d> &solutions)
{
    assert(bearing_vectors.cols() >= 3 && world_points.cols() >= 3);
    const Eigen::Vector3d w1 = world_points.col(0);
    const Eigen::Vector3d w2 = world_points.col(1);
    const Eigen::Vector3d w3 = world_points.col(2);

    const Eigen::Vector3d b1 = bearing_vectors.col(0);
    const Eigen::Vector3d b2 = bearing_vectors.col(1);
    const Eigen::Vector3d b3 = bearing_vectors.col(2);

    // calculate k1, k2, k3
    const Eigen::Vector3d u0 = w1 - w2;
    const double nu0 = u0.norm();
    const Eigen::Vector3d k1 = u0.normalized();
    Eigen::Vector3d k3 = b1.cross(b2);
    const double nk3 = k3.norm();
    k3 = k3.normalized();

    const Eigen::Vector3d tz = b1.cross(k3);
    // ui, vi
    const Eigen::Vector3d v1 = b1.cross(b3);
    const Eigen::Vector3d v2 = b2.cross(b3);

    const Eigen::Vector3d u1 = w1 - w3;
    // coefficients related terms
    const double u1k1 = u1.dot(k1);
    const double k3b3 = k3.dot(b3);
    // f1i
    double f11 = k3b3;
    double f13 = k3.dot(v1);
    const double f15 = -u1k1 * f11;
    // delta
    const Eigen::Vector3d nl = u1.cross(k1).normalized();
    const double delta = u1.cross(k1).norm();
    f11 *= delta;
    f13 *= delta;
    // f2i
    const double u2k1 = u1k1 - nu0;
    double f21 = tz.dot(v2);
    double f22 = nk3 * k3b3;
    double f23 = k3.dot(v2);
    const double f24 = u2k1 * f22;
    const double f25 = -u2k1 * f21;
    f21 *= delta;
    f22 *= delta;
    f23 *= delta;
    const double g1 = f13 * f22;
    const double g2 = f13 * f25 - f15 * f23;
    const double g3 = f11 * f23 - f13 * f21;
    const double g4 = -f13 * f24;
    const double g5 = f11 * f22;
    const double g6 = f11 * f25 - f15 * f21;
    const double g7 = -f15 * f24;
    const std::array<double, 5> coeffs = {{g5 * g5 + g1 * g1 + g3 * g3, 2 * (g5 * g6 + g1 * g2 + g3 * g4),
                                           g6 * g6 + 2 * g5 * g7 + g2 * g2 + g4 * g4 - g1 * g1 - g3 * g3,
                                           2 * (g6 * g7 - g1 * g2 - g3 * g4), g7 * g7 - g2 * g2 - g4 * g4}};
    std::array<double, 4> s;
    solveQuarticPolynomial(coeffs, s);
    polishQuarticPolynomialRoots(coeffs, s);

    const Eigen::Vector3d temp = k1.cross(nl);

    Eigen::Matrix3d Ck1nl;
    Ck1nl << k1, nl, temp;

    Eigen::Matrix3d Cb1k3tzT;
    Cb1k3tzT << b1.transpose(), k3.transpose(), tz.transpose();

    const Eigen::Vector3d b3p = b3 * (delta / k3b3);

    for (const auto ctheta1p : s)
    {
        if (std::abs(ctheta1p) > 1)
            continue;
        const double stheta1p = ((k3b3 > 0) ? 1 : -1) * sqrt(1 - ctheta1p * ctheta1p);
        const double ntheta3 = stheta1p / ((g5 * ctheta1p + g6) * ctheta1p + g7);
        const double ctheta3 = (g1 * ctheta1p + g2) * ntheta3;
        const double stheta3 = (g3 * ctheta1p + g4) * ntheta3;

        Eigen::Matrix3d C13;
        C13 << ctheta3, 0.0, -stheta3, stheta1p * stheta3, ctheta1p, stheta1p * ctheta3, ctheta1p * stheta3, -stheta1p,
            ctheta1p * ctheta3;

        const Eigen::Matrix3d R = (Ck1nl * C13) * Cb1k3tzT;
        // R' * p3
        const Eigen::Vector3d rp3 = R.transpose() * w3;
        Eigen::Matrix4d T;
        T.setIdentity();
        T.block<3, 3>(0, 0) = R.transpose();
        Eigen::Vector3d t = (b3p * stheta1p) - rp3;
        T.block<3, 1>(0, 3) = t;
        solutions.emplace_back(T);
    }
}

// still use pixel error as reprojection error to evcaluate solution
int P3PSolver::evaluateSolutions(const Eigen::Matrix4d &solution,
                                 std::map<int, int> &valid_matches,
                                 double &mean_reprojection_error)
{
    int valid_measurement = initialMeasurement();

    valid_matches.clear();
    for (size_t i = 0; i < v_map_point_set_.size(); ++i)
    {
        valid_matches[i] = -1;
    }

    mean_reprojection_error = 0.0;

    double eps = std::numeric_limits<double>::epsilon();
    const double img_left_bound = ParamsConfig::GetImageBound();
    const double img_right_bound = img_width_ - img_left_bound;
    const double img_up_bound = ParamsConfig::GetImageBound();
    const double img_bottom_bound = img_height_ - img_up_bound;

    for (size_t i = 0; i < v_map_point_set_.size(); ++i)
    {

        Eigen::Vector3d map_point = v_map_point_set_[i];

        Eigen::Vector3d proj_point = (solution * map_point.homogeneous()).hnormalized();
        if (proj_point[2] <= eps)
        {
            continue;
        }

        Eigen::Vector3d img_point = cam_intrinsic_ * proj_point;
        double img_x = img_point[0] / img_point[2];
        double img_y = img_point[1] / img_point[2];

        if (img_left_bound < img_x && img_x < img_right_bound &&
            img_up_bound < img_y && img_y < img_bottom_bound)
        {
            for (size_t j = 0; j < v_keypoint_set_.size(); ++j)
            {
                double x_err = (img_x - v_keypoint_set_[j].x);
                double y_err = (img_y - v_keypoint_set_[j].y);
                double dist = std::sqrt(x_err * x_err + y_err * y_err);
                if (dist < reproject_err_thre_)
                {
                    valid_matches[i] = j;
                    valid_measurement += 1;
                    mean_reprojection_error += dist;

                    // {
                    //     const cv::Scalar color(0, 0, 255);
                    //     cv::line(debug_img_mat_, cv::Point(img_x - 20, img_y), cv::Point(img_x + 20, img_y),
                    //              color, 1);
                    //     cv::line(debug_img_mat_, cv::Point(img_x, img_y - 20), cv::Point(img_x, img_y + 20),
                    //              color, 1);
                    // }
                }
            }
        }
    }

    // valid_measurement must fall into [0, v_keypoint_set_.size()]
    if (valid_measurement == 0 || valid_measurement > static_cast<int>(v_keypoint_set_.size()))
    {
        valid_measurement = 0;
        mean_reprojection_error = std::numeric_limits<double>::max();
    }
    else
    {
        mean_reprojection_error /= valid_measurement;
    }

    return valid_measurement;
}

bool P3PSolver::calcTransform(const std::vector<unsigned int> &sample_kpt_indices,
                              const std::vector<unsigned int> &sample_map_indices)
{
    assert(sample_kpt_indices.size() > 2 && sample_kpt_indices.size() == sample_map_indices.size());

    Eigen::MatrixXd bearing_mat(3, 3);
    Eigen::MatrixXd world_mat(3, 3);
    for (int i = 0; i < 3; ++i)
    {
        if (sample_kpt_indices[i] >= v_bearing_set_.size() || sample_map_indices[i] >= v_map_point_set_.size())
        {
            ERROR_STREAM("[calcTransform] Invalid sample indices!");
            return false;
        }

        bearing_mat.col(i) = v_bearing_set_[sample_kpt_indices[i]];
        world_mat.col(i) = v_map_point_set_[sample_map_indices[i]];
    }

    // although AP3P is more robust to amgubility
    // we still judge degenerate conditions:
    // 1. colinear
    // 2. 2 points
    {
        const double eps = std::numeric_limits<double>::epsilon();
        double len01 = (world_mat.col(0) - world_mat.col(1)).norm();
        double len12 = (world_mat.col(1) - world_mat.col(2)).norm();
        double len02 = (world_mat.col(2) - world_mat.col(0)).norm();
        // 2 point
        if (len01 <= eps || len02 <= eps || len12 <= eps)
        {
            ERROR_STREAM("[P3PSolver::calcTransform] Invalid world map points!");
            return false;
        }
        // colinear, using heron's formula
        double p = 0.5 * (len01 + len02 + len12);
        double area = std::sqrt(p * (p - len01) * (p - len02) * (p - len12));
        if (area <= eps)
        {
            ERROR_STREAM("[P3PSolver::calcTransform] Invalid world map points' area!");
            return false;
        }
    }

    std::vector<Eigen::Matrix4d> solutions;
    solveAP3P(bearing_mat, world_mat, solutions);
    if (solutions.empty())
    {
        // ERROR_STREAM("[P3PSolver::calcTransform] Fail to solve relative pose from P3P!");
        return false;
    }

    best_inlier_nb_ = 0;
    // check each solutions to avoid amgubility
    for (size_t i = 0; i < solutions.size(); ++i)
    {
        // evaluate each solution on the whole input matches
        Eigen::Matrix4d T = solutions[i];
        if (T.hasNaN())
            continue;

        std::map<int, int> inlier_matches;
        double mean_reproject_error = 0.0;
        int measure = evaluateSolutions(T, inlier_matches, mean_reproject_error);
        if (measure < minimalDataNumber())
            continue;

        if (measure > best_inlier_nb_ || (measure == best_inlier_nb_ && mean_reproject_error < best_reprojection_error_))
        {
            T_cw_ = T;
            best_inlier_nb_ = measure;
            best_reprojection_error_ = mean_reproject_error;
            best_valid_matches_.swap(inlier_matches);
            // DEBUG_STREAM("[calcTransform] best_inlier_num: " << best_inlier_nb_);
            // DEBUG_STREAM("[calcTransform] best_mean_reprojection_error: " << best_reprojection_error_);
        }
    }

    return true;
}

int P3PSolver::validateData(std::map<int, int> &inlier_matches)
{
    if (best_inlier_nb_ > 0)
    {
        inlier_matches.swap(best_valid_matches_);
        return best_inlier_nb_;
    }

    for (size_t i = 0; i < v_map_point_set_.size(); ++i)
    {
        inlier_matches[i] = -1;
    }
    return worstMeasurement();
}

} // namespace fast_p3p


// TRENTO: Reduced Thickness Event-by-event Nuclear Topology
// Copyright 2015 Jonah E. Bernhard, J. Scott Moreland
// MIT License

#include "nucleus.h"

#include <algorithm>
#include <cmath>
#include <memory>
#include <stdexcept>
#include <string>
#include <utility>

#include <boost/math/constants/constants.hpp>
#ifdef TRENTO_HDF5
// include multi_array for use with ManualNucleus
#ifdef NDEBUG
#define BOOST_DISABLE_ASSERTS
#endif
#include <boost/multi_array.hpp>
#endif

#include "hdf5_utils.h"
#include "random.h"

namespace trento {

NucleusPtr Nucleus::create(const std::string& species, double nucleon_dmin) {
  // W-S params ref. in header
  // XXX: remember to add new species to the help output in main() and the readme
  if (species == "p")
    return NucleusPtr{new Proton{}};
  else if (species == "d")
    return NucleusPtr{new Deuteron{}};
  else if (species == "Cu")
    return NucleusPtr{new WoodsSaxonNucleus{
       63, 4.20, 0.596, 0.0, nucleon_dmin
    }};
  else if (species == "Cu2")
    return NucleusPtr{new DeformedWoodsSaxonNucleus{
       63, 4.20, 0.596, 0.162, -0.006, nucleon_dmin
    }};
  else if (species == "O")
      return NucleusPtr{new WoodsSaxonNucleus{
              16, 2.608, 0.513, -0.051, nucleon_dmin
      }};
  else if (species == "Xe")
    return NucleusPtr{new WoodsSaxonNucleus{
      129, 5.36, 0.590, 0.0, nucleon_dmin
    }};
  else if (species == "Xe2")
    return NucleusPtr{new DeformedWoodsSaxonNucleus{
      129, 5.36, 0.590, 0.162, -0.003, nucleon_dmin
    }};
  else if (species == "XeTri")
    return NucleusPtr{new DeformedWoodsSaxonNucleus{
     129, 5.60, 0.49, 0.21, 0.0, nucleon_dmin
    }};
    else if (species == "XeALICE")
    return NucleusPtr{new DeformedWoodsSaxonNucleus{
     129, 5.36, 0.590, 0.18, 0.0, nucleon_dmin
    }};
  else if (species == "Au")
    return NucleusPtr{new WoodsSaxonNucleus{
      197, 6.38, 0.535, 0.0, nucleon_dmin
    }};
  else if (species == "Au2")
    return NucleusPtr{new DeformedWoodsSaxonNucleus{
      197, 6.38, 0.535, -0.131, -0.031, nucleon_dmin
    }};
  else if (species == "Pb")
    return NucleusPtr{new WoodsSaxonNucleus{
      208, 6.62, 0.546, 0.0, nucleon_dmin
    }};
  else if (species == "PbTri")
    return NucleusPtr{new WoodsSaxonNucleus{
      208, 6.65, 0.54, 0.0, nucleon_dmin
    }};
  else if (species == "U")
    return NucleusPtr{new DeformedWoodsSaxonNucleus{
      238, 6.81, 0.600, 0.280, 0.093, nucleon_dmin
    }};
  else if (species == "U2")
    return NucleusPtr{new DeformedWoodsSaxonNucleus{
      238, 6.86, 0.420, 0.265, 0.000, nucleon_dmin
    }};
  else if (species == "U3")
    return NucleusPtr{new DeformedWoodsSaxonNucleus{
      238, 6.67, 0.440, 0.280, 0.093, nucleon_dmin
    }};
  // Read nuclear configurations from HDF5.
  else if (hdf5::filename_is_hdf5(species)) {
#ifdef TRENTO_HDF5
    return ManualNucleus::create(species);
#else
    throw std::invalid_argument{"HDF5 output was not compiled"};
#endif  // TRENTO_HDF5
  }
  else
    throw std::invalid_argument{"unknown projectile species: " + species};
}

Nucleus::Nucleus(std::size_t A) : nucleons_(A), offset_(0) {}

void Nucleus::sample_nucleons(double offset) {
  offset_ = offset;
  sample_nucleons_impl();
}

void Nucleus::set_nucleon_position(
    NucleonData& nucleon, double x, double y, double z) {
  nucleon.set_position(x + offset_, y, z);
}

Proton::Proton() : Nucleus(1) {}

/// Always zero.
double Proton::radius() const {
  return 0.;
}

/// Always place the nucleon at the origin.
void Proton::sample_nucleons_impl() {
  set_nucleon_position(*begin(), 0., 0., 0.);
}

// Without loss of generality, let the internal a_ parameter be the minimum of
// the given (a, b) and the internal b_ be the maximum.
Deuteron::Deuteron(double a, double b)
    : Nucleus(2),
      a_(std::fmin(a, b)),
      b_(std::fmax(a, b))
{}

double Deuteron::radius() const {
  // The quantile function for the exponential distribution exp(-2*a*r) is
  // -log(1-q)/(2a).  Return the 99% quantile.
  return -std::log(.01)/(2*a_);
}

void Deuteron::sample_nucleons_impl() {
  // Sample the inter-nucleon radius using rejection sampling with an envelope
  // function.  The Hulthén wavefunction including the r^2 Jacobian expands to
  // three exponential terms:  exp(-2*a*r) + exp(-2*b*r) - 2*exp(-(a+b)*r).
  // This does not have a closed-form inverse CDF, however we can easily sample
  // exponential numbers from the term that falls off the slowest, i.e.
  // exp(-2*min(a,b)*r).  In the ctor initializer list the "a" parameter is
  // always set to the minimum, so we should sample from exp(-2*a*r).
  double r, prob;
  do {
    // Sample a uniform random number, u = exp(-2*a*r).
    auto u = random::canonical<double>();
    // Invert to find the actual radius.
    r = -std::log(u) / (2*a_);
    // The acceptance probability is now the radial wavefunction over the
    // envelope function, both evaluated at the proposal radius r.
    // Conveniently, the envelope evaluated at r is just the uniform random
    // number u.
    prob = std::pow(std::exp(-a_*r) - std::exp(-b_*r), 2) / u;
  } while (prob < random::canonical<double>());

  // Now sample spherical rotation angles.
  auto cos_theta = random::cos_theta<double>();
  auto phi = random::phi<double>();

  // And compute the Cartesian coordinates of one nucleon.
  auto r_sin_theta = r * std::sqrt(1. - cos_theta*cos_theta);
  auto x = r_sin_theta * std::cos(phi);
  auto y = r_sin_theta * std::sin(phi);
  auto z = r * cos_theta;

  // Place the first nucleon at the sampled coordinates.
  set_nucleon_position(*begin(), x, y, z);
  // Place the second nucleon opposite to the first.
  set_nucleon_position(*std::next(begin()), -x, -y, -z);
}

MinDistNucleus::MinDistNucleus(std::size_t A, double dmin)
    : Nucleus(A),
      dminsq_(dmin*dmin)
{}

bool MinDistNucleus::is_too_close(const_iterator nucleon) const {
  if (dminsq_ < 1e-10)
    return false;
  for (const_iterator nucleon2 = begin(); nucleon2 != nucleon; ++nucleon2) {
    auto dx = nucleon->x() - nucleon2->x();
    auto dy = nucleon->y() - nucleon2->y();
    auto dz = nucleon->z() - nucleon2->z();
    if (dx*dx + dy*dy + dz*dz < dminsq_)
      return true;
  }
  return false;
}

// Extend the W-S dist out to R + 10a; for typical values of (R, a), the
// probability of sampling a nucleon beyond this radius is O(10^-5).
WoodsSaxonNucleus::WoodsSaxonNucleus(
    std::size_t A, double R, double a, double w, double dmin)
    : MinDistNucleus(A, dmin),
      R_(R),
      a_(a),
      woods_saxon_dist_(1000, 0., R + 10.*a,
        [R, a, w](double r) { return r*r*(1+w*(r/R)*(r/R))/(1.+std::exp((r-R)/a)); })
{}

/// Return something a bit smaller than the true maximum radius.  The
/// Woods-Saxon distribution falls off very rapidly (exponentially), and since
/// this radius determines the impact parameter range, the true maximum radius
/// would cause far too many events with zero participants.
double WoodsSaxonNucleus::radius() const {
  return R_ + 3.*a_;
}

/// Sample Woods-Saxon nucleon positions.
void WoodsSaxonNucleus::sample_nucleons_impl() {
  // When placing nucleons with a minimum distance criterion, resample spherical
  // angles until the nucleon is not too close to a previously sampled nucleon,
  // but do not resample radius -- this could modify the Woods-Saxon dist.

  // Because of the r^2 Jacobian, there is less available space at smaller
  // radii.  Therefore, pre-sample all radii first, sort them, and then place
  // nucleons starting with the smallest radius and working outwards.  This
  // dramatically reduces the chance that a nucleon cannot be placed.
  std::vector<double> radii(size());
  for (auto&& r : radii)
    r = woods_saxon_dist_(random::engine);
  std::sort(radii.begin(), radii.end());

  // Place each nucleon at a pre-sampled radius.
  auto r_iter = radii.cbegin();
  for (iterator nucleon = begin(); nucleon != end(); ++nucleon) {
    // Get radius and advance iterator.
    auto& r = *r_iter++;

    // Sample angles until the minimum distance criterion is satisfied.
    auto ntries = 0;
    do {
      // Sample isotropic spherical angles.
      auto cos_theta = random::cos_theta<double>();
      auto phi = random::phi<double>();

      // Convert to Cartesian coordinates.
      auto r_sin_theta = r * std::sqrt(1. - cos_theta*cos_theta);
      auto x = r_sin_theta * std::cos(phi);
      auto y = r_sin_theta * std::sin(phi);
      auto z = r * cos_theta;

      set_nucleon_position(*nucleon, x, y, z);

      // Retry sampling a reasonable number of times.  If a nucleon cannot be
      // placed, give up and leave it at its last sampled position.  Some
      // approximate numbers for Pb nuclei:
      //
      //   dmin = 0.5 fm, < 0.001% of nucleons cannot be placed
      //          1.0 fm, ~0.005%
      //          1.5 fm, ~0.1%
      //          1.73 fm, ~1%
    } while (++ntries < 1000 && is_too_close(nucleon));
  }
  // XXX: re-center nucleon positions?
}

// Set rmax like the non-deformed case (R + 10a), but for the maximum
// "effective" radius.  The numerical coefficients for beta2 and beta4 are the
// approximate values of Y20 and Y40 at theta = 0.
DeformedWoodsSaxonNucleus::DeformedWoodsSaxonNucleus(
    std::size_t A, double R, double a, double beta2, double beta4, double dmin)
    : MinDistNucleus(A, dmin),
      R_(R),
      a_(a),
      beta2_(beta2),
      beta4_(beta4),
      rmax_(R*(1. + .63*std::fabs(beta2) + .85*std::fabs(beta4)) + 10.*a)
{}

/// Return something a bit smaller than the true maximum radius.  The
/// Woods-Saxon distribution falls off very rapidly (exponentially), and since
/// this radius determines the impact parameter range, the true maximum radius
/// would cause far too many events with zero participants.
double DeformedWoodsSaxonNucleus::radius() const {
  return rmax_ - 7.*a_;
}

double DeformedWoodsSaxonNucleus::deformed_woods_saxon_dist(
    double r, double cos_theta) const {
  auto cos_theta_sq = cos_theta*cos_theta;

  // spherical harmonics
  using math::double_constants::one_div_root_pi;
  auto Y20 = std::sqrt(5)/4. * one_div_root_pi * (3.*cos_theta_sq - 1.);
  auto Y40 = 3./16. * one_div_root_pi *
             (35.*cos_theta_sq*cos_theta_sq - 30.*cos_theta_sq + 3.);

  // "effective" radius
  auto Reff = R_ * (1. + beta2_*Y20 + beta4_*Y40);

  return 1. / (1. + std::exp((r - Reff) / a_));
}

/// Sample deformed Woods-Saxon nucleon positions.
void DeformedWoodsSaxonNucleus::sample_nucleons_impl() {
  // The deformed W-S distribution is defined so the symmetry axis is aligned
  // with the Z axis, so e.g. the long axis of uranium coincides with Z.
  //
  // After sampling positions, they must be randomly rotated.  In general this
  // requires three Euler rotations, but in this case we only need two
  // because there is no use in rotating about the nuclear symmetry axis.
  //
  // The two rotations are:
  //  - a polar "tilt", i.e. rotation about the X axis
  //  - an azimuthal "spin", i.e. rotation about the original Z axis

  // "tilt" angle
  const auto cos_a = random::cos_theta<double>();
  const auto sin_a = std::sqrt(1. - cos_a*cos_a);

  // "spin" angle
  const auto angle_b = random::phi<double>();
  const auto cos_b = std::cos(angle_b);
  const auto sin_b = std::sin(angle_b);

  // Pre-sample and sort (r, cos_theta) points from the deformed W-S dist.
  // See comments in WoodsSaxonNucleus (above) for rationale.
  struct Sample {
    double r, cos_theta;
  };

  std::vector<Sample> samples(size());

  for (auto&& sample : samples) {
    // Sample (r, cos_theta) using a standard rejection method.
    // Remember to include the phase-space factors.
    do {
      sample.r = rmax_ * std::cbrt(random::canonical<double>());
      sample.cos_theta = random::cos_theta<double>();
    } while (
      random::canonical<double>() >
      deformed_woods_saxon_dist(sample.r, sample.cos_theta)
    );
  }

  // Sort by radius.  Could also sort by e.g. the perpendicular distance from
  // the z-axis, or by descending W-S density.  Empirically, radius leads to the
  // smallest failure rate.
  std::sort(
    samples.begin(), samples.end(),
    [](const Sample& a, const Sample& b) {
      return a.r < b.r;
    }
  );

  // Place each nucleon at a pre-sampled (r, cos_theta).
  auto sample = samples.cbegin();
  for (iterator nucleon = begin(); nucleon != end(); ++nucleon, ++sample) {
    auto& r = sample->r;
    auto& cos_theta = sample->cos_theta;

    auto r_sin_theta = r * std::sqrt(1. - cos_theta*cos_theta);
    auto z = r * cos_theta;

    // Sample azimuthal angle until the minimum distance criterion is satisfied.
    auto ntries = 0;
    do {
      // Choose azimuthal angle.
      auto phi = random::phi<double>();

      // Convert to Cartesian coordinates.
      auto x = r_sin_theta * std::cos(phi);
      auto y = r_sin_theta * std::sin(phi);

      // Rotate.
      // The rotation formula was derived by composing the "tilt" and "spin"
      // rotations described above.
      auto x_rot = x*cos_b - y*cos_a*sin_b + z*sin_a*sin_b;
      auto y_rot = x*sin_b + y*cos_a*cos_b - z*sin_a*cos_b;
      auto z_rot =           y*sin_a       + z*cos_a;

      set_nucleon_position(*nucleon, x_rot, y_rot, z_rot);

      // In addition to resampling phi, flip the z-coordinate each time.  This
      // works because the deformed WS dist is symmetric in z.  Effectively
      // doubles the available space for the nucleon.
      z *= -1;

      // Retry a reasonable number of times.  Unfortunately the failure rate is
      // worse than non-deformed sampling because there is less freedom to place
      // each nucleon.  Some approximate numbers for U nuclei:
      //
      //   dmin = 0.5 fm, < 0.001% of nucleons cannot be placed
      //          1.0 fm, ~0.03%
      //          1.3 fm, ~0.3%
      //          1.5 fm, ~1.2%
    } while (++ntries < 1000 && is_too_close(nucleon));
  }
}

#ifdef TRENTO_HDF5

namespace {

// Read a slice of an HDF5 dataset into a boost::multi_array.
template <typename T, std::size_t FileDims, std::size_t MemDims>
boost::multi_array<T, MemDims>
read_dataset(
    const H5::DataSet& dataset,
    const std::array<hsize_t, FileDims>& count,
    const std::array<hsize_t, FileDims>& start,
    const std::array<hsize_t, MemDims>& shape) {
  boost::multi_array<T, MemDims> array{shape};
  auto filespace = dataset.getSpace();
  filespace.selectHyperslab(H5S_SELECT_SET, count.data(), start.data());
  auto memspace = hdf5::make_dataspace(shape);

  dataset.read(array.data(), hdf5::type<T>(), memspace, filespace);

  return array;
}

}  // unnamed namespace

std::unique_ptr<ManualNucleus> ManualNucleus::create(const std::string& path) {
  auto file = hdf5::try_open_file(path);

  // Check that there is a single dataset in the file.
  // Might relax this constraint in the future.
  if (file.getNumObjs() != 1)
    throw std::invalid_argument{
      "file '" + path + "' must contain exactly one object"
    };

  auto name = file.getObjnameByIdx(0);
#if H5_VERSION_GE(1, 8, 13)
  if (file.childObjType(name) != H5O_TYPE_DATASET)  // added v1.8.13
#else
  if (file.getObjTypeByIdx(0) != H5G_DATASET)  // deprecated fall back
#endif
    throw std::invalid_argument{
      "object '" + name + "' in file '" + path + "' is not a dataset"
    };

  // Make dataset object in a unique_ptr for eventual passing to ctor.
  auto dataset = std::unique_ptr<H5::DataSet>{
    new H5::DataSet{file.openDataSet(name)}
  };

  // Verify that the dataset has the correct dimensionality and shape.
  std::array<hsize_t, 3> shape;
  auto ndim = dataset->getSpace().getSimpleExtentDims(shape.data());

  if (ndim != 3)
    throw std::invalid_argument{
      "dataset '" + name + "' in file '" + path + "' has " +
      std::to_string(ndim) + " dimensions (need 3)"
    };

  if (shape[2] != 3)
    throw std::invalid_argument{
      "dataset '" + name + "' in file '" + path + "' has " +
      std::to_string(shape[2]) + " columns (need 3)"
    };

  // Deduce number of configs and number of nucleons (A) from the shape.
  const auto& nconfigs = shape[0];
  const auto& A = shape[1];

  // Estimate the max radius from at least 500 nucleon positions.
  auto n = std::min(500/A + 1, nconfigs);
  std::array<hsize_t, 3> count = {n, A, 3};
  std::array<hsize_t, 3> start = {0, 0, 0};
  std::array<hsize_t, 2> shape_n = {n*A, 3};
  auto positions = read_dataset<float>(*dataset, count, start, shape_n);

  auto rmax_sq = 0.;

  for (const auto& position : positions) {
    auto& x = position[0];
    auto& y = position[1];
    auto& z = position[2];
    auto r_sq = x*x + y*y + z*z;
    if (r_sq > rmax_sq)
      rmax_sq = r_sq;
  }

  auto rmax = std::sqrt(rmax_sq);

  return std::unique_ptr<ManualNucleus>{
    new ManualNucleus{std::move(dataset), nconfigs, A, rmax}
  };
}

ManualNucleus::ManualNucleus(std::unique_ptr<H5::DataSet> dataset,
                             std::size_t nconfigs, std::size_t A, double rmax)
    : Nucleus(A),
      dataset_(std::move(dataset)),
      rmax_(rmax),
      index_dist_(0, nconfigs - 1)
{}

ManualNucleus::~ManualNucleus() = default;

double ManualNucleus::radius() const {
  return rmax_;
}

void ManualNucleus::sample_nucleons_impl() {
  // Sample Euler rotation angles.
  // First is an azimuthal spin about the Z axis.
  const auto angle_1 = random::phi<double>();
  const auto c1 = std::cos(angle_1);
  const auto s1 = std::sin(angle_1);
  // Then a polar tilt about the original X axis, uniform in cos(theta).
  const auto c2 = random::cos_theta<double>();
  const auto s2 = std::sqrt(1. - c2*c2);
  // Finally another azimuthal spin about the original Z axis.
  const auto angle_3 = random::phi<double>();
  const auto c3 = std::cos(angle_3);
  const auto s3 = std::sin(angle_3);

  // Choose and read a random config from the dataset.
  std::array<hsize_t, 3> count = {1, size(), 3};
  std::array<hsize_t, 3> start = {index_dist_(random::engine), 0, 0};
  std::array<hsize_t, 2> shape = {size(), 3};
  const auto positions = read_dataset<float>(*dataset_, count, start, shape);

  // Loop over positions and nucleons.
  auto positions_iter = positions.begin();
  for (iterator nucleon = begin(); nucleon != end(); ++nucleon) {
    // Extract position vector and increment iterator.
    auto position = *positions_iter++;
    auto& x = position[0];
    auto& y = position[1];
    auto& z = position[2];

    // Rotate.
    auto x_rot = x*(c1*c3 - c2*s1*s3) - y*(c3*s1 + c1*c2*s3) + z*s2*s3;
    auto y_rot = x*(c1*s3 + c2*c3*s1) - y*(s1*s3 - c1*c2*c3) - z*c3*s2;
    auto z_rot = x*s1*s2              + y*c1*s2              + z*c2;

    set_nucleon_position(*nucleon, x_rot, y_rot, z_rot);
  }
}

#endif  // TRENTO_HDF5

}  // namespace trento


// Boost.Geometry

// Copyright (c) 2016-2017 Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_FORMULAS_MAXIMUM_LATITUDE_HPP
#define BOOST_GEOMETRY_FORMULAS_MAXIMUM_LATITUDE_HPP

#include <boost/geometry/formulas/flattening.hpp>
#include <boost/geometry/formulas/spherical.hpp>

#include <boost/mpl/assert.hpp>

namespace boost { namespace geometry { namespace formula
{

/*!
\brief Algorithm to compute the vertex latitude of a geodesic segment. Vertex is
a point on the geodesic that maximizes (or minimizes) the latitude.
\author See
    [Wood96] Wood - Vertex Latitudes on Ellipsoid Geodesics, SIAM Rev., 38(4),
             637–644, 1996
*/

template <typename CT>
class vertex_latitude_on_sphere
{

public:
    template<typename T1, typename T2>
    static inline CT apply(T1 const& lat1,
                           T2 const& alp1)
    {
        return std::acos( math::abs(cos(lat1) * sin(alp1)) );
    }
};

template <typename CT>
class vertex_latitude_on_spheroid
{

public:
/*
 * formula based on paper
 *   [Wood96] Wood - Vertex Latitudes on Ellipsoid Geodesics, SIAM Rev., 38(4),
 *            637–644, 1996
    template <typename T1, typename T2, typename Spheroid>
    static inline CT apply(T1 const& lat1,
                           T2 const& alp1,
                           Spheroid const& spheroid)
    {
        CT const f = formula::flattening<CT>(spheroid);

        CT const e2 = f * (CT(2) - f);
        CT const sin_alp1 = sin(alp1);
        CT const sin2_lat1 = math::sqr(sin(lat1));
        CT const cos2_lat1 = CT(1) - sin2_lat1;

        CT const e2_sin2 = CT(1) - e2 * sin2_lat1;
        CT const cos2_sin2 = cos2_lat1 * math::sqr(sin_alp1);
        CT const vertex_lat = std::asin( math::sqrt((e2_sin2 - cos2_sin2)
                                                    / (e2_sin2 - e2 * cos2_sin2)));
        return vertex_lat;
    }
*/

    // simpler formula based on Clairaut relation for spheroids
    template <typename T1, typename T2, typename Spheroid>
    static inline CT apply(T1 const& lat1,
                           T2 const& alp1,
                           Spheroid const& spheroid)
    {
        CT const f = formula::flattening<CT>(spheroid);

        CT const one_minus_f = (CT(1) - f);

        //get the reduced latitude
        CT const bet1 = atan( one_minus_f * tan(lat1) );

        //apply Clairaut relation
        CT const betv =  vertex_latitude_on_sphere<CT>::apply(bet1, alp1);

        //return the spheroid latitude
        return atan( tan(betv) / one_minus_f );
    }

    /*
    template <typename T>
    inline static void sign_adjustment(CT lat1, CT lat2, CT vertex_lat, T& vrt_result)
    {
        // signbit returns a non-zero value (true) if the sign is negative;
        // and zero (false) otherwise.
        bool sign = std::signbit(std::abs(lat1) > std::abs(lat2) ? lat1 : lat2);

        vrt_result.north = sign ? std::max(lat1, lat2) : vertex_lat;
        vrt_result.south = sign ? vertex_lat * CT(-1) : std::min(lat1, lat2);
    }

    template <typename T>
    inline static bool vertex_on_segment(CT alp1, CT alp2, CT lat1, CT lat2, T& vrt_result)
    {
        CT const half_pi = math::pi<CT>() / CT(2);

        // if the segment does not contain the vertex of the geodesic
        // then return the endpoint of max (min) latitude
        if ((alp1 < half_pi && alp2 < half_pi)
                || (alp1 > half_pi && alp2 > half_pi))
        {
            vrt_result.north = std::max(lat1, lat2);
            vrt_result.south = std::min(lat1, lat2);
            return false;
        }
        return true;
    }
    */
};

template <typename CT, typename CS_Tag>
struct vertex_latitude
{
    BOOST_MPL_ASSERT_MSG
         (
             false, NOT_IMPLEMENTED_FOR_THIS_COORDINATE_SYSTEM, (types<CS_Tag>)
         );

};

template <typename CT>
struct vertex_latitude<CT, spherical_equatorial_tag>
        : vertex_latitude_on_sphere<CT>
{};

template <typename CT>
struct vertex_latitude<CT, geographic_tag>
        : vertex_latitude_on_spheroid<CT>
{};

}}} // namespace boost::geometry::formula

#endif // BOOST_GEOMETRY_FORMULAS_MAXIMUM_LATITUDE_HPP


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

    @copyright 2016 NumScale SAS

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

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

  /*!

    @ingroup group-arithmetic
    This function object computes two reals of the type of the inputs
    (in an std::pair)  @c r0 and @c r1 such that:

    @code
    r0 = x * y
    r1 = r0 -(x * y)
    @endcode

    using perfect arithmetic.

    Its main usage is to be able to compute
    prod of reals and the residual error using IEEE 754 arithmetic.

    @par Header <boost/simd/function/two_prod.hpp>

    @par Example:

      @snippet two_prod.cpp two_prod

    @par Possible output:

      @snippet two_prod.txt two_prod

  **/
  std::pair<Value, Value> two_prod(Value const& x, Value const& y);

} }
#endif

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

#endif


//
// Copyright 1997, 1998, 1999 University of Notre Dame.
// Authors: Andrew Lumsdaine, Jeremy G. Siek, Lie-Quan Lee
//
// This file is part of the Matrix Template Library
//
// You should have received a copy of the License Agreement for the
// Matrix Template Library along with the software;  see the
// file LICENSE.  If not, contact Office of Research, University of Notre
// Dame, Notre Dame, IN  46556.
//
// Permission to modify the code and to distribute modified code is
// granted, provided the text of this NOTICE is retained, a notice that
// the code was modified is included with the above COPYRIGHT NOTICE and
// with the COPYRIGHT NOTICE in the LICENSE file, and that the LICENSE
// file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//

#include <iostream>
#include <mtl/matrix.h>

int
main()
{
#ifdef MTL_DISABLE_BLOCKING
  std::cout << "Static blocking unsupported for this compiler" << std::endl;
#else
  using namespace mtl;
  //begin
  const int M = 4;
  const int N = 4;
  typedef matrix<double,
                 rectangle<>,
                 dense<>,
                 column_major >::type Matrix;
  Matrix A(M,N);

  for (int i = 0; i < M; ++i)
    for (int j = 0; j < N; ++j)
      A(i, j) = i * N + j;
  print_all_matrix(A);

  block_view<Matrix,2,2>::type
                     bA = blocked(A, blk<2,2>());
  print_partitioned_matrix(bA);

  block_view<Matrix>::type cA = blocked(A, 2, 2);
  print_partitioned_by_column(cA);
  //end
  return 0;
#endif
}


#ifndef ALEPH_GEOMETRY_TANGENT_SPACE_HH__
#define ALEPH_GEOMETRY_TANGENT_SPACE_HH__

#include <aleph/config/Eigen.hh>

#include <aleph/geometry/BruteForce.hh>
#include <aleph/geometry/FLANN.hh>

#include <aleph/geometry/distances/Euclidean.hh>

#include <aleph/math/AlgebraicSphere.hh>
#include <aleph/math/KahanSummation.hh>

#ifdef ALEPH_WITH_EIGEN
  #include <Eigen/Core>
  #include <Eigen/Cholesky>
  #include <Eigen/Eigenvalues>
  #include <Eigen/SVD>
#endif

#include <cassert>

#include <set>
#include <vector>

namespace aleph
{

namespace geometry
{

namespace detail
{

/**
  Model of a smooth decreasing weight function according to the
  original paper *Algebraic Point Set Surfaces* by Guennebaud &
  Gross.
*/

template <class T> T phi( T x )
{
  return x < 1 ? std::pow( 1 - x*x, T(4) ) : T();
}

} // namespace detail

#ifdef ALEPH_WITH_EIGEN

// Previous versions of Eigen have a bug that occurs when mixing dynamic
// and fixed-sized vectors:
//
//   http://eigen.tuxfamily.org/bz/show_bug.cgi?id=654
//
// Until a workaround has been identified, tangent space estimation will
// not be enabled for older versions.
#if EIGEN_VERSION_AT_LEAST(3,3,0)

class TangentSpace
{
public:
  using T        = double;

  using Matrix = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
  using Vector = Eigen::Matrix<T, 1, Eigen::Dynamic>;

  using Sphere = math::AlgebraicSphere<T>;

#if EIGEN_VERSION_AT_LEAST(3,3,0)
  using Index = Eigen::Index;
#else
  using Index = typename Matrix::Index;
#endif

  struct LocalTangentSpace
  {
    Matrix tangents;
    Vector normal;
    Vector position;

    T localFeatureSize;
    std::vector<std::size_t> indices;
  };

  template <class Container> std::vector<T> operator()( const Container& container, unsigned k )
  {
    std::vector<double> curvature;
    curvature.reserve( container.size() );

    auto lts     = localTangentSpaces( container, k );
    auto spheres = fitSpheresWithoutNormals( container, lts );

    std::transform( spheres.begin(), spheres.end(), std::back_inserter( curvature ),
      [] ( const Sphere& sphere )
      {
        return sphere.meanCurvature();
      }
    );

    return curvature;
  }

private:

  /**
    Given a container and a number for determining the local
    neighbourhood of points, this function estimates (!) the
    tangent space structure around every point, resulting in
    a set of normal vectors and tangent vectors.

    @param container Container
    @param k         Local neighbourhood size
  */

  template <class Container> std::vector<LocalTangentSpace> localTangentSpaces( const Container& container, unsigned k )
  {
    using ElementType = typename Container::ElementType;
    using Distance    = distances::Euclidean<ElementType>;

#ifdef ALEPH_WITH_FLANN
    using NearestNeighbours = FLANN<Container, Distance>;
#else
    using NearestNeighbours = BruteForce<Container, Distance>;
#endif

    NearestNeighbours nearestNeighbours( container );
    using IndexType = typename NearestNeighbours::IndexType;

    std::vector< std::vector<IndexType> > indices;
    std::vector< std::vector<ElementType> > distances;

    nearestNeighbours.neighbourSearch( k,
                                       indices,
                                       distances );

    std::vector<LocalTangentSpace> localTangentSpaces;

    auto n = container.size();
    auto d = container.dimension();

    for( std::size_t i = 0; i < n; i++ )
    {
      // This coordinate matrix will contain the differences to the
      // centroid coordinate. The matrix will be transformed via an
      // SVD.
      Matrix M = Matrix::Zero( Index(k), Index(d) );

      // Centroid calculation ------------------------------------------

      Vector centroid  = Vector::Zero(1, Index(d) );

      for( std::size_t j = 0; j < indices[i].size(); j++ )
      {
        auto&& neighbourIndex = indices[i][j];
        auto v                = getPosition( container, neighbourIndex );

        centroid          += v;
        M.row( Index(j) )  = v;
      }

      centroid /= static_cast<T>( indices.size() );

      // Coordinate matrix setup ---------------------------------------

      M = M.rowwise() - centroid;

      Eigen::JacobiSVD<Matrix> svd( M, Eigen::ComputeThinV );

      LocalTangentSpace lts;
      lts.tangents = Matrix::Zero( Index(d), Index(d - 1) );

      // The singular vectors of all but the *smallest* singular value
      // form the tangential directions of the tangent space.

      auto&& V = svd.matrixV();

      // Actual or "effective" dimensionality of the input data. If the
      // matrix is rectangular (and most input matrices will be), there
      // is not a full system of singular vectors available.
      auto dEffective  = std::min( Index(d-1), Index(k-1) );

      for( Index j = 0; j < dEffective; j++ )
        lts.tangents.col(j) = V.col(j);

      lts.normal           = V.col( dEffective  ).normalized();
      lts.position         = getPosition( container, i );
      lts.indices          = indices[i];

      // Take the *maximum distance* in which we can find all of the
      // neighbours as a *rough*  approximation to the local feature
      // size.
      lts.localFeatureSize
        = distances[i].empty() == false ?
            *std::max_element( distances[i].begin(), distances[i].end() )
          : T();

      localTangentSpaces.push_back( lts );

      // TODO:
      //
      // 1. Calculate raw approximation (reconstruction) error by
      //    assessing how well the space fits the original data
      //
      // 2. Make normal orientation consistent. I am unsure as to
      //    whether this will improve the results or not.
    }

    propagateOrientation( localTangentSpaces );
    return localTangentSpaces;
  }

  template <class Container>
    std::vector<Sphere> fitSpheres( const Container& container,
                                    const std::vector<LocalTangentSpace>& localTangentSpaces )
  {
    using namespace detail;

    std::vector<Sphere> spheres;
    spheres.reserve( container.size() );

    for( auto&& lts : localTangentSpaces )
    {
      auto d   = Index( container.dimension() );
      Matrix A = Matrix::Zero( d+2, d+2 );
      Vector b = Vector::Zero( 1,   d+2 );

      auto&& indices = lts.indices;

      // Pre-processing --------------------------------------------------
      //
      // Choose a value for the beta parameter, based on the weighted
      // neighbourhood sizes of *all* points. This requires iterating
      // over all points prior to calculating anything else.

      T beta = T();

      {
        std::vector<T> W;
        std::vector<T> H;
        W.reserve( container.size() );
        H.reserve( container.size() );

        for( auto&& index : indices )
        {
          auto&& neighbour = getPosition( container, index );
          W.emplace_back( phi( ( lts.position - neighbour ).norm() / lts.localFeatureSize ) );
          H.emplace_back( lts.localFeatureSize );
        }

        // Sum of weights *before* applying the local feature size
        // multiplier; we need to save this result because it will
        // be required below.
        auto ws = math::accumulate_kahan_sorted( W.begin(), W.end(), T() );

        // Apply weights to local feature size estimates; afterwards we
        // can finally obtain the scaling factor from this weighted sum
        for( std::size_t i = 0; i < W.size(); i++ )
          W[i] = W[i] * H[i];

        // TODO: make initial guess for beta (10e6) configurable?
        auto h = math::accumulate_kahan_sorted( W.begin(), W.end(), T() ) / ws;
        beta   = 10e6 * h * h;
      }

      Index k  = Index( indices.size() );
      Matrix W = Matrix::Zero( (d+1)*k, (d+1)*k );
      Matrix D = Matrix::Zero( (d+1)*k, (d+2)   );
      Matrix c = Matrix::Zero( (d+1)*k, 1       );

      {
        Index i = Index();

        for( auto&& index : indices )
        {
          auto neighbour       = getPosition( container, index );
          auto w               = phi( ( lts.position - neighbour ).norm() / lts.localFeatureSize );
          W( i*(d+1),i*(d+1) ) = w;

          for( Index(j) = 0; j < d; j++ )
          {
            assert( W( i*(d+1)+j+1, i*(d+1)+j+1 ) == 0 );
            W( i*(d+1)+j+1, i*(d+1)+j+1 ) = beta * w;
          }

          ++i;
        }
      }

      {
        Index i = Index();

        for( auto&& index : indices )
        {
          auto neighbour       = getPosition( container, index );
          D( i*(d+1), 0 )      = 1.0;

          for( Index(j) = 0; j < d; j++ )
            D( i*(d+1), j+1 ) = neighbour(j);

          D( i*(d+1), d+1) = neighbour * neighbour.transpose();

          for( Index(j) = 0; j < d; j++ )
          {
            D( i*(d+1)+j+1, j+1 ) = 1;
            D( i*(d+1)+j+1, d+1 ) = 2 * neighbour(j);
            c( i*(d+1)+j+1, 0   ) = localTangentSpaces.at(index).normal(j);
          }

          ++i;
        }
      }

      for( auto&& index : indices )
      {
        auto neighbour           = getPosition( container, index );
        auto squaredNeigbourNorm = neighbour.squaredNorm();
        auto w                   = phi( ( lts.position - neighbour ).norm() / lts.localFeatureSize );

        A(   0,   0) += w;
        A( d+1,   0) += w * squaredNeigbourNorm;
        A( d+1, d+1) += w * squaredNeigbourNorm * squaredNeigbourNorm;

        for( Index(i) = 1; i < d+1; i++ )
        {
          A(  i,   i) += w * ( neighbour(i-1)*neighbour(i-1) + 1 ) * beta;
          A(  i,   0) += w * ( neighbour(i-1) );
          A(d+1,   i) += w * ( neighbour(i-1)*squaredNeigbourNorm + 2 * beta * neighbour(i-1) );
          A(d+1, d+1) += w * ( 4*neighbour(i-1)*neighbour(i-1) ) * beta;

          // re-establish symmetry
          A(  0,   i)  = A(i,0);
          A(  i, d+1)  = A(d+1, i);

          b(i  ) +=       beta * w * localTangentSpaces.at(index).normal(i-1);
          b(d+1) += 2.0 * beta * w * localTangentSpaces.at(index).normal(i-1) * neighbour(i-1);

          for( Index(j) = i+1; j < d+1; j++ )
          {
            A(j, i) += w * neighbour(i-1) * neighbour(j-1);
            A(i, j)  = A(j,i);
          }
        }

        // re-establish symmetry
        A(0, d+1) = A(d+1, 0);
      }

      // Solve the linear system ---------------------------------------
      //
      // The solution of the system Ax = b is used to obtain the
      // coefficients of the algebraic sphere.

      using Solver = Eigen::LDLT<Matrix>;
      Solver solver(A);

      Vector u = solver.solve( b.transpose() );

      spheres.emplace_back( Sphere( u.data(), u.data() + u.size() ) );
    }

    return spheres;
  }

  template <class Container>
    std::vector<Sphere> fitSpheresWithoutNormals( const Container& container,
                                                  const std::vector<LocalTangentSpace>& localTangentSpaces )
  {
    using namespace detail;

    std::vector<Sphere> spheres;
    spheres.reserve( container.size() );

    for( auto&& lts : localTangentSpaces )
    {
      auto&& indices = lts.indices;
      auto d         = Index( container.dimension() );
      auto k         = Index( indices.size() );

      Matrix W = Matrix::Zero(k,k  );
      Matrix D = Matrix::Zero(k,d+2);
      Matrix C = Matrix::Identity(d+2,d+2);

      C(0  ,  0) =  0;
      C(0  ,d+1) = -2;
      C(d+1,  0) = -2;
      C(d+1,d+1) =  0;

      {
        Index i = Index();
        for( auto&& index : indices )
        {
          auto neighbour = getPosition( container, index );
          W(i,  i)       = phi( ( lts.position - neighbour ).norm() / lts.localFeatureSize );
          D(i,  0)       = 1;
          D(i,d+1)       = neighbour * neighbour.transpose();

          for( Index j = 0; j < d; j++ )
            D(i,j+1) = neighbour(j);

          ++i;
        }
      }

      // Solve the linear system ---------------------------------------
      //
      // The solution of the system Ax = b is used to obtain the
      // coefficients of the algebraic sphere.

      using Solver = Eigen::GeneralizedEigenSolver<Matrix>;
      Solver solver;
      solver.compute( D.transpose() * W * D, C );

      auto eigenvalues = solver.eigenvalues();
      Vector u         = Vector::Zero( 1, d+2 );

      for( Index i = 0; i < d+2; i++ )
      {
        if( eigenvalues(i).real() > 0 && eigenvalues(i).imag() == 0 )
        {
          u = solver.eigenvectors().real().col(i);
          break;
        }
      }

      spheres.emplace_back( Sphere( u.data(), u.data() + u.size() ) );
    }

    return spheres;
  }

  void propagateOrientation( std::vector<LocalTangentSpace>& localTangentSpaces )
  {
    using Edge = std::pair<std::size_t, std::size_t>;

    std::set<Edge> edges;
    for( std::size_t i = 0; i < localTangentSpaces.size(); i++ )
    {
      for( auto&& index : localTangentSpaces.at(i).indices )
      {
        if( i < index )
          edges.insert( std::make_pair(i,index) );
        else
          edges.insert( std::make_pair(index,i) );
      }
    }

    for( auto&& edge : edges )
    {
      auto i    = edge.first;
      auto&& ni = localTangentSpaces.at(i).normal;
      auto j    = edge.second;
      auto&& nj = localTangentSpaces.at(j).normal;

      if( ni * nj.transpose() < 0 )
        nj = -nj;
    }
  }

  /**
    Auxiliary function for extracting and converting a position from
    a given container, storing it as a (mathematical) vector.
  */

  template <class Container> Vector getPosition( const Container& container, std::size_t i )
  {
    auto d   = container.dimension();
    auto p   = container[i];
    Vector v = Vector::Zero(1, Index(d) );

    // copy (and transform!) the vector; there's an implicit type
    // conversion going on here
    for( std::size_t l = 0; l < d; l++ )
      v( Index(l) ) = p[l];

    return v;
  }
};

#endif

#endif

} // namespace geometry

} // namespace aleph

#endif


/*
 * Copyright (c) Contributors to the Open 3D Engine Project.
 * For complete copyright and license terms please see the LICENSE at the root of this distribution.
 *
 * SPDX-License-Identifier: Apache-2.0 OR MIT
 *
 */

#include <NumericalMethods_precompiled.h>
#include <NumericalMethods/Eigenanalysis.h>
#include <NumericalMethods/Optimization.h>
#include <Optimization/SolverBFGS.h>
#include <Eigenanalysis/Solver3x3.h>

namespace NumericalMethods
{
    namespace Optimization
    {
        SolverResult SolverBFGS(const Function& function, const AZStd::vector<double>& initialGuess)
        {
            return MinimizeBFGS(function, initialGuess);
        }
    }

    namespace Eigenanalysis
    {
        SolverResult<Real, 3> Solver3x3RealSymmetric(const SquareMatrix<Real, 3>& matrix)
        {
            // The matrix must be symmetric.
            if (matrix[0][1] == matrix[1][0] && matrix[0][2] == matrix[2][0] && matrix[1][2] == matrix[2][1])
            {
                return NonIterativeSymmetricEigensolver3x3(
                    matrix[0][0], matrix[0][1], matrix[0][2],
                                  matrix[1][1], matrix[1][2],
                                                matrix[2][2]
                );
            }
            else
            {
                return SolverResult<Real, 3>{SolverOutcome::FailureInvalidInput};
            }
        }
    }
} // namespace NumericalMethods


//#include <lapacke.h>
#include <stdio.h>
#include <Eigen/Dense>
#include <stdexcept>
#include <iostream>

char lower = 'L';

// TODO, remove dependency on lapacke.h
extern "C" void dpotrf_(char *uplo, int *n, double *a, int *lda, int *info);
extern "C" void dpotrs_(char *uplo, int* n, int* nrhs, double* A, int* lda, double* B, int* ldb, int* info);

void chol_decomp(double* A, int n) {
  int info;
  dpotrf_(&lower, &n, A, &n, &info);
  if(info != 0){ throw std::runtime_error("c++ error: Cholesky decomp failed"); }
}

void chol_decomp(Eigen::MatrixXd & A) {
  int info, n = A.rows();
  dpotrf_(&lower, &n, A.data(), &n, &info);
  if(info != 0) {
    throw std::runtime_error(
        std::string("c++ error: Cholesky decomp failed (for ")
        + std::to_string(n)
        + " x "
        + std::to_string(n)
        + " eigen matrix)");
  }
}

void chol_solve(double* A, int n, double* B, int nrhs) {
  int info;
  dpotrs_(&lower, &n, &nrhs, A, &n, B, &n, &info);
  if(info != 0){ throw std::runtime_error("c++ error: Cholesky solve failed");}
}

void chol_solve(Eigen::MatrixXd & A, Eigen::MatrixXd & B) {
  if (A.rows() != B.rows()) {throw std::runtime_error("A.rows() must equal B.rows()");}
  int info;
  int n    = A.rows();
  int nrhs = B.cols();
  dpotrs_(&lower, &n, &nrhs, A.data(), &n, B.data(), &n, &info);
  if(info != 0){ throw std::runtime_error("c++ error: Cholesky solve failed (for eigen matrix)");}
}

/** solves A * X' = B' for X in place */
void chol_solve_t(Eigen::MatrixXd & A, Eigen::MatrixXd & B) {
  if (A.rows() != B.cols()) {throw std::runtime_error("A.rows() must equal B.cols()");}
  B.transposeInPlace();
  chol_solve(A, B);
  B.transposeInPlace();
}