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

#include "theia/sfm/pose/upnp.h"

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <glog/logging.h>

#include <algorithm>
#include <complex>
#include <utility>
#include <vector>

#include "theia/alignment/alignment.h"
#include "theia/math/util.h"
#include "theia/sfm/pose/build_upnp_action_matrix.h"
#include "theia/sfm/pose/build_upnp_action_matrix_using_symmetry.h"

namespace theia {

namespace {
typedef Eigen::Matrix<double, 3, 10> Matrix3x10d;
typedef Eigen::Matrix<double, 8, 8> Matrix8d;
typedef Eigen::Matrix<std::complex<double>, 8, 8> Matrix8cd;
typedef Eigen::Matrix<double, 10, 10> Matrix10d;
typedef Eigen::Matrix<double, 16, 16> Matrix16d;
typedef Eigen::Matrix<std::complex<double>, 16, 16> Matrix16cd;
typedef Eigen::Matrix<double, 10, 1> Vector10d;

const int kNumMaxRotations = 16;
const int kNumMaxRotationsExploitingSymmetry = 8;
const int kNumMinCorrespondences = 4;

// Helper structure to simplify function argument list.
struct InputDatum {
  InputDatum(const std::vector<Eigen::Vector3d>& _ray_origins,
             const std::vector<Eigen::Vector3d>& _ray_directions,
             const std::vector<Eigen::Vector3d>& _world_points) :
      ray_origins(_ray_origins),
      ray_directions(_ray_directions),
      world_points(_world_points) {}
  ~InputDatum() = default;

