#include <boost/graph/adjacency_list.hpp>
#include <tuple>
#include <algorithm>
#include <iterator>
#include <iostream>

int main()
{
  typedef boost::adjacency_list<boost::setS, boost::vecS,
    boost::undirectedS> graph;
  graph g;

  enum { topLeft, topRight, bottomRight, bottomLeft };

  boost::add_edge(topLeft, topRight, g);
  boost::add_edge(topRight, bottomRight, g);
  boost::add_edge(bottomRight, bottomLeft, g);
  boost::add_edge(bottomLeft, topLeft, g);

  graph::edge_iterator it, end;
  std::tie(it, end) = boost::edges(g);
  std::copy(it, end,
    std::ostream_iterator<graph::edge_descriptor>{std::cout, "\n"});
}


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
/// \file find_test.cpp
/// \brief Test for find functions.

#define BOOST_TEST_MODULE find_test
#include <boost/test/included/unit_test.hpp>

#include "mcrl2/data/consistency.h"
#include "mcrl2/lps/find.h"
#include "mcrl2/lps/parse.h"

using namespace mcrl2;
using namespace mcrl2::lps;

inline
data::variable nat(const std::string& name)
{
  return data::variable(core::identifier_string(name), data::sort_nat::nat());
}

inline
data::variable pos(const std::string& name)
{
  return data::variable(core::identifier_string(name), data::sort_pos::pos());
}

inline
data::variable bool_(const std::string& name)
{
  return data::variable(core::identifier_string(name), data::bool_());
}

template <typename T>
std::string print_set(const std::set<T>& v)
{
  std::set<std::string> s;
  for (const T& v_i: v)
  {
    s.insert(data::pp(v_i));
  }
  return core::detail::print_set(s);
}

BOOST_AUTO_TEST_CASE(test_find)
{
  using utilities::detail::contains;

  std::string SPEC =
    "glob                      \n"
    "  m: Nat;                 \n"
    "                          \n"
    "act                       \n"
    "  a: Nat;                 \n"
    "                          \n"
    "proc                      \n"
    "  P(n:Nat) = a(m).P(n+1); \n"
    "                          \n"
    "init P(0);                \n"
  ;

  specification lpsspec = parse_linear_process_specification(SPEC);
  action_summand s = lpsspec.process().action_summands().front();
  process::action a = s.multi_action().actions().front();

  BOOST_CHECK_EQUAL(print_set(lps::find_all_variables(s)), "{ m, n }");
  BOOST_CHECK_EQUAL(print_set(lps::find_sort_expressions(a)), "{ Nat }");
}

BOOST_AUTO_TEST_CASE(test_free_variables)
{
  std::set<data::variable> free_variables;

  lps::specification specification(parse_linear_process_specification(
                                     "act a : Bool;\n"
                                     "proc X = a((forall x : Nat. exists y : Nat. x < y)).X;\n"
                                     "init X;\n"
                                   ));

  free_variables = find_free_variables(specification.process());
  BOOST_CHECK(free_variables.find(data::variable("x", data::sort_nat::nat())) == free_variables.end());
  BOOST_CHECK(free_variables.find(data::variable("y", data::sort_nat::nat())) == free_variables.end());

  specification = parse_linear_process_specification(
                    "act a;\n"
                    "proc X(z : Bool) = (z && (forall x : Nat. exists y : Nat. x < y)) -> a.X(!z);\n"
                    "init X(true);\n"
                  );
  free_variables = find_free_variables(specification);
  BOOST_CHECK(free_variables.empty());

  free_variables = find_free_variables(specification.process());
  BOOST_CHECK(free_variables.empty());

  free_variables = find_free_variables(specification.process().action_summands().front());
  BOOST_CHECK(free_variables.size() == 1);
  BOOST_CHECK(free_variables.find(data::variable("z", data::bool_())) != free_variables.end());

  BOOST_CHECK(check_well_typedness(specification));
}

BOOST_AUTO_TEST_CASE(test_search)
{
  lps::specification spec(parse_linear_process_specification(
                            "glob dc: Nat;\n"
                            "act a : Bool;\n"
                            "proc X = a((forall x : Nat. exists y : Nat. (x < y) && (y < dc))).X;\n"
                            "init X;\n"
                          ));
  data::variable x("x", data::sort_nat::nat());
  BOOST_CHECK(!lps::search_free_variable(spec.process().action_summands(), x));

  data::variable y("y", data::sort_nat::nat());
  BOOST_CHECK(!lps::search_free_variable(spec.process().action_summands(), y));

  data::variable dc("dc", data::sort_nat::nat());
  BOOST_CHECK(lps::search_free_variable(spec.process().action_summands(), dc));
}

BOOST_AUTO_TEST_CASE(test_search_sort_expression)
{
  std::string text =
    "act a: List(Bool);                   \n"
    "proc X(x: List(Bool)) = a(x) . X(x); \n"
    "init X([true]);                      \n"
    ;
  lps::specification spec = parse_linear_process_specification(text);
  data::sort_expression s = data::parse_sort_expression("List(Bool)");
  BOOST_CHECK(data::search_sort_expression(spec.data().sorts(), s));
}


#include <Eigen/Core>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/time.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/passthrough.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <tf/transform_listener.h>

#include <kuri_mbzirc_challenge_2_tools/pose_conversion.h>

//convenient typedefs
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;

// PCL Visualization
pcl::visualization::PCLVisualizer *p;
int vp_1, vp_2;

// State variables
bool is_initialized_ = false;
bool is_done_ = false;
PointCloud::Ptr pc_current_, pc_prev_;

tf::TransformListener* listener;

////////////////////////////////////////////////////////////////////////////////
/** \brief Display source and target on the first viewport of the visualizer
 *
 */
void showCloudsLeft(const PointCloud::Ptr cloud_target, const PointCloud::Ptr cloud_source)
{
  p->removePointCloud ("vp1_target");
  p->removePointCloud ("vp1_source");

  pcl::visualization::PointCloudColorHandlerCustom<PointT> tgt_h (cloud_target, 0, 255, 0);
  pcl::visualization::PointCloudColorHandlerCustom<PointT> src_h (cloud_source, 255, 0, 0);
  p->addPointCloud (cloud_target, tgt_h, "vp1_target", vp_1);
  p->addPointCloud (cloud_source, src_h, "vp1_source", vp_1);
}

////////////////////////////////////////////////////////////////////////////////
/** \brief Display source and target on the second viewport of the visualizer
 *
 */
void showCloudsRight(const PointCloud::Ptr cloud_target, const PointCloud::Ptr cloud_source)
{
  p->removePointCloud ("vp2_target");
  p->removePointCloud ("vp2_source");

  pcl::visualization::PointCloudColorHandlerCustom<PointT> tgt_h (cloud_target, 0, 255, 0);
  pcl::visualization::PointCloudColorHandlerCustom<PointT> src_h (cloud_source, 255, 0, 0);
  p->addPointCloud (cloud_target, tgt_h, "vp2_target", vp_2);
  p->addPointCloud (cloud_source, src_h, "vp2_source", vp_2);
}

////////////////////////////////////////////////////////////////////////////////
/** \brief Align a pair of PointCloud datasets and return the result
  * \param cloud_src the source PointCloud
  * \param cloud_tgt the target PointCloud
  * \param output the resultant aligned source PointCloud
  * \param final_transform the resultant transform between source and target
  */
void pairAlign (const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr output, Eigen::Matrix4f &final_transform, bool downsample = false)
{
  //
  // Downsample for consistency and speed
  // \note enable this for large datasets
  PointCloud::Ptr src (new PointCloud);
  PointCloud::Ptr tgt (new PointCloud);
  pcl::VoxelGrid<PointT> grid;
  if (downsample)
  {
    double leaf = 0.2;
    grid.setLeafSize (leaf, leaf, leaf);
    grid.setInputCloud (cloud_src);
    grid.filter (*src);

    grid.setInputCloud (cloud_tgt);
    grid.filter (*tgt);
  }
  else
  {
    src = cloud_src;
    tgt = cloud_tgt;
  }

  //
  // Run the same optimization in a loop and visualize the results
  Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity (), prev, targetToSource;
  PointCloud::Ptr reg_result (new PointCloud);

  //
  // Align
  pcl::IterativeClosestPoint<PointT, PointT> reg;
  reg.setMaxCorrespondenceDistance (10.0);

  reg.setTransformationEpsilon (1e-8);
  reg.setMaximumIterations (50);
  reg.setInputSource (src);
  reg.setInputTarget (tgt);
  reg.align (*reg_result);

  // Get the transformation from source to target
  Ti = reg.getFinalTransformation ();
  //targetToSource = Ti.inverse();

  //
  // Transform target back in source frame
  pcl::transformPointCloud (*cloud_src, *output, Ti);
  final_transform = Ti;
 }

