#include "teca_tc_candidates.h"

#include "teca_cartesian_mesh.h"
#include "teca_variant_array.h"
#include "teca_table.h"
#include "teca_database.h"
#include "teca_calendar.h"
#include "teca_coordinate_util.h"
#include "gfdl_tc_candidates.h"

#include <iostream>
#include <sstream>
#include <limits>
#include <algorithm>
#include <cmath>
#include <vector>
#include <set>
#include <chrono>

#if defined(TECA_HAS_BOOST)
#include <boost/program_options.hpp>
#endif

//#define TECA_DEBUG 2

using std::cerr;
using std::endl;
using seconds_t = std::chrono::duration<double, std::chrono::seconds::period>;

// --------------------------------------------------------------------------
teca_tc_candidates::teca_tc_candidates() :
    max_core_radius(2.0),
    min_vorticity_850mb(1.6e-4),
    vorticity_850mb_window(7.75),
    max_pressure_delta(400.0),
    max_pressure_radius(5.0),
    max_core_temperature_delta(0.8),
    max_core_temperature_radius(5.0),
    max_thickness_delta(50.0),
    max_thickness_radius(4.0),
    search_lat_low(1.0),
    search_lat_high(0.0),
    search_lon_low(1.0),
    search_lon_high(0.0),
    minimizer_iterations(50)
{
    this->set_number_of_input_connections(1);
    this->set_number_of_output_ports(1);
}

// --------------------------------------------------------------------------
teca_tc_candidates::~teca_tc_candidates()
{}

#if defined(TECA_HAS_BOOST)
// --------------------------------------------------------------------------
void teca_tc_candidates::get_properties_description(
    const std::string &prefix, options_description &opts)
{
    options_description ard_opts("Options for "
        + (prefix.empty()?"teca_tc_candidates":prefix));

    ard_opts.add_options()
        TECA_POPTS_GET(std::string, prefix, surface_wind_speed_variable,
            "name of wind speed variable")
        TECA_POPTS_GET(std::string, prefix, vorticity_850mb_variable,
            "name of 850 mb vorticity variable")
        TECA_POPTS_GET(std::string, prefix, sea_level_pressure_variable,
            "name of sea level pressure variable")
        TECA_POPTS_GET(std::string, prefix, core_temperature_variable,
            "name of core temperature variable")
        TECA_POPTS_GET(double, prefix, max_core_radius,
            "maximum number of degrees latitude separation between "
            "vorticity max and pressure min defining a storm (2.0)")
        TECA_POPTS_GET(double, prefix, min_vorticity_850mb,
            "minimum vorticty to be considered a tropical storm (1.6e-4)")
        TECA_POPTS_GET(double, prefix, vorticity_850mb_window,
            "size of the search window in degrees. storms core must have a "
            "local vorticity max centered on this window (7.74446)")
        TECA_POPTS_GET(double, prefix, max_pressure_delta,
            "maximum pressure change within specified radius (400.0)")
        TECA_POPTS_GET(double, prefix, max_pressure_radius,
            "radius in degrees over which max pressure change is computed (5.0)")
        TECA_POPTS_GET(double, prefix, max_core_temperature_delta,
            "maximum core temperature change over the specified radius (0.8)")
        TECA_POPTS_GET(double, prefix, max_core_temperature_radius,
            "radius in degrees over which max core temperature change is computed (5.0)")
        TECA_POPTS_GET(double, prefix, max_thickness_delta,
            "maximum thickness change over the specified radius (50.0)")
        TECA_POPTS_GET(double, prefix, max_thickness_radius,
            "radius in degrees over with max thickness change is comuted (4.0)")
        TECA_POPTS_GET(double, prefix, search_lat_low,
            "lowest latitude in degrees to search for storms (-80.0)")
        TECA_POPTS_GET(double, prefix, search_lat_high,
            "highest latitude in degrees to search for storms (80.0)")
        TECA_POPTS_GET(double, prefix, search_lon_low,
            "lowest longitude in degrees to search for stroms (1)")
        TECA_POPTS_GET(double, prefix, search_lon_high,
            "highest longitude in degrees to search for storms (0)")
        ;

    opts.add(ard_opts);
}

// --------------------------------------------------------------------------
void teca_tc_candidates::set_properties(
    const std::string &prefix, variables_map &opts)
{
    TECA_POPTS_SET(opts, std::string, prefix, surface_wind_speed_variable)
    TECA_POPTS_SET(opts, std::string, prefix, vorticity_850mb_variable)
    TECA_POPTS_SET(opts, std::string, prefix, sea_level_pressure_variable)
    TECA_POPTS_SET(opts, std::string, prefix, core_temperature_variable)
    TECA_POPTS_SET(opts, double, prefix, max_core_radius)
    TECA_POPTS_SET(opts, double, prefix, min_vorticity_850mb)
    TECA_POPTS_SET(opts, double, prefix, vorticity_850mb_window)
    TECA_POPTS_SET(opts, double, prefix, max_pressure_delta)
    TECA_POPTS_SET(opts, double, prefix, max_pressure_radius)
    TECA_POPTS_SET(opts, double, prefix, max_core_temperature_delta)
    TECA_POPTS_SET(opts, double, prefix, max_core_temperature_radius)
    TECA_POPTS_SET(opts, double, prefix, max_thickness_delta)
    TECA_POPTS_SET(opts, double, prefix, max_thickness_radius)
    TECA_POPTS_SET(opts, double, prefix, search_lat_high)
    TECA_POPTS_SET(opts, double, prefix, search_lat_low)
    TECA_POPTS_SET(opts, double, prefix, search_lon_high)
    TECA_POPTS_SET(opts, double, prefix, search_lon_low)
}
#endif

// --------------------------------------------------------------------------
teca_metadata teca_tc_candidates::get_output_metadata(unsigned int port,
    const std::vector<teca_metadata> &input_md)
{
#if TECA_DEBUG > 1
    cerr << teca_parallel_id()
        << "teca_tc_candidates::get_output_metadata" << endl;
#endif
    (void)port;

    teca_metadata output_md(input_md[0]);
    return output_md;
}

// --------------------------------------------------------------------------
int teca_tc_candidates::get_active_extent(p_teca_variant_array lat,
    p_teca_variant_array lon, std::vector<unsigned long> &extent) const
{
    extent = {1, 0, 1, 0, 0, 0};

    unsigned long high_i = lon->size() - 1;
    if (this->search_lon_low > this->search_lon_high)
    {
        extent[0] = 0l;
        extent[1] = high_i;
    }
    else
    {
        TEMPLATE_DISPATCH_FP(
            teca_variant_array_impl,
            lon.get(),
            NT *p_lon = std::dynamic_pointer_cast<TT>(lon)->get();

            if (teca_coordinate_util::index_of(p_lon, 0, high_i, static_cast<NT>(this->search_lon_low), false, extent[0])
                || teca_coordinate_util::index_of(p_lon, 0, high_i, static_cast<NT>(this->search_lon_high), true, extent[1]))
            {
                TECA_ERROR(
                    << "requested longitude ["
                    << this->search_lon_low << ", " << this->search_lon_high << ", "
                    << "] is not contained in the current dataset bounds ["
                    << p_lon[0] << ", " << p_lon[high_i] << "]")
                return -1;
            }
            )

    }
    if (extent[0] > extent[1])
    {
        TECA_ERROR("invalid longitude coordinate array type")
        return -1;
    }

    unsigned long high_j = lat->size() - 1;
    if (this->search_lat_low > this->search_lat_high)
    {
        extent[2] = 0l;
        extent[3] = high_j;
    }
    else
    {
        TEMPLATE_DISPATCH_FP(
            teca_variant_array_impl,
            lat.get(),
            NT *p_lat = std::dynamic_pointer_cast<TT>(lat)->get();

            if (teca_coordinate_util::index_of(p_lat, 0, high_j, static_cast<NT>(this->search_lat_low), false, extent[2])
                || teca_coordinate_util::index_of(p_lat, 0, high_j, static_cast<NT>(this->search_lat_high), true, extent[3]))
            {
                TECA_ERROR(
                    << "requested latitude ["
                    << this->search_lat_low << ", " << this->search_lat_high
                    << "] is not contained in the current dataset bounds ["
                    << p_lat[0] << ", " << p_lat[high_j] << "]")
                return -1;
            }
            )

    }
    if (extent[2] > extent[3])
    {
        TECA_ERROR("invalid latitude coordinate array type")
        return -1;
    }

    return 0;
}

// --------------------------------------------------------------------------
std::vector<teca_metadata> teca_tc_candidates::get_upstream_request(
    unsigned int port, const std::vector<teca_metadata> &input_md,
    const teca_metadata &request)
{
#if TECA_DEBUG > 1
    cerr << teca_parallel_id()
        << "teca_tc_candidates::get_upstream_request" << endl;
#endif
    (void)port;

    std::vector<teca_metadata> up_reqs;
    teca_metadata md = input_md[0];

    // locate the extents of the user supplied region of
    // interest
    teca_metadata coords;
    if (md.get("coordinates", coords))
    {
        TECA_ERROR("metadata is missing \"coordinates\"")
        return up_reqs;
    }

    p_teca_variant_array lat;
    p_teca_variant_array lon;
    if (!(lat = coords.get("y")) || !(lon = coords.get("x")))
    {
        TECA_ERROR("metadata missing lat lon coordinates")
        return up_reqs;
    }

    std::vector<unsigned long> extent(6, 0l);
    if (this->get_active_extent(lat, lon, extent))
    {
        TECA_ERROR("failed to determine the active extent")
        return up_reqs;
    }

#if TECA_DEBUG > 1
    cerr << teca_parallel_id() << "active_bound = "
        << this->search_lon_low<< ", " << this->search_lon_high
        << ", " << this->search_lat_low << ", " << this->search_lat_high
        << endl;
    cerr << teca_parallel_id() << "active_extent = "
        << extent[0] << ", " << extent[1] << ", " << extent[2] << ", "
        << extent[3] << ", " << extent[4] << ", " << extent[5] << endl;
#endif

    // build the request
    std::set<std::string> arrays;
    request.get("arrays", arrays);
    arrays.insert(this->surface_wind_speed_variable);
    arrays.insert(this->vorticity_850mb_variable);
    arrays.insert(this->sea_level_pressure_variable);
    arrays.insert(this->core_temperature_variable);
    arrays.insert(this->thickness_variable);

    teca_metadata up_req(request);
    up_req.set("arrays", arrays);
    up_req.set("extent", extent);

    up_reqs.push_back(up_req);
    return up_reqs;
}