  const std::vector<Eigen::Vector3d>& ray_origins;
  const std::vector<Eigen::Vector3d>& ray_directions;
  const std::vector<Eigen::Vector3d>& world_points;
};

// Computes the H Matrix (see Eq. (6)) and the outer products of the ray
// directions, since these are used to compute matrix V (Eq. (5)).
inline Eigen::Matrix3d ComputeHMatrixAndRayDirectionsOuterProducts(
    const InputDatum& input_datum,
    std::vector<Eigen::Matrix3d>* outer_products) {
  const std::vector<Eigen::Vector3d>& ray_directions =
      input_datum.ray_directions;
  CHECK_NOTNULL(outer_products)->reserve(ray_directions.size());
  Eigen::Matrix3d h_inverse;
  h_inverse.setZero();
  for (const Eigen::Vector3d& ray : ray_directions) {
    outer_products->emplace_back(ray * ray.transpose());
    h_inverse -= outer_products->back();
  }
  h_inverse += ray_directions.size() * Eigen::Matrix3d::Identity();
  return h_inverse.inverse();
}

inline Matrix3x10d LeftMultiply(const Eigen::Vector3d& point) {
  Matrix3x10d phi_mat;
  // Row 0.
  phi_mat(0, 0) = point.x();
  phi_mat(0, 1) = point.x();
  phi_mat(0, 2) = -point.x();
  phi_mat(0, 3) = -point.x();
  phi_mat(0, 4) = 0.0;
  phi_mat(0, 5) = 2 * point.z();
  phi_mat(0, 6) = -2 * point.y();
  phi_mat(0, 7) = 2 * point.y();
  phi_mat(0, 8) = 2 * point.z();
  phi_mat(0, 9) = 0.0;

  // Row 1.
  phi_mat(1, 0) = point.y();
  phi_mat(1, 1) = -point.y();
  phi_mat(1, 2) = point.y();
  phi_mat(1, 3) = -point.y();
  phi_mat(1, 4) = -2.0 * point.z();
  phi_mat(1, 5) = 0.0;
  phi_mat(1, 6) = 2 * point.x();
  phi_mat(1, 7) = 2 * point.x();
  phi_mat(1, 8) = 0.0;
  phi_mat(1, 9) = 2 * point.z();

  // Row 3.
  phi_mat(2, 0) = point.z();
  phi_mat(2, 1) = -point.z();
  phi_mat(2, 2) = -point.z();
  phi_mat(2, 3) = point.z();
  phi_mat(2, 4) = 2.0 * point.y();
  phi_mat(2, 5) = -2.0 * point.x();
  phi_mat(2, 6) = 0.0;
  phi_mat(2, 7) = 0.0;
  phi_mat(2, 8) = 2.0 * point.x();
  phi_mat(2, 9) = 2.0 * point.y();
  return phi_mat;
}

inline std::vector<Eigen::Matrix3d> ComputeHelperMatrices(
    const InputDatum& input_datum,
    const std::vector<Eigen::Matrix3d>& outer_products,
    const Eigen::Matrix3d& h_matrix,
    Matrix3x10d* g_matrix,
    Eigen::Vector3d* j_matrix) {
  const std::vector<Eigen::Vector3d>& world_points = input_datum.world_points;
  const std::vector<Eigen::Vector3d>& ray_origins = input_datum.ray_origins;
  CHECK_EQ(ray_origins.size(), outer_products.size());
  CHECK_NOTNULL(g_matrix)->setZero();
  CHECK_NOTNULL(j_matrix)->setZero();
  const Eigen::Matrix3d identity = Eigen::Matrix3d::Identity();
  std::vector<Eigen::Matrix3d> v_matrices(
      world_points.size(), Eigen::Matrix3d::Zero());
  for (int i = 0; i < ray_origins.size(); ++i) {
    const Eigen::Matrix3d& outer_product = outer_products[i];
    // Computation following Eq. (5).
    v_matrices[i] = h_matrix * (outer_product - identity);
    const Eigen::Matrix3d& v_matrix = v_matrices[i];
    // Compute the left multiplication matrix or Phi matrix in the paper.
    const Matrix3x10d left_multiply_mat = LeftMultiply(world_points[i]);
    *j_matrix += v_matrix * ray_origins[i];
    *g_matrix += v_matrix * left_multiply_mat;
  }
  return v_matrices;
}

// Computes the block matrices that compose the M matrix in Eq. 17. These
// blocks are:
// quadratic_penalty_matrix = \sum A_i^T * A_i,
// linear_penalty_vector = \sum A_i^T * b_i ,
// gamma = \sum b_i^T * b_i.
void ComputeCostParameters(
    const InputDatum& input_datum,
    const std::vector<Eigen::Matrix3d>& outer_products,
    const Matrix3x10d& g_matrix,
    const Eigen::Vector3d& j_matrix,
    Upnp::CostParameters* cost_params) {
  const Eigen::Matrix3d identity = Eigen::Matrix3d::Identity();
  const std::vector<Eigen::Vector3d>& world_points = input_datum.world_points;
  const std::vector<Eigen::Vector3d>& ray_origins = input_datum.ray_origins;
  Matrix10d& a_matrix = CHECK_NOTNULL(cost_params)->quadratic_penalty_mat;
  Vector10d& b_vector = cost_params->linear_penalty_vector;
  double& gamma = cost_params->gamma;

  // Gamma is the sum of the dot products of b_matrices.
  for (int i = 0; i < world_points.size(); ++i) {
    // Compute the left multiplication matrix or Phi matrix in the paper.
    const Matrix3x10d left_multiply_mat = LeftMultiply(world_points[i]);
    const Eigen::Matrix3d outer_prod_minus_identity =
        outer_products[i] - identity;

    // Compute the i-th a_matrix.
    const Matrix3x10d temp_a_mat =
        outer_prod_minus_identity * (left_multiply_mat + g_matrix);
    a_matrix += temp_a_mat.transpose() * temp_a_mat;

    // Compute the i-th b_vector.
    const Eigen::Vector3d temp_b_mat =
        -outer_prod_minus_identity * (ray_origins[i] + j_matrix);
    b_vector += temp_a_mat.transpose() * temp_b_mat;

    // Compute the i-th gamma.
    gamma += temp_b_mat.squaredNorm();
  }
}

// Constructs the vector s as indicated in Eq. 12.
inline Vector10d ComputeRotationVector(const Eigen::Quaterniond& rotation) {
  Vector10d rotation_vector;
  // Set the values of the rotation vector.
  rotation_vector[0] = rotation.w() * rotation.w();
  rotation_vector[1] = rotation.x() * rotation.x();
  rotation_vector[2] = rotation.y() * rotation.y();
  rotation_vector[3] = rotation.z() * rotation.z();
  rotation_vector[4] = rotation.w() * rotation.x();
  rotation_vector[5] = rotation.w() * rotation.y();
  rotation_vector[6] = rotation.w() * rotation.z();
  rotation_vector[7] = rotation.x() * rotation.y();
  rotation_vector[8] = rotation.x() * rotation.z();
  rotation_vector[9] = rotation.y() * rotation.z();
  return rotation_vector;
}

Eigen::Vector3d ComputeTranslation(
    const InputDatum& input_datum,
    const Eigen::Quaterniond& rotation,
    const std::vector<Eigen::Matrix3d>& v_matrices) {
  Eigen::Vector3d translation = Eigen::Vector3d::Zero();
  for (int i = 0; i < input_datum.world_points.size(); ++i) {
    translation +=
        v_matrices[i] *
        (rotation * input_datum.world_points[i] - input_datum.ray_origins[i]);
  }
  return translation;
}

std::vector<Eigen::Vector3d> ComputeTranslations(
    const InputDatum& input_datum,
    const std::vector<Eigen::Quaterniond>& rotations,
    const std::vector<Eigen::Matrix3d>& v_matrices) {
  std::vector<Eigen::Vector3d> translations(rotations.size());
  const int num_points = input_datum.world_points.size();
  const int num_rotations = rotations.size();
  for (int i = 0; i < num_rotations; ++i) {
    translations[i] = ComputeTranslation(input_datum, rotations[i], v_matrices);
  }
  return translations;
}

std::vector<double> ComputeCostsAndRankSolutions(
    const Upnp::CostParameters& cost_params,
    std::vector<Eigen::Quaterniond>* solution_rotations,
    std::vector<Eigen::Vector3d>* solution_translations) {
  std::vector<double> costs(solution_rotations->size(), 0.0);
  std::vector<std::pair<int, double>> indexes_and_costs;
  indexes_and_costs.reserve(costs.size());
  // 1. Compute the costs of the solutions.
  for (int i = 0; i < costs.size(); ++i) {
    costs[i] = Upnp::EvaluateCost(cost_params, solution_rotations->at(i));
    indexes_and_costs.emplace_back(i, costs[i]);
  }

  // 2. Sort the costs such that the best rotation (i.e., lowest error) is the
  // first solution.
  std::sort(indexes_and_costs.begin(), indexes_and_costs.end(),
            [](const std::pair<int, double>& lhs,
               const std::pair<int, double>& rhs) {
              return lhs.second < rhs.second;
            });

  // 3. Rank the solutions.
  std::vector<Eigen::Quaterniond> ranked_rotations(costs.size());
  std::vector<Eigen::Vector3d> ranked_translations(costs.size());
  for (int i = 0; i < costs.size(); ++i) {
    costs[i] = indexes_and_costs[i].second;
    ranked_rotations[i] = solution_rotations->at(indexes_and_costs[i].first);
    ranked_translations[i] =
        solution_translations->at(indexes_and_costs[i].first);
  }

  *solution_rotations = std::move(ranked_rotations);
  *solution_translations = std::move(ranked_translations);

  return costs;
}

void DiscardBadSolutions(const InputDatum& input_datum,
                         std::vector<Eigen::Quaterniond>* solution_rotations,
                         std::vector<Eigen::Vector3d>* solution_translations) {
  CHECK_EQ(CHECK_NOTNULL(solution_rotations)->size(),
           CHECK_NOTNULL(solution_translations)->size());
  std::vector<Eigen::Quaterniond> final_rotations;
  std::vector<Eigen::Vector3d> final_translations;
  final_rotations.reserve(solution_rotations->size());
  final_translations.reserve(solution_translations->size());

  // Useful aliases.
  const std::vector<Eigen::Vector3d>& world_points = input_datum.world_points;
  const std::vector<Eigen::Vector3d>& ray_origins = input_datum.ray_origins;
  const std::vector<Eigen::Vector3d>& ray_directions =
      input_datum.ray_directions;

  // For every computed solution, check that points are in front of camera.
  for (int i = 0; i < solution_rotations->size(); ++i) {
    const Eigen::Quaterniond& soln_rotation = solution_rotations->at(i);
    const Eigen::Vector3d& soln_translation = solution_translations->at(i);

    // Check that all points are in front of the camera. Discard the solution
    // if this is not the case.
    bool all_points_in_front_of_camera = true;

    for (int j = 0; j < world_points.size(); ++j) {
      const Eigen::Vector3d transformed_point =
          soln_rotation * world_points[j] + soln_translation - ray_origins[j];

      // Find the rotation that puts the image ray at [0, 0, 1] i.e. looking
      // straightforward from the camera.
      const Eigen::Quaterniond unrot =
          Eigen::Quaterniond::FromTwoVectors(ray_directions[j],
                                             Eigen::Vector3d::UnitZ());

      // Rotate the transformed point and check if the z coordinate is
      // negative. This will indicate if the point is projected behind the
      // camera.
      const Eigen::Vector3d rotated_projection = unrot * transformed_point;
      if (rotated_projection.z() < 0) {
        all_points_in_front_of_camera = false;
        break;
      }
    }

    if (all_points_in_front_of_camera) {
      final_rotations.emplace_back(soln_rotation);
      final_translations.emplace_back(soln_translation);
    }
  }

  // Set the final solutions.
  std::swap(*solution_rotations, final_rotations);
  std::swap(*solution_translations, final_translations);
}

// The observed pattern is that duplicate rotations appear consequtively in the
// vector, i.e., rotation[i] == rotation[i + 1] is common.
std::vector<Eigen::Quaterniond> RemoveDuplicateRotations(
    const std::vector<Eigen::Quaterniond>& candidate_rotations) {
  const double kAngleThreshold = DegToRad(0.1);
  std::vector<Eigen::Quaterniond> rotations;
  rotations.reserve(candidate_rotations.size());

  // If no rotations then return empty vector.
  if (candidate_rotations.empty()) {
    return rotations;
  }

  for (int i = 0; i < candidate_rotations.size(); ++i) {
    bool duplicate_rotation = false;
    const Eigen::Quaterniond& candidate_rotation = candidate_rotations[i];
    for (int j = rotations.size() - 1; j >= 0; --j) {
      if (candidate_rotation.angularDistance(rotations[j]) < kAngleThreshold) {
        duplicate_rotation = true;
        break;
      }
    }
    if (!duplicate_rotation) {
      rotations.push_back(candidate_rotation);
    }
  }
  return rotations;
}

}  // namespace

inline std::vector<Eigen::Quaterniond>
Upnp::ComputeRotations(const int num_correspondences) {
  // Build the action matrix.
  std::vector<Eigen::Quaterniond> candidate_rotations;
  if (use_minimal_template_ && num_correspondences <= kNumMinCorrespondences) {
    candidate_rotations = SolveForRotationsFromMinimalSample();
  }
  candidate_rotations = SolveForRotationsFromNonMinimalSample();
  // Remove duplicate solutions.
  return RemoveDuplicateRotations(candidate_rotations);
}

double Upnp::EvaluateCost(const Upnp::CostParameters& parameters,
                          const Eigen::Quaterniond& rotation) {
  // Compute the quaternion vector.
  const Vector10d rotation_vector = ComputeRotationVector(rotation);
  return (rotation_vector.transpose() *
          parameters.quadratic_penalty_mat * rotation_vector +
          2.0 * parameters.linear_penalty_vector.transpose() *
          rotation_vector)(0, 0) + parameters.gamma;
}

double Upnp::ComputeResidual(const Eigen::Vector3d& ray_origin,
                             const Eigen::Vector3d& ray_direction,
                             const Eigen::Vector3d& world_point,
                             const Eigen::Quaterniond& rotation,
                             const Eigen::Vector3d& translation) {
  const Eigen::Quaterniond unrot =
      Eigen::Quaterniond::FromTwoVectors(ray_direction,
                                         Eigen::Vector3d::UnitZ());
  const Eigen::Vector3d reprojected_point =
      rotation * world_point + translation - ray_origin;
  const Eigen::Vector3d unrot_reprojected_point = unrot * reprojected_point;
  const Eigen::Vector3d unrot_ray_direction = unrot * ray_direction;
  return (unrot_reprojected_point.hnormalized() -
          unrot_ray_direction.hnormalized()).norm();
}

std::vector<Eigen::Matrix3d> Upnp::ComputeCostParameters(
    const std::vector<Eigen::Vector3d>& ray_origins,
    const std::vector<Eigen::Vector3d>& ray_directions,
    const std::vector<Eigen::Vector3d>& world_points) {
  const InputDatum input_datum(ray_origins, ray_directions, world_points);
  // 1. Compute the H matrix and the outer products of the ray directions.
  std::vector<Eigen::Matrix3d> outer_products;
  const Eigen::Matrix3d h_matrix =
      ComputeHMatrixAndRayDirectionsOuterProducts(input_datum, &outer_products);

  // 2. Compute matrices J and G from page 132 or 6-th page in the paper.
  Matrix3x10d g_matrix;
  Eigen::Vector3d j_matrix;
  const std::vector<Eigen::Matrix3d> v_matrices =
      ComputeHelperMatrices(input_datum,
                            outer_products,
                            h_matrix,
                            &g_matrix,
                            &j_matrix);

  // 3. Compute matrix the block-matrix of matrix M from Eq. 17.
  theia::ComputeCostParameters(input_datum,
                               outer_products,
                               g_matrix,
                               j_matrix,
                               &cost_params_);

  return v_matrices;
}

std::vector<Eigen::Quaterniond> Upnp::SolveForRotationsFromNonMinimalSample() {
  std::vector<Eigen::Quaterniond> rotations(kNumMaxRotationsExploitingSymmetry);
  // Build action matrix.
  const Matrix8d action_matrix = BuildActionMatrixUsingSymmetry(
      cost_params_.quadratic_penalty_mat,
      cost_params_.linear_penalty_vector,
      &non_minimal_sample_template_matrix_);

  const Eigen::EigenSolver<Matrix8d> eigen_solver(action_matrix);
  const Matrix8cd eigen_vectors = eigen_solver.eigenvectors();

  for (int i = 0; i < rotations.size(); ++i) {
    // According to the original implementation, the complex solutions
    // can be good, in particular when the number of correspondences is really
    // low. The solutions simply ignore the imaginary part.
    rotations[i] = Eigen::Quaterniond(eigen_vectors(4, i).real(),
                                      eigen_vectors(5, i).real(),
                                      eigen_vectors(6, i).real(),
                                      eigen_vectors(7, i).real()).normalized();
  }

  return rotations;
}

std::vector<Eigen::Quaterniond> Upnp::SolveForRotationsFromMinimalSample() {
  std::vector<Eigen::Quaterniond> rotations(kNumMaxRotations);
  // Build action matrix.
  const Matrix16d action_matrix =
      BuildActionMatrix(cost_params_.quadratic_penalty_mat,
                        cost_params_.linear_penalty_vector,
                        &minimal_sample_template_matrix_);

  const Eigen::EigenSolver<Matrix16d> eigen_solver(action_matrix, true);
  const Matrix16cd eigen_vectors = eigen_solver.eigenvectors();

  for (int i = 0; i < rotations.size(); ++i) {
    // According to the original implementation, the complex solutions
    // can be good, in particular when the number of correspondences is really
    // low. The solutions simply ignore the imaginary part.
    Eigen::Vector4d quaternion(eigen_vectors(11, i).real(),
                               eigen_vectors(12, i).real(),
                               eigen_vectors(13, i).real(),
                               eigen_vectors(14, i).real());

    if (quaternion[0] < 0.0) {
      quaternion *= -1.0;
    }

    rotations[i] = Eigen::Quaterniond(quaternion[0],
                                      quaternion[1],
                                      quaternion[2],
                                      quaternion[3]).normalized();
  }

  return rotations;
}

bool Upnp::EstimatePose(const std::vector<Eigen::Vector3d>& ray_origins,
                        const std::vector<Eigen::Vector3d>& ray_directions,
                        const std::vector<Eigen::Vector3d>& world_points,
                        std::vector<Eigen::Quaterniond>* solution_rotations,
                        std::vector<Eigen::Vector3d>* solution_translations,
                        std::vector<double>* solution_costs) {
  CHECK_NOTNULL(solution_rotations)->clear();
  CHECK_NOTNULL(solution_translations)->clear();
  CHECK_EQ(ray_origins.size(), ray_directions.size());
  CHECK_EQ(world_points.size(), ray_directions.size());

  // Compute Upnp cost parameters.
  const InputDatum input_datum(ray_origins, ray_directions, world_points);
  const std::vector<Eigen::Matrix3d> v_matrices =
      ComputeCostParameters(ray_origins, ray_directions, world_points);

  // Compute rotations.
  *solution_rotations = ComputeRotations(world_points.size());

  // Compute translation.
  *solution_translations =
      ComputeTranslations(input_datum, *solution_rotations, v_matrices);

  // Discard solutions that have points behind the camera.
  DiscardBadSolutions(input_datum, solution_rotations, solution_translations);

  if (solution_costs) {
    *solution_costs = ComputeCostsAndRankSolutions(cost_params_,
                                                   solution_rotations,
                                                   solution_translations);
  }

  return !solution_rotations->empty();
}

Upnp::CostParameters Upnp(const std::vector<Eigen::Vector3d>& ray_origins,
                          const std::vector<Eigen::Vector3d>& ray_directions,
                          const std::vector<Eigen::Vector3d>& world_points,
                          std::vector<Eigen::Quaterniond>* solution_rotations,
                          std::vector<Eigen::Vector3d>* solution_translations) {
  class Upnp estimator;
  CHECK(estimator.EstimatePose(ray_origins,
                               ray_directions,
                               world_points,
                               solution_rotations,
                               solution_translations))
      << "Could not estimate pose";
  return estimator.cost_params();
}

Upnp::CostParameters Upnp(const std::vector<Eigen::Vector2d>& normalized_pixels,
                          const std::vector<Eigen::Vector3d>& world_points,
                          std::vector<Eigen::Quaterniond>* solution_rotations,
                          std::vector<Eigen::Vector3d>* solution_translations) {
  CHECK_EQ(normalized_pixels.size(), world_points.size());
  std::vector<Eigen::Vector3d> ray_directions(world_points.size());
  std::vector<Eigen::Vector3d> ray_origins(world_points.size());

  // Compute the ray directions and origins from the normalized pixels.
  for (int i = 0; i < world_points.size(); ++i) {
    ray_directions[i] = normalized_pixels[i].homogeneous().normalized();
    ray_origins[i].setZero();
  }

  return Upnp(ray_origins, ray_directions, world_points,
              solution_rotations, solution_translations);
}

}  // namespace theia


#include <iostream>
#include <string>
#include <armadillo>
#include <time.h>
#include "ga.h"

using namespace std;
using namespace arma;