void callbackVelo(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
{
  if (is_done_)
    return;

  // Convert msg to pointcloud
  PointCloud cloud;

  pcl::fromROSMsg (*cloud_msg, cloud);
  pc_current_ = cloud.makeShared();

  // Transform to the more stable odom frame
  tf::StampedTransform transform;
  try
  {
    listener->lookupTransform("odom", "velodyne", ros::Time(0), transform);
  }
  catch (tf::TransformException ex)
  {
    ROS_ERROR("%s",ex.what());
    ros::Duration(1.0).sleep();
  }

  Eigen::Matrix4d Ti = pose_conversion::convertStampedTransform2Matrix4d(transform);

  // Transform cloud
  //PointCloud::Ptr output (new PointCloud);
  //pcl::transformPointCloud (*pc_current_, *output, Ti);
  pcl::transformPointCloud (*pc_current_, *pc_current_, Ti);

  if (!is_initialized_)
  {
    pc_prev_ = pc_current_;
    is_initialized_ = true;

    PCL_INFO ("Press q to begin the registration.\n");
    showCloudsLeft(pc_prev_, pc_current_);
    p-> spin();
    return;
  }

  //Filter inputs
  pcl::console::print_highlight ("Filtering z...\n");
  pcl::PassThrough<pcl::PointXYZ> pass;
  pass.setFilterFieldName ("z");
  pass.setFilterLimits (0.2, 3);
  //pass.setFilterLimitsNegative (true);
  pass.setInputCloud (pc_prev_);
  pass.filter (*pc_prev_);
  pass.setInputCloud (pc_current_);
  pass.filter (*pc_current_);

  // Align
  PointCloud::Ptr result (new PointCloud);
  Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity (), pairTransform;

  // Add visualization data
  showCloudsLeft(pc_prev_, pc_current_);

  PointCloud::Ptr temp (new PointCloud);
  //PCL_INFO ("Aligning %s (%d) with %s (%d).\n", data[i-1].f_name.c_str (), source->points.size (), data[i].f_name.c_str (), target->points.size ());
  {
    pcl::ScopeTime t("Alignment");
    pairAlign (pc_prev_, pc_current_, temp, pairTransform, true);
  }

  showCloudsRight(pc_current_, temp);
  p->spinOnce();

  //std::cout << pairTransform << "\n";

  // Update clouds
  pc_prev_ = pc_current_;
  //is_done_ = true;

  p->spinOnce();

}

// Align a rigid object to a scene with clutter and occlusions
int main (int argc, char **argv)
{
  ros::init(argc, argv, "test_velodyne_alignment");
  ros::NodeHandle node;

  listener = new tf::TransformListener();
  ros::Subscriber sub_velo  = node.subscribe("/velodyne_points", 1, callbackVelo);

  // Create a PCLVisualizer object
  p = new pcl::visualization::PCLVisualizer (argc, argv, "Pairwise Incremental Registration example");
  p->createViewPort (0.0, 0, 0.5, 1.0, vp_1);
  p->createViewPort (0.5, 0, 1.0, 1.0, vp_2);

  ros::spin();
  return 0;
}


/******************************************************************************
Copyright (c) 2017, Farbod Farshidian. All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of the copyright holder nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
******************************************************************************/

#include <Eigen/Core>
#include <cppad/cg.hpp>
#include <iostream>
#include <memory>
#include <vector>

int main(void) {
  typedef double scalar_t;
  typedef CppAD::cg::CG<scalar_t> ad_base_t;
  typedef CppAD::AD<ad_base_t> ad_t;
  typedef CppAD::ADFun<ad_base_t> ad_fun_t;

  const size_t DOMAIN_DIM = 3;
  const size_t RANGE_DIM = 1;

  /***************************************************************************
   *                               the model
   **************************************************************************/

  // set and declare independent variables and start tape recording
  Eigen::Matrix<ad_t, Eigen::Dynamic, 1> x(DOMAIN_DIM);
  x.setZero();
  CppAD::Independent(x);

  // dependent variable vector
  Eigen::Matrix<ad_t, Eigen::Dynamic, 1> y(RANGE_DIM);

  // the model equation
  y(0) = (x(0) / 1.0 + x(1) * x(1)) / 2.0 + -5.0 * x(2);
  //    y(1) = -2.0 * x(0) - 3.0 * x(1) + -4.0 * x(2);

  // create f: x -> y and stop tape recording
  ad_fun_t fun(x, y);

  // Optimize the operation sequence
  fun.optimize();

  /***************************************************************************
   *                              Sparsity Pattern
   **************************************************************************/
  std::vector<size_t> rows;
  std::vector<size_t> cols;

  rows.push_back(0);
  cols.push_back(0);

  rows.push_back(0);
  cols.push_back(1);

  rows.push_back(1);
  cols.push_back(1);

  /***************************************************************************
   *                       Create the dynamic library
   *                  (generates and compiles source code)
   **************************************************************************/
  // generates source code
  CppAD::cg::ModelCSourceGen<scalar_t> cgen(fun, "model");
  cgen.setCreateSparseHessian(true);
  cgen.setCustomSparseHessianElements(rows, cols);

  CppAD::cg::ModelLibraryCSourceGen<scalar_t> libcgen(cgen);

  // compile source code
  const std::string libraryName = "cppad_bug_model";
  CppAD::cg::DynamicModelLibraryProcessor<scalar_t> p(libcgen, libraryName);

  CppAD::cg::GccCompiler<scalar_t> compiler;
  std::unique_ptr<CppAD::cg::DynamicLib<scalar_t>> dynamicLib(p.createDynamicLibrary(compiler));

  /***************************************************************************
   *                       Use the dynamic library
   **************************************************************************/

  std::unique_ptr<CppAD::cg::GenericModel<scalar_t>> model(dynamicLib->model("model"));

  Eigen::Matrix<scalar_t, 1, Eigen::Dynamic> xv(DOMAIN_DIM);
  xv << 2.5, 3.5, 0.0;

  if (model->isSparseHessianAvailable()) {
    Eigen::Matrix<scalar_t, 1, Eigen::Dynamic> weight(RANGE_DIM);
    Eigen::Matrix<scalar_t, 1, Eigen::Dynamic> hess(DOMAIN_DIM * DOMAIN_DIM);
    Eigen::Matrix<scalar_t, DOMAIN_DIM, DOMAIN_DIM> hessMat;

    for (size_t i = 0; i < RANGE_DIM; i++) {
      weight.setZero();
      weight(i) = 1.0;

      model->SparseHessian(xv, weight, hess);

      hessMat = Eigen::Map<Eigen::Matrix<scalar_t, DOMAIN_DIM, DOMAIN_DIM>>(hess.data());

      // print out the result
      std::cout << "hessMat[" << i << "]: " << std::endl;
      std::cout << hessMat << std::endl;
    }
  }
}


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

#ifndef KALMAN_FILTER_HPP_
#define KALMAN_FILTER_HPP_

#include <Eigen/Dense>

namespace mineral_deposit_tracking
{

class KalmanFilter
{
public:
  KalmanFilter(
    const Eigen::Matrix2d & transition_matrix,
    const Eigen::Matrix2d & process_covariance,
    const Eigen::Matrix2d & observation_matrix);

  void TimeUpdate();

  void MeasurementUpdate(
    const Eigen::Vector2d & measurement,
    const Eigen::Matrix2d & measurement_covariance);

  void Reset(const Eigen::Vector2d & initial_state, const Eigen::Matrix2d & initial_covariance);

  const Eigen::Vector2d & GetEstimate();

  const Eigen::Matrix2d & GetEstimateCovariance();

private:
  const Eigen::Matrix2d transition_matrix_;
  const Eigen::Matrix2d process_covariance_;
  const Eigen::Matrix2d observation_matrix_;
  Eigen::Vector2d estimate_;
  Eigen::Matrix2d estimate_covariance_;
};

}  // namespace mineral_deposit_tracking

#endif  // KALMAN_FILTER_HPP_


#include <iostream>
#include <utility>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>

class Graph {
  public:
    using AdjacencyList = boost::adjacency_list<
      boost::vecS,        // OutEdgeListS
      boost::vecS,        // VertexListS
      boost::directedS,   // DirectedS
      boost::no_property, // VertexProperty
      boost::no_property, // EdgeProperty
      boost::no_property, // GraphProperty
      boost::vecS>;       // EdgeListS
    using Vertex = boost::graph_traits<AdjacencyList>::vertex_descriptor;
    using Edge = std::pair<int, int>;

    template <class EdgeIterator>
    Graph(EdgeIterator begin, EdgeIterator end, int vcount = 0)
    : adjlist_(begin, end, vcount) {}

    const AdjacencyList& data() {return adjlist_;}
  private:
    AdjacencyList adjlist_;
};

int main() {
  std::vector<Graph::Edge> edges{
    Graph::Edge(0, 1),
    Graph::Edge(0, 2),
    Graph::Edge(1, 3),
    Graph::Edge(1, 4),
    Graph::Edge(2, 5),
    Graph::Edge(2, 6)
  };
  Graph g(edges.begin(), edges.end());
  boost::print_graph(g.data(), std::cout);

  Graph::Vertex from(0);
  Graph::Vertex to(6);
  const auto vcount = boost::num_vertices(g.data());
  std::vector<int> distances(vcount);
  std::vector<Graph::Vertex> predecessors(vcount);

  boost::breadth_first_search(g.data(), from,
    boost::visitor(boost::make_bfs_visitor(std::make_pair(
      boost::record_distances(distances.data(), boost::on_tree_edge()),
      boost::record_predecessors(predecessors.data(), boost::on_tree_edge())
    )))
  );
  std::cout << "distances:\n";
  for (const auto x: distances) {
    std::cout << x << "\n";
  }
  std::cout << "predecessors:\n";
  for (const auto x: predecessors) {
    std::cout << x << "\n";
  }

  std::deque<Graph::Vertex> route;
  for (auto v = to; v != from; v = predecessors[v]) {
    route.push_front(v);
  }
  route.push_front(from);

  std::cout << "shortest path:\n";
  for (const auto v: route) {
      std::cout << v << " ";
  }
  std::cout << "\n";

  return 0;
}


//==================================================================================================
/*!
  @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_NEGATENZ_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_NEGATENZ_HPP_INCLUDED

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

 /*!

    @ingroup group-ieee
    Function object implementing negatenz capabilities

    Returns x multiplied by the @ref signnz of y.

    The result is unspecified if y is @ref Nan

    @par Semantic:

    @code
    auto r = negatenz(x,y);
    @endcode

    is similar to:

    @code
    auto r = x*signnz(y);
    @endcode

    @par Note
    If x is  null the result is @ref \f$\pm0\f$. Take care that negatenz distinguish
    between the signs of T(0) and -T(0) when T is a floating type.

    @see negatenz, Mzero, is_positive, is_negative

  **/
  Value negatenz(Value const & x, value const& y);
} }
#endif

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