// --------------------------------------------------------------------------
const_p_teca_dataset teca_tc_candidates::execute(unsigned int port,
    const std::vector<const_p_teca_dataset> &input_data,
    const teca_metadata &request)
{
#if TECA_DEBUG > 1
    cerr << teca_parallel_id() << "teca_tc_candidates::execute" << std::endl;
#endif
    (void)port;
    (void)request;

    if (!input_data.size())
    {
        TECA_ERROR("empty input")
        return nullptr;
    }

    // get the input dataset
    const_p_teca_cartesian_mesh mesh
        = std::dynamic_pointer_cast<const teca_cartesian_mesh>(input_data[0]);
    if (!mesh)
    {
        TECA_ERROR("teca_cartesian_mesh is required")
        return nullptr;
    }

    // get coordinate arrays
    const_p_teca_variant_array y = mesh->get_y_coordinates();
    const_p_teca_variant_array x = mesh->get_x_coordinates();

    if (!x || !y)
    {
        TECA_ERROR("mesh coordinates are missing.")
        return nullptr;
    }

    // get time step
    unsigned long time_step;
    mesh->get_time_step(time_step);

    // get temporal offset of the current timestep
    double time_offset = 0.0;
    mesh->get_time(time_offset);

    // get offset units
    std::string time_units;
    mesh->get_time_units(time_units);

    // get offset calendar
    std::string calendar;
    mesh->get_calendar(calendar);

    // get extent of data passed in
    std::vector<unsigned long> extent;
    mesh->get_extent(extent);

    long nlat = extent[3] - extent[2] + 1;
    long nlon = extent[1] - extent[0] + 1;

    // get wind speed array
    const_p_teca_variant_array surface_wind_speed
        = mesh->get_point_arrays()->get(this->surface_wind_speed_variable);

    if (!surface_wind_speed)
    {
        TECA_ERROR("Dataset missing wind speed variable \""
            << this->surface_wind_speed_variable << "\"")
        return nullptr;
    }

    // get vorticity_850mb array
    const_p_teca_variant_array vorticity_850mb
        = mesh->get_point_arrays()->get(this->vorticity_850mb_variable);

    if (!vorticity_850mb)
    {
        TECA_ERROR("Dataset missing vorticity_850mb variable \""
            << this->vorticity_850mb_variable << "\"")
        return nullptr;
    }

    // get core_temperature array
    const_p_teca_variant_array core_temperature
        = mesh->get_point_arrays()->get(this->core_temperature_variable);

    if (!core_temperature)
    {
        TECA_ERROR("Dataset missing core_temperature variable \""
            << this->core_temperature_variable << "\"")
        return nullptr;
    }

    // get sea_level_pressure array
    const_p_teca_variant_array sea_level_pressure
        = mesh->get_point_arrays()->get(this->sea_level_pressure_variable);

    if (!sea_level_pressure)
    {
        TECA_ERROR("Dataset missing sea_level_pressure variable \""
            << this->sea_level_pressure_variable << "\"")
        return nullptr;
    }

    // get thickness array
    const_p_teca_variant_array thickness;
    if (!(thickness = mesh->get_point_arrays()->get(this->thickness_variable)))
    {
        TECA_ERROR("Dataset missing thickness variable \""
            << this->thickness_variable << "\"")
        return nullptr;
    }

    // identify candidates
    p_teca_table candidates = teca_table::New();

    std::chrono::high_resolution_clock::time_point t0, t1;

    NESTED_TEMPLATE_DISPATCH_FP(const teca_variant_array_impl,
        x.get(), _COORD,

        const NT_COORD *lon = static_cast<const TT_COORD*>(x.get())->get();
        const NT_COORD *lat = static_cast<const TT_COORD*>(y.get())->get();

        NESTED_TEMPLATE_DISPATCH_FP(const teca_variant_array_impl,
            surface_wind_speed.get(), _VAR,

            // configure the candidate table
            candidates->declare_columns("storm_id", int(),
                "lon", NT_COORD(), "lat", NT_COORD(), "surface_wind", NT_VAR(),
                "850mb_vorticity", NT_VAR(), "sea_level_pressure", NT_VAR(),
                "have_core_temp", int(), "have_thickness", int(),
                "core_temp", NT_VAR(), "thickness", NT_VAR());

            const NT_VAR *v = dynamic_cast<const TT_VAR*>(surface_wind_speed.get())->get();
            const NT_VAR *w = dynamic_cast<const TT_VAR*>(vorticity_850mb.get())->get();
            const NT_VAR *P = dynamic_cast<const TT_VAR*>(sea_level_pressure.get())->get();
            const NT_VAR *T = dynamic_cast<const TT_VAR*>(core_temperature.get())->get();
            const NT_VAR *th = dynamic_cast<const TT_VAR*>(thickness.get())->get();

            t0 = std::chrono::high_resolution_clock::now();
            // invoke the detector
            if (teca_gfdl::tc_candidates(this->max_core_radius,
                this->min_vorticity_850mb, this->vorticity_850mb_window,
                this->max_pressure_delta, this->max_pressure_radius,
                this->max_core_temperature_delta, this->max_core_temperature_radius,
                this->max_thickness_delta, this->max_thickness_radius, v, w,
                T, P, th, lat, lon, nlat, nlon, this->minimizer_iterations,
                time_step, candidates.get()))
            {
                TECA_ERROR("GFDL TC detector encountered an error")
                return nullptr;
            }
            t1 = std::chrono::high_resolution_clock::now();
            )
        )

    // build the output
    p_teca_table out_table = teca_table::New();
    out_table->set_calendar(calendar);
    out_table->set_time_units(time_units);

    // add time stamp
    out_table->declare_columns("step", long(), "time", double());
    unsigned long n_candidates = candidates->get_number_of_rows();
    for (unsigned long i = 0; i < n_candidates; ++i)
        out_table << time_step << time_offset;

    // add the candidates
    out_table->concatenate_cols(candidates);

#if TECA_DEBUG > 1
    out_table->to_stream(cerr);
    cerr << std::endl;
#endif
    seconds_t dt(t1 - t0);
    TECA_STATUS("teca_tc_candidates step=" << time_step
        << " t=" << time_offset << ", dt=" << dt.count() << " sec")

    return out_table;
}

// --------------------------------------------------------------------------
void teca_tc_candidates::to_stream(std::ostream &os) const
{
    (void)os;
}


/**
 * @file bcsr.cpp
 * @brief
 * @author Oleg Borschuk
 * @date 2009-08-18
 */
#ifdef BSPY_EXPORTING_PLUGIN
#include <boost/python.hpp>
#endif

#include "bcsr.h"

using namespace std;
using namespace boost::python;

namespace blue_sky
{
  bcsr::bcsr (bs_type_ctor_param)
        : bcsr_amg_matrix_iface (),
        values (BS_KERNEL.create_object (v_float::bs_type ())),
        cols_ind (BS_KERNEL.create_object (v_long::bs_type ())),
        rows_ptr (BS_KERNEL.create_object (v_long::bs_type ()))

    {
      n_rows = n_cols = n_block_size = 0;
    }
  bcsr::bcsr (const bcsr & /*src*/) : bs_refcounter (),
        values (BS_KERNEL.create_object (v_float::bs_type ())),
        cols_ind (BS_KERNEL.create_object (v_long::bs_type ())),
        rows_ptr (BS_KERNEL.create_object (v_long::bs_type ()))
     {
      n_rows = n_cols = n_block_size = 0;
     }

  int
  bcsr::matrix_vector_product_t (spv_double v_, spv_double r_) const
  {
    t_long i, j1, j2, j, cl;

    t_long *r_ptr = &(*rows_ptr)[0];
    t_long *c_ind = &(*cols_ind)[0];
    t_float *val = &(*values)[0];
    t_double *v = &(*v_)[0];
    t_double *r = &(*r_)[0];

    //BS_ASSERT (v.size ());
    //BS_ASSERT (r.size ());
    //BS_ASSERT (v.size () >= r.size ()) (v.size ()) (r.size ());
    //BS_ASSERT (n_rows == (t_long)v.size ());

    //BS_ASSERT (n_block_size <= 1);
    if (n_block_size > 1)
      return -1;

    ////////////////////////////////////////////
    // loop through columns in transform matrix
    ////////////////////////////////////////////
    for (i = 0; i < n_rows; ++i)
      {
        j1 = r_ptr[i];
        j2 = r_ptr[i + 1];

        ////////////////////////////////////////////
        // loop through rows in transform matrix
        ////////////////////////////////////////////
        for (j = j1; j < j2; ++j)
          {
            cl = c_ind[j];
            r[cl] += v[i] * val[j];
          }
      }
    return 0;
  }

  // calculate linear combination with current matrix A:
  // r = alpha * A * u + beta * v
  // 1 step: r = beta * v / alpha
  // 2 step: r += alpha * matrix * u
  int
  bcsr::calc_lin_comb (t_double alpha,
                       t_double beta,
                       spv_double u_,
                       spv_double v_,
                       spv_double r_) const
  {
    static const t_double eps = t_double (1.0e-12);
    t_long i, n;
    int r_code = 0;

    t_double *v = &(*v_)[0];
    t_double *r = &(*r_)[0];

    t_double d = beta;

    if (fabs (alpha) > eps)
      {
        //BS_ASSERT (u.size ());
        //BS_ASSERT (r.size () >= (size_t)(n_rows * n_block_size));
        d /= alpha;
      }
    if (fabs (beta) > eps)
      {
        //BS_ASSERT (v.size ());
        //BS_ASSERT (v.size () == r.size ())(v.size ())(r.size ());
        //BS_ASSERT (r.size () >= (size_t)(n_rows * n_block_size)) (r.size ()) (n_rows) (n_block_size) (n_rows * n_block_size);
      }

    n = (t_long)r_->size ();
    if (fabs (beta) > eps)
      {
        i = 0;
        //t_long n2 = n - (n % 4);
        // TODO:
        for (; i < n; ++i)
          {
            r[i] = v[i] * d;
          }
      }
    else
      {
        memset (r, 0, sizeof (t_double) * n);
        //r.assign (n, 0);
        //assign (r, 0);
      }

    if (fabs (alpha) > eps)
      {
        matrix_vector_product_internal <true> (u_, r_, alpha);
      }

    return r_code;
  }

  t_double
  bcsr::get_allocated_memory_in_mbytes () const
    {
      t_double d = 0;

      d += sizeof (this);
      d += sizeof (t_float) * values->size ();
      d += sizeof (t_long) * rows_ptr->size ();
      d += sizeof (t_long) * cols_ind->size ();
      d /= 1024 * 1024;
      return d;
    }

  int
  bcsr::init (const t_long new_n_rows,
              const t_long new_n_cols,
              const t_long new_n_block_size,
              const t_long new_n_non_zeros)
  {
    if (new_n_block_size < 1 || new_n_cols < 1)
      return -1;

    n_block_size = new_n_block_size;
    n_cols = new_n_cols;

    if (alloc_rows_ptr (new_n_rows)
        || alloc_cols_ind_and_values (new_n_non_zeros, new_n_block_size))
      {
        return -2;
      }

    return 0;
  }

  int
  bcsr::alloc_rows_ptr (const t_long new_n_rows)
  {
    if (new_n_rows < 1)
      return -1;

    n_rows = new_n_rows;

    rows_ptr->resize (n_rows + 1);
    memset (&(*rows_ptr)[0], 0, sizeof (t_long) * (n_rows + 1));

    return 0;
  }

  int
  bcsr::alloc_cols_ind (const t_long new_n_non_zeros)
  {
    t_long nnz = new_n_non_zeros;

    if (nnz < 1)
      nnz = 1;

    cols_ind->resize (nnz);
    memset (&(*cols_ind)[0], -1, sizeof (t_long) * nnz);
    return 0;
  }

  int
  bcsr::alloc_values (const t_long new_n_non_zeros,
                               const t_long new_n_block_size)
  {
    t_long b_sqr = new_n_block_size * new_n_block_size;
    t_long nnz = new_n_non_zeros;

    n_block_size = new_n_block_size;
    if (nnz < 1)
      nnz = 1;
    if (b_sqr < 1)
      b_sqr = 1;

    values->resize (b_sqr * nnz);
    memset (&(*values)[0], 0, sizeof (t_float) * nnz * b_sqr);

    return 0;
  }

  int
  bcsr::alloc_cols_ind_and_values (const t_long new_n_non_zeros,
                                   const t_long new_n_blok_size)
  {
    if (alloc_cols_ind (new_n_non_zeros) || alloc_values (new_n_non_zeros, new_n_blok_size))
      return -2;

    return 0;
  }

  int
  bcsr::init_by_matrix  (sp_bcsr_matrix_iface_t m)
    {
      rows_ptr = m->get_rows_ptr ()->clone ();
      cols_ind = m->get_cols_ind ()->clone ();
      values = m->get_values ()->clone ();
      n_rows = m->get_n_rows ();
      n_cols = m->get_n_cols ();
      n_block_size = m->get_n_block_size ();
      return 0;
    }

  int
  bcsr::init_struct (const t_long new_n_rows,
                     const t_long new_n_cols,
                     const t_long new_n_non_zeros)
{
  if (new_n_cols < 1)
    return -1;

  n_cols = new_n_cols;
  if (alloc_rows_ptr (new_n_rows) || alloc_cols_ind (new_n_non_zeros))
    return -2;

  return 0;
}