//------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------
mat ga_eval(mat DNAS, vec fitness, double alpha, double reg, double temperature){

  int DNASrows = DNAS.n_rows;
  int DNAScols = DNAS.n_cols;
//  fitness *= 1/max(abs(fitness));

  double myMax=max(max(abs(DNAS)));

	double randBuff=0;
  double aveDNAS=sum(sum(abs(DNAS),0)) /DNASrows/DNAScols;

	double mutRat=0.1*temperature;//*DNAScols;
	double mutAmmount=0.001*aveDNAS*temperature;

  double bigMutRat=0.001*temperature;//*DNAScols;
	double bigMutAmmount=0.005*aveDNAS*temperature;

  double veryBigMutRat=0.0001*temperature;//*DNAScols;
	double verBigMutAmmount=1.0*aveDNAS*temperature;

	int decision[2*DNASrows];
	int myindex;

  rowvec baby = zeros<rowvec>(DNAScols);
	vec orderedV(DNASrows);

	mat orderedDNAS(DNASrows,DNAScols);
	mat newDNAS(DNASrows,DNAScols);
  // without this activation, higher fitness the better.
 fitness.save("fitness.save",raw_ascii);
	fitness=-fitness - reg*sum(DNAS%DNAS,1);
 vec regVec = reg*sum(DNAS%DNAS,1);
 regVec.save("regVec.save",raw_ascii);

	uvec q = sort_index(fitness,"descend");

  for(int j=0;j < DNASrows; j++){
     orderedDNAS.row(j)= DNAS.row(q(j));
	   orderedV(j) = fitness(q(j));
	}

 fitness.save("fitness2.save",raw_ascii);
 q.save("q.save",raw_ascii);
 vec y = exp(alpha*orderedV)/sum(exp(alpha*orderedV));
 y.save("y.save",raw_ascii);
 vec z = cumsum(y);
 z.save("z.save",raw_ascii);

 y.save("distri.dat",raw_ascii);
 z.save("cumsum.dat",raw_ascii);

  for(int j=0;j < 2*DNASrows; j++){
    myindex = index_min(abs(z - (double)(rand() + 1)/(RAND_MAX))  );
    decision[j] = myindex;
  }

    //BEGIN MATING RITUAL
	  for(int i=0; i < 2*DNAS.n_rows-1; i = i + 2){
		//cout << "THE I" << (i)/2 << endl;
       baby =  orderedDNAS.row(decision[i]);
	   //MIXING
	  for(int j=0; j <DNAScols; j++){
       if((double)(rand() + 1)/(RAND_MAX) < 0.5) baby(j) = orderedDNAS.row(decision[i+1])(j);
	  }
	  //SMALL MUTATION
	  for(int j=0; j <DNAScols; j++){
       randBuff = (double)(rand() + 1)/(RAND_MAX);
       if((double)(rand() + 1)/(RAND_MAX) < mutRat) baby(j) = baby(j) + baby(j) * mutAmmount*(2*randBuff-1);
	  }
	  //LARGE MUTATION
	  for(int j=0; j <DNAScols; j++){
       randBuff = (double)(rand() + 1)/(RAND_MAX);
       if((double)(rand() + 1)/(RAND_MAX) < bigMutRat) baby(j) = baby(j) + baby(j) * bigMutAmmount*(2*randBuff-1);
	  }
	  for(int j=0; j <DNAScols; j++){
       randBuff = (double)(rand() + 1)/(RAND_MAX);
       if((double)(rand() + 1)/(RAND_MAX) < veryBigMutRat) baby(j) = baby(j) + baby(j) * verBigMutAmmount*(2*randBuff-1);
	  }

	newDNAS.row(i/2) = baby;
	}

	return newDNAS;
}
//------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------


/*
* Copyright 2019 © Centre Interdisciplinaire de développement en Cartographie des Océans (CIDCO), Tous droits réservés
*/

 /*
 * \author Christian Bouchard
 */

#ifndef HULLOVERLAP_HPP
#define HULLOVERLAP_HPP

#include <iostream>
#include <cstdint>

#include <vector>

#include <utility>      // std::pair, std::make_pair

#include <pcl/common/common_headers.h>

#include <pcl/point_types.h>
#include <pcl/surface/concave_hull.h>

#include <pcl/PointIndices.h>

#include <pcl/ModelCoefficients.h>
#include <pcl/filters/project_inliers.h>

#include <pcl/segmentation/extract_polygonal_prism_data.h>

#include <Eigen/Dense>
#include <Eigen/Geometry> // For cross product

class HullOverlap
{

public:

	/**
	* Creates a HullOverlap
	*
	* @param line1In Point cloud for line #1
	* @param line2In Point cloud for line #2
    * @param a projection plane coefficient 'a' in ax + by + cz + d = 0
    * @param b projection plane coefficient 'b' in ax + by + cz + d = 0
    * @param c projection plane coefficient 'c' in ax + by + cz + d = 0
    * @param d projection plane coefficient 'd' in ax + by + cz + d = 0
    * @param alpha1 Concave hull computation parameter to use with line #1
    * @param alpha2 Concave hull computation parameter to use with line #2
	*/
    HullOverlap( pcl::PointCloud<pcl::PointXYZ>::ConstPtr line1In,
                    pcl::PointCloud<pcl::PointXYZ>::ConstPtr line2In,
                    double a, double b, double c, double d,
                    double alphaLine1 = 1.0, double alphaLine2 = 1.0 )
                    :   line1( line1In ), line2( line2In ),
                        a( a ), b( b ), c( c ), d( d ),
                        alphaLine1( alphaLine1 ), alphaLine2( alphaLine2 ),

                        coefficients ( new pcl::ModelCoefficients() ),

                        line1InPlane (new pcl::PointCloud<pcl::PointXYZ>),
                        line2InPlane (new pcl::PointCloud<pcl::PointXYZ>),

                        line1InPlane2D (new pcl::PointCloud<pcl::PointXYZ>),
                        line2InPlane2D (new pcl::PointCloud<pcl::PointXYZ>),

                        hull1Vertices (new pcl::PointCloud<pcl::PointXYZ>),
                        hull2Vertices (new pcl::PointCloud<pcl::PointXYZ>),

                       // Initialize to dummy values
                       vector1( 1, 0, 0 ), vector2( 0, 1, 0 ), refPoint( 0.0, 0.0, 0.0 )

    {
        coefficients->values.resize(4);
        coefficients->values[0] = a;
        coefficients->values[1] = b;
        coefficients->values[2] = c;
        coefficients->values[3] = d;
    }

	/**
	* Returns a pair with the number of points in line #1 and in line #2 that are in the overlap area of the two lines.
	* The points are place in the point clouds pointed to by line1InBothHull and line2InBothHull
    *
	* @param[out] line1InBothHull Point cloud of points in line #1 in the overlap area of the two lines
	* @param[out] line2InBothHull Point cloud of points in line #2 in the overlap area of the two lines
	*/
    std::pair< uint64_t, uint64_t > computePointsInBothHulls( pcl::PointCloud<pcl::PointXYZ>::Ptr line1InBothHull,
                                                              pcl::PointCloud<pcl::PointXYZ>::Ptr line2InBothHull )
    {
        return computeHullsAndPointsInBothHulls( line1InBothHull, line2InBothHull, true );
    }

private:

	/**
	* Returns a pair with the number of points in line #1 and in line #2 that are in the overlap area of the two lines,
	* the points are place in the point clouds pointed to by line1InBothHull and line2InBothHull
    *
	* @param[out] line1InBothHull Point cloud of points in line #1 in the overlap area of the two lines
	* @param[out] line2InBothHull Point cloud of points in line #2 in the overlap area of the two lines
    * @param[in] line2InBothHull minimalMemory bool variable, true to specify to try and minimize the memory usage
	*/
    // Was public when wanted to look at details of projection, etc
    std::pair< uint64_t, uint64_t > computeHullsAndPointsInBothHulls( pcl::PointCloud<pcl::PointXYZ>::Ptr line1InBothHull = nullptr,
                                                                        pcl::PointCloud<pcl::PointXYZ>::Ptr line2InBothHull = nullptr,
                                                                        const bool minimalMemory = false )
    {

        if ( line1InBothHull != nullptr )
            line1InBothHull->clear();

        if ( line2InBothHull != nullptr )
            line2InBothHull->clear();

        std::cout << "\nProjecting line 1 in plane\n" << std::endl;

        // Project line 1 in plane
        createCloudFromProjectionInPlane( line1, line1InPlane );

        std::cout << "line1InPlane->points.size(): " << line1InPlane->points.size() << "\n" << std::endl;

        computeTwoVectorsAndRefPoint();

        std::cout << "\nExpressing points of line 1 in the projection plane using a 2D coordinate system\n" << std::endl;

        createCloudInPlane2D( line1InPlane, line1InPlane2D );

        if ( minimalMemory )
        {
            // Delete the dynamically allocated memory
            line1InPlane.reset();
            line1InPlane = nullptr;
        }

        std::cout << "line1InPlane2D->points.size(): " << line1InPlane2D->points.size() << "\n" << std::endl;

        std::cout << "\nProjecting line 2 in plane\n" << std::endl;

        // Project line 2 in plane
        createCloudFromProjectionInPlane( line2, line2InPlane );

        std::cout << "line2InPlane->points.size(): " << line2InPlane->points.size() << "\n" << std::endl;

        std::cout << "\nExpressing points of line 2 in the projection plane using a 2D coordinate system\n" << std::endl;

        createCloudInPlane2D( line2InPlane, line2InPlane2D );

        if ( minimalMemory )
        {
            // Delete the dynamically allocated memory
            line2InPlane.reset();
            line2InPlane = nullptr;
        }

        std::cout << "line2InPlane2D->points.size(): " << line2InPlane2D->points.size() << "\n" << std::endl;

        //http://www.pointclouds.org/documentation/tutorials/hull_2d.php

        std::cout << "\nFinding Hull 1\n" << std::endl;

        // Create a Concave Hull for line 1
        computeVerticesOfConcaveHull( line1InPlane2D, alphaLine1, hull1Vertices, hull1PointIndices, ! minimalMemory );

        std::cout << "Finding Hull 2\n" << std::endl;

        // Create a Concave Hull for line 2
        computeVerticesOfConcaveHull( line2InPlane2D, alphaLine2, hull2Vertices, hull2PointIndices, ! minimalMemory );

        std::cout << "hull1Vertices->points.size(): " << hull1Vertices->points.size() << "\n"
            << "hull2Vertices->points.size(): " << hull2Vertices->points.size() << "\n" << std::endl;

        // If the hulls of the lines where found correctly, points of line 1 are within the hull of line 1.
        // So only need to check that a point of line 1 is part of hull 2 to know that it is part of both hulls.
        // Same idea for points of line 2.

        if ( line1InBothHull != nullptr && line2InBothHull != nullptr )
        {
            if ( minimalMemory )
            {
                std::cout << "Finding points of Line 1 inside Hull 2\n\n" << std::endl;

                findPointsInHullOnlyPoints( line1, line1InPlane2D, line1InBothHull, hull2Vertices );

                // Delete the dynamically allocated memory
                line1InPlane2D.reset();
                line1InPlane2D = nullptr;

                hull2Vertices.reset();
                hull2Vertices = nullptr;

                std::cout << "Finding points of Line 2 inside Hull 1\n\n" << std::endl;

                findPointsInHullOnlyPoints( line2, line2InPlane2D, line2InBothHull, hull1Vertices );

                // Delete the dynamically allocated memory
                line2InPlane2D.reset();
                line2InPlane2D = nullptr;

                hull1Vertices.reset();
                hull1Vertices = nullptr;

                std::cout << "line1InBothHull->points.size(): " << line1InBothHull->points.size() << "\n"
                    << "line2InBothHull->points.size(): " << line2InBothHull->points.size() << "\n" << std::endl;

                return std::make_pair( line1InBothHull->size(), line2InBothHull->size() );

            }
            else
            {

                std::cout << "Finding points of Line 1 inside Hull 2 (and the indices)\n\n" << std::endl;

                findPointsInHull( line1, line1InPlane2D, line1InBothHull, line1InBothHullPointIndices, hull2Vertices );

                std::cout << "Finding points of Line 2 inside Hull 1 (and the indices)\n\n" << std::endl;

                findPointsInHull( line2, line2InPlane2D, line2InBothHull, line2InBothHullPointIndices, hull1Vertices );

                std::cout << "line1InBothHull->points.size(): " << line1InBothHull->points.size() << "\n"
                    << "line2InBothHull->points.size(): " << line2InBothHull->points.size() << "\n" << std::endl;

                return std::make_pair( line1InBothHull->size(), line2InBothHull->size() );

            }

        }
        else
        {
            std::cout << "Finding indices of points of Line 1 inside Hull 2\n\n" << std::endl;

            findPointsInHullOnlyPointIndices( line1InPlane2D, line1InBothHullPointIndices, hull2Vertices );

            std::cout << "Finding indices of points of Line 2 inside Hull 1\n\n" << std::endl;

            findPointsInHullOnlyPointIndices( line2InPlane2D, line2InBothHullPointIndices, hull1Vertices );

            std::cout << "line1InBothHullPointIndices.size(): " << line1InBothHullPointIndices.size() << "\n"
                << "line2InBothHullPointIndices.size(): " << line2InBothHullPointIndices.size() << "\n" << std::endl;

            return std::make_pair( line1InBothHullPointIndices.size(), line2InBothHullPointIndices.size() );
        }

    }