#endif


// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// 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)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

// Purpose:  Implementation of the aeqd (Azimuthal Equidistant) projection.
// Author:   Gerald Evenden
// Copyright (c) 1995, Gerald Evenden

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#ifndef BOOST_GEOMETRY_PROJECTIONS_AEQD_HPP
#define BOOST_GEOMETRY_PROJECTIONS_AEQD_HPP

#include <boost/config.hpp>
#include <boost/geometry/formulas/vincenty_direct.hpp>
#include <boost/geometry/formulas/vincenty_inverse.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/math/special_functions/hypot.hpp>

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/impl/aasincos.hpp>
#include <boost/geometry/srs/projections/impl/pj_mlfn.hpp>

#include <boost/type_traits/is_same.hpp>

namespace boost { namespace geometry
{

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace aeqd
    {

            static const double epsilon10 = 1.e-10;
            static const double tolerance = 1.e-14;
            enum mode_type {
                n_pole = 0,
                s_pole = 1,
                equit  = 2,
                obliq  = 3
            };

            template <typename T>
            struct par_aeqd
            {
                T    sinph0;
                T    cosph0;
                detail::en<T> en;
                T    M1;
                T    N1;
                T    Mp;
                T    He;
                T    G;
                mode_type mode;
                srs::spheroid<T> spheroid;
            };

            template <typename T, typename Par, typename ProjParm>
            inline void e_forward(T const& lp_lon, T const& lp_lat, T& xy_x, T& xy_y, Par const& par, ProjParm const& proj_parm)
            {
                T coslam, cosphi, sinphi, rho;
                //T azi1, s12;
                //T lam1, phi1, lam2, phi2;

                coslam = cos(lp_lon);
                cosphi = cos(lp_lat);
                sinphi = sin(lp_lat);
                switch (proj_parm.mode) {
                case n_pole:
                    coslam = - coslam;
                    BOOST_FALLTHROUGH;
                case s_pole:
                    xy_x = (rho = fabs(proj_parm.Mp - pj_mlfn(lp_lat, sinphi, cosphi, proj_parm.en))) *
                        sin(lp_lon);
                    xy_y = rho * coslam;
                    break;
                case equit:
                case obliq:
                    if (fabs(lp_lon) < epsilon10 && fabs(lp_lat - par.phi0) < epsilon10) {
                        xy_x = xy_y = 0.;
                        break;
                    }

                    //phi1 = par.phi0; lam1 = par.lam0;
                    //phi2 = lp_lat;  lam2 = lp_lon + par.lam0;

                    formula::result_inverse<T> const inv =
                        formula::vincenty_inverse
                            <
                                T, true, true
                            >::apply(par.lam0, par.phi0, lp_lon + par.lam0, lp_lat, proj_parm.spheroid);
                    //azi1 = inv.azimuth; s12 = inv.distance;
                    xy_x = inv.distance * sin(inv.azimuth) / par.a;
                    xy_y = inv.distance * cos(inv.azimuth) / par.a;
                    break;
                }
            }

            template <typename T, typename Par, typename ProjParm>
            inline void e_inverse(T const& xy_x, T const& xy_y, T& lp_lon, T& lp_lat, Par const& par, ProjParm const& proj_parm)
            {
                T c;

                if ((c = boost::math::hypot(xy_x, xy_y)) < epsilon10) {
                    lp_lat = par.phi0;
                    lp_lon = 0.;
                        return;
                }
                if (proj_parm.mode == obliq || proj_parm.mode == equit) {
                    T const x2 = xy_x * par.a;
                    T const y2 = xy_y * par.a;
                    //T const lat1 = par.phi0;
                    //T const lon1 = par.lam0;
                    T const azi1 = atan2(x2, y2);
                    T const s12 = sqrt(x2 * x2 + y2 * y2);
                    formula::result_direct<T> const dir =
                        formula::vincenty_direct
                            <
                                T, true
                            >::apply(par.lam0, par.phi0, s12, azi1, proj_parm.spheroid);
                    lp_lat = dir.lat2;
                    lp_lon = dir.lon2;
                    lp_lon -= par.lam0;
                } else { /* Polar */
                    lp_lat = pj_inv_mlfn(proj_parm.mode == n_pole ? proj_parm.Mp - c : proj_parm.Mp + c,
                        par.es, proj_parm.en);
                    lp_lon = atan2(xy_x, proj_parm.mode == n_pole ? -xy_y : xy_y);
                }
            }

            template <typename T, typename Par, typename ProjParm>
            inline void e_guam_fwd(T const& lp_lon, T const& lp_lat, T& xy_x, T& xy_y, Par const& par, ProjParm const& proj_parm)
            {
                T cosphi, sinphi, t;

                cosphi = cos(lp_lat);
                sinphi = sin(lp_lat);
                t = 1. / sqrt(1. - par.es * sinphi * sinphi);
                xy_x = lp_lon * cosphi * t;
                xy_y = pj_mlfn(lp_lat, sinphi, cosphi, proj_parm.en) - proj_parm.M1 +
                    .5 * lp_lon * lp_lon * cosphi * sinphi * t;
            }

            template <typename T, typename Par, typename ProjParm>
            inline void e_guam_inv(T const& xy_x, T const& xy_y, T& lp_lon, T& lp_lat, Par const& par, ProjParm const& proj_parm)
            {
                T x2, t = 0.0;
                int i;

                x2 = 0.5 * xy_x * xy_x;
                lp_lat = par.phi0;
                for (i = 0; i < 3; ++i) {
                    t = par.e * sin(lp_lat);
                    lp_lat = pj_inv_mlfn(proj_parm.M1 + xy_y -
                        x2 * tan(lp_lat) * (t = sqrt(1. - t * t)), par.es, proj_parm.en);
                }
                lp_lon = xy_x * t / cos(lp_lat);
            }

            template <typename T, typename Par, typename ProjParm>
            inline void s_forward(T const& lp_lon, T lp_lat, T& xy_x, T& xy_y, Par const& /*par*/, ProjParm const& proj_parm)
            {
                static const T half_pi = detail::half_pi<T>();

                T coslam, cosphi, sinphi;

                sinphi = sin(lp_lat);
                cosphi = cos(lp_lat);
                coslam = cos(lp_lon);
                switch (proj_parm.mode) {
                case equit:
                    xy_y = cosphi * coslam;
                    goto oblcon;
                case obliq:
                    xy_y = proj_parm.sinph0 * sinphi + proj_parm.cosph0 * cosphi * coslam;
            oblcon:
                    if (fabs(fabs(xy_y) - 1.) < tolerance)
                        if (xy_y < 0.)
                            BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                        else
                            xy_x = xy_y = 0.;
                    else {
                        xy_y = acos(xy_y);
                        xy_y /= sin(xy_y);
                        xy_x = xy_y * cosphi * sin(lp_lon);
                        xy_y *= (proj_parm.mode == equit) ? sinphi :
                                proj_parm.cosph0 * sinphi - proj_parm.sinph0 * cosphi * coslam;
                    }
                    break;
                case n_pole:
                    lp_lat = -lp_lat;
                    coslam = -coslam;
                    BOOST_FALLTHROUGH;
                case s_pole:
                    if (fabs(lp_lat - half_pi) < epsilon10)
                        BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                    xy_x = (xy_y = (half_pi + lp_lat)) * sin(lp_lon);
                    xy_y *= coslam;
                    break;
                }
            }

            template <typename T, typename Par, typename ProjParm>
            inline void s_inverse(T xy_x, T xy_y, T& lp_lon, T& lp_lat, Par const& par, ProjParm const& proj_parm)
            {
                static const T pi = detail::pi<T>();
                static const T half_pi = detail::half_pi<T>();

                T cosc, c_rh, sinc;

                if ((c_rh = boost::math::hypot(xy_x, xy_y)) > pi) {
                    if (c_rh - epsilon10 > pi)
                        BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                    c_rh = pi;
                } else if (c_rh < epsilon10) {
                    lp_lat = par.phi0;
                    lp_lon = 0.;
                        return;
                }
                if (proj_parm.mode == obliq || proj_parm.mode == equit) {
                    sinc = sin(c_rh);
                    cosc = cos(c_rh);
                    if (proj_parm.mode == equit) {
                                    lp_lat = aasin(xy_y * sinc / c_rh);
                        xy_x *= sinc;
                        xy_y = cosc * c_rh;
                    } else {
                        lp_lat = aasin(cosc * proj_parm.sinph0 + xy_y * sinc * proj_parm.cosph0 /
                            c_rh);
                        xy_y = (cosc - proj_parm.sinph0 * sin(lp_lat)) * c_rh;
                        xy_x *= sinc * proj_parm.cosph0;
                    }
                    lp_lon = xy_y == 0. ? 0. : atan2(xy_x, xy_y);
                } else if (proj_parm.mode == n_pole) {
                    lp_lat = half_pi - c_rh;
                    lp_lon = atan2(xy_x, -xy_y);
                } else {
                    lp_lat = c_rh - half_pi;
                    lp_lon = atan2(xy_x, xy_y);
                }
            }

            // Azimuthal Equidistant
            template <typename Params, typename Parameters, typename T>
            inline void setup_aeqd(Params const& params, Parameters& par, par_aeqd<T>& proj_parm, bool is_sphere, bool is_guam)
            {
                static const T half_pi = detail::half_pi<T>();

                par.phi0 = pj_get_param_r<T, srs::spar::lat_0>(params, "lat_0", srs::dpar::lat_0);
                if (fabs(fabs(par.phi0) - half_pi) < epsilon10) {
                    proj_parm.mode = par.phi0 < 0. ? s_pole : n_pole;
                    proj_parm.sinph0 = par.phi0 < 0. ? -1. : 1.;
                    proj_parm.cosph0 = 0.;
                } else if (fabs(par.phi0) < epsilon10) {
                    proj_parm.mode = equit;
                    proj_parm.sinph0 = 0.;
                    proj_parm.cosph0 = 1.;
                } else {
                    proj_parm.mode = obliq;
                    proj_parm.sinph0 = sin(par.phi0);
                    proj_parm.cosph0 = cos(par.phi0);
                }
                if (is_sphere) {
                    /* empty */
                } else {
                    proj_parm.en = pj_enfn<T>(par.es);
                    if (is_guam) {
                        proj_parm.M1 = pj_mlfn(par.phi0, proj_parm.sinph0, proj_parm.cosph0, proj_parm.en);
                    } else {
                        switch (proj_parm.mode) {
                        case n_pole:
                            proj_parm.Mp = pj_mlfn<T>(half_pi, 1., 0., proj_parm.en);
                            break;
                        case s_pole:
                            proj_parm.Mp = pj_mlfn<T>(-half_pi, -1., 0., proj_parm.en);
                            break;
                        case equit:
                        case obliq:
                            proj_parm.N1 = 1. / sqrt(1. - par.es * proj_parm.sinph0 * proj_parm.sinph0);
                            proj_parm.G = proj_parm.sinph0 * (proj_parm.He = par.e / sqrt(par.one_es));
                            proj_parm.He *= proj_parm.cosph0;
                            break;
                        }
                        // Boost.Geometry specific, in proj4 geodesic is initialized at the beginning
                        T const b = math::sqrt(math::sqr(par.a) * (1. - par.es));
                        proj_parm.spheroid = srs::spheroid<T>(par.a, b);
                    }
                }
            }

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_aeqd_e
                : public base_t_fi<base_aeqd_e<T, Parameters>, T, Parameters>
            {
                par_aeqd<T> m_proj_parm;

                inline base_aeqd_e(const Parameters& par)
                    : base_t_fi<base_aeqd_e<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(e_forward)  elliptical
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T const& lp_lon, T const& lp_lat, T& xy_x, T& xy_y) const
                {
                    e_forward(lp_lon, lp_lat, xy_x, xy_y, this->m_par, this->m_proj_parm);
                }

                // INVERSE(e_inverse)  elliptical
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T const& xy_x, T const& xy_y, T& lp_lon, T& lp_lat) const
                {
                    e_inverse(xy_x, xy_y, lp_lon, lp_lat, this->m_par, this->m_proj_parm);
                }