  int
  bcsr::build_transpose_struct (sp_bcsr_matrix_iface_t m,
                                const t_long rows_offset,
                                const t_long cols_offset,
                                const t_long new_n_rows)
  {

    t_long i,j;
    t_long row_ind, j1, j2;
    t_long nnz;
    t_long nr;
    t_long *r_ptr;
    t_long *c_ind;
    t_long *in_rows_ptr = &(*(m->get_rows_ptr ()))[0];
    t_long *in_cols_ind = &(*(m->get_cols_ind ()))[0];
    //t_float *v_ptr;

    nnz = m->get_n_non_zeros ();
    t_long in_n_rows = m->get_n_rows ();
    t_long in_n_cols = m->get_n_cols ();
    t_long in_n_block_size = m->get_n_block_size ();

    if (new_n_rows == 0)
      nr = in_n_cols;
    else
      nr = new_n_rows;

    if (init (nr, in_n_rows, in_n_block_size, nnz))
      return -1;

    r_ptr = &(*rows_ptr)[0];
    c_ind = &(*cols_ind)[0];
    // Count the number of entries in each column of matrix (row of transposed matrix) and fill the rows_ptr array.

    for (i = 0; i < nnz; i++)
      {
        ++r_ptr[in_cols_ind[i] + 1 - rows_offset];
      }

    for (i = 2; i <= n_rows; i++)
      {
        r_ptr[i] += r_ptr[i - 1];
      }

    // Load the values and column numbers of transposed matrix

    for (i = 0; i < in_n_rows; i++)
      {
        j1 = in_rows_ptr[i];
        j2 = in_rows_ptr[i + 1];
        for (j = j1; j < j2; j++)
          {
            row_ind = in_cols_ind[j] - rows_offset;
            c_ind[r_ptr[row_ind]] = i + cols_offset;
            r_ptr[row_ind]++;
          }
      }
    // rows_ptr now points to the *end* of the jth row of entries
    // instead of the beginning.  Restore rows_ptr to front of row.

    for (i = n_rows; i > 0; i--)
      {
        r_ptr[i] = r_ptr[i - 1];
      }

    r_ptr[0] = 0;

    return 0;
  }

  int
  bcsr::build_transpose (sp_bcsr_matrix_iface_t m,
                         const t_long rows_offset,
                         const t_long cols_offset,
                         const t_long new_n_rows)
  {

    t_long i,j;
    t_long row_ind, j1, j2;
    t_long nnz;
    t_long nr;
    t_long *r_ptr;
    t_long *c_ind;
    t_long *in_rows_ptr = &(*(m->get_rows_ptr ()))[0];
    t_long *in_cols_ind = &(*(m->get_cols_ind ()))[0];
    t_float *v_ptr;
    t_float *in_values = &(*(m->get_values ()))[0];

    nnz = m->get_n_non_zeros ();
    t_long in_n_rows = m->get_n_rows ();
    t_long in_n_cols = m->get_n_cols ();
    t_long in_n_block_size = m->get_n_block_size ();

    if (new_n_rows == 0)
      nr = in_n_cols;
    else
      nr = new_n_rows;

    if (init (nr, in_n_rows, in_n_block_size, nnz))
      return -1;

    r_ptr = &(*rows_ptr)[0];
    c_ind = &(*cols_ind)[0];
    v_ptr = &(*values)[0];
    // Count the number of entries in each column of matrix (row of transposed matrix) and fill the rows_ptr array.

    for (i = 0; i < nnz; i++)
      {
        ++r_ptr[in_cols_ind[i] + 1 - rows_offset];
      }

    for (i = 2; i <= n_rows; i++)
      {
        r_ptr[i] += r_ptr[i - 1];
      }

    // Load the values and column numbers of transposed matrix

    for (i = 0; i < in_n_rows; i++)
      {
        j1 = in_rows_ptr[i];
        j2 = in_rows_ptr[i + 1];
        for (j = j1; j < j2; j++)
          {
            row_ind = in_cols_ind[j] - rows_offset;
            c_ind[r_ptr[row_ind]] = i + cols_offset;
            v_ptr[r_ptr[row_ind]] = in_values[j];
            r_ptr[row_ind]++;
          }
      }
    // rows_ptr now points to the *end* of the jth row of entries
    // instead of the beginning.  Restore rows_ptr to front of row.

    for (i = n_rows; i > 0; i--)
      {
        r_ptr[i] = r_ptr[i - 1];
      }

    r_ptr[0] = 0;

    return 0;
  }
  int
  bcsr::triple_matrix_product (sp_bcsr_matrix_iface_t r_matrix, sp_bcsr_matrix_iface_t a_matrix,
                               sp_bcsr_matrix_iface_t p_matrix, const bool update)
  {
    t_long i, i1, i2, j;
    t_long jr1, jr2, jr;
    t_long ja1, ja2, ja;
    t_long jp1, jp2, jp;
    t_long index_counter, i_row_ptr; // indexes of RAP values - current element and current row start element respectively
    t_long thread_num = 0, n_threads = 1, row_begin, row_end, row_begin_s, row_end_s;
    t_float r_a_product;
    t_long *r_ptr;
    t_long *c_ind;
    t_float *v_ptr;

    t_long *r_rows_ptr = &(*(r_matrix->get_rows_ptr ()))[0];
    t_long *r_cols_ind = &(*(r_matrix->get_cols_ind ()))[0];
    t_float *r_values   = &(*(r_matrix->get_values ()))[0];
    t_long  r_n_rows   = r_matrix->get_n_rows ();

    t_long *a_rows_ptr = &(*(a_matrix->get_rows_ptr ()))[0];
    t_long *a_cols_ind = &(*(a_matrix->get_cols_ind ()))[0];
    t_float *a_values   = &(*(a_matrix->get_values ()))[0];
    t_long  a_n_cols   = a_matrix->get_n_cols ();

    t_long *p_rows_ptr = &(*(p_matrix->get_rows_ptr ()))[0];
    t_long *p_cols_ind = &(*(p_matrix->get_cols_ind ()))[0];
    t_float *p_values   = &(*(p_matrix->get_values ()))[0];
    t_long  p_n_cols   = p_matrix->get_n_cols ();
    t_long block_size  = a_matrix->get_n_block_size ();

    // works only with non-blocked matrices
    if (block_size > 1)
      {
        //TODO: print error message
        return -2;
      }

    if (!update)
      {
        // initializing RAP matrix
        //result->n_block_size = A->n_block_size;
        //result->n_cols = P->n_cols;
        if (alloc_rows_ptr (r_n_rows))
          {
            //TODO: print error message
            return -1;
          }
      }
    r_ptr = &(*rows_ptr)[0];

    // initializing some stuff (c)

#ifdef TRIPLE_MATRIX_PRODUCT_PARALLEL
#pragma omp master
    {
      r_matrix->get_n_rows () < omp_get_max_threads () ? n_threads = r_matrix->get_n_rows () : n_threads = omp_get_max_threads ();
    }
#endif //TRIPLE_MATRIX_PRODUCT_PARALLEL

    a_marker_vec.assign (a_n_cols * n_threads, -1 );
    p_marker_vec.assign (p_n_cols * n_threads, -1 );
    t_long *a_marker = &a_marker_vec[0];
    t_long *p_marker = &p_marker_vec[0];

    r_ptr[0] = 0;

    if (!update)
      {
        ///////////////////////////////////////
        //  STEP 1 - calculating non-zero RAP elements count
        ///////////////////////////////////////
        // loop through R rows
#ifdef TRIPLE_MATRIX_PRODUCT_PARALLEL
#pragma omp parallel  firstprivate (A_marker, P_marker)                                \
                          private (i, thread_num, row_begin, row_end, i_row_ptr, jr1, jr2, jr, ja1, ja2, ja, \
                                   jp1, jp2, jp, i1, i2, j, index_counter)
#endif //TRIPLE_MATRIX_PRODUCT_PARALLEL
        {

#ifdef TRIPLE_MATRIX_PRODUCT_PARALLEL
          thread_num = omp_get_thread_num ();
          a_marker += a_matrix->get_n_cols () * thread_num;
          p_marker += p_matrix->get_n_cols () * thread_num;
#endif //TRIPLE_MATRIX_PRODUCT_PARALLEL
          index_counter = 0;
          if (thread_num < n_threads) // thead is used
            {
              row_begin = r_n_rows * thread_num / n_threads;
              row_end = r_n_rows * (thread_num + 1) / n_threads;
            }
          else
            row_begin = row_end = 0;

          for (i = row_begin; i < row_end; i++)
            {
              i_row_ptr = index_counter;

              jr1 = r_rows_ptr[i];
              jr2 = r_rows_ptr[i + 1];
              // loop through R columns in i row
              for (jr = jr1; jr < jr2; jr++)
                {
                  i1 = r_cols_ind[jr];

                  ja1 = a_rows_ptr[i1];
                  ja2 = a_rows_ptr[i1 + 1];
                  // loop through A columns in i1 row
                  for (ja = ja1; ja < ja2; ja++)
                    {
                      i2 = a_cols_ind[ja];

                      //  Check A_marker to see if point i2 has been previously
                      //  visited. New entries in RAP only occur from unmarked points.

                      if (a_marker[i2] != i)
                        {

                          //  Mark i2 as visited.

                          a_marker[i2] = i;

                          jp1 = p_rows_ptr[i2];
                          jp2 = p_rows_ptr[i2 + 1];
                          // loop through P columns in i2 row
                          for (jp = jp1; jp < jp2; jp++)
                            {
                              j = p_cols_ind[jp];

                              //  Check P_marker to see that RAP{i,j} has not already
                              //  been accounted for. If it has not, mark it and increment
                              //  counter.

                              if (p_marker[j] < i_row_ptr)
                                {
                                  // we have new entry in RAP matrix
                                  p_marker[j] = index_counter;
                                  index_counter++;
                                }
                            }
                        }
                    }
                }
              r_ptr[i + 1] = index_counter;
            }
#ifdef TRIPLE_MATRIX_PRODUCT_PARALLEL
#pragma omp barrier
#pragma omp single
#endif //TRIPLE_MATRIX_PRODUCT_PARALLEL
          // collect results of all threads at the head of each thread's chunk
          {
            i_row_ptr = 0;
            for (j = 1; j < n_threads - 1; j++)
              {
                row_begin_s = r_n_rows * j / n_threads;
                row_end_s = r_n_rows * (j + 1) / n_threads;
                r_ptr[row_end_s] += r_ptr[row_begin_s];
              }
          }
          // each thread adds collected head value to each member of own chunk
          if (thread_num != 0)
            {
              for (i = row_begin; i < row_end - 1; i++)
                {
                  r_ptr[i + 1] += r_ptr[row_begin];
                }
              if (row_end == r_n_rows)
                r_ptr[row_end] += r_ptr[row_begin];
            }
        } // end omp parallel

        // now we have (RAP_rows_ptr[result->n_rows]) non-zero values in RAP matrix.
        // Let`s finish it`s initialization
        if (alloc_cols_ind_and_values(r_ptr[n_rows], 1))
          //TODO: print error message
          return -1;
        set_n_cols (n_rows);
      }
    c_ind = &(*cols_ind)[0];
    v_ptr = &(*values)[0];

    // initializing some stuff (c)

    a_marker_vec.assign (a_n_cols * n_threads, -1 );
    p_marker_vec.assign (p_n_cols * n_threads, -1 );
    a_marker = &a_marker_vec[0];
    p_marker = &p_marker_vec[0];

    ///////////////////////////////////////
    //  STEP 2 - calculating a product
    ///////////////////////////////////////
#ifdef TRIPLE_MATRIX_PRODUCT_PARALLEL
#pragma omp parallel  firstprivate (a_marker, p_marker)                                   \
                      private (i, thread_num, row_begin, row_end, i_row_ptr, jr1, jr2, jr, ja1, ja2, ja, \
                               jp1, jp2, jp, i1, i2, j, index_counter, r_a_product)
#endif //TRIPLE_MATRIX_PRODUCT_PARALLEL
    {
#ifdef TRIPLE_MATRIX_PRODUCT_PARALLEL
      thread_num = omp_get_thread_num ();
      a_marker += a_matrix.get_n_cols () * thread_num;
      p_marker += p_matrix.get_n_cols () * thread_num;
#endif //TRIPLE_MATRIX_PRODUCT_PARALLEL

      if (thread_num < n_threads) // thead is used
        {
          row_begin = r_n_rows * thread_num / n_threads;
          row_end = r_n_rows * (thread_num + 1) / n_threads;
        }
      else
        row_begin = row_end = 0;
      index_counter = r_ptr[row_begin];
      for (i = row_begin; i < row_end; i++)
        {
          i_row_ptr = index_counter;
          p_marker[i] = index_counter;
          v_ptr[index_counter] = 0.0;
          c_ind[index_counter] = i;
          ++index_counter;

          jr1 = r_rows_ptr[i];
          jr2 = r_rows_ptr[i + 1];

          // loop through R columns in i row
          for (jr = jr1; jr < jr2; jr++)
            {
              i1 = r_cols_ind[jr];

              ja1 = a_rows_ptr[i1];
              ja2 = a_rows_ptr[i1 + 1];

              // loop through A columns in i1 row
              for (ja = ja1; ja < ja2; ja++)
                {
                  i2 = a_cols_ind[ja];

                  r_a_product = r_values[jr] * a_values[ja];

                  //  Check A_marker to see if point i2 has been previously
                  //  visited. New entries in RAP only occur from unmarked points.

                  if (a_marker[i2] != i)
                    {

                      //  Mark i2 as visited.

                      a_marker[i2] = i;

                      jp1 = p_rows_ptr[i2];
                      jp2 = p_rows_ptr[i2 + 1];

                      for (jp = jp1; jp < jp2; jp++)
                        {
                          j = p_cols_ind[jp];

                          //  Check P_marker to see that RAP{i,j} has not already
                          //  been accounted for. If it has not, mark it and increment
                          //  counter.

                          if (p_marker[j] < i_row_ptr)
                            {
                              // we have new entry in RAP matrix
                              v_ptr[index_counter] = p_values[jp] * r_a_product;
                              c_ind[index_counter] = j;

                              p_marker[j] = index_counter;
                              index_counter++;
                            }
                          else
                            {
                              // we have already visited this element, upgrade it
                              v_ptr[p_marker[j]] += p_values[jp] * r_a_product;
                            }
                        }
                    }
                  else
                    {
                      //  i2 is already visited. No new RAP entries. Just add contribution.

                      jp1 = p_rows_ptr[i2];
                      jp2 = p_rows_ptr[i2 + 1];

                      for (jp = jp1; jp < jp2; jp++)
                        {
                          j = p_cols_ind[jp];
                          v_ptr[p_marker[j]] += p_values[jp] * r_a_product;
                        }
                    }
                }
            }
        }
    } // end parallel

    return 0;
  }
/////////////////////////////////BS Register
/////////////////////////////////Stuff//////////////////////////