    // These functions were public when wanted to look at details of projection, etc
    // pcl::PointCloud<pcl::PointXYZ>::ConstPtr getConstPtrLineInPlane( const bool isLine1 )
    // {
    //     if ( isLine1 )
    //         return line1InPlane;
    //     else
    //         return line2InPlane;
    // }

    // const std::vector< int > * getConstPtrVerticesIndices( const bool isLine1 )
    // {
    //     if ( isLine1 )
    //         return & ( hull1PointIndices.indices );
    //     else
    //         return & ( hull2PointIndices.indices );
    // }

    // const std::vector< uint64_t > * getConstPtrlineInBothHullPointIndices( const bool isLine1 )
    // {
    //     if ( isLine1 )
    //         return & ( line1InBothHullPointIndices );
    //     else
    //         return & ( line2InBothHullPointIndices );
    // }

	/**
	* Computes the projection of a point cloud onto a plane
    *
    * @param[in] cloudIn Point cloud to project on the plane
    * @param[out] cloudOut Point cloud resulting from the projection
	*/
    void createCloudFromProjectionInPlane( pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloudIn,
                                                pcl::PointCloud<pcl::PointXYZ>::Ptr cloudOut )
    {
        cloudOut->clear();
        cloudOut->reserve( cloudIn->points.size() );

        // Create the filtering object
        pcl::ProjectInliers<pcl::PointXYZ> proj;
        proj.setModelType( pcl::SACMODEL_PLANE );

        proj.setInputCloud( cloudIn );
        proj.setModelCoefficients( coefficients );

        proj.filter( *cloudOut );
    }

	/**
	* Computes two vectors and sets a reference point used to express point positions on the
    * projection plane using only two dimensions
	*/
    void computeTwoVectorsAndRefPoint()
    {

        // Two vectors and a reference point to span the projection plane
        // so that points in the projection plane can be expressed in
        // a coordinate system with vector1, vector2, and refPoint.

        refPoint = line1InPlane->points[ 0 ];

        // Vector #1: from first point in line to last point in line, normalized

        const uint64_t nbPointLine1 = line1InPlane->points.size();

        vector1 << line1InPlane->points[ nbPointLine1 - 1 ].x - line1InPlane->points[ 0 ].x,
                    line1InPlane->points[ nbPointLine1 - 1 ].y - line1InPlane->points[ 0 ].y,
                    line1InPlane->points[ nbPointLine1 - 1 ].z - line1InPlane->points[ 0 ].z;

        std::cout << "vector1 before normalization:\n" << vector1 << "\n\n";

        vector1 = vector1 / vector1.norm();

        std::cout << "vector1 after normalization:\n" << vector1 << "\n\n";

        Eigen::Vector3d normalToPlane;

        normalToPlane <<  a, b, c;

        // Vector #2: perpendicular to the normal to the plane and to vector #1
        vector2 = normalToPlane.cross( vector1 );

        std::cout << "vector2 before normalization:\n" << vector2 << "\n\n";

        vector2 = vector2 / vector2.norm();

        std::cout << "vector2 after normalization:\n" << vector2 << "\n\n";

        // Sanity check
        std::cout << "vector1 dot vector2: " << vector1.dot( vector2 ) << "    (should be 0)\n\n";
    }

	/**
	* Computes a 2D representation of points on the projection plane
    *
    * @param[in] cloudIn Point cloud on the projection plane expressed in 3D
    * @param[out] cloudOut Point cloud on the projection plane expressed in 2D
	*/
    void createCloudInPlane2D( pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloudIn,
                            pcl::PointCloud<pcl::PointXYZ>::Ptr cloudOut )
    {
        // Build a point cloud where points in the projection plane are expressed in
        // the coordinate system with vector1, vector2, and refPoint.

        cloudOut->clear();
        cloudOut->reserve( cloudIn->points.size() );

        for ( uint64_t count = 0; count < cloudIn->points.size(); count++ )
        {
            pcl::PointXYZ point;

            // projection along vector 1
            point.x = ( cloudIn->points[ count ].x - refPoint.x ) * vector1( 0 )
                        + ( cloudIn->points[ count ].y - refPoint.y ) * vector1( 1 )
                        + ( cloudIn->points[ count ].z - refPoint.z ) * vector1( 2 );

            // projection along vector 2
            point.y = ( cloudIn->points[ count ].x - refPoint.x ) * vector2( 0 )
                        + ( cloudIn->points[ count ].y - refPoint.y ) * vector2( 1 )
                        + ( cloudIn->points[ count ].z - refPoint.z ) * vector2( 2 );

            point.z = 0;

            cloudOut->push_back( point );

        }
    }

	/**
	* Computes the vertices of a concave hull for points on the projection plane
    *
    * @param[in] cloudIn Point cloud on the projection plane expressed in 2D
    * @param[in] alpha Concave hull computation parameter to use
    * @param[out] hullVertices Computed vertices of the concave hull
    * @param[out] hullPointIndices Indices of the points in cloudIn making up the hull
    * @param[in] keepInformation bool variable, true to specify to put the indices of the points in cloudIn in hullPointIndices
	*/
    void computeVerticesOfConcaveHull( pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloudIn,
                                        const double alpha,
                                        pcl::PointCloud<pcl::PointXYZ>::Ptr hullVertices,
                                        pcl::PointIndices & hullPointIndices, const bool keepInformation = true )
    {

        hullVertices->clear();

        pcl::ConcaveHull<pcl::PointXYZ> concaveHull;

        if ( keepInformation )
            concaveHull.setKeepInformation( true ); // To be able to use function getHullPointIndices()

        concaveHull.setInputCloud( cloudIn );
        concaveHull.setAlpha( alpha );
        concaveHull.reconstruct( * hullVertices );

        if ( keepInformation )
            // Get indices of points making the hull
            concaveHull.getHullPointIndices( hullPointIndices );
    }

	/**
	* Find points that are within a concave hull.
    * Provides the points and their indices within the original line
    *
    * @param[in] lineOriginal Point cloud of points on the line
    * @param[in] cloudIn Point cloud on the projection plane expressed in 2D
    * @param[out] cloudOut Point cloud of points on the line that are within the hull
    * @param[out] indexPointInHull Indices of the points on the line that are within the hull
    * @param[in] hullVertices Vertices of the concave hull
	*/
    void findPointsInHull( pcl::PointCloud<pcl::PointXYZ>::ConstPtr lineOriginal,
                                pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloudIn,
                                pcl::PointCloud<pcl::PointXYZ>::Ptr cloudOut,
                                std::vector< uint64_t > & indexPointInHull,
                                pcl::PointCloud<pcl::PointXYZ>::ConstPtr hullVertices )
    {
        cloudOut->clear();
        indexPointInHull.clear();

        for ( uint64_t count = 0; count < cloudIn->points.size(); count++ )
        {
            if ( pcl::isXYPointIn2DXYPolygon( cloudIn->points[ count ], *hullVertices ) )
            {
                cloudOut->push_back( lineOriginal->points[ count ] );
                indexPointInHull.push_back( count );
            }
        }

    }

	/**
	* Find indices of points that are within a concave hull.
    *
    * @param[in] cloudIn Point cloud on the projection plane expressed in 2D
    * @param[out] indexPointInHull Indices of the points on the line that are within the hull
    * @param[in] hullVertices Vertices of the concave hull
	*/
    void findPointsInHullOnlyPointIndices( pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloudIn,
                                    std::vector< uint64_t > & indexPointInHull,
                                    pcl::PointCloud<pcl::PointXYZ>::ConstPtr hullVertices )
    {
        indexPointInHull.clear();

        for ( uint64_t count = 0; count < cloudIn->points.size(); count++ )
        {
            if ( pcl::isXYPointIn2DXYPolygon( cloudIn->points[ count ], *hullVertices ) )
                indexPointInHull.push_back( count );
        }

    }

	/**
	* Find points that are within a concave hull.
    *
    * @param[in] lineOriginal Point cloud of points on the line
    * @param[in] cloudIn Point cloud on the projection plane expressed in 2D
    * @param[out] cloudOut Point cloud of points on the line that are within the hull
    * @param[in] hullVertices Vertices of the concave hull
	*/
    void findPointsInHullOnlyPoints( pcl::PointCloud<pcl::PointXYZ>::ConstPtr lineOriginal,
                                    pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloudIn,
                                    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudOut,
                                    pcl::PointCloud<pcl::PointXYZ>::ConstPtr hullVertices )
    {
        cloudOut->clear();

        for ( uint64_t count = 0; count < cloudIn->points.size(); count++ )
        {
            if ( pcl::isXYPointIn2DXYPolygon( cloudIn->points[ count ], *hullVertices ) )
                cloudOut->push_back( lineOriginal->points[ count ] );
        }

    }

// ----------------------------- Variables ------------------------------------------------

    /**Point cloud for line #1*/
    const pcl::PointCloud<pcl::PointXYZ>::ConstPtr line1;

    /**Point cloud for line #2*/
    const pcl::PointCloud<pcl::PointXYZ>::ConstPtr line2;

    /**Projection plane coefficient 'a' in ax + by + cz + d = 0*/
    const double a;

    /**Projection plane coefficient 'b' in ax + by + cz + d = 0*/
    const double b;

    /**Projection plane coefficient 'c' in ax + by + cz + d = 0*/
    const double c;

    /**Projection plane coefficient 'd' in ax + by + cz + d = 0*/
    const double d;

    /**Concave hull computation parameter to use with line #1*/
    double alphaLine1; // Alpha value to compute the concave hull for line #1

    /**Concave hull computation parameter to use with line #2*/
    double alphaLine2; // Alpha value to compute the concave hull for line #2

    /**Coefficients for the plane, ax + by + cz + d = 0 */
    pcl::ModelCoefficients::Ptr coefficients;

    /**Point cloud of the projection of line #1 on the plane, expressed in 3D*/
    pcl::PointCloud<pcl::PointXYZ>::Ptr line1InPlane;

    /**Point cloud of the projection of line #2 on the plane, expressed in 3D*/
    pcl::PointCloud<pcl::PointXYZ>::Ptr line2InPlane;

    /**Point cloud of the projection of line #1 on the plane, expressed in 2D*/
    pcl::PointCloud<pcl::PointXYZ>::Ptr line1InPlane2D;

    /**Point cloud of the projection of line #2 on the plane, expressed in 2D*/
    pcl::PointCloud<pcl::PointXYZ>::Ptr line2InPlane2D;

    /**Vertices of the concave hull for line #1*/
    pcl::PointCloud<pcl::PointXYZ>::Ptr hull1Vertices;

    /**Vertices of the concave hull for line #2*/
    pcl::PointCloud<pcl::PointXYZ>::Ptr hull2Vertices;

    /**Indices of the points in line #1 whose projection on the plane makes up its hull*/
    pcl::PointIndices hull1PointIndices;

    /**Indices of the points in line #2 whose projection on the plane makes up its hull*/
    pcl::PointIndices hull2PointIndices;

    /**Indices of the points in line #1 that are within both hulls*/
    std::vector< uint64_t > line1InBothHullPointIndices;