                static inline std::string get_name()
                {
                    return "aeqd_e";
                }

            };

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_aeqd_e_guam
                : public base_t_fi<base_aeqd_e_guam<T, Parameters>, T, Parameters>
            {
                par_aeqd<T> m_proj_parm;

                inline base_aeqd_e_guam(const Parameters& par)
                    : base_t_fi<base_aeqd_e_guam<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(e_guam_fwd)  Guam elliptical
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T const& lp_lon, T const& lp_lat, T& xy_x, T& xy_y) const
                {
                    e_guam_fwd(lp_lon, lp_lat, xy_x, xy_y, this->m_par, this->m_proj_parm);
                }

                // INVERSE(e_guam_inv)  Guam elliptical
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T const& xy_x, T const& xy_y, T& lp_lon, T& lp_lat) const
                {
                    e_guam_inv(xy_x, xy_y, lp_lon, lp_lat, this->m_par, this->m_proj_parm);
                }

                static inline std::string get_name()
                {
                    return "aeqd_e_guam";
                }

            };

            // template class, using CRTP to implement forward/inverse
            template <typename BGParameters, typename T, typename Parameters>
            struct base_aeqd_e_static
                : public base_t_fi<base_aeqd_e_static<BGParameters, T, Parameters>, T, Parameters>
            {
                par_aeqd<T> m_proj_parm;

                static const bool is_guam = ! boost::is_same
                    <
                        typename srs::spar::detail::tuples_find_if
                            <
                                BGParameters,
                                //srs::par4::detail::is_guam
                                srs::spar::detail::is_param<srs::spar::guam>::pred
                            >::type,
                        void
                    >::value;

                inline base_aeqd_e_static(const Parameters& par)
                    : base_t_fi<base_aeqd_e_static<BGParameters, T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(e_forward or e_guam_fwd)  elliptical
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T const& lp_lon, T const& lp_lat, T& xy_x, T& xy_y) const
                {
                    if (is_guam)
                        e_guam_fwd(lp_lon, lp_lat, xy_x, xy_y, this->m_par, this->m_proj_parm);
                    else
                        e_forward(lp_lon, lp_lat, xy_x, xy_y, this->m_par, this->m_proj_parm);
                }

                // INVERSE(e_inverse or e_guam_inv)  elliptical
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T const& xy_x, T const& xy_y, T& lp_lon, T& lp_lat) const
                {
                    if (is_guam)
                        e_guam_inv(xy_x, xy_y, lp_lon, lp_lat, this->m_par, this->m_proj_parm);
                    else
                        e_inverse(xy_x, xy_y, lp_lon, lp_lat, this->m_par, this->m_proj_parm);
                }

                static inline std::string get_name()
                {
                    return "aeqd_e_static";
                }

            };

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_aeqd_s
                : public base_t_fi<base_aeqd_s<T, Parameters>, T, Parameters>
            {
                par_aeqd<T> m_proj_parm;

                inline base_aeqd_s(const Parameters& par)
                    : base_t_fi<base_aeqd_s<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(s_forward)  spherical
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T const& lp_lon, T const& lp_lat, T& xy_x, T& xy_y) const
                {
                    s_forward(lp_lon, lp_lat, xy_x, xy_y, this->m_par, this->m_proj_parm);
                }

                // INVERSE(s_inverse)  spherical
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T const& xy_x, T const& xy_y, T& lp_lon, T& lp_lat) const
                {
                    s_inverse(xy_x, xy_y, lp_lon, lp_lat, this->m_par, this->m_proj_parm);
                }

                static inline std::string get_name()
                {
                    return "aeqd_s";
                }

            };

    }} // namespace detail::aeqd
    #endif // doxygen

    /*!
        \brief Azimuthal Equidistant projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal
         - Spheroid
         - Ellipsoid
        \par Projection parameters
         - lat_0: Latitude of origin (degrees)
         - guam (boolean)
        \par Example
        \image html ex_aeqd.gif
    */
    template <typename T, typename Parameters>
    struct aeqd_e : public detail::aeqd::base_aeqd_e<T, Parameters>
    {
        template <typename Params>
        inline aeqd_e(Params const& params, Parameters const& par)
            : detail::aeqd::base_aeqd_e<T, Parameters>(par)
        {
            detail::aeqd::setup_aeqd(params, this->m_par, this->m_proj_parm, false, false);
        }
    };

    /*!
        \brief Azimuthal Equidistant projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal
         - Spheroid
         - Ellipsoid
        \par Projection parameters
         - lat_0: Latitude of origin (degrees)
         - guam (boolean)
        \par Example
        \image html ex_aeqd.gif
    */
    template <typename T, typename Parameters>
    struct aeqd_e_guam : public detail::aeqd::base_aeqd_e_guam<T, Parameters>
    {
        template <typename Params>
        inline aeqd_e_guam(Params const& params, Parameters const& par)
            : detail::aeqd::base_aeqd_e_guam<T, Parameters>(par)
        {
            detail::aeqd::setup_aeqd(params, this->m_par, this->m_proj_parm, false, true);
        }
    };

    /*!
        \brief Azimuthal Equidistant projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal
         - Spheroid
         - Ellipsoid
        \par Projection parameters
         - lat_0: Latitude of origin (degrees)
         - guam (boolean)
        \par Example
        \image html ex_aeqd.gif
    */
    template <typename SParams, typename T, typename Parameters>
    struct aeqd_e_static : public detail::aeqd::base_aeqd_e_static<SParams, T, Parameters>
    {
        inline aeqd_e_static(SParams const& params, Parameters const& par)
            : detail::aeqd::base_aeqd_e_static<SParams, T, Parameters>(par)
        {
            detail::aeqd::setup_aeqd(params, this->m_par, this->m_proj_parm,
                                     false,
                                     detail::aeqd::base_aeqd_e_static<SParams, T, Parameters>::is_guam);
        }
    };

    /*!
        \brief Azimuthal Equidistant projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal
         - Spheroid
         - Ellipsoid
        \par Projection parameters
         - lat_0: Latitude of origin (degrees)
         - guam (boolean)
        \par Example
        \image html ex_aeqd.gif
    */
    template <typename T, typename Parameters>
    struct aeqd_s : public detail::aeqd::base_aeqd_s<T, Parameters>
    {
        template <typename Params>
        inline aeqd_s(Params const& params, Parameters const& par)
            : detail::aeqd::base_aeqd_s<T, Parameters>(par)
        {
            detail::aeqd::setup_aeqd(params, this->m_par, this->m_proj_parm, true, false);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        template <typename BGP, typename CT, typename P>
        struct static_projection_type<srs::spar::proj_aeqd, srs_sphere_tag, BGP, CT, P>
        {
            typedef aeqd_s<CT, P> type;
        };
        template <typename BGP, typename CT, typename P>
        struct static_projection_type<srs::spar::proj_aeqd, srs_spheroid_tag, BGP, CT, P>
        {
            typedef aeqd_e_static<BGP, CT, P> type;
        };

        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_ENTRY_BEGIN(aeqd_entry)
        {
            bool const guam = pj_get_param_b<srs::spar::guam>(params, "guam", srs::dpar::guam);

            if (parameters.es && ! guam)
                return new base_v_fi<aeqd_e<T, Parameters>, T, Parameters>(params, parameters);
            else if (parameters.es && guam)
                return new base_v_fi<aeqd_e_guam<T, Parameters>, T, Parameters>(params, parameters);
            else
                return new base_v_fi<aeqd_s<T, Parameters>, T, Parameters>(params, parameters);
        }
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_ENTRY_END

        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_BEGIN(aeqd_init)
        {
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(aeqd, aeqd_entry)
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_AEQD_HPP


#include <mvp/OrbitalImageFootprint.h>

#include <vw/Cartography/SimplePointImageManipulation.h>
#include <vw/FileIO/DiskImageResource.h>

#include <boost/format.hpp>
#include <boost/filesystem.hpp>

namespace mvp {

vw::Vector2 OrbitalImageFootprint::backproj_px(vw::camera::PinholeModel const& cam,
                                               vw::Vector2 const& px,
                                               vw::cartography::Datum const& datum,
                                               double alt)
{
  VW_ASSERT(datum.semi_major_axis() == datum.semi_minor_axis(), vw::NoImplErr() << "Spheroid datums not supported");

  double sphere_rad = datum.semi_major_axis() + alt;

  vw::Vector3 dir = cam.pixel_to_vector(px);
  vw::Vector3 cntr = cam.camera_center();

  double a = dot_prod(dir, dir);
  double b = 2 * dot_prod(cntr, dir);
  double c = dot_prod(cntr, cntr) - sphere_rad * sphere_rad;

  double t = (-b - sqrt(b*b-4*a*c))/2/a;

  vw::Vector3 isect = cntr + t * dir;

  vw::Vector3 llr = vw::cartography::xyz_to_lon_lat_radius(isect);

  return vw::math::subvector(llr, 0, 2);
}

OrbitalImageFootprint OrbitalImageFootprint::construct_from_paths(std::string const& image_path,
                                                                  std::string const& camera_path,
                                                                  vw::cartography::Datum const& datum,
                                                                  vw::Vector2 const& alt_limits)
{
  OrbitalImageFileDescriptor image_file;
  image_file.set_camera_path(camera_path);
  image_file.set_image_path(image_path);

  boost::scoped_ptr<vw::DiskImageResource> rsrc(vw::DiskImageResource::open(image_file.image_path()));
  vw::Vector2i image_size(rsrc->cols(), rsrc->rows());

  std::vector<vw::Vector2> fp_points(8);

  vw::camera::PinholeModel camera(image_file.camera_path());
  fp_points[0] = backproj_px(camera, vw::Vector2i(0, 0), datum, alt_limits[0]);
  fp_points[1] = backproj_px(camera, vw::Vector2i(image_size.x(), 0), datum, alt_limits[0]);
  fp_points[2] = backproj_px(camera, vw::Vector2i(image_size.x(), image_size.y()), datum, alt_limits[0]);
  fp_points[3] = backproj_px(camera, vw::Vector2i(0, image_size.y()), datum, alt_limits[0]);
  fp_points[4] = backproj_px(camera, vw::Vector2i(0, 0), datum, alt_limits[1]);
  fp_points[5] = backproj_px(camera, vw::Vector2i(image_size.x(), 0), datum, alt_limits[1]);
  fp_points[6] = backproj_px(camera, vw::Vector2i(image_size.x(), image_size.y()), datum, alt_limits[1]);
  fp_points[7] = backproj_px(camera, vw::Vector2i(0, image_size.y()), datum, alt_limits[1]);

  // TODO: Need to take care of wrapping around poles!
  vw::BBox2 maxbounds(-180,-180,360,360);
  BOOST_FOREACH(vw::Vector2 v, fp_points) {
    if (!maxbounds.contains(v)) {
      VW_ASSERT(maxbounds.contains(v), vw::NoImplErr() << "Lonlat wrapping in footprints needs to be implemented");
    }
  }

  return OrbitalImageFootprint(image_file, image_size, fp_points);
}

int OrbitalImageFootprint::equal_resolution_level() const {
  vw::BBox2 fp_bbox(bounding_box());

  double x_res_lvl = log(360.0 / fp_bbox.width()) / log(2);
  double y_res_lvl = log(360.0 / fp_bbox.height()) / log(2);

  return round(std::max(x_res_lvl, y_res_lvl));
}

int OrbitalImageFootprint::equal_density_level(int tile_size) const {
  vw::BBox2 fp_bbox(bounding_box());

  double x_dens_lvl = log(360.0 * m_image_size.x() / fp_bbox.width() / tile_size) / log(2);
  double y_dens_lvl = log(360.0 * m_image_size.y() / fp_bbox.height() / tile_size) / log(2);

  return ceil(std::max(x_dens_lvl, y_dens_lvl));
}

} // namespace mvp


#include <boost/polygon/voronoi_diagram.hpp>


// Main code for peak bagging by means of nested sampling analysis
// Created by Enrico Corsaro @ INAF-OACT - January 2019
// e-mail: emncorsaro@gmail.com
// Source code file "PeakBagging.cpp"

#include <cstdlib>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <Eigen/Dense>
#include "Functions.h"
#include "File.h"
#include "MultiEllipsoidSampler.h"
#include "KmeansClusterer.h"
#include "EuclideanMetric.h"
#include "Prior.h"
#include "UniformPrior.h"
#include "ExponentialLikelihood.h"
#include "SingleLorentzianFixedLinewidthModel.h"
#include "DoubleLorentzianFixedLinewidthModel.h"
#include "SlidingPatternModel.h"
#include "LorentzianMixtureModel.h"
#include "LorentzianSincMixtureModel.h"
#include "LorentzianRotationMixtureModel.h"
#include "PeakTestBackgroundModel.h"
#include "PeakTestLorentzianBackgroundModel.h"
#include "PeakTestSincBackgroundModel.h"
#include "PeakTestBlendingBackgroundModel.h"
#include "PeakTestLorentzianModel.h"
#include "PeakTestDipoleRotationModel.h"
#include "PeakTestDupletModel.h"
#include "BackgroundModel.h"
#include "PowerlawReducer.h"
#include "Results.h"
#include "Ellipsoid.h"
#include "PrincipalComponentProjector.h"

int main(int argc, char *argv[])
{
    // Check number of arguments for main function

    if (argc != 11)
    {
        cerr << "Usage: ./peakbagging <Catalog ID> <Star ID> <output sub-directory> <run number> <background model> <input prior base filename> "
        << "<input linewidth> <Peak-Test flag> <Asymptotic-pattern flag> <Background level output flag>" << endl;
        exit(EXIT_FAILURE);
    }

    // ---------------------------
    // ----- Read input data -----
    // ---------------------------

    unsigned long Nrows;
    int Ncols;
    ArrayXXd data;
    string CatalogID(argv[1]);
    string StarID(argv[2]);
    string runNumber(argv[4]);
    string backgroundModelName(argv[5]);
    string inputPriorBaseName(argv[6]);
    string inputLinewidth(argv[7]);
    string inputPeakTestFlag(argv[8]);
    string inputAsymptoticFlag(argv[9]);
    string inputBackgroundLevelOutputFlag(argv[10]);
    double peakLinewidth = stod(inputLinewidth);
    int peakTestFlag = stoi(inputPeakTestFlag);
    int asymptoticFlag = stoi(inputAsymptoticFlag);
    int backgroundLevelOutputFlag = stoi(inputBackgroundLevelOutputFlag);

    // Read the local path for the working session from an input ASCII file

    ifstream inputFile;
    File::openInputFile(inputFile, "localPath.txt");
    File::sniffFile(inputFile, Nrows, Ncols);
    vector<string> myLocalPath;
    myLocalPath = File::vectorStringFromFile(inputFile, Nrows);
    inputFile.close();

    // Set up some string paths used in the computation

    string baseInputDirName = myLocalPath[0] + "data/";
    string inputFileName = baseInputDirName + CatalogID + StarID + ".txt";
    string outputSubDirName(argv[3]);
    string baseOutputDirName = myLocalPath[0] + "results/" + CatalogID + StarID + "/";
    string outputDirName = baseOutputDirName + outputSubDirName + "/";
    string outputPathPrefix = outputDirName + runNumber + "/peakbagging_";

    cout << "------------------------------------------------------ " << endl;
    if (asymptoticFlag == 1)
    {
        cout << " Performing asymptotic pattern fit for " + CatalogID + StarID << endl;
    }
    else
    {
        if (peakTestFlag == 1)
        {
                cout << " Performing peak significance test for " + CatalogID + StarID << endl;
        }
        else
        {
            if (peakLinewidth >= 0.0)
            {
                cout << " Multi-modal Peak Bagging analysis for " + CatalogID + StarID << endl;

                if (peakLinewidth > 0) cout << " Using input linewidth " + inputLinewidth + " microHz." << endl;
                if (peakLinewidth == 0) cout << " Using frequencyResolution as input linewidth." << endl;
            }
            else
            {
                if (peakLinewidth == -1)
                {
                cout << " Uni-modal Peak Bagging analysis without rotation for " + CatalogID + StarID << endl;
                }
                else
                {
                cout << " Uni-modal Peak Bagging analysis with rotation for " + CatalogID + StarID << endl;
                }
            }
        }
    }
    cout << "------------------------------------------------------ " << endl;
    cout << endl;

    // Read the input dataset

    File::openInputFile(inputFile, inputFileName);
    File::sniffFile(inputFile, Nrows, Ncols);
    data = File::arrayXXdFromFile(inputFile, Nrows, Ncols);
    inputFile.close();

    // Creating frequency and PSD arrays

    ArrayXd covariates = data.col(0);
    ArrayXd observations = data.col(1);
    double frequencyResolution = covariates(1) - covariates(0);
    double lowerFrequency;
    double upperFrequency;

    // -------------------------------------------------------
    // ----- First step. Set up all prior distributions ------
    // -------------------------------------------------------

    unsigned long Nparameters;              // Number of parameters for which prior distributions are defined

    // Read prior hyper parameters from an input file

    inputFileName = outputDirName + inputPriorBaseName + "_" + runNumber + ".txt";
    File::openInputFile(inputFile, inputFileName);
    File::sniffFile(inputFile, Nparameters, Ncols);
    ArrayXXd hyperParameters;

    if (asymptoticFlag == 1)
    {
        if (Nparameters != 8)
        {
            cerr << "Wrong number of input prior hyper-parameters." << endl;
            cerr << "When performing an asymptotic pattern fit, there are up to 8 free parameters." << endl;
            exit(EXIT_FAILURE);
        }
    }
    else
    {
        if (peakTestFlag == 1)
        {
            if (Nparameters > 7)
            {
                cerr << "Wrong number of input prior hyper-parameters." << endl;
                cerr << "When performing a peak significance test, the code allows for the following cases: " << endl;
                cerr << "(A) - Only background (Ndimensions = 1). " << endl;
                cerr << "(B) - Background plus Lorentzian profile (Ndimensions = 4). " << endl;
                cerr << "(C) - Background plus Sinc^2 profile (Ndimensions = 3). " << endl;
                cerr << "(D) - Background plus two Lorentzian profiles for blending test (Ndimensions = 7). " << endl;
                cerr << "(E) - One Lorentzian profile with fixed background (Ndimensions = 3). " << endl;
                cerr << "(F) - One Lorentzian profile with rotationally split components and fixed background (Ndimensions = 5). " << endl;
                cerr << "(G) - Two Lorentzian profiles with fixed background for duplet fit (Ndimensions = 6). " << endl;
                exit(EXIT_FAILURE);
            }
        }
        else
        {
            if (peakLinewidth >= 0.0)
            {
                if (Nparameters != 2 && Nparameters != 3)
                {
                    cerr << "Wrong number of input prior hyper-parameters." << endl;
                    cerr << "There are only two free parameters for the single-peak model and three for the double-peak model." << endl;
                    exit(EXIT_FAILURE);
                }
            }
            else
            {
                if (peakLinewidth == -1)
                {
                    if (fmod(Nparameters,3) != 0.0)
                    {
                        cerr << "Wrong number of input prior hyper-parameters for resolved peaks." << endl;
                        exit(EXIT_FAILURE);
                    }
                }
                else
                {
                    if (fmod((Nparameters-2),3) != 0.0)
                    {
                        cerr << "Wrong number of input prior hyper-parameters for resolved peaks." << endl;
                        exit(EXIT_FAILURE);
                    }
                }
            }
        }
    }

    if (Ncols == 1)
    {
        Ncols = 2;
        hyperParameters.conservativeResize(Nparameters,Ncols);
    }

    cout << "-------------------------------------------------- " << endl;
    cout << " Reading prior file " + inputPriorBaseName + "_" + runNumber + ".txt" << endl;
    cout << "-------------------------------------------------- " << endl;
    cout << " " << endl;

    hyperParameters = File::arrayXXdFromFile(inputFile, Nparameters, Ncols);
    inputFile.close();

    ArrayXd hyperParametersMinima = hyperParameters.col(0);
    ArrayXd hyperParametersMaxima = hyperParameters.col(1);
    // ---------------------------------------------------------

    int Ndimensions = Nparameters;              // Total number of dimensions of the peak bagging model

    // Uniform Prior

    int NpriorTypes = 1;                                        // Total number of prior types included in the computation
    vector<Prior*> ptrPriors(NpriorTypes);

    ArrayXd parametersMinima;                      // Minima for prior PDF
    ArrayXd parametersMaxima;                      // Maxima for prior PDF

    double sincProfileIsUsed = false;

    // Set sliding pattern parameters to be free by default

    ArrayXd slidingPatternParameters = ArrayXd::Constant(6,-99);

    if (peakTestFlag == 1 && asymptoticFlag != 1)
    {
        // Assume by default that all the prior parameters are used.
        // This represents the case of the peak test models (A), (E), (F), (G).

        parametersMinima.resize(Ndimensions);
        parametersMaxima.resize(Ndimensions);
        parametersMinima << hyperParametersMinima;
        parametersMaxima << hyperParametersMaxima;

        if (Ndimensions == 7)
        {
            // This can be either model (D), or model (G)
            // if the last parameter (background factor) is fixed.

            if (hyperParametersMinima(6) == hyperParametersMaxima(6))
            {
                Ndimensions--;

                parametersMinima.resize(Ndimensions);
                parametersMaxima.resize(Ndimensions);
                parametersMinima << hyperParametersMinima.segment(0,Ndimensions);
                parametersMaxima << hyperParametersMaxima.segment(0,Ndimensions);
            }
        }

        if (Ndimensions == 4)
        {
            // In this case the peak test contains cases (B), (C).
            // Here one has to distinguish between the case of a Lorentzian
            // and that of a Sinc^2 profile for the peak significance test.

            if (hyperParametersMinima(3) < 0.0)
            {
                // Here the last prior parameter is set to be a negative value.
                // This means that it has to be ignored, therefore the profile
                // is that of a sinc^2. Then the effective number of dimensions is 3.

                sincProfileIsUsed = true;
                Ndimensions = 3;

                parametersMinima.resize(Ndimensions);
                parametersMaxima.resize(Ndimensions);
                parametersMinima << hyperParametersMinima.segment(0,Ndimensions);
                parametersMaxima << hyperParametersMaxima.segment(0,Ndimensions);
            }
        }
    }
    else
    {
        // Consider the case of the sliding pattern model separately

        if (asymptoticFlag == 1)
        {
            // Assume that the first two parameters are always free (namely frequency centroid and peak profile height).

            parametersMinima.resize(2);
            parametersMaxima.resize(2);
            parametersMinima << hyperParametersMinima.segment(0,2);
            parametersMaxima << hyperParametersMaxima.segment(0,2);
            Ndimensions = 2;

            // If DeltaNu, deltaNu02, deltaNu01, deltaNu03, rotationalSplitting and cosi are free parameters, add them to the prior list,
            // otherwise set their value to the one given in the prior file.
            // If a value of +99 is used for any of the small frequency spacings deltaNu02, deltaNu01, deltaNu03, this implies that
            // the corresponding peak is excluded from the sliding pattern.

            for (int freeParameter = 2; freeParameter < hyperParametersMinima.size(); freeParameter++)
            {
                if (hyperParametersMinima(freeParameter) != hyperParametersMaxima(freeParameter))
                {
                    Ndimensions++;
                    parametersMinima.conservativeResize(Ndimensions);
                    parametersMaxima.conservativeResize(Ndimensions);
                    parametersMinima(Ndimensions-1) = hyperParametersMinima(freeParameter);
                    parametersMaxima(Ndimensions-1) = hyperParametersMaxima(freeParameter);
                }
                else
                {
                    slidingPatternParameters(freeParameter-2) = hyperParametersMinima(freeParameter);
                }
            }
        }
        else
        {
            parametersMinima.resize(Ndimensions);
            parametersMaxima.resize(Ndimensions);
            parametersMinima << hyperParametersMinima;
            parametersMaxima << hyperParametersMaxima;
        }
    }

    UniformPrior uniformPrior(parametersMinima, parametersMaxima);
    ptrPriors[0] = &uniformPrior;

    string fullPathHyperParameters = outputPathPrefix + "hyperParametersUniform.txt";
    uniformPrior.writeHyperParametersToFile(fullPathHyperParameters);

    // If the fit has to be done in a uni-modal high-dimensional manner, then expect an input file to provide the frequency boundaries to perform the fit.
    // In this case the PSD will be trimmed according to an input frequency range. The same applies if the fit is a multi-modal sliding pattern fit and
    // to the suit of peak testing models.
    // Otherwise, if the fit has to be done in a standard multi-modal low-dimensional manner then trim the dataset according to the input prior
    // for the frequency range.

    if ((peakTestFlag != 1 && peakLinewidth < 0.0) || asymptoticFlag == 1 || peakTestFlag == 1)
    {
        // Read input frequency range of the PSD

        inputFileName = outputDirName + "frequencyRange_" + runNumber + ".txt";
        File::openInputFile(inputFile, inputFileName);
        File::sniffFile(inputFile, Nrows, Ncols);
        ArrayXXd frequencyRange(Nrows, Ncols);
        frequencyRange = File::arrayXXdFromFile(inputFile, Nrows, Ncols);
        inputFile.close();

        lowerFrequency = frequencyRange(0);        // muHz
        upperFrequency = frequencyRange(1);        // muHz
    }
    else
    {
        lowerFrequency = hyperParameters(0,0);        // muHz
        upperFrequency = hyperParameters(0,1);

        if (Ndimensions == 3)
        {
            // For the double Lorentzian multi-modal fit, take as the lower frequency bound
            // the lower prior bound for the radial mode frequency minus the upper prior bound
            // for the small spacing deltaNu02.

            lowerFrequency = fabs(hyperParameters(0,0) - hyperParameters(2,1));
        }
    }

    // Trim input dataset in the given frequency range

    vector<int> trimIndices = Functions::findArrayIndicesWithinBoundaries(covariates, lowerFrequency, upperFrequency);
    int Nbins = trimIndices.size();
    ArrayXd trimmedArray(Nbins);

    trimmedArray = covariates.segment(trimIndices[0],Nbins);
    covariates.resize(Nbins);
    covariates = trimmedArray;
    trimmedArray = observations.segment(trimIndices[0],Nbins);
    observations.resize(Nbins);
    observations = trimmedArray;

    cout << " Frequency range: [" << setprecision(4) << covariates.minCoeff() << ", "
         << covariates.maxCoeff() << "] muHz" << endl;
    cout << " Frequency resolution: " << setprecision(4) << frequencyResolution << " muHz" << endl;
    cout << endl;

    // -------------------------------------------------------------------
    // ---- Second step. Set up the background model ---------------------
    // -------------------------------------------------------------------

    BackgroundModel backgroundModel(covariates, backgroundModelName);
    inputFileName = baseOutputDirName + "NyquistFrequency.txt";
    backgroundModel.readNyquistFrequencyFromFile(inputFileName);
    string backgroundConfiguringParameters = baseOutputDirName + "backgroundParameters.txt";
    backgroundModel.readConfiguringParametersFromFile(backgroundConfiguringParameters);

    if (backgroundLevelOutputFlag == 1)
    {
        string outputBackgroundLevelFileName = baseOutputDirName + "backgroundLevel.txt";
        backgroundModel.writeBackgroundPredictionToFile(outputBackgroundLevelFileName);
    }

    // -------------------------------------------------------------------
    // ---- Third step. Set up the peak bagging model --------------------
    // -------------------------------------------------------------------

    Model *model = nullptr;

    if (asymptoticFlag == 1)
    {
        string asymptoticParametersFileName = baseOutputDirName + "asymptoticParameters.txt";
        string gaussianEnvelopeParametersFileName = baseOutputDirName + "gaussianEnvelopeParameters.txt";
        model = new SlidingPatternModel(covariates, peakLinewidth, slidingPatternParameters,
            asymptoticParametersFileName, gaussianEnvelopeParametersFileName, backgroundModel);
    }
    else
    {
        if (peakTestFlag == 1)
        {
            // Set the model to perform the peak significance test. This can be one of the
            //  models (A), (B), (C), (D), (E), (F), (G), depending on the number of
            // dimensions as set out from the input prior list.

            if (Ndimensions == 1)
            {
                model = new PeakTestBackgroundModel(covariates, backgroundModel);
            }

            if (Ndimensions == 3)
            {
                if (sincProfileIsUsed)
                {
                    model = new PeakTestSincBackgroundModel(covariates, frequencyResolution, backgroundModel);
                }
                else
                {
                    model = new PeakTestLorentzianModel(covariates, backgroundModel);
                }
            }

            if (Ndimensions == 4)
            {
                model = new PeakTestLorentzianBackgroundModel(covariates, backgroundModel);
            }

            if (Ndimensions == 5)
            {
                model = new PeakTestDipoleRotationModel(covariates, backgroundModel);
            }

            if (Ndimensions == 6)
            {
                model = new PeakTestDupletModel(covariates, backgroundModel);
            }

            if (Ndimensions == 7)
            {
                model = new PeakTestBlendingBackgroundModel(covariates, backgroundModel);
            }
        }
        else
        {
            if (peakLinewidth >= 0)
            {
                if (peakLinewidth == 0)
                {
                    peakLinewidth = frequencyResolution;
                }

                // If the input linewidth is < 0, use the frequency resolution as peak linewidth for the multi-modal fitting (linewidth = 0).
                // This is useful in case narrow (unresolved) mixed modes are expected.

                if (Nparameters == 2)
                {
                    model = new SingleLorentzianFixedLinewidthModel(covariates, peakLinewidth, backgroundModel);
                }
                else
                {
                    string asymptoticParametersFileName = baseOutputDirName + "asymptoticParameters.txt";
                    model = new DoubleLorentzianFixedLinewidthModel(covariates, peakLinewidth, backgroundModel, asymptoticParametersFileName);
                }
            }
            else
            {
                int Nresolved;

                if (peakLinewidth == -1)
                {
                    // No input linewidth has to be used (= -1). In this case perform the standard uni-modal fitting
                    // by assuming a mixture of Lorentzian profiles without rotational splitting.

                    Nresolved = Ndimensions/3;
                    model = new LorentzianMixtureModel(covariates, Nresolved, backgroundModel);
                }
                else
                {
                    // No input linewidth has to be used (< 0 but != -1). In this case perform the standard uni-modal fitting
                    // by assuming a mixture of Lorentzian profiles and include also the effect of rotation.
                    // This requires an input list of angular degrees to be fed into the peak bagging model.

                    Nresolved = (Ndimensions-2)/3;
                    string angularDegreesFileName = outputDirName + "angularDegrees_" + runNumber + ".txt";
                    model = new LorentzianRotationMixtureModel(covariates, Nresolved, angularDegreesFileName, backgroundModel);
                }
            }
        }
    }

    // -----------------------------------------------------------------
    // ---- Fourth step. Set up the likelihood function to be used -----
    // -----------------------------------------------------------------

    ExponentialLikelihood likelihood(observations, *model);

    // -------------------------------------------------------------------------------
    // ----- Fifth step. Set up the X-means clusterer using an Euclidean metric ------
    // -------------------------------------------------------------------------------

    inputFileName = outputDirName + "Xmeans_configuringParameters.txt";
    File::openInputFile(inputFile, inputFileName);
    File::sniffFile(inputFile, Nparameters, Ncols);

    if (Nparameters != 2)
    {
        cerr << "Wrong number of input parameters for X-means algorithm." << endl;
        exit(EXIT_FAILURE);
    }

    ArrayXd configuringParameters;
    configuringParameters = File::arrayXXdFromFile(inputFile, Nparameters, Ncols);
    inputFile.close();

    int minNclusters = configuringParameters(0);
    int maxNclusters = configuringParameters(1);

    if ((minNclusters <= 0) || (maxNclusters <= 0) || (maxNclusters < minNclusters))
    {
        cerr << "Minimum or maximum number of clusters cannot be <= 0, and " << endl;
        cerr << "minimum number of clusters cannot be larger than maximum number of clusters." << endl;
        exit(EXIT_FAILURE);
    }

    int Ntrials = 10;
    double relTolerance = 0.01;     // k-means

    bool printNdimensions = false;
    PrincipalComponentProjector projector(printNdimensions);
    bool featureProjectionActivated = false;
    EuclideanMetric myMetric;

    KmeansClusterer clusterer(myMetric, projector, featureProjectionActivated,
                           minNclusters, maxNclusters, Ntrials, relTolerance);

    // ---------------------------------------------------------------------
    // ----- Sixth step. Configure and start nested sampling inference -----
    // ---------------------------------------------------------------------

    inputFileName = outputDirName + "NSMC_configuringParameters.txt";
    File::openInputFile(inputFile, inputFileName);
    File::sniffFile(inputFile, Nparameters, Ncols);
    configuringParameters.setZero();
    configuringParameters = File::arrayXXdFromFile(inputFile, Nparameters, Ncols);
    inputFile.close();

    if (Nparameters > 9 || Nparameters < 8)
    {
        cerr << "Wrong number of input parameters for NSMC algorithm." << endl;
        cerr << "There must be either 8 or 9 parameters (the latter specified only if " <<
        "a fixed number of nested iterations is required, i.e. for the multi-modal fit." << endl;
        exit(EXIT_FAILURE);
    }

    // Print results on the screen

    bool printOnTheScreen = true;

    // Initial number of live points

    int initialNobjects = configuringParameters(0);

    // Minimum number of live points

    int minNobjects = configuringParameters(1);

    // Maximum number of attempts when trying to draw a new sampling point

    int maxNdrawAttempts = configuringParameters(2);

    // The first N iterations, we assume that there is only 1 cluster

    int NinitialIterationsWithoutClustering = configuringParameters(3);

    // Clustering is only happening every N iterations.

    int NiterationsWithSameClustering = configuringParameters(4);

    // Fraction by which each axis in an ellipsoid has to be enlarged
    // It can be a number >= 0, where 0 means no enlargement.
    // Calibration from Corsaro et al. (2018)

    double initialEnlargementFraction;

    if (initialNobjects <= 500)
    {
        initialEnlargementFraction = 0.369*pow(Ndimensions,0.574);
    }
    else
    {
        initialEnlargementFraction = 0.310*pow(Ndimensions,0.598);
    }

    // Exponent for remaining prior mass in ellipsoid enlargement fraction.
    // It is a number between 0 and 1. The smaller the slower the shrinkage of the ellipsoids.

    double shrinkingRate = configuringParameters(6);

    // Termination factor for nested sampling process.

    double terminationFactor = configuringParameters(7);

    // Total maximum number of nested iterations required to carry out the computation.
    // This is used only in the multi-modal approach.

    int maxNiterations = 0;
    if ((peakLinewidth >= 0 && peakTestFlag != 1) || asymptoticFlag == 1)
    {
        maxNiterations = configuringParameters(8);
    }

    MultiEllipsoidSampler nestedSampler(printOnTheScreen, ptrPriors, likelihood, myMetric, clusterer,
                                        initialNobjects, minNobjects, initialEnlargementFraction, shrinkingRate);

    double tolerance = 1.e2;
    double exponent = 0.4;
    PowerlawReducer livePointsReducer(nestedSampler, tolerance, exponent, terminationFactor);

    nestedSampler.run(livePointsReducer, NinitialIterationsWithoutClustering, NiterationsWithSameClustering,
                      maxNdrawAttempts, terminationFactor, maxNiterations, outputPathPrefix);

    nestedSampler.outputFile << "# List of configuring parameters used for the ellipsoidal sampler and X-means" << endl;
    nestedSampler.outputFile << "# Row #1: Minimum Nclusters" << endl;
    nestedSampler.outputFile << "# Row #2: Maximum Nclusters" << endl;
    nestedSampler.outputFile << "# Row #3: Initial Enlargement Fraction" << endl;
    nestedSampler.outputFile << "# Row #4: Shrinking Rate" << endl;
    nestedSampler.outputFile << minNclusters << endl;
    nestedSampler.outputFile << maxNclusters << endl;
    nestedSampler.outputFile << initialEnlargementFraction << endl;
    nestedSampler.outputFile << shrinkingRate << endl;
    nestedSampler.outputFile << "# Other information on the run" << endl;
    nestedSampler.outputFile << "# Row #1: Local working path used" << endl;
    nestedSampler.outputFile << "# Row #2: Catalog and Star ID" << endl;
    nestedSampler.outputFile << "# Row #3: Run Directory" << endl;
    nestedSampler.outputFile << "# Row #4: Run Number" << endl;
    nestedSampler.outputFile << "# Row #5: Peak significance test (from A to G, 0 if not a test)" << endl;
    nestedSampler.outputFile << "# Row #6: Asymptotic pattern fit (1 if activated, 0 otherwise)" << endl;
    nestedSampler.outputFile << "# Row #7: Background model adopted" << endl;
    nestedSampler.outputFile << "# Row #8: Peak linewidth in muHz (0 if uni-modal peak bagging or peak test)" << endl;
    nestedSampler.outputFile << myLocalPath[0] << endl;
    nestedSampler.outputFile << CatalogID + StarID << endl;
    nestedSampler.outputFile << outputSubDirName << endl;
    nestedSampler.outputFile << runNumber << endl;

    if (peakTestFlag == 1 && asymptoticFlag != 1)
    {
        if (Ndimensions == 1)
        {
            nestedSampler.outputFile << "A" << endl;
        }

        if (Ndimensions == 4)
        {
            nestedSampler.outputFile << "B" << endl;
        }

        if (Ndimensions == 7)
        {
            nestedSampler.outputFile << "D" << endl;
        }

        if (Ndimensions == 3)
        {
            if (sincProfileIsUsed)
            {
                nestedSampler.outputFile << "C" << endl;
            }
            else
            {
                nestedSampler.outputFile << "E" << endl;
            }
        }

        if (Ndimensions == 5)
        {
            nestedSampler.outputFile << "F" << endl;
        }

        if (Ndimensions == 6)
        {
            nestedSampler.outputFile << "G" << endl;
        }
    }
    else
    {
        nestedSampler.outputFile << 0 << endl;
    }

    if (asymptoticFlag == 1)
    {
        nestedSampler.outputFile << 1 << endl;
    }
    else
    {
        nestedSampler.outputFile << 0 << endl;
    }

    nestedSampler.outputFile << backgroundModelName << endl;

    if (peakLinewidth < 0 || peakTestFlag == 1)
    {
        nestedSampler.outputFile << setprecision(2) << 0.0 << endl;
    }
    else
    {
        nestedSampler.outputFile << setprecision(3) << peakLinewidth << endl;
    }

    nestedSampler.outputFile.close();

    // -------------------------------------------------------
    // ----- Last step. Save the results in output files -----
    // -------------------------------------------------------

    Results results(nestedSampler);
    results.writeParametersToFile("parameter");
    results.writeLogLikelihoodToFile("logLikelihood.txt");
    results.writePosteriorProbabilityToFile("posteriorDistribution.txt");
    results.writeLogEvidenceToFile("logEvidence.txt");
    results.writeLogMeanLiveEvidenceToFile("logMeanLiveEvidence.txt");
    results.writeEvidenceInformationToFile("evidenceInformation.txt");

    // Print out parameter estimates only in the case of a uni-modal high-dimensional fit or a peak test.

    if ((peakLinewidth < 0.0 && peakTestFlag != 1) || peakTestFlag == 1)
    {
        double credibleLevel = 68.3;
        bool writeMarginalDistributionToFile = true;
        results.writeParametersSummaryToFile("parameterSummary.txt", credibleLevel, writeMarginalDistributionToFile);
    }

    cout << "Process # " << runNumber << " under subdir: " + outputSubDirName + " has been completed." << endl;

    return EXIT_SUCCESS;
}