  BLUE_SKY_TYPE_STD_CREATE (bcsr);
  BLUE_SKY_TYPE_STD_COPY (bcsr);

  BLUE_SKY_TYPE_IMPL (bcsr, bcsr_amg_matrix_iface, "bcsr_matrix", "Block CSR Matrix class", "Realization of Block CSR Matricies");
}  // blue_sky namespace


/*
 *            Copyright 2009-2018 The VOTCA Development Team
 *                       (http://www.votca.org)
 *
 *      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 <votca/xtp/forces.h>
#include <boost/format.hpp>

#include "votca/xtp/statefilter.h"

namespace votca {
    namespace xtp {

      using std::flush;
        void Forces::Initialize(tools::Property &options) {

            std::vector<std::string> choices = {"forward", "central"};
            _force_method = options.ifExistsAndinListReturnElseThrowRuntimeError<std::string>(".method", choices);

            _noisy_output = options.ifExistsReturnElseReturnDefault<bool>(".noisy", false);

            if ((_force_method == "forward") || (_force_method == "central")) {
                _displacement = options.ifExistsReturnElseReturnDefault<double>(".displacement", 0.001); // Angstrom
            }
            _displacement*=tools::conv::ang2bohr;

            // check for force removal options
            choices = {"total", "none"};
            std::string _force_removal = options.ifExistsAndinListReturnElseThrowRuntimeError<std::string>(".removal", choices);
            if (_force_removal == "total") _remove_total_force = true;

            _natoms = _orbitals.QMAtoms().size();
            _forces =Eigen::MatrixX3d::Zero(_natoms,3);

            return;
        }

        void Forces::Calculate(double energy) {

            ctp::TLogLevel ReportLevel = _pLog->getReportLevel(); // backup report level
            if ( ! _noisy_output ){
                _pLog->setReportLevel(ctp::logERROR); // go silent for force calculations
            }
            std::vector<QMAtom*>& atoms=_orbitals.QMAtoms();
            for (unsigned atom_index=0;atom_index<atoms.size();atom_index++) {
                if ( _noisy_output ){
                    CTP_LOG(ctp::logINFO, *_pLog) << "FORCES--DEBUG working on atom " << atom_index<< flush;
                }
                Eigen::Vector3d atom_force=Eigen::Vector3d::Zero();
                // Calculate Force on this atom
                if (_force_method == "forward") atom_force=NumForceForward(energy, atom_index);
                if (_force_method == "central") atom_force=NumForceCentral(energy, atom_index);
                _forces.row(atom_index)=atom_force.transpose();
            }
            _pLog->setReportLevel(ReportLevel); //
            if (_remove_total_force) RemoveTotalForce();
            return;
        }

        void Forces::Report() const{

            CTP_LOG(ctp::logINFO, *_pLog) << (boost::format("   ---- FORCES (Hartree/Bohr)   ")).str() << flush;
            CTP_LOG(ctp::logINFO, *_pLog) << (boost::format("        %1$s differences   ") % _force_method).str() << flush;
            CTP_LOG(ctp::logINFO, *_pLog) << (boost::format("        displacement %1$1.4f Angstrom   ") % (_displacement*tools::conv::bohr2ang)).str() << flush;
            CTP_LOG(ctp::logINFO, *_pLog) << (boost::format("   Atom\t x\t  y\t  z ")).str() << flush;

            for (unsigned _i = 0; _i < _forces.rows(); _i++) {
                CTP_LOG(ctp::logINFO, *_pLog) << (boost::format(" %1$4d    %2$+1.4f  %3$+1.4f  %4$+1.4f")
                        % _i % _forces(_i, 0) % _forces(_i, 1) % _forces(_i, 2)).str() << flush;
            }
            return;
        }

        /* Calculate forces on an atom numerically by forward differences */
        Eigen::Vector3d Forces::NumForceForward(double energy,int atom_index) {
            Eigen::Vector3d force=Eigen::Vector3d::Zero();
            // get this atoms's current coordinates
            QMAtom* atom= _orbitals.QMAtoms()[atom_index];
            const tools::vec current_pos =atom->getPos();

            for (unsigned i_cart = 0; i_cart < 3; i_cart++) {
                tools::vec displacement_vec(0, 0, 0);
                displacement_vec[i_cart]=_displacement;
                // update the coordinate
                tools::vec pos_displaced = current_pos + displacement_vec;
                atom->setPos(pos_displaced);
                _gwbse_engine.ExcitationEnergies(_orbitals);
                double energy_displaced = _orbitals.getTotalStateEnergy(_filter.CalcState(_orbitals));
                force(i_cart) = (energy - energy_displaced) / _displacement;
                 atom->setPos(current_pos); // restore original coordinate into segment
            } // Cartesian directions
            return force;
        }

        /* Calculate forces on atoms numerically by central differences */
        Eigen::Vector3d Forces::NumForceCentral(double energy,int atom_index) {
            QMAtom* atom= _orbitals.QMAtoms()[atom_index];
            const tools::vec current_pos =atom->getPos();
            Eigen::Vector3d force=Eigen::Vector3d::Zero();
            for (unsigned i_cart = 0; i_cart < 3; i_cart++) {
                if ( _noisy_output ){
                    CTP_LOG(ctp::logINFO, *_pLog) << "FORCES--DEBUG           Cartesian component " << i_cart << flush;
                }
                tools::vec displacement_vec(0, 0, 0);
                displacement_vec[i_cart]=_displacement;
                // update the coordinate
                tools::vec pos_displaced = current_pos + displacement_vec;
                atom->setPos(pos_displaced);
                _gwbse_engine.ExcitationEnergies(_orbitals);
                double energy_displaced_plus = _orbitals.getTotalStateEnergy(_filter.CalcState(_orbitals));
                // update the coordinate
                pos_displaced = current_pos - displacement_vec;
                atom->setPos(pos_displaced);
                _gwbse_engine.ExcitationEnergies(_orbitals);
                double energy_displaced_minus = _orbitals.getTotalStateEnergy(_filter.CalcState(_orbitals));
                force(i_cart) = 0.5 * (energy_displaced_minus - energy_displaced_plus) / _displacement;
                atom->setPos(current_pos); // restore original coordinate into orbital
            }
            return force;
        }

        void Forces::RemoveTotalForce() {
            Eigen::Vector3d total_force = _forces.colwise().sum();
            for (unsigned i_atom = 0; i_atom < _natoms; i_atom++) {
                _forces.row(i_atom)-=total_force/double(_natoms);
            }
            return;
        }

    }
}


/*
Compute Jensen-Shannon distance between an assembly's average tri-nucleotide
or amino acid distribution and its genes' tri-nucleotide or amino acid distributions.
*/
#include <iostream>
#include <string>
#include <boost/program_options.hpp>
#include "./task/compute_distance_task.cpp"

using namespace std;
namespace po = boost::program_options;