    /**Indices of the points in line #2 that are within both hulls*/
    std::vector< uint64_t > line2InBothHullPointIndices;

    /**First computed orthonormal vector used to express points on the projection plane in 2D*/
    Eigen::Vector3d vector1;

    /**Second computed orthonormal vector used to express points on the projection plane in 2D*/
    Eigen::Vector3d vector2;

    /**Referenced point used to express points on the projection plane in 2D*/
    pcl::PointXYZ refPoint;

};

#endif


#include <Eigen/Core>
#include <Eigen/Geometry>

namespace dr {

template<typename Scalar, int AmbientDim, int MatrixOptions, int PlaneOptions>
Eigen::Matrix<Scalar, AmbientDim, 1, MatrixOptions> rejection(
	Eigen::Matrix<Scalar, AmbientDim, 1, MatrixOptions> const & vector,
	Eigen::Hyperplane<Scalar, AmbientDim, PlaneOptions> const & plane
) {
	return vector - plane.projection(vector);
}

template<typename Scalar, int AmbientDim, int MatrixOptions, int PlaneOptions>
Eigen::Matrix<Scalar, AmbientDim, 1, MatrixOptions> reflection(
	Eigen::Matrix<Scalar, AmbientDim, 1, MatrixOptions> const & vector,
	Eigen::Hyperplane<Scalar, AmbientDim, PlaneOptions> const & plane
) {
	return vector - 2 * rejection(vector, plane);
}

}


#pragma once

#include "coma/Core"
#include <Eigen/Core>
#include <RBDyn/MultiBody.h>

template <int Order>
Eigen::MatrixXd makeDiag(const Eigen::MatrixXd& mat)
{
    constexpr int ord = Order;
    Eigen::MatrixXd out = Eigen::MatrixXd::Zero(ord * mat.rows(), ord * mat.cols());
    for (int i = 0; i < ord; ++i)
        out.block(i * mat.rows(), i * mat.cols(), mat.rows(), mat.cols()) = mat;

    return out;
}

template <typename CrossNOp>
Eigen::MatrixXd generateD(const CrossNOp& cx)
{
    constexpr int n_vec = coma::internal::traits<CrossNOp>::n_vec;
    using Scalar = typename coma::internal::traits<CrossNOp>::Scalar;
    using mat_t = Eigen::Matrix<Scalar, 6 * n_vec, 6 * n_vec>;
    using sub_mat_t = Eigen::Matrix<Scalar, 6, 6 * n_vec>;
    Eigen::MatrixXd D_N = Eigen::MatrixXd::Zero(6 * (n_vec - 1), 6 * (n_vec - 1));
    for (int i = 0; i < n_vec - 1; ++i)
        D_N.block<6, 6>(6 * i, 6 * i) = Eigen::Matrix6d::Identity() / (i + 1);
    return mat_t::Identity() + (mat_t() << sub_mat_t::Zero(), D_N * cx.dualMatrix().template topRows<6 * (n_vec - 1)>()).finished();
}

// M = I + Cd * I * C
template <typename Tree>
std::vector<Eigen::MatrixXd> getSubTreeInertia(const rbd::MultiBody& mb, const Tree& tree)
{
    constexpr int ord = Tree::order;
    std::vector<Eigen::MatrixXd> M(mb.nrBodies(), Eigen::MatrixXd::Zero(6 * ord, 6 * ord));
    const auto& bodies = mb.bodies();
    const auto& pred = mb.predecessors();
    for (int i = mb.nrBodies() - 1; i >= 0; --i) {
        M[i] += makeDiag<ord>(bodies[i].inertia().matrix());
        int p = pred[i]; // parent
        if (p != -1) {
            auto C_p_b = tree.links[p].inverse() * tree.links[i];
            M[p] += C_p_b.template dualMatrix<ord>() * M[i] * C_p_b.inverse().template matrix<ord>();
        }
    }

    return M;
}

namespace detail {

template <typename Transf, typename CMTM, size_t Order>
struct CMTMSetter {
    static void impl(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints);
};

template <typename Transf, typename CMTM>
struct CMTMSetter<Transf, CMTM, 0> {
    static void impl(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints)
    {
        joints.set(T);
    }
};

template <typename Transf, typename CMTM>
struct CMTMSetter<Transf, CMTM, 1> {
    static void impl(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints)
    {
        joints.set(T, S * dqs[0].col(t).segment(pos, dof));
    }
};

template <typename Transf, typename CMTM>
struct CMTMSetter<Transf, CMTM, 2> {
    static void impl(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints)
    {
        joints.set(T, S * dqs[0].col(t).segment(pos, dof), S * dqs[1].col(t).segment(pos, dof));
    }
};

template <typename Transf, typename CMTM>
struct CMTMSetter<Transf, CMTM, 3> {
    static void impl(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints)
    {
        joints.set(T, S * dqs[0].col(t).segment(pos, dof), S * dqs[1].col(t).segment(pos, dof), S * dqs[2].col(t).segment(pos, dof));
    }
};

template <typename Transf, typename CMTM>
struct CMTMSetter<Transf, CMTM, 4> {
    static void impl(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints)
    {
        joints.set(T, S * dqs[0].col(t).segment(pos, dof), S * dqs[1].col(t).segment(pos, dof), S * dqs[2].col(t).segment(pos, dof),
            S * dqs[3].col(t).segment(pos, dof));
    }
};

template <typename Transf, typename CMTM>
struct CMTMSetter<Transf, CMTM, 5> {
    static void impl(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints)
    {
        joints.set(T, S * dqs[0].col(t).segment(pos, dof), S * dqs[1].col(t).segment(pos, dof), S * dqs[2].col(t).segment(pos, dof),
            S * dqs[3].col(t).segment(pos, dof), S * dqs[4].col(t).segment(pos, dof));
    }
};

template <typename Transf, typename CMTM>
struct CMTMSetter<Transf, CMTM, 6> {
    static void impl(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints)
    {
        joints.set(T, S * dqs[0].col(t).segment(pos, dof), S * dqs[1].col(t).segment(pos, dof), S * dqs[2].col(t).segment(pos, dof),
            S * dqs[3].col(t).segment(pos, dof), S * dqs[4].col(t).segment(pos, dof), S * dqs[5].col(t).segment(pos, dof));
    }
};

template <typename Transf, typename CMTM>
struct CMTMSetter<Transf, CMTM, 7> {
    static void impl(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints)
    {
        joints.set(T, S * dqs[0].col(t).segment(pos, dof), S * dqs[1].col(t).segment(pos, dof), S * dqs[2].col(t).segment(pos, dof),
            S * dqs[3].col(t).segment(pos, dof), S * dqs[4].col(t).segment(pos, dof), S * dqs[5].col(t).segment(pos, dof),
            S * dqs[6].col(t).segment(pos, dof));
    }
};

template <typename Transf, typename CMTM>
struct CMTMSetter<Transf, CMTM, 8> {
    static void impl(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints)
    {
        joints.set(T, S * dqs[0].col(t).segment(pos, dof), S * dqs[1].col(t).segment(pos, dof), S * dqs[2].col(t).segment(pos, dof),
            S * dqs[3].col(t).segment(pos, dof), S * dqs[4].col(t).segment(pos, dof), S * dqs[5].col(t).segment(pos, dof),
            S * dqs[6].col(t).segment(pos, dof), S * dqs[7].col(t).segment(pos, dof));
    }
};

template <typename Transf, typename CMTM>
struct CMTMSetter<Transf, CMTM, 9> {
    static void impl(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints)
    {
        joints.set(T, S * dqs[0].col(t).segment(pos, dof), S * dqs[1].col(t).segment(pos, dof), S * dqs[2].col(t).segment(pos, dof),
            S * dqs[3].col(t).segment(pos, dof), S * dqs[4].col(t).segment(pos, dof), S * dqs[5].col(t).segment(pos, dof),
            S * dqs[6].col(t).segment(pos, dof), S * dqs[7].col(t).segment(pos, dof), S * dqs[8].col(t).segment(pos, dof));
    }
};

template <typename Transf, typename CMTM>
struct CMTMSetter<Transf, CMTM, 10> {
    static void impl(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints)
    {
        joints.set(T, S * dqs[0].col(t).segment(pos, dof), S * dqs[1].col(t).segment(pos, dof), S * dqs[2].col(t).segment(pos, dof),
            S * dqs[3].col(t).segment(pos, dof), S * dqs[4].col(t).segment(pos, dof), S * dqs[5].col(t).segment(pos, dof),
            S * dqs[6].col(t).segment(pos, dof), S * dqs[7].col(t).segment(pos, dof), S * dqs[8].col(t).segment(pos, dof),
            S * dqs[9].col(t).segment(pos, dof));
    }
};

template <typename Transf, typename CMTM>
struct CMTMSetter<Transf, CMTM, 11> {
    static void impl(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints)
    {
        joints.set(T, S * dqs[0].col(t).segment(pos, dof), S * dqs[1].col(t).segment(pos, dof), S * dqs[2].col(t).segment(pos, dof),
            S * dqs[3].col(t).segment(pos, dof), S * dqs[4].col(t).segment(pos, dof), S * dqs[5].col(t).segment(pos, dof),
            S * dqs[6].col(t).segment(pos, dof), S * dqs[7].col(t).segment(pos, dof), S * dqs[8].col(t).segment(pos, dof),
            S * dqs[9].col(t).segment(pos, dof), S * dqs[10].col(t).segment(pos, dof));
    }
};

template <typename Transf, typename CMTM>
struct CMTMSetter<Transf, CMTM, 12> {
    static void impl(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints)
    {
        joints.set(T, S * dqs[0].col(t).segment(pos, dof), S * dqs[1].col(t).segment(pos, dof), S * dqs[2].col(t).segment(pos, dof),
            S * dqs[3].col(t).segment(pos, dof), S * dqs[4].col(t).segment(pos, dof), S * dqs[5].col(t).segment(pos, dof),
            S * dqs[6].col(t).segment(pos, dof), S * dqs[7].col(t).segment(pos, dof), S * dqs[8].col(t).segment(pos, dof),
            S * dqs[9].col(t).segment(pos, dof), S * dqs[10].col(t).segment(pos, dof), S * dqs[11].col(t).segment(pos, dof));
    }
};

}

template<typename Transf, typename CMTM, size_t Order>
void CMTMSet(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints)
{
    detail::CMTMSetter<Transf, CMTM, Order>::impl(T, S, dqs, t, pos, dof, joints);
}


#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <armadillo>

namespace py = pybind11;

using namespace arma;

typedef py::array_t<double, py::array::f_style | py::array::forcecast> array_tf;
typedef py::array_t<double, py::array::c_style | py::array::forcecast> array_tc;

cube array_to_cube(array_tf m) {

    py::buffer_info _m_buff = m.request();
    int n_rows = _m_buff.shape[0];
    int n_cols = _m_buff.shape[1];
    int n_slices = _m_buff.shape[2];

    cube _m_arma((double *)_m_buff.ptr, n_rows, n_cols, n_slices);

    return _m_arma;
}

mat array_to_mat(array_tf m) {

    py::buffer_info _m_buff = m.request();
    int n_rows = _m_buff.shape[0];
    int n_cols = _m_buff.shape[1];

    mat _m_arma((double *)_m_buff.ptr, n_rows, n_cols);

    return _m_arma;
}

vec array_to_vec(array_tf m) {

    py::buffer_info _m_buff = m.request();
    int n_rows = _m_buff.shape[0];

    vec _m_vec((double *)_m_buff.ptr, n_rows);

    return _m_vec;
}

array_tf cube_to_array(cube m) {

    auto _m_array = array_tf({m.n_rows, m.n_cols, m.n_slices});

    py::buffer_info _m_buff = _m_array.request();
    std::memcpy(_m_buff.ptr, m.memptr(), sizeof(double) * m.n_rows * m.n_cols * m.n_slices);

    return _m_array;
}

array_tf mat_to_array(mat m) {

    auto _m_array = array_tf({m.n_rows, m.n_cols});

    py::buffer_info _m_buff = _m_array.request();
    std::memcpy(_m_buff.ptr, m.memptr(), sizeof(double) * m.n_rows * m.n_cols);

    return _m_array;
}

array_tf vec_to_array(vec m) {

    auto _m_array = array_tf({m.n_rows});

    py::buffer_info _m_buff = _m_array.request();
    std::memcpy(_m_buff.ptr, m.memptr(), sizeof(double) * m.n_rows);

    return _m_array;
}

py::tuple kl_divergence(array_tf _p_K, array_tf _p_kff, array_tf _p_sigma_ctl,
                        array_tf _q_K, array_tf _q_kff, array_tf _q_sigma_ctl,
                        array_tf _mu_x, array_tf _sigma_x,
                        int dm_state, int dm_act, int nb_steps) {

    cube p_K = array_to_cube(_p_K);
    mat p_kff = array_to_mat(_p_kff);
    cube p_sigma_ctl = array_to_cube(_p_sigma_ctl);

    cube q_K = array_to_cube(_q_K);
    mat q_kff = array_to_mat(_q_kff);
    cube q_sigma_ctl = array_to_cube(_q_sigma_ctl);

    mat mu_x  = array_to_mat(_mu_x);
    cube sigma_x = array_to_cube(_sigma_x);

    vec kl(nb_steps);

    for(int i = 0; i < nb_steps; i++) {
        mat q_lambda_ctl = inv_sympd(q_sigma_ctl.slice(i));

        mat diff_K = (q_K.slice(i) - p_K.slice(i)).t() * q_lambda_ctl * (q_K.slice(i) - p_K.slice(i));
        mat diff_crs = (q_K.slice(i) - p_K.slice(i)).t() * q_lambda_ctl * (- q_kff.col(i) + p_kff.col(i));
        mat diff_kff = (- q_kff.col(i) + p_kff.col(i)).t() * q_lambda_ctl * (- q_kff.col(i) + p_kff.col(i));

        kl(i) = as_scalar(0.5 * log( det(q_sigma_ctl.slice(i)) / det(p_sigma_ctl.slice(i)) )
		                  + 0.5 * trace(q_lambda_ctl * p_sigma_ctl.slice(i))
		                  - 0.5 * dm_act
		                  + 0.5 * trace(diff_K * sigma_x.slice(i))
		                  + 0.5 * mu_x.col(i).t() * diff_K * mu_x.col(i)
		                  - mu_x.col(i).t() * diff_crs
		                  + 0.5 * diff_kff);
    }

    array_tf _kl = vec_to_array(kl);

    py::tuple output =  py::make_tuple(_kl);
    return output;
}

double quad_expectation(array_tf _mu, array_tf _sigma_s,
                        array_tf _Q, array_tf _q, double _q0) {

    vec mu  = array_to_vec(_mu);
    mat sigma_s = array_to_mat(_sigma_s);

    mat Q = array_to_mat(_Q);
    vec q = array_to_vec(_q);

	double result = as_scalar(mu.t() * Q * mu) + as_scalar(mu.t() * q) + _q0 + trace(Q * sigma_s);
	return result;
}

py::tuple augment_cost(array_tf _Cxx, array_tf _cx, array_tf _Cuu,
                       array_tf _cu, array_tf _Cxu, array_tf _c0,
                       array_tf _K, array_tf _kff, array_tf _sigma_ctl,
                       array_tf _alpha, int dm_state, int dm_act, int nb_steps) {

    // inputs
    cube Cxx = array_to_cube(_Cxx);
    mat cx = array_to_mat(_cx);
    cube Cuu = array_to_cube(_Cuu);
    mat cu = array_to_mat(_cu);
    cube Cxu = array_to_cube(_Cxu);
    vec c0 = array_to_vec(_c0);

    cube K = array_to_cube(_K);
    mat kff = array_to_mat(_kff);
    cube sigma_ctl = array_to_cube(_sigma_ctl);

    vec alpha = array_to_vec(_alpha);

    // outputs
    cube agCxx(dm_state, dm_state, nb_steps + 1);
    mat agcx(dm_state, nb_steps + 1);
    cube agCuu(dm_act, dm_act, nb_steps + 1);
    mat agcu(dm_act, nb_steps + 1);
    cube agCxu(dm_state, dm_act, nb_steps + 1);
    vec agc0(nb_steps + 1);

    for (int i = 0; i < nb_steps; i++) {
        mat lambda_ctl = inv_sympd(sigma_ctl.slice(i));

        agCxx.slice(i) = Cxx.slice(i) + 0.5 * alpha(i) * K.slice(i).t() * lambda_ctl * K.slice(i);
        agCuu.slice(i) = Cuu.slice(i) + 0.5 * alpha(i) * lambda_ctl;
        agCxu.slice(i) = Cxu.slice(i) - 0.5 * alpha(i) * K.slice(i).t() * lambda_ctl;
        agcx.col(i) = cx.col(i) + alpha(i) * K.slice(i).t() * lambda_ctl * kff.col(i);
        agcu.col(i) = cu.col(i) - alpha(i) * lambda_ctl * kff.col(i);
        agc0(i) = as_scalar(c0(i) + 0.5 * alpha(i) * log( det(2. * datum::pi * sigma_ctl.slice(i)) )
                            + 0.5 * alpha(i) * kff.col(i).t() * lambda_ctl * kff.col(i));
    }

    // last time step
    agCxx.slice(nb_steps) = Cxx.slice(nb_steps);
    agcx.col(nb_steps) = cx.col(nb_steps);
    agCuu.slice(nb_steps) = Cuu.slice(nb_steps);
    agcu.col(nb_steps) = cu.col(nb_steps);
    agCxu.slice(nb_steps) = Cxu.slice(nb_steps);
    agc0(nb_steps) = c0(nb_steps);

    // transform outputs to numpy
    array_tf _agCxx = cube_to_array(agCxx);
    array_tf _agcx = mat_to_array(agcx);
    array_tf _agCuu =  cube_to_array(agCuu);
    array_tf _agcu = mat_to_array(agcu);
    array_tf _agCxu =  cube_to_array(agCxu);
    array_tf _agc0 = vec_to_array(agc0);

    py::tuple output =  py::make_tuple(_agCxx, _agcx, _agCuu, _agcu, _agCxu, _agc0);
    return output;
}

py::tuple forward_pass(array_tf _mu_x0, array_tf _sigma_x0,
                       array_tf _A, array_tf _B, array_tf _c, array_tf _sigma_dyn,
                       array_tf _K, array_tf _kff, array_tf _sigma_ctl,
                       int dm_state, int dm_act, int nb_steps) {

    // inputs
    vec mu_x0 = array_to_vec(_mu_x0);
    mat sigma_x0 = array_to_mat(_sigma_x0);

    cube A = array_to_cube(_A);
    cube B = array_to_cube(_B);
    mat c = array_to_mat(_c);
    cube sigma_dyn = array_to_cube(_sigma_dyn);

    cube K = array_to_cube(_K);
    mat kff = array_to_mat(_kff);
    cube sigma_ctl = array_to_cube(_sigma_ctl);

    // outputs
    mat mu_x(dm_state, nb_steps + 1);
    cube sigma_x(dm_state, dm_state, nb_steps + 1);

    mat mu_u(dm_act, nb_steps);
    cube sigma_u(dm_act, dm_act, nb_steps);

    mat mu_xu(dm_state + dm_act, nb_steps + 1);
    cube sigma_xu(dm_state + dm_act, dm_state + dm_act, nb_steps + 1);

    mu_x.col(0) = mu_x0;
    sigma_x.slice(0) = sigma_x0;

    for (int i = 0; i < nb_steps; i++) {

        // mu_u = K * mu_x + k
        mu_u.col(i) = K.slice(i) * mu_x.col(i) + kff.col(i);

        // sigma_u = sigma_ctl + K * sigma_x * K_T
        sigma_u.slice(i) = sigma_ctl.slice(i) + K.slice(i) * sigma_x.slice(i) * K.slice(i).t();
        sigma_u.slice(i) = 0.5 * (sigma_u.slice(i) + sigma_u.slice(i).t());

        // sigma_xu =   [[sigma_x,      sigma_x * K_T],
        //               [K*sigma_x,    sigma_u    ]]
        sigma_xu.slice(i) = join_vert(join_horiz(sigma_x.slice(i), sigma_x.slice(i) * K.slice(i).t()),
                                        join_horiz(K.slice(i) * sigma_x.slice(i), sigma_u.slice(i)));
        sigma_xu.slice(i) = 0.5 * (sigma_xu.slice(i) + sigma_xu.slice(i).t());

        // mu_xu =  [[mu_x],
        //           [mu_u]],
        mu_xu.col(i) = join_vert(mu_x.col(i), mu_u.col(i));

        // sigma_x_next = sigma_dyn + [A B] * sigma_xu * [A B]^T
        sigma_x.slice(i+1) = sigma_dyn.slice(i) + join_horiz(A.slice(i), B.slice(i)) * sigma_xu.slice(i) *
                                                              join_vert(A.slice(i).t(), B.slice(i).t());
        sigma_x.slice(i+1) = 0.5 * (sigma_x.slice(i+1) + sigma_x.slice(i+1).t());

        // mu_x_next = [A B] * [s a]^T + c
        mu_x.col(i+1) = join_horiz(A.slice(i), B.slice(i)) * mu_xu.col(i) + c.col(i);

        if(i == nb_steps - 1) {
            mu_xu.col(i+1) = join_vert(mu_x.col(i+1), zeros<vec>(dm_act));
            sigma_xu.slice(i+1).submat(0, 0, dm_state - 1, dm_state - 1) = sigma_x.slice(i+1);
        }
    }

    // transform outputs to numpy
    array_tf _mu_x = mat_to_array(mu_x);
    array_tf _sigma_x = cube_to_array(sigma_x);
    array_tf _mu_u =  mat_to_array(mu_u);
    array_tf _sigma_u = cube_to_array(sigma_u);
    array_tf _mu_xu =  mat_to_array(mu_xu);
    array_tf _sigma_xu = cube_to_array(sigma_xu);

    py::tuple output =  py::make_tuple(_mu_x, _sigma_x, _mu_u, _sigma_u, _mu_xu, _sigma_xu);
    return output;
}