int main(int ac, char* av[]) {
	try {
		po::options_description desc(
			"Compute Jensen-Shannon distance between an assembly's average "
            "tri-nucleotide (or amino acid) distribution and its genes' "
            "tri-nucleotide (or amino acid) distributions"
		);
		desc.add_options()
            (
            	"help,h",
            	"Print help message"
            )
            (
                "kind,k",
                po::value<string>()->default_value(""),
                "One of \"tri-nucleotide\" or \"amino-acid\""
            )
            (
            	"n_threads,t",
            	po::value<int>()->default_value(4),
            	"Number of threads to use."
            )
        ;

        po::variables_map vm;
        po::store(po::parse_command_line(ac, av, desc), vm);
        po::notify(vm);

        if (vm.count("help")) {
            cerr << desc << endl;
            return 0;
        }

        cerr << "Computing Jensen-Shannon distance" << endl;

        string kind = vm["kind"].as<string>();
        if (kind == "tri-nucleotide" || kind == "amino-acid") {
            cerr << "Kind: " << kind << endl;
        } else if (kind.empty()) {
            cerr << "Error: parameter \"kind\" not set.";
            cerr << " See --help for usage." << endl;
            return 1;
        } else {
            cerr << "Error: unknown value for parameter \"kind\": \"" << kind;
            cerr << "\". See --help for usage." << endl;
            return 1;
        }

        const int n_threads = vm["n_threads"].as<int>();
        cerr << "Threads: " << n_threads << endl;

        compute_distance(kind, n_threads);
	}
	catch (exception& e) {
		cerr << "Exception raised: " << e.what() << endl;
		return 1;
	}
	catch (...) {
		cerr << "Unknown exception raised" << endl;
		return 1;
	}
	return 0;
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2012-2014 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef THREADING_HH
#define THREADING_HH

#include <functional>
#include <future>
#include <map>
#include <queue>
#include <utility>
#include <vector>

#define BOOST_THREAD_PROVIDES_INTERRUPTIONS

#ifndef BOOST_DISABLE_THREADS
#include <boost/thread/condition_variable.hpp>
#include <boost/thread/locks.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/thread/thread.hpp>
#endif

//#include <boost/timer/timer.hpp>  // xzl: this seems to conflict with boost/progress.hpp

#include "utilities/kalloc.hh"

namespace Kaskade
{
#ifndef BOOST_DISABLE_THREADS
  /**
   * \ingroup threading
   * \brief A global lock for the Dune::QuadratureRules factory, which is not thread-safe as of 2015-01-01.
   */
  extern boost::mutex DuneQuadratureRulesMutex;

  /**
   * \ingroup threading
   * \brief A global lock for the Dune::GenericReferenceElement singletons, which are not thread-safe.
   */
  extern boost::mutex refElementMutex;
#endif

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

  /**
   * \ingroup threading
   * \brief Computes partitioning points such that the sum of weights in each partition is roughly the same.
   *
   * Let \f$ w_i, 0\le i < N \f$ denote the weights x[i]. On exit, x has size \f$ k=n+1 \f$ with values \f$ x_i \f$
   * such that \f$ x_0 = 0 \f$, \f$ x_{k-1} = N\f$, and
   * \f[ \sum_{j=x_i}^{x_{i+1}-1} w_j \approx \frac{1}{k} \sum_{j=0}^{N-1} w_j. \f]
   *
   * \param[in,out] x the array of (nonnegative) weights
   * \param[in]     n the desired number of partitions (positive).
   */
  void equalWeightRanges(std::vector<size_t>& x, size_t n);

  /**
   * \ingroup threading
   * \brief Computes partitioning points of ranges for uniform weight distributions.
   *
   * The functionality is similar to \ref equalWeightRanges, but for uniform weight distribution, the
   * partitioning points can be computed directly.
   *
   * \tparam Index an integral type
   * \param i the number of the range for which the starting point is to be computed. Has to be in [0,n]
   * \param n the number of ranges
   * \param m the total number of entries
   */
  template <class BlockIndex, class Index>
  Index uniformWeightRangeStart(BlockIndex i, BlockIndex n, Index m)
  {
    assert(i>=0 && i<=n && n>0 && m>0);
    return (i*m)/n;
  }

  /**
   * \ingroup threading
   * \brief Computes the range in which an index is to be found when partitioned for uniform weights.
   *
   * \tparam Index an integral type
   * \param j the index for which the range number is to be computed. Has to be in [0,m[
   * \param n the number of ranges
   * \param m the total number of entries
   */
  template <class Index>
  Index uniformWeightRange(Index j, Index n, Index m)
  {
    assert(j>=0 && j<m && n>0 && m>0);

    // We're looking for i such that floor(i*m/n) <= j < floor((i+1)*m/n), i.e. index j has
    // to be in the half-open range given by the partitioning points of the range. Now this
    // implies i*m/n-1 <= j < (i+1)*m/n and also j*n/m-1 < i <= (j+1)*n/m. As i is integer,
    // this implies floor(j*n/m) <= i <= floor((j+1)*n/m). Typically, n/m is small, in which
    // case this closed interval is likely to contain just one natural number - the result.
    Index low = (j*n)/m;       // floor is implied by integer arithmetics
    Index high = ((j+1)*n)/m;  // floor is implied by integer arithmetics

    if (low==high)
    {
      assert(uniformWeightRangeStart(low,n,m)<=j && j<uniformWeightRangeStart(low+1,n,m));
      return low;
    }

    // The implied inequality chains above are not sharp estimates, therefore the interval
    // [low,high] can contain several natural numbers. This is always the case if n>m, i.e., we have
    // more ranges than entries and several ranges are empty. But it can also happen if j*n/m is
    // just below the next integral number. Now we have to walk through all natural numbers in
    // the interval to find the correct range. TODO: is there a direct way?
    Index i = low;
    while (i<high && !(uniformWeightRangeStart(i,n,m)<=j && j<uniformWeightRangeStart(i+1,n,m)))
      ++i;

    assert(uniformWeightRangeStart(i,n,m)<=j && j<uniformWeightRangeStart(i+1,n,m));
    return i;
  }

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

  #ifndef BOOST_DISABLE_THREADS
  /**
   * \ingroup threading
   * \brief A concurrent fifo queue.
   *
   * The queue is not copyable as it is intended to hold non-copyable std:packaged_task objects.
   */
  template <class T>
  class ConcurrentQueue {
  public:
    /**
     * \brief Constructs an empty queue.
     */
    ConcurrentQueue()
    {
    }

    /**
     * \brief Moves a queue.
     */
    ConcurrentQueue(ConcurrentQueue&& q)
    {
      boost::lock_guard<boost::mutex> lock(q.mutex);
      queue = std::move(q.queue);
    }

    /**
     * \brief Assignment.
     */
    ConcurrentQueue& operator=(ConcurrentQueue const& q)
    {
      boost::lock_guard<boost::mutex> lock(q.mutex);
      queue = q.queue;
      return *this;
    }

    bool empty() const
    {
      boost::lock_guard<boost::mutex> lock(mutex);
      return queue.empty();
    }

    size_t size() const
    {
      boost::lock_guard<boost::mutex> lock(mutex);
      return queue.size();
    }

    /**
     * \brief Stores an element at the end of the queue.
     */
    void push_back(T&& t)
    {
      {
      boost::lock_guard<boost::mutex> lock(mutex);
      queue.push(std::move(t));
      } // release lock before waking up consumers
      filled.notify_one();
    }

    /**
     * \brief Retrieves the foremost element.
     *
     * This method blocks if the queue is empty and waits for data to become available.
     */
    T pop_front()
    {
      boost::unique_lock<boost::mutex> lock(mutex);

      // Wait for data to become available.
      while (queue.empty())
        filled.wait(lock);

      // extract and remove data. When assignment throws an exception, the queue
      // remains unmodified
      T t = std::move(queue.front());
      queue.pop();
      return t;
    }

  private:
    std::queue<T> queue;
    mutable boost::mutex mutex; // has to be mutable because of empty/size
    boost::condition_variable filled;
  };
#endif

  //----------------------------------------------------------------------------
#ifndef BOOST_DISABLE_THREADS
  /**
   * \ingroup threading
   * \brief Abstract interface for tasks to be scheduled for concurrent execution.
   */
  typedef std::packaged_task<void()> Task;

  /**
   * \ingroup threading
   * \brief Abstract waitable job ticket for submitted tasks.
   */
  typedef std::future<void> Ticket;
#else
  /**
   * \ingroup threading
   * \brief Abstract interface for tasks to be scheduled for concurrent execution. This is just a workaround for missing std::packaged_task on GCC/Windows.
   */
  typedef std::function<void()> Task;

  /**
   * \ingroup threading
   * \brief Abstract waitable job ticket for submitted tasks. This is just a workaround for missing std::future on GCC/Windows.
   */
  struct Ticket
  {
    void get() const {}
    void wait() const {}
  };
#endif

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

  /**
   * \ingroup threading
   * \brief Implementation of thread pools suitable for parallelization of (more or less) memory-bound algorithms (not only) on NUMA machines.
   *
   * This class maintains two thread pools satisfying different needs.
   *
   * The threads in the first (global) pool can be moved by the operating system freely between nodes and CPUs, and should be used (by submitting tasks via \ref run)
   * whenever there is no particular need to execute the task on a particular NUMA node, i.e. if the task is not memory-bandwidth-bound or works on
   * data that is not located on a particular NUMA node. The number of global threads defaults to twice the number of available CPUs (such that all CPUs can be
   * busy even if some threads wait on a mutex), but is at least 4 unless limited to a smaller number on construction of the thread pool, see \ref instance.
   *
   * The threads in the second (NUMA) pool are pinned to nodes (the OS is allowed to move them between CPUS on the same node), and should be used
   * (by submitting tasks via \ref runOnNode) whenever the tasks are memory-bandwidth-bound and the data resides on a particular NUMA node.
   * Note that as the threads are locked to the nodes, the operating system cannot move the threads. This is intended to keep
   * the thread close to its data in order to have local memory access. On the other hand, on multi-user machines it can lead to
   * several threads competing for the same CPU while other CPUs are idle. Use the node-locked threads only if locality of memory
   * access is of top priority.
   *
   * Relying on the first-touch policy for controlling memory locality is not guaranteed to keep threads and data close to each other.
   * First, the allocator may re-use memory blocks previously touched and released by a different thread, leading to remote data
   * access. Second, the operating system may decide to move a thread to a different node without knowledge of which thread
   * is memory-bound or compute-bound.
   *
   * Caveat: When submitting tasks recursively to the task pool, it is easy to create a deadlock. While the top level tasks wait for the
   *         completion of the lower level tasks, the lower level task is not processed as all threads in the pool are occupied by the
   *         top level tasks. Take care not to submit more recursive tasks than there are worker threads available.
   */
  class NumaThreadPool {
  public:
    /**
     * \brief Returns a globally unique thread pool instance.
     *
     * On the very first call of this method, the singleton thread pool is created with a limitation of the number of global (unpinned)
     * threads limited by the given number of maxThreads. Later calls with different value of maxThreads do not change the number of threads.
     * In case the number of global threads shall be limited throughout, call this method at the very start of the program.
     *
     * \param maxThreads an upper bound for the number of global threads to create.
     *
     * As it makes little sense to have multiple thread pools fight for physical ressources, a single instance should be employed.
     */
    static NumaThreadPool& instance(int maxThreads = std::numeric_limits<int>::max());

    /**
     * \name System information
     * @{
     */

    /**
     * \brief Reports the number of NUMA nodes (i.e., memory interfaces/CPU sockets)
     */
    int nodes() const
    {
      return nNode;
    }

    /**
     * \brief Reports the total number of CPUs (usually a multiple of nodes)
     */
    int cpus() const
    {
      return nCpu;
    }

    /**
     * \brief Reports the number of CPUs on the given node (usually the same for all nodes).
     */
    int cpus(int node) const
    {
      return cpuByNode[node].size();
    }

    /**
     * \brief Reports the maximal number of CPUs on one node.
     */
    int maxCpusOnNode() const
    {
      return maxCpusPerNode;
    }

    /**
     * @}
     */

    /**
     * \name Task submission
     * @{
     */

    /**
     * \brief Schedules a task to be executed on an arbitrary CPU.
     *
     * Waiting for tasks to be completed can be done by obtaining a waitable Ticket object from the task
     * before submitting it.
     *
     * \param task the task object to be executed.
     */
    Ticket run(Task&& task);

    /**
     * \brief Schedules a task to be executed on a CPU belonging to the given NUMA node.
     *
     * Waiting for tasks to be completed can be done by obtaining a waitable Ticket object from the task
     * before submitting it.
     *
     * \param node the number of the node on which to execute the task. 0 <= node < nodes().
     * \param task the task object to be executed. The object has to live at least until the call to its call operator returns.
     */
    Ticket runOnNode(int node, Task&& task);

    /**
     * @}
     */

    /**
     * \name NUMA-aware memory management
     * @{
     */

    /**
     * \brief Returns the allocator used for the given node.
     */
    Kalloc& allocator(int node);

    /**
     * \brief Allocates memory on a specific node.
     *
     * Note: this is comparatively slow and should only be used for allocating large chunks of memory
     * to be managed locally. The memory has to be released by a subsequent call to deallocate.
     */
    void* allocate(size_t n, int node);

    /**
     * \brief frees a chunk of memory previously allocated
     */
    void deallocate(void* p, size_t n, int node);

    /**
     * \brief Reports the alignment size of allocator at given NUMA node.
     */
    size_t alignment(int node) const;

    /**
     * \brief Tells the allocator to prepare for subsequent allocation of several memory blocks of same size.
     * \param n the requested size of the memory blocks
     * \param k the number of memory blocks that will be requested
     * \param node on which NUMA node
     *
     * Use this as a hint for the allocator that allows to improve its performance.
     */
    void reserve(size_t n, size_t k, int node);

    /**
     * @}
     */

  private:

    // private constructor to be called by instance()
    NumaThreadPool(int maxThreads);
    ~NumaThreadPool();

#ifndef BOOST_DISABLE_THREADS
    Ticket runOnQueue(ConcurrentQueue<Task>& queue, Task&& task);
#endif

    int nCpu, nNode, maxCpusPerNode;
#ifndef BOOST_DISABLE_THREADS
public: // xzl debugging
    std::vector<ConcurrentQueue<Task>>    nodeQueue;  // task queues for passing to worker threads on nodes
private:
    ConcurrentQueue<Task>                 globalQueue;
    boost::thread_group                   threads;    // worker threads
#endif
    std::vector<int>                      nodeByCpu;  // NUMA topology info
    std::vector<std::vector<int>>         cpuByNode;  // NUMA topology info
    std::map<void*,std::pair<size_t,int>> memBlocks;  // NUMA memory allocations
    std::vector<Kalloc>                   nodeMemory; // NUMA memory management
  };

  /**
   * \ingroup threading
   * \brief A parallel for loop that executes the given functor in parallel on different CPUs
   *
   * \tparam Func a functor with an operator ()(int i, int n)
   *
   * \param f the functor to call
   * \param maxTasks the maximal number of tasks to create
   *
   * The given function object is called in parallel for values (i,n) with i ranging from 0 to n-1. The number n is
   * the same for all calls and guaranteed not to exceed maxTasks (but may be smaller).
   *
   * The function returns after the last task has been completed, therefore the computational
   * effort for tasks 0,...,n-1 should be roughly equal, no matter which value n has. For an optimal performance,
   * maxTasks should be either a small multiple of the number of CPUs in the system (such that ideally all CPUs are
   * busy the whole time) or much larger than that (such that an imbalance has only a small impact).
   */
  template <class Func>
  void parallelFor(Func const& f, int maxTasks = std::numeric_limits<int>::max())
  {
    NumaThreadPool& pool = NumaThreadPool::instance();
    int nTasks = std::min(4*pool.cpus(),maxTasks);

    std::vector<Ticket> tickets(nTasks);
    for (int i=0; i<nTasks; ++i)
      tickets[i] = pool.run(Task([i,&f,nTasks] { f(i,nTasks); }));

    for (auto& ticket: tickets)
    {
      ticket.wait();
    }
  }

  /**
   * \ingroup threading
   * \brief A parallel for loop that executes the given functor in parallel on different NUMA nodes
   *
   * \tparam Func a functor with an operator ()(int i, int n)
   *
   * \param f the functor to call
   * \param maxTasks the maximal number of tasks to create
   *
   * The given function object is called in parallel for values (i,n) with i ranging from 0 to n-1. The number n is
   * min(nNodes,maxTasks) for all calls to f. Every task is executed on the node given by arguemnt i.
   *
   * The function returns after the last task has been completed, therefore the computational
   * effort for tasks 0,...,n-1 should be roughly equal, no matter which value n has.
   */
  template <class Func>
  void parallelForNodes(Func const& f, int maxTasks = std::numeric_limits<int>::max())
  {
    NumaThreadPool& pool = NumaThreadPool::instance();
    int nTasks = std::min(pool.nodes(),maxTasks);
    std::vector<Ticket> tickets(nTasks);
    for (int i=0; i<nTasks; ++i)
      tickets[i] = pool.runOnNode(i,Task([i,&f,&nTasks] { f(i,nTasks); }));
    for (auto& ticket: tickets)
      ticket.wait();
  }

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

  /**
   * \cond internals
   */
  namespace ThreadingDetail
  {
    class NumaAllocatorBase
    {
    public:
      NumaAllocatorBase(int node);

      size_t max_size() const;

      void* allocate(size_t n);

      void deallocate(void* p, size_t n);

      int node() const
      {
        return nod;
      }

    private:
      int nod;
    public:  // xzl: for debugging
      Kalloc* allocator; // xzl: ctor will point this to existing threadpool's NUMA allocator
    };
  }
  /**
   * \endcond
   */

  /**
   * \ingroup threading
   * \brief An STL allocator that uses memory of a specific NUMA node only.
   */
  template <class T>
  class NumaAllocator
  {
  public:
    typedef T value_type;
    typedef T* pointer;
    typedef T const* const_pointer;
    typedef T& reference;
    typedef T const& const_reference;
    typedef std::size_t size_type;
    typedef std::ptrdiff_t difference_type;

    // make sure that on copying, the target copy has its data in the same
    // NUMA memory region as the source by default -- this improves data locality.
    typedef std::true_type propagate_on_container_copy_assignment;
    typedef std::true_type propagate_on_container_move_assignment;
    typedef std::true_type propagate_on_container_swap;

    template <class U>
    struct rebind
    {
      typedef NumaAllocator<U> other;
    };

    /**
     * \brief Construct an allocator for allocating on the given NUMA node.
     *
     * \param node the NUMA node on which to allocate the memory. This has to be less than NumaThreadPool::instance().nodes().
     *             For negative values, the memory is allocated in interleaved mode.
     */
    NumaAllocator(int node): alloc(node)
    {
    }

    /* xzl: the following three appears necessary, otherwise rebinding will run into errors
     * cf: alloc.cpp
     */
    NumaAllocator(const NumaAllocator & other) : alloc(other.node()) { }

    template <class U>
    NumaAllocator(const NumaAllocator<U> & other) : alloc(other.node()) { }

    ~NumaAllocator() {}

    /**
     * \brief Reports the node on which we allocate.
     */
    int node() const
    {
      return alloc.node();
    }

    pointer address( reference x ) const
    {
      return &x;
    }

    const_pointer address( const_reference x ) const
    {
      return &x;
    }

    size_type max_size() const
    {
      return alloc.max_size() / sizeof(T);
    }

    /**
     * \brief Allocates the requested amount of memory.
     *
     * If \arg n == 0, a null pointer is returned.
     *
     * \param n number of objects of type T
     */
    pointer allocate( size_type n, std::allocator<void>::const_pointer /* hint */ = 0 )
    {
      if (n>0)
        return static_cast<pointer>(alloc.allocate(n*sizeof(T)));
      else
        return nullptr;
    }

    void deallocate(pointer p, size_type n)
    {
      if (p)
        alloc.deallocate(static_cast<void*>(p),n*sizeof(T));
    }

    template< class U, class... Args >
    void construct( U* p, Args&&... args )
    {
      ::new((void*)p) U(std::forward<Args>(args)...);
    }

    template <class U>
    void destroy( U* p )
    {
      p->~U();
    }

    /**
     * \brief comparison for equality
     *
     * Allocators compare equal, if they allocate on the same NUMA node.
     */
    template <class U>
    bool operator==(NumaAllocator<U> const& other) const
    {
      return node()==other.node();
    }

    template <class U>
    bool operator!=(NumaAllocator<U> const& other) const
    {
      return !(node() == other.node());
    }

//  private:
  public:			// xzl: for debugging
    ThreadingDetail::NumaAllocatorBase alloc; // xzl: storing a ptr to threadpool's existing allocator
  };

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

  /**
   * \brief A utility class implementing appropriate copy semantics for boost mutexes.
   *
   * \todo: design appropriate semantics for move construction/assignment
   */
  class Mutex
  {
  public:
    /**
     * \brief Default constructor.
     *
     * The constructed object's mutex is unlocked.
     */
    Mutex() = default;

    /**
     * \brief Copy constructor.
     *
     * The constructed object's mutex is unlocked, independently of the state of m.
     */
    Mutex(Mutex const& m) {}

    /**
     * \brief Assignment.
     *
     * Our mutex doesn't change its state.
     */
    Mutex& operator=(Mutex const& m) { return *this; }

    #ifndef KASKADE_SEQUENTIAL
    /**
     * \brief provides access to the mutex to perform the locking.
     */
    boost::mutex& get() { return mutex; }
    #endif

  private:
    #ifndef KASKADE_SEQUENTIAL
    boost::mutex mutex;
    #endif
  };

}

#endif


// This file is a part of the OpenSurgSim project.
// Copyright 2013-2016, SimQuest Solutions Inc.
//
// 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 "SurgSim/Blocks/KnotIdentificationBehavior.h"

#include <boost/assign/list_of.hpp>
#include <boost/math/special_functions/sign.hpp>

#include "SurgSim/Framework/SceneElement.h"
#include "SurgSim/Graphics/OsgPointCloudRepresentation.h"
#include "SurgSim/Graphics/OsgTextRepresentation.h"
#include "SurgSim/Math/Geometry.h"
#include "SurgSim/Math/RigidTransform.h"
#include "SurgSim/Physics/Fem1DLocalization.h"
#include "SurgSim/Physics/Fem1DRepresentation.h"
#include "SurgSim/Physics/FemElement.h"

using SurgSim::Math::Vector3d;
using SurgSim::Math::Vector2d;

namespace SurgSim
{
namespace Blocks
{
SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Blocks::KnotIdentificationBehavior,
				 KnotIdentificationBehavior);

KnotIdentificationBehavior::KnotIdentificationBehavior(const std::string& name) :
	Framework::Behavior(name), m_knotName("Undetermined")
{
	using boost::assign::list_of;

	SURGSIM_ADD_SERIALIZABLE_PROPERTY(KnotIdentificationBehavior,
		std::shared_ptr<SurgSim::Physics::Fem1DRepresentation>, Fem1d, getFem1d, setFem1d);

	addKnownKnotCode("Trefoil Knot", boost::assign::list_of(1)(-2)(3)(-1)(2)(-3),
		boost::assign::list_of(1)(1)(1)(1)(1)(1));
	addKnownKnotCode("Granny Knot", boost::assign::list_of(1)(-2)(3)(-4)(5)(-6)(4)(-5)(6)(-1)(2)(-3),
		boost::assign::list_of(-1)(-1)(-1)(-1)(-1)(-1)(-1)(-1)(-1)(-1)(-1)(-1));
	addKnownKnotCode("Granny Knot", boost::assign::list_of(1)(-2)(3)(4)(-5)(6)(-4)(5)(-6)(-1)(2)(-3),
		boost::assign::list_of(-1)(-1)(-1)(-1)(-1)(-1)(-1)(-1)(-1)(-1)(-1)(-1));
	addKnownKnotCode("Granny Knot", boost::assign::list_of(1)(-2)(3)(-4)(5)(-6)(4)(-5)(6)(-1)(2)(-3),
		boost::assign::list_of(1)(1)(1)(1)(1)(1)(1)(1)(1)(1)(1)(1));
	addKnownKnotCode("Granny Knot", boost::assign::list_of(1)(-2)(3)(4)(-5)(6)(-4)(5)(-6)(-1)(2)(-3),
		boost::assign::list_of(1)(1)(1)(1)(1)(1)(1)(1)(1)(1)(1)(1));
	addKnownKnotCode("Square Knot", boost::assign::list_of(1)(-2)(3)(4)(-5)(6)(-4)(5)(-6)(-1)(2)(-3),
		boost::assign::list_of(-1)(-1)(-1)(1)(1)(1)(1)(1)(1)(-1)(-1)(-1));
	addKnownKnotCode("Square Knot", boost::assign::list_of(1)(-2)(3)(-4)(5)(-6)(4)(-5)(6)(-1)(2)(-3),
		boost::assign::list_of(-1)(-1)(-1)(1)(1)(1)(1)(1)(1)(-1)(-1)(-1));
	addKnownKnotCode("Square Knot", boost::assign::list_of(1)(-2)(3)(4)(-5)(6)(-4)(5)(-6)(-1)(2)(-3),
		boost::assign::list_of(1)(1)(1)(-1)(-1)(-1)(-1)(-1)(-1)(1)(1)(1));
	addKnownKnotCode("Square Knot", boost::assign::list_of(1)(-2)(3)(-4)(5)(-6)(4)(-5)(6)(-1)(2)(-3),
		boost::assign::list_of(1)(1)(1)(-1)(-1)(-1)(-1)(-1)(-1)(1)(1)(1));
}

void KnotIdentificationBehavior::setFem1d(const std::shared_ptr<Framework::Component>& fem1d)
{
	m_fem1d = Framework::checkAndConvert<Physics::Fem1DRepresentation>(fem1d,
		"SurgSim::Physics::Fem1DRepresentation");
}

const std::shared_ptr<Physics::Fem1DRepresentation>& KnotIdentificationBehavior::getFem1d() const
{
	return m_fem1d;
}

const std::string& KnotIdentificationBehavior::getKnotName()
{
	boost::lock_guard<boost::mutex> lock(m_mutex);
	return m_knotName;
}

void KnotIdentificationBehavior::update(double dt)
{
	std::vector<std::string> results;
	for (const auto& projection : m_projections)
	{
		const auto& knotName = detectAndIdentifyKnot(projection);
		if (knotName != "No Knot")
		{
			results.push_back(knotName);
		}
	}
	std::string knotName = "No Knot";
	if (results.size() > 0)
	{
		knotName = results.back();
		if (results.size() > 1)
		{
			std::vector<std::string> known;
			for (const auto& r : results)
			{
				if (r != "Unknown Knot")
				{
					known.push_back(r);
				}
			}
			ptrdiff_t maxCount = 0;
			for (const auto& k : known)
			{
				auto count = std::count(known.begin(), known.end(), k);
				if (count > maxCount)
				{
					maxCount = count;
					knotName = k;
				}
			}
		}
	}
	boost::lock_guard<boost::mutex> lock(m_mutex);
	m_knotName = knotName;
}

int KnotIdentificationBehavior::getTargetManagerType() const
{
	return Framework::MANAGER_TYPE_PHYSICS;
}

bool KnotIdentificationBehavior::doInitialize()
{
	using Math::makeRigidTransform;
	Math::Matrix33d projection;
	m_projections.clear();
	m_projections.push_back(Math::Matrix33d::Identity() * 100.0);
	{
		projection <<
			1.0, 0.0, 0.0,
			0.0, 0.0, -1.0,
			0.0, 1.0, 0.0;
		m_projections.push_back(projection * 100.0);
	}
	{
		projection <<
			0.0, 0.0, -1.0,
			0.0, 1.0, 0.0,
			1.0, 0.0, 0.0;
		m_projections.push_back(projection * 100.0);
	}
	{
		projection <<
			0.0, -0.707107, -0.707107,
			0.0, 0.707107, -0.707107,
			1.0, 0.0, 0.0;
		m_projections.push_back(projection * 100.0);
	}
	{
		projection <<
			-0.707107, 0.0, -0.707107,
			0.0, 1.0, -0.0,
			0.707107, -0.0, -0.707107;
		m_projections.push_back(projection * 100.0);
	}
	{
		projection <<
			-1.0, 0.0, -0.0,
			0.0, 0.707107, -0.707107,
			0.0, -0.707107, -0.707107;
		m_projections.push_back(projection * 100.0);
	}
	return true;
}

bool KnotIdentificationBehavior::doWakeUp()
{
	if (m_fem1d == nullptr)
	{
		SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger()) << getClassName() << " named '" +
			getName() + "' must have a Fem1D Representation to detect knots in it.";
		return false;
	}
	return true;
}

void KnotIdentificationBehavior::addKnownKnotCode(const std::string& name, const std::vector<int>& code,
	const std::vector<int>& signs)
{
	std::vector<Crossing> enhancedGaussCode;
	auto codeLength = code.size();
	SURGSIM_ASSERT(codeLength == signs.size()) << " " <<  getClassName() << "::" << __func__ <<
		" passed a vector of signs that is not the same length as the code.";
	for (size_t id = 0; id < codeLength; ++id)
	{
		enhancedGaussCode.push_back(Crossing(code[id], 0, 0.0, signs[id]));
	}
	if (m_knownLists.find(name) == m_knownLists.end())
	{

		auto v = std::vector<std::vector<Crossing>>();
		v.push_back(enhancedGaussCode);
		m_knownLists[name] = v;
	}
	else
	{
		m_knownLists[name].push_back(enhancedGaussCode);
	}
}

void KnotIdentificationBehavior::clearKnownKnotCodes()
{
	m_knownLists.clear();
}

std::string KnotIdentificationBehavior::detectAndIdentifyKnot(
	const Math::Matrix33d& projection)
{
	auto gaussCode = getGaussCode(projection);

	// Perform reidmeister moves.
	performReidmeisterMoves(&gaussCode);

	// Identify the knot.
	return identifyKnot(gaussCode);
}

std::vector<KnotIdentificationBehavior::Crossing> KnotIdentificationBehavior::getGaussCode(
	const Math::Matrix33d& projection)
{
	std::vector<Vector3d> nodes3d;
	std::vector<Vector2d> nodes2d;
	std::vector<Vector3d> segments3d;

	buildNodeData(projection.col(0), projection.col(1), &nodes3d, &nodes2d, &segments3d);

	auto crossings = calculateCrossings(projection.col(2), nodes3d, nodes2d, segments3d);

	if (crossings.empty())
	{
		return std::vector<Crossing>();
	}

	std::sort(crossings.begin(), crossings.end(), [](const Crossing& i, const Crossing& j)
	{
		return (i.segmentId == j.segmentId) ? (i.segmentLocation < j.segmentLocation) : (i.segmentId < j.segmentId);
	});
	return crossings;
}

void KnotIdentificationBehavior::buildNodeData(
	const Vector3d& projectionX,
	const Vector3d& projectionY,
	std::vector<Vector3d>* nodes3d,
	std::vector<Vector2d>* nodes2d,
	std::vector<Vector3d>* segments3d)
{
	// Reserve the memory
	nodes3d->reserve(m_fem1d->getNumFemElements() + 2);
	nodes2d->reserve(m_fem1d->getNumFemElements() + 2);
	segments3d->reserve(m_fem1d->getNumFemElements() + 1);

	const auto& finalState = m_fem1d->getFinalState();

	Vector3d node3d;
	Vector2d node2d;
	for (size_t i = 0; i < m_fem1d->getNumFemElements(); ++i)
	{
		if (i == 0)
		{
			node3d = finalState->getPosition(m_fem1d->getFemElement(i)->getNodeIds()[0]);
			node2d[0] = projectionX.dot(node3d);
			node2d[1] = projectionY.dot(node3d);
			nodes3d->push_back(node3d);
			nodes2d->push_back(node2d);
		}

		node3d = finalState->getPosition(m_fem1d->getFemElement(i)->getNodeIds()[1]);
		node2d[0] = projectionX.dot(node3d);
		node2d[1] = projectionY.dot(node3d);

		segments3d->push_back(node3d - nodes3d->back());

		nodes3d->push_back(node3d);
		nodes2d->push_back(node2d);
	}
	const auto& lastNode =
		finalState->getPosition(m_fem1d->getFemElement(m_fem1d->getNumFemElements() - 1)->getNodeIds()[1]);
	node3d = lastNode + 0.99 * (finalState->getPosition(m_fem1d->getFemElement(0)->getNodeIds()[0]) - lastNode);
	node2d[0] = projectionX.dot(node3d);
	node2d[1] = projectionY.dot(node3d);
	segments3d->push_back(node3d - nodes3d->back());
	nodes3d->push_back(node3d);
	nodes2d->push_back(node2d);
}

std::vector<KnotIdentificationBehavior::Crossing> KnotIdentificationBehavior::calculateCrossings(
	const Math::Vector3d& projectionZ,
	const std::vector<SurgSim::Math::Vector3d>& nodes3d,
	const std::vector<SurgSim::Math::Vector2d>& nodes2d,
	const std::vector<SurgSim::Math::Vector3d>& segments3d)
{
	std::vector<Crossing> crossings;
	int crossingId = 1;
	double s, t;
	size_t numSegments = segments3d.size();
	for (size_t i = 0; i < numSegments - 2; ++i)
	{
		for (size_t j = i + 2; j < numSegments; ++j)
		{
			if (Math::doesIntersectSegmentSegment(nodes2d[i], nodes2d[i + 1], nodes2d[j], nodes2d[j + 1], &s, &t))
			{
				int sign =
					((nodes3d[i] + segments3d[i] * s) - (nodes3d[j] + segments3d[j] * t)).dot(projectionZ) > 0.0
					? 1 : -1;
				auto under = i;
				auto over = j;
				if (sign > 0)
				{
					under = j;
					over = i;
				}
				SurgSim::Math::Vector2d segments2dUnder = nodes2d[under + 1] - nodes2d[under];
				SurgSim::Math::Vector2d segments2dOver = nodes2d[over + 1] - nodes2d[over];
				double dot = segments2dUnder.dot(segments2dOver);
				double determinant = segments2dUnder.x() * segments2dOver.y() -
					segments2dUnder.y() * segments2dOver.x();
				int handedness = boost::math::sign(std::atan2(determinant, dot));
				crossings.push_back(Crossing(crossingId * sign, i, s, handedness));
				crossings.push_back(Crossing(crossingId * -sign, j, t, handedness));
				crossingId++;
			}
		}
	}

	return crossings;
}

void KnotIdentificationBehavior::performReidmeisterMoves(std::vector<Crossing>* gaussCode)
{
	if (gaussCode->empty())
	{
		return;
	}

	// Setup some variables for Reidmeister Move 3.
	ReidmeisterMove3Data move3Data;

	// Perform Reidmeister moves.
	std::vector<int> erased;
	bool reidmeisterMovePerformed = false;
	do
	{
		reidmeisterMovePerformed =
			tryReidmeisterMove1(gaussCode, &erased) ||
			tryReidmeisterMove2(gaussCode, &erased) ||
			tryReidmeisterMove3(gaussCode, &move3Data);
	} while (reidmeisterMovePerformed && !gaussCode->empty());

	adjustGaussCodeForErasedCrossings(gaussCode);
}

bool KnotIdentificationBehavior::tryReidmeisterMove1(std::vector<Crossing>* gaussCode,
													 std::vector<int>* erased)
{
	bool movePerformed = false;
	size_t i = 0;
	size_t j;
	while (i < gaussCode->size())
	{
		j = nextIndex(*gaussCode, i);
		if (isSameCross(*gaussCode, i, j))
		{
			erased->push_back(std::abs((*gaussCode)[i].id));
			erase(gaussCode, i, j);
			movePerformed = true;
		}
		else
		{
			++i;
		}
	}
	return movePerformed;
}

bool KnotIdentificationBehavior::tryReidmeisterMove2(std::vector<Crossing>* gaussCode,
													 std::vector<int>* erased)
{
	size_t i = 0;
	size_t j;
	size_t k;
	size_t l;
	while (i < gaussCode->size())
	{
		j = nextIndex(*gaussCode, i);
		if (isSameSign(*gaussCode, i, j))
		{
			k = nextIndex(*gaussCode, j);
			l = nextIndex(*gaussCode, k);
			// If the gaussCode is correctly formed, then it has to have more than 2 crossings by the time this gets
			// executed, so we don't need to check the case that k == i before l == i.
			while (l != i)
			{
				if (doesOverlap(*gaussCode, i, j, k, l))
				{
					erased->push_back(std::abs((*gaussCode)[i].id));
					erased->push_back(std::abs((*gaussCode)[j].id));
					erase(gaussCode, i, j, k, l);
					return true;
				}
				k = l;
				l = nextIndex(*gaussCode, k);
			}
		}
		++i;
	}
	return false;
}

bool KnotIdentificationBehavior::tryReidmeisterMove3(std::vector<Crossing>* gaussCode, ReidmeisterMove3Data* data)
{
	size_t i = 0;
	size_t j;
	size_t k;
	size_t l;
	size_t m;
	size_t n;
	if (data->code.empty() || data->code.size() > gaussCode->size())
	{
		data->code = *gaussCode;
		data->i = std::numeric_limits<size_t>::max();
		data->iCount = 0;
		data->m = std::numeric_limits<size_t>::max();
		data->n = std::numeric_limits<size_t>::max();
	}
	else
	{
		*gaussCode = data->code;
		i = data->i;
	}

	while (i < gaussCode->size())
	{
		j = nextIndex(*gaussCode, i);
		if (isSameSign(*gaussCode, i, j) && data->iCount < 2)
		{
			k = getComplementCross(*gaussCode, i);
			l = getComplementCross(*gaussCode, j);
			m = (i == data->i) ? data->m : std::numeric_limits<size_t>::max();
			n = (i == data->i) ? data->n : std::numeric_limits<size_t>::max();
			if (hasCommonNeighbor(*gaussCode, k, l, &m, &n))
			{
				data->iCount = (i == data->i) ? (data->iCount + 1) : 1;
				data->i = i;
				data->m = m;
				data->n = n;
				std::swap(gaussCode->at(k), gaussCode->at(l));
				std::swap(gaussCode->at(k), gaussCode->at(m));
				std::swap(gaussCode->at(l), gaussCode->at(n));
				return true;
			}
		}
		++i;
	}
	return false;
}

void KnotIdentificationBehavior::adjustGaussCodeForErasedCrossings(std::vector<Crossing>* gaussCode)
{
	using boost::math::sign;
	std::map<int, int> oldToNew;
	int next = 1;
	for (auto& code : *gaussCode)
	{
		const auto id = std::abs(code.id);
		int newId;
		if (oldToNew.find(id) == oldToNew.end())
		{
			newId = next;
			++next;
			oldToNew[id] = newId;
		}
		else
		{
			newId = oldToNew[id];
		}
		code.id = sign(code.id) * newId;
	}
}

std::string KnotIdentificationBehavior::identifyKnot(const std::vector<Crossing>& gaussCode)
{
	if (gaussCode.empty())
	{
		return "No Knot";
	}

	for (const auto& knotVector : m_knownLists)
	{
		for (const auto& knot : knotVector.second)
		{
			if (isSameCode(gaussCode, knot))
			{
				return knotVector.first;
			}
		}
	}

	return "Unknown Knot";
}

size_t KnotIdentificationBehavior::nextIndex(const std::vector<Crossing>& code, size_t i)
{
	return (++i) % code.size();
}

size_t KnotIdentificationBehavior::prevIndex(const std::vector<Crossing>& code, size_t i)
{
	return (i + code.size() - 1) % code.size();
}

bool KnotIdentificationBehavior::isSameSign(const std::vector<Crossing>& code, size_t i, size_t j)
{
	using boost::math::sign;
	return sign(code[i].id) == sign(code[j].id);
}

bool KnotIdentificationBehavior::isSameCross(const std::vector<Crossing>& code, size_t i, size_t j)
{
	return code[i].id == -code[j].id;
}

bool KnotIdentificationBehavior::doesOverlap(const std::vector<Crossing>& code, size_t i, size_t j, size_t k, size_t l)
{
	return isSameSign(code, i, j) && isSameSign(code, k, l) && !isSameSign(code, i, k) &&
		   ((isSameCross(code, i, k) && isSameCross(code, j, l)) ||
			(isSameCross(code, i, l) && isSameCross(code, j, k)));
}

void KnotIdentificationBehavior::erase(std::vector<Crossing>* code, size_t i, size_t j)
{
	code->erase(code->begin() + std::max(i, j));
	code->erase(code->begin() + std::min(i, j));
}

void KnotIdentificationBehavior::erase(std::vector<Crossing>* code, size_t i, size_t j, size_t k, size_t l)
{
	std::vector<size_t> list = { i, j, k, l };
	std::sort(list.begin(), list.end(), std::greater<size_t>());
	code->erase(code->begin() + list[0]);
	code->erase(code->begin() + list[1]);
	code->erase(code->begin() + list[2]);
	code->erase(code->begin() + list[3]);
}

size_t KnotIdentificationBehavior::getComplementCross(const std::vector<Crossing>& code, size_t i)
{
	for (size_t j = 0; j < code.size(); ++j)
	{
		if (code[j].id == -code[i].id)
		{
			return j;
		}
	}
	SURGSIM_FAILURE() << "Cannot find the complement cross for " << i;
	return std::numeric_limits<size_t>::max();
}

bool KnotIdentificationBehavior::hasCommonNeighbor(const std::vector<Crossing>& code, size_t i, size_t j,
												   size_t* k, size_t* l)
{
	size_t iPrev = prevIndex(code, i);
	size_t iNext = nextIndex(code, i);
	size_t jPrev = prevIndex(code, j);
	size_t jNext = nextIndex(code, j);

	if (isSameCross(code, iPrev, jPrev) && !(*k == iPrev && *l == jPrev))
	{
		*k = iPrev;
		*l = jPrev;
		return true;
	}
	if (isSameCross(code, iPrev, jNext) && !(*k == iPrev && *l == jNext))
	{
		*k = iPrev;
		*l = jNext;
		return true;
	}
	if (isSameCross(code, iNext, jPrev) && !(*k == iNext && *l == jPrev))
	{
		*k = iNext;
		*l = jPrev;
		return true;
	}
	if (isSameCross(code, iNext, jNext) && !(*k == iNext && *l == jNext))
	{
		*k = iNext;
		*l = jNext;
		return true;
	}

	return false;
}

bool KnotIdentificationBehavior::isSameCode(const std::vector<Crossing>& code, const std::vector<Crossing>& knot)
{
	if (code.size() != knot.size())
	{
		return false;
	}

	bool isSame = true;
	for (size_t i = 0; i < code.size() && isSame; ++i)
	{
		isSame = (code[i].id == knot[i].id) && (code[i].sign == knot[i].sign);
	}

	if (!isSame)
	{
		// Check if (code * -1) is the same as knot.
		isSame = true;
		for (size_t i = 0; i < code.size() && isSame; ++i)
		{
			isSame = (-code[i].id == knot[i].id) && (code[i].sign == knot[i].sign);
		}
	}

	return isSame;
}

} //namespace Blocks
} //namespace SurgSim