py::tuple backward_pass(array_tf _Cxx, array_tf _cx, array_tf _Cuu,
                        array_tf _cu, array_tf _Cxu, array_tf _c0,
                        array_tf _A, array_tf _B, array_tf _c, array_tf _sigma_dyn,
                        array_tf _alpha, int dm_state, int dm_act, int nb_steps) {

    // inputs
    cube Cxx = array_to_cube(_Cxx);
    mat cx = array_to_mat(_cx);
    cube Cuu = array_to_cube(_Cuu);
    mat cu = array_to_mat(_cu);
    cube Cxu = array_to_cube(_Cxu);
    vec c0 = array_to_vec(_c0);

    cube A = array_to_cube(_A);
    cube B = array_to_cube(_B);
    mat c = array_to_mat(_c);
    cube sigma_dyn = array_to_cube(_sigma_dyn);

    vec alpha = array_to_vec(_alpha);

    // outputs
    cube Q(dm_state + dm_act, dm_state + dm_act, nb_steps);
    cube Qxx(dm_state, dm_state, nb_steps);
    cube Qux(dm_act, dm_state, nb_steps);
    cube Quu(dm_act, dm_act, nb_steps);
    cube Quu_inv(dm_act, dm_act, nb_steps);
    mat qx(dm_state, nb_steps);
    mat qu(dm_act, nb_steps);
    vec q0(nb_steps);

    cube V(dm_state, dm_state, nb_steps + 1);
    mat v(dm_state, nb_steps + 1);
    vec v0(nb_steps + 1);

    cube K(dm_act, dm_state, nb_steps);
    mat kff(dm_act, nb_steps);
    cube sigma_ctl(dm_act, dm_act, nb_steps);
    cube lambda_ctl(dm_act, dm_act, nb_steps);

    int _diverge = 0;

    // last time step
    V.slice(nb_steps) = Cxx.slice(nb_steps);
    v.col(nb_steps) = cx.col(nb_steps);
    v0(nb_steps) = c0(nb_steps);

	for(int i = nb_steps - 1; i>= 0; --i)
	{
        Qxx.slice(i) = - (Cxx.slice(i) + A.slice(i).t() * V.slice(i+1) * A.slice(i)) / alpha(i);
        Quu.slice(i) = - (Cuu.slice(i) + B.slice(i).t() * V.slice(i+1) * B.slice(i)) / alpha(i);
        Qux.slice(i) = - (Cxu.slice(i) + A.slice(i).t() * V.slice(i+1) * B.slice(i)).t() / alpha(i);

        qu.col(i) = - (cu.col(i) + 2.0 * B.slice(i).t() * V.slice(i+1) * c.col(i) + B.slice(i).t() * v.col(i+1)) / alpha(i);
        qx.col(i) = - (cx.col(i) + 2.0 * A.slice(i).t() * V.slice(i+1) * c.col(i) + A.slice(i).t() * v.col(i+1)) / alpha(i);
        q0(i) = - as_scalar(c0(i) + v0(i+1) + c.col(i).t() * V.slice(i+1) * c.col(i)
                            + trace(V.slice(i+1) * sigma_dyn.slice(i)) + v.col(i+1).t() * c.col(i)) / alpha(i);

        if ((Quu.slice(i)).is_sympd()) {
            _diverge = i;
            break;
        }

        Quu_inv.slice(i) = inv(Quu.slice(i));
        K.slice(i) = - Quu_inv.slice(i) * Qux.slice(i);
        kff.col(i) = - 0.5 * Quu_inv.slice(i) * qu.col(i);

        sigma_ctl.slice(i) = - 0.5 * Quu_inv.slice(i);
        sigma_ctl.slice(i) = 0.5 * (sigma_ctl.slice(i).t() + sigma_ctl.slice(i));

        lambda_ctl.slice(i) = - (Quu.slice(i).t() + Quu.slice(i));
        lambda_ctl.slice(i) = 0.5 * (lambda_ctl.slice(i).t() + lambda_ctl.slice(i));

        V.slice(i) = - alpha(i) * (Qxx.slice(i) + Qux.slice(i).t() * K.slice(i));
        V.slice(i) = 0.5 * (V.slice(i) + V.slice(i).t());

        v.col(i) = - alpha(i) * (qx.col(i) + 2. * Qux.slice(i).t() * kff.col(i));
        v0(i) = - alpha(i) * (as_scalar(0.5 * qu.col(i).t() * kff.col(i)) + q0(i)
                              + 0.5 * (dm_act * log (2. * datum::pi) - log(det(- 2. * Quu.slice(i)))));
	}

    // transform outputs to numpy
    array_tf _Qxx = cube_to_array(Qxx);
    array_tf _Qux = cube_to_array(Qux);
    array_tf _Quu = cube_to_array(Quu);

    array_tf _qx = mat_to_array(qx);
    array_tf _qu = mat_to_array(qu);
    array_tf _q0 = mat_to_array(q0);

    array_tf _V = cube_to_array(V);
    array_tf _v = mat_to_array(v);
    array_tf _v0 = vec_to_array(v0);

    array_tf _K = cube_to_array(K);
    array_tf _kff = mat_to_array(kff);
    array_tf _sigma_ctl = cube_to_array(sigma_ctl);

    py::tuple output =  py::make_tuple(_Qxx, _Qux, _Quu, _qx, _qu, _q0,
                                        _V, _v, _v0,
                                        _K, _kff, _sigma_ctl, _diverge);

    return output;
}

PYBIND11_MODULE(core, m)
{
    m.def("kl_divergence", &kl_divergence);
    m.def("quad_expectation", &quad_expectation);
    m.def("augment_cost", &augment_cost);
    m.def("forward_pass", &forward_pass);
    m.def("backward_pass", &backward_pass);
}


/*
! Program for 2D explicit finite element analysis of incompressible Navier-Stokes
!
!
! Author: Dr. Chennakesava Kadapa
! Date  : 17-May-2018
! Place : Swansea, UK
!
!
*/

#include "headersBasic.h"
#include "headersEigen.h"
#include "elementutilitiescfd.h"
#include "SolutionData.h"
#include "BernsteinElem2DINSTria6Node.h"
#include "BernsteinElem2DINSQuad9Node.h"
#include <Eigen/SuperLUSupport>

using namespace std;