#ifndef BOOST_GEOMETRY_PROJECTIONS_BACON_HPP
#define BOOST_GEOMETRY_PROJECTIONS_BACON_HPP

// Boost.Geometry - extensions-gis-projections (based on PROJ4)
// This file is automatically generated. DO NOT EDIT.

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

// This file was modified by Oracle on 2017.
// Modifications copyright (c) 2017, 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: 4.9.1

// Original copyright notice:

// 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.

#include <boost/geometry/util/math.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>

namespace boost { namespace geometry
{

namespace srs { namespace par4
{
    struct apian {};
    struct ortel {};
    struct bacon {};

}} //namespace srs::par4

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace bacon
    {

            //static const double HLFPI2 = 2.46740110027233965467;
            static const double EPS = 1e-10;

            struct par_bacon
            {
                int bacn;
                int ortl;
            };

            // template class, using CRTP to implement forward/inverse
            template <typename CalculationType, typename Parameters>
            struct base_bacon_spheroid : public base_t_f<base_bacon_spheroid<CalculationType, Parameters>,
                     CalculationType, Parameters>
            {

                typedef CalculationType geographic_type;
                typedef CalculationType cartesian_type;

                par_bacon m_proj_parm;

                inline base_bacon_spheroid(const Parameters& par)
                    : base_t_f<base_bacon_spheroid<CalculationType, Parameters>,
                     CalculationType, Parameters>(*this, par) {}

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    static const CalculationType HALFPI = detail::HALFPI<CalculationType>();
                    static const CalculationType HLFPI2 = detail::HALFPI_SQR<CalculationType>();

                    CalculationType ax, f;

                    xy_y = this->m_proj_parm.bacn ? HALFPI * sin(lp_lat) : lp_lat;
                    if ((ax = fabs(lp_lon)) >= EPS) {
                        if (this->m_proj_parm.ortl && ax >= HALFPI)
                            xy_x = sqrt(HLFPI2 - lp_lat * lp_lat + EPS) + ax - HALFPI;
                        else {
                            f = 0.5 * (HLFPI2 / ax + ax);
                            xy_x = ax - f + sqrt(f * f - xy_y * xy_y);
                        }
                        if (lp_lon < 0.) xy_x = - xy_x;
                    } else
                        xy_x = 0.;
                }

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

            };

            // Apian Globular I
            template <typename Parameters>
            inline void setup_apian(Parameters& par, par_bacon& proj_parm)
            {
                proj_parm.bacn = proj_parm.ortl = 0;
                par.es = 0.;
            }

            // Ortelius Oval
            template <typename Parameters>
            inline void setup_ortel(Parameters& par, par_bacon& proj_parm)
            {
                proj_parm.bacn = 0;
                proj_parm.ortl = 1;
                par.es = 0.;
            }

            // Bacon Globular
            template <typename Parameters>
            inline void setup_bacon(Parameters& par, par_bacon& proj_parm)
            {
                proj_parm.bacn = 1;
                proj_parm.ortl = 0;
                par.es = 0.;
            }

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

    /*!
        \brief Apian Globular I projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Miscellaneous
         - Spheroid
         - no inverse
        \par Example
        \image html ex_apian.gif
    */
    template <typename CalculationType, typename Parameters>
    struct apian_spheroid : public detail::bacon::base_bacon_spheroid<CalculationType, Parameters>
    {
        inline apian_spheroid(const Parameters& par) : detail::bacon::base_bacon_spheroid<CalculationType, Parameters>(par)
        {
            detail::bacon::setup_apian(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Ortelius Oval projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Miscellaneous
         - Spheroid
         - no inverse
        \par Example
        \image html ex_ortel.gif
    */
    template <typename CalculationType, typename Parameters>
    struct ortel_spheroid : public detail::bacon::base_bacon_spheroid<CalculationType, Parameters>
    {
        inline ortel_spheroid(const Parameters& par) : detail::bacon::base_bacon_spheroid<CalculationType, Parameters>(par)
        {
            detail::bacon::setup_ortel(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Bacon Globular projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Miscellaneous
         - Spheroid
         - no inverse
        \par Example
        \image html ex_bacon.gif
    */
    template <typename CalculationType, typename Parameters>
    struct bacon_spheroid : public detail::bacon::base_bacon_spheroid<CalculationType, Parameters>
    {
        inline bacon_spheroid(const Parameters& par) : detail::bacon::base_bacon_spheroid<CalculationType, Parameters>(par)
        {
            detail::bacon::setup_bacon(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::apian, apian_spheroid, apian_spheroid)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::bacon, bacon_spheroid, bacon_spheroid)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::ortel, ortel_spheroid, ortel_spheroid)

        // Factory entry(s)
        template <typename CalculationType, typename Parameters>
        class apian_entry : public detail::factory_entry<CalculationType, Parameters>
        {
            public :
                virtual base_v<CalculationType, Parameters>* create_new(const Parameters& par) const
                {
                    return new base_v_f<apian_spheroid<CalculationType, Parameters>, CalculationType, Parameters>(par);
                }
        };

        template <typename CalculationType, typename Parameters>
        class ortel_entry : public detail::factory_entry<CalculationType, Parameters>
        {
            public :
                virtual base_v<CalculationType, Parameters>* create_new(const Parameters& par) const
                {
                    return new base_v_f<ortel_spheroid<CalculationType, Parameters>, CalculationType, Parameters>(par);
                }
        };

        template <typename CalculationType, typename Parameters>
        class bacon_entry : public detail::factory_entry<CalculationType, Parameters>
        {
            public :
                virtual base_v<CalculationType, Parameters>* create_new(const Parameters& par) const
                {
                    return new base_v_f<bacon_spheroid<CalculationType, Parameters>, CalculationType, Parameters>(par);
                }
        };

        template <typename CalculationType, typename Parameters>
        inline void bacon_init(detail::base_factory<CalculationType, Parameters>& factory)
        {
            factory.add_to_factory("apian", new apian_entry<CalculationType, Parameters>);
            factory.add_to_factory("ortel", new ortel_entry<CalculationType, Parameters>);
            factory.add_to_factory("bacon", new bacon_entry<CalculationType, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_BACON_HPP