int main(int argc, char* argv[])
{
    double tstart, tend;

    int ndim=2, ndof=2, npElem=6;
    double fact, xNode[50], yNode[50];

    vector<vector<int> > ElemDofArray;

    int  nElem, nNode, nDBC, nFBC;
    int  ee, ii, jj, kk, ind, count, row, col;
    int  n1, n2, n3, n4, n5, n6, nsize;
    int  nn, dof;
    int  rr, cc;

    string  infileNodes, infileElems, infileDBCs, infileOutput;
    string  infileFBCs, charTemp, outFileName;

    //Set file names
    //The file names are specified as inputs from the command line
    if(argc == 0)
    {
        cerr << " Error in input data " << endl;
        cerr <<  "Number of input files is not sufficient " << endl;
        cerr <<  "You must enter names of THREE files" << endl;
        cerr <<  "a.) Node file, b.) Element file, and c.) Dirichlet BC file" << endl;
        cerr << "Aborting..." << endl;
    }
    else
    {
       infileNodes = argv[1];
       infileElems = argv[2];
       infileDBCs  = argv[3];

       //if(argc == 5)
         //infileFBCs  = argv[3];

       if(argc == 5)
         infileOutput = argv[4];
    }

    // Read nodal data files
    /////////////////////////////////////

    std::ifstream  infile_nodes(infileNodes);
    std::ifstream  infile_elems(infileElems);
    std::ifstream  infile_DBCs(infileDBCs);
    std::ifstream  infile_FBCs(infileFBCs);

    if(infile_nodes.fail())
    {
       cout << " Could not open the input nodes file " << endl;
       exit(1);
    }

    double  val[50];
    int  val2[50];

    std::string line;

    // read nodal coordinates
    ////////////////////////////////////////////

    cout << " reading nodes " << endl;

    nNode = 0;
    while (std::getline(infile_nodes, line))
      ++nNode;

    vector<vector<double> >  node_coords(nNode, vector<double>(3));

    infile_nodes.clear();
    infile_nodes.seekg(0, infile_nodes.beg);

      ii=0;
      while(infile_nodes >> val[0] >> val[1] >> val[2] )
      {
        //printf("%12.6f \t %12.6f \t %12.6f \n", val[0], val[1], val[2]);

        node_coords[ii][0] = val[1];
        node_coords[ii][1] = val[2];
        node_coords[ii][2] = 0.0;

        ii++;
      }

    // read elements
    ////////////////////////////////////////////
    cout << " reading elements " << endl;

    if(infile_elems.fail())
    {
       cout << " Could not open the input elements file " << endl;
       exit(1);
    }

    nElem = 0;
    while (std::getline(infile_elems, line))
      ++nElem;

    cout << " nElem   " << nElem << endl;

    infile_elems.clear();
    infile_elems.seekg(0, infile_elems.beg);

    vector<vector<int> >  elemNodeConn(nElem, vector<int>(npElem));

    ee=0;
    if(npElem == 6)
    {
      while(infile_elems >> val2[0] >> val2[1] >> val2[2] >> val2[3] >> val2[4] >> val2[5] >> val2[6] >> val2[7] >> val2[8] >> val2[9] )
      {
        //printf("%6d \t %6d \t %6d \t %6d \n", val2[4], val2[5], val2[6], val2[7]);

        for(ii=0; ii<npElem; ii++)
          elemNodeConn[ee][ii] = val2[4+ii]-1;

        ee++;
      }
    }
    else if(npElem == 9)
    {
      while(infile_elems >> val2[0] >> val2[1] >> val2[2] >> val2[3] >> val2[4] >> val2[5] >> val2[6] >> val2[7] >> val2[8] >> val2[9] >> val2[10] >> val2[11] >> val2[12] )
      {
        //printf("%6d \t %6d \t %6d \t %6d \n", val2[4], val2[5], val2[6], val2[7]);

        for(ii=0; ii<npElem; ii++)
          elemNodeConn[ee][ii] = val2[4+ii]-1;

        ee++;
      }
    }
    else
    {
      cerr << " Invalid npElem " << npElem << endl;
      exit(-1);
    }
    //
    // Read Dirichlet BC data
    //
    ////////////////////////////////////////////
    cout << " reading DBCs " << endl;

    if(infile_DBCs.fail())
    {
       cout << " Could not open the input elements file " << endl;
       exit(1);
    }

    vector<vector<double> >  DirichletBCs;
    vector<double>  vecDblTemp(3);

    nDBC = 0;
    while(infile_DBCs >> val[0] >> val[1] >> val[2] )
    {
      vecDblTemp[0] = val[0]-1;
      vecDblTemp[1] = val[1]-1;
      vecDblTemp[2] = val[2];

      //if( val[1] <= ndof )
      //{
        DirichletBCs.push_back(vecDblTemp);
        nDBC++;
      //}
    }

    cout << " nDBC  = " << '\t' << nDBC << endl;

    infile_nodes.close();
    infile_elems.close();
    infile_DBCs.close();

    //
    // Read Output data
    //
    ////////////////////////////////////////////

    vector<int>  OutputData;

    if( !infileOutput.empty() )
    {
      std::ifstream  infile_output(infileOutput);

      cout << " reading output data " << endl;

      if(infile_output.fail())
      {
        cout << " Could not open the input elements file " << endl;
        exit(1);
      }

      ii = 0;
      while (std::getline(infile_output, line))
        ++ii;

      infile_output.clear();
      infile_output.seekg(0, infile_output.beg);

      OutputData.resize(ii);

      ee=0;
      while(infile_output >> val[0] )
      {
        OutputData[ee] = val[0]-1;
        ee++;
      }

      infile_output.close();
    }

    cout << " Input files have been read successfully \n\n " << endl;

      vector<vector<int> >  midNodeData;

      midNodeData.resize(nNode);

      for(ii=0; ii<nNode; ii++)
      {
        midNodeData[ii].resize(3);

        midNodeData[ii][0] = 0;  midNodeData[ii][1] = 0;  midNodeData[ii][2] = 0;
      }

    int nNode_Pres=0;
    vector<int>  pressure_nodes;
    if(npElem == 6)
    {
      for(ee=0; ee<nElem; ee++)
      {
        ii = elemNodeConn[ee][3];
        midNodeData[ii][0] = 1;
        midNodeData[ii][1] = elemNodeConn[ee][0];
        midNodeData[ii][2] = elemNodeConn[ee][1];

        ii = elemNodeConn[ee][4];
        midNodeData[ii][0] = 1;
        midNodeData[ii][1] = elemNodeConn[ee][1];
        midNodeData[ii][2] = elemNodeConn[ee][2];

        ii = elemNodeConn[ee][5];
        midNodeData[ii][0] = 1;
        midNodeData[ii][1] = elemNodeConn[ee][2];
        midNodeData[ii][2] = elemNodeConn[ee][0];

        pressure_nodes.push_back(elemNodeConn[ee][0]);
        pressure_nodes.push_back(elemNodeConn[ee][1]);
        pressure_nodes.push_back(elemNodeConn[ee][2]);
      }
    }
    else if(npElem == 9)
    {
      for(ee=0; ee<nElem; ee++)
      {
        ii = elemNodeConn[ee][4];
        midNodeData[ii][0] = 1;
        midNodeData[ii][1] = elemNodeConn[ee][0];
        midNodeData[ii][2] = elemNodeConn[ee][1];

        ii = elemNodeConn[ee][5];
        midNodeData[ii][0] = 1;
        midNodeData[ii][1] = elemNodeConn[ee][1];
        midNodeData[ii][2] = elemNodeConn[ee][2];

        ii = elemNodeConn[ee][6];
        midNodeData[ii][0] = 1;
        midNodeData[ii][1] = elemNodeConn[ee][2];
        midNodeData[ii][2] = elemNodeConn[ee][3];

        ii = elemNodeConn[ee][7];
        midNodeData[ii][0] = 1;
        midNodeData[ii][1] = elemNodeConn[ee][3];
        midNodeData[ii][2] = elemNodeConn[ee][0];

        pressure_nodes.push_back(elemNodeConn[ee][0]);
        pressure_nodes.push_back(elemNodeConn[ee][1]);
        pressure_nodes.push_back(elemNodeConn[ee][2]);
        pressure_nodes.push_back(elemNodeConn[ee][3]);
      }
    }

    findUnique(pressure_nodes);
    nNode_Pres = pressure_nodes.size();

    vector<int>  pressure_nodes_map(nNode,-1);

    for(ii=0; ii<nNode_Pres; ii++)
    {
      pressure_nodes_map[pressure_nodes[ii]] = ii;
    }

    vector<vector<int> >  ID;
    vector<vector<bool> >  NodeType;
    vector<int>  assyForSoln;

    NodeType.resize(nNode);

    ID.resize(nNode);

    for(ii=0;ii<nNode;ii++)
    {
      NodeType[ii].resize(ndof);
      ID[ii].resize(ndof);

      for(jj=0;jj<ndof;jj++)
      {
        NodeType[ii][jj] = false;
        ID[ii][jj] = -1;
      }
    }

    // fix the pressure at all the mid nodes
    for(ii=0; ii<nNode; ii++)
    {
      if(midNodeData[ii][0])
        NodeType[ii][2] = true;
    }

    if(npElem == 9)
    {
      for(ee=0; ee<nElem; ee++)
      {
        NodeType[elemNodeConn[ee][8]][2] = true;
      }
    }

    // fix the specified Dirichlet BCs
    for(ii=0; ii<nDBC; ii++)
    {
      //cout << ii << '\t' << DirichletBCs[ii][0] << '\t' << DirichletBCs[ii][1] << endl;
      NodeType[DirichletBCs[ii][0]][DirichletBCs[ii][1]] = true;
    }

    //for(ii=0; ii<totalDOF; ii++)
      //cout << ii << '\t' << assyForSoln[ii] << endl;

    int totalDOF = 0;
    for(ii=0;ii<nNode;ii++)
    {
      for(jj=0;jj<ndof;jj++)
      {
        //cout << ii << '\t' << jj << '\t' << NodeType[ii][jj] << endl;
        if(!NodeType[ii][jj])
        {
          ID[ii][jj] = totalDOF++;
          assyForSoln.push_back(ii*ndof+jj);
        }
      }
    }

      cout << " Mesh statistics .....\n" << endl;
      cout << " nElem          = " << '\t' << nElem << endl;
      cout << " nNode          = " << '\t' << nNode  << endl;
      cout << " npElem         = " << '\t' << npElem << endl;
      cout << " ndof           = " << '\t' << ndof << endl;
      cout << " Total DOF      = " << '\t' << totalDOF << endl;

      vector<vector<int> >  LM;

      LM.resize(nElem);

      for(ee=0;ee<nElem;ee++)
      {
        npElem = elemNodeConn[ee].size();

        //printVector(IEN[ee]);

        ind = ndof*npElem;
        LM[ee].resize(ind);

        for(ii=0;ii<npElem;ii++)
        {
          ind = ndof*ii;

          kk = elemNodeConn[ee][ii];

          for(jj=0;jj<ndof;jj++)
          {
            LM[ee][ind+jj] = ID[kk][jj];
          }
        }
      }

      printf("\n element DOF values initialised \n\n");

      /////////////////////////////////////////
      //
      // Eigen based solver
      //
      /////////////////////////////////////////

      cout << " Eigen based solver " << totalDOF << endl;

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

      double  xx, yy;

      // loop over the nodes and adjust nodal coordinates
      for(nn=0; nn<nNode; nn++)
      {
        if( midNodeData[nn][0] )
        {
          n1 = midNodeData[nn][1];
          n2 = midNodeData[nn][2];

          xx = 0.25*node_coords[n1][0] + 0.25*node_coords[n2][0];
          yy = 0.25*node_coords[n1][1] + 0.25*node_coords[n2][1];

          node_coords[nn][0] = 2.0*(node_coords[nn][0] - xx);
          node_coords[nn][1] = 2.0*(node_coords[nn][1] - yy);
        }
      }

      double  elemData[50];

      //density
      elemData[0] = 1.0;
      //viscosity
      //elemData[1] = 1.0;
      elemData[1] = 0.025;
      //Body force in X-, Y- and Z- direction
      elemData[2] = 0.0;   elemData[3] = 0.0; elemData[4] = 0.0;
      //beta
      elemData[5] = 2.0;

      double  fact1, fact2;

      // create elements and prepare element data
      BernsteinElem2DINSTria6Node   **elems;
      //BernsteinElem2DINSQuad9Node   **elems;

      elems = new BernsteinElem2DINSTria6Node* [nElem];
      //elems = new BernsteinElem2DINSQuad9Node* [nElem];

      for(ee=0;ee<nElem;ee++)
      {
        elems[ee] = new BernsteinElem2DINSTria6Node;
        //elems[ee] = new BernsteinElem2DINSQuad9Node;

        elems[ee]->nodeNums = elemNodeConn[ee];

        //elems[ee]->SolnData = &(SolnData);

        elems[ee]->prepareElemData(node_coords);

        elems[ee]->forAssyVec = LM[ee];
      }

      cout << " elements are created and prepated " << endl;
      cout << " Computing the solution \n" << endl;

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

     cout << " nNode_Pres = " << nNode_Pres << endl;

      vector<int>  vecTempInt1, vecTempInt2;

    // to compute inf-sup number

    //nNode_Pres = nNode;

    ind = npElem*ndim;
    MatrixXd  Kuu(ind,ind), Kup(ind,9), Kpp(9,9);

    MatrixXd  Kglobal, Mglobal, eigen_vectors;
    VectorXd  eigen_values, eigvec, Flocal, vecTemp;
    MatrixXd  Vmat, Qmat, Bmat;

    Vmat.resize(totalDOF, totalDOF);
    Bmat.resize(totalDOF, nNode_Pres);
    Qmat.resize(totalDOF, totalDOF);

    Kglobal.resize(totalDOF, totalDOF);
    Mglobal.resize(nNode_Pres, nNode_Pres);

    Kglobal.setZero();
    Mglobal.setZero();
    Vmat.setZero();
    Bmat.setZero();
    Qmat.setZero();

    /////////////////////////////////////////
    // compute and assemble matrices
    /////////////////////////////////////////

    for(ee=0; ee<nElem; ee++)  // loop over all the elements
    {
        //cout << "       elem... : " << (ee+1) << endl;

        elems[ee]->toComputeInfSupCondition(node_coords, elemData, Kuu, Kup, Kpp);

        vecTempInt1=elems[ee]->forAssyVec;
        vecTempInt2=elems[ee]->nodeNums;

        n1=vecTempInt1.size();
        n2=3;

        //printVector(vecTempInt1);
        //printVector(vecTempInt2);

        // Kuu
        for(ii=0; ii<n1; ii++)
        {
          rr = vecTempInt1[ii];
          if( rr != -1 )
          {
            for(jj=0; jj<n1; jj++)
            {
              cc = vecTempInt1[jj];
              if( cc != -1 )
              {
                 Kglobal(rr, cc) += Kuu(ii,jj);
              }
            }
          }
        }

        //cout << " aaaaaaaaaaa " << endl;

        // Kup
        for(ii=0; ii<n1; ii++)
        {
          rr = vecTempInt1[ii];
          if( rr != -1 )
          {
            for(jj=0; jj<n2; jj++)
            {
              cc = pressure_nodes_map[vecTempInt2[jj]];
              //cc = vecTempInt2[jj];

              Bmat(rr,cc) += Kup(ii,jj);
            }
          }
        }

        //cout << " aaaaaaaaaaa " << endl;

        // Kpp
        for(ii=0; ii<n2; ii++)
        {
          rr = pressure_nodes_map[vecTempInt2[ii]];
          //rr = vecTempInt2[ii];

          for(jj=0; jj<n2; jj++)
          {
            cc = pressure_nodes_map[vecTempInt2[jj]];
            //cc = vecTempInt2[jj];

            Mglobal(rr, cc) += Kpp(ii,jj);
          }
        }
    }

    cout << "  Solving eigenvalue problem ... " << endl;

    Kglobal = Kglobal.inverse();
    //printMatrix(Vmat);
    //printf("\n\n");
    //printMatrix(globalK);
    //printf("\n\n");
    //printMatrix(globalM);
    //printf("\n\n");
    Kglobal = (Bmat.transpose()*Kglobal)*Bmat;

    GeneralizedSelfAdjointEigenSolver<MatrixXd> es(Kglobal, Mglobal, EigenvaluesOnly);
    eigen_values = es.eigenvalues();
    //eigen_vectors = es.eigenvectors();

    //EigenSolver<MatrixXd>  es(Kglobal, EigenvaluesOnly);
    //cout << "The eigenvalues of A are:" << endl << es.eigenvalues() << endl;
    //eigen_values = es.eigenvalues().col(0);

    cout << "  Eigen analysis successfully completed ... " << endl;

    cout << " The first " << min(20, (int) eigen_values.rows()) << " eigenvalues are ... " << endl;

    for(int ii=0;ii<min(20, (int) eigen_values.rows());ii++)
      printf("\t %5d \t %12.10f \t %12.10f \n", (ii+1), eigen_values(ii), sqrt(abs(eigen_values(ii))));

    if(elems != NULL)
    {
      for(ii=0;ii<nElem;ii++)
        delete elems[ii];

      delete [] elems;
      elems = NULL;
    }

    cout << " Program is successful \n " << endl;

    return 1;
}


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/config.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/detail/constant_traits.hpp>
#include <boost/simd/detail/dispatch/function/make_callable.hpp>
#include <boost/simd/detail/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch/as.hpp>

/*

    @ingroup group-constant

    This constant is such that, for pairs of types (T, Tup)
    (namely (float,  double) and (double, long double)) the sum:

    abs(Tup(Pio_2lo<T>())+Tup(Pio_2<T>())-Pio_2\< Tup \>()) is  less than
    a few Eps<Tup>().

    This is used to improve accurracy when computing sums of the kind
    \f$\pi/2 + x\f$ with x small,  by replacing them by
    Pio_2 + (Pio_2lo + x)

    @par Semantic:

    For type T:

    @code
    T r = Pio_2lo<T>();
    @endcode

    is similar to:

    @code
    if T is double
      r = 6.123233995736766e-17
    else if T is float
      r = -4.3711388e-08
    @endcode

    @return a value of type T

*/

namespace boost { namespace simd
{
  namespace tag
  {
    struct pio_2lo_ : boost::dispatch::constant_value_<pio_2lo_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,pio_2lo_,boost::dispatch::constant_value_<pio_2lo_>);
      BOOST_SIMD_REGISTER_CONSTANT(0, 0XB33BBD2EUL, 0X3C91A62633145C07ULL);
    };
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag, pio_2lo_)
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::pio_2lo_,pio_2lo);
  }

  template<typename T> BOOST_FORCEINLINE auto Pio_2lo()
  BOOST_NOEXCEPT_DECLTYPE(detail::pio_2lo( boost::dispatch::as_<T>{}))
  {
    return detail::pio_2lo( boost::dispatch::as_<T>{} );
  }
} }

#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif