/*
 * WaveEquationBase.cpp
 *
 *  Created on: 23.07.2017
 *      Author: thies
 */

#include <deal.II/lac/precondition.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/solver_control.h>
#include <deal.II/numerics/matrix_tools.h>
#include <forward/MatrixCreator.h>
#include <forward/WaveEquationBase.h>

namespace wavepi {
namespace forward {

using namespace dealii;
using namespace wavepi::base;

template <int dim>
void WaveEquationBase<dim>::fill_matrices(std::shared_ptr<SpaceTimeMesh<dim>> mesh, size_t time_idx,
                                          SparseMatrix<double> &dst_A, SparseMatrix<double> &dst_B,
                                          SparseMatrix<double> &dst_C) {
  //   const double time = mesh->get_time(time_idx);

  // this helps only a bit because each of the operations is already parallelized
  // tests show about 20%-30% (depending on dim) speedup on my Intel i5 4690
  // this assembling could be done even more efficient, by looping through the mesh once and assembling all matrices.

  //   Threads::TaskGroup<void> task_group;

  if (!rho_time_dependent || time_idx == mesh->length() - 1) {
    //      task_group += Threads::new_task(&WaveEquationBase<dim>::fill_A, *this, mesh, time_idx, dst_A);
    //      task_group += Threads::new_task(&WaveEquationBase<dim>::fill_B, *this, mesh, time_idx, dst_B);
    //      task_group += Threads::new_task(&WaveEquationBase<dim>::fill_C, *this, mesh, time_idx, dst_C);

    this->fill_A(mesh, time_idx, dst_A);
    this->fill_B(mesh, time_idx, dst_B);
    this->fill_C(mesh, time_idx, dst_C);

    matrix_C_intermediate.clear();
    matrix_D_intermediate.clear();
  } else {
    // possible here because fill_*_intermediate can access different time steps of ρ in parallel
    // (ρ is discretized and this is exploited by assembly tasks)

    //      task_group += Threads::new_task(&WaveEquationBase<dim>::fill_A, *this, mesh, time_idx, dst_A);
    //      task_group += Threads::new_task(&WaveEquationBase<dim>::fill_B, *this, mesh, time_idx, dst_B);
    //      task_group += Threads::new_task(&WaveEquationBase<dim>::fill_C, *this, mesh, time_idx, dst_C);

    this->fill_A(mesh, time_idx, dst_A);
    this->fill_B(mesh, time_idx, dst_B);
    this->fill_C(mesh, time_idx, dst_C);

    //      task_group += Threads::new_task(&WaveEquationBase<dim>::fill_C_intermediate, *this, mesh, time_idx);
    //      task_group += Threads::new_task(&WaveEquationBase<dim>::fill_D_intermediate, *this, mesh, time_idx);

    this->fill_C_intermediate(mesh, time_idx);
    this->fill_D_intermediate(mesh, time_idx);
  }

  //   task_group.join_all();
}

// let dst <- (D^n)^{-1} D^{n-1} M^{-1} src
// ( i.e. dst <- src for time-independent D)
template <int dim>
void WaveEquationBase<dim>::vmult_D_intermediate(const SparseMatrix<double> &mass_matrix, Vector<double> &dst,
                                                 const Vector<double> &src, double tolerance) const {
  if (rho_time_dependent) {
    Vector<double> tmp(src.size());

    SolverControl solver_control(2000, tolerance * src.l2_norm());
    SolverCG<> cg(solver_control);

    // PreconditionIdentity precondition = PreconditionIdentity();
    PreconditionSSOR<SparseMatrix<double>> precondition;
    precondition.initialize(mass_matrix, PreconditionSSOR<SparseMatrix<double>>::AdditionalData(1.0));

    cg.solve(mass_matrix, tmp, src, precondition);

    AssertThrow(matrix_D_intermediate.n() > 0, ExcInternalError("matrix_D_intermediate is missing"));
    matrix_D_intermediate.vmult(dst, tmp);
  } else {
    dst.equ(1.0, src);
  }
}

// let dst <- M^{-1} (D^n)^{-1} D^{n-1} src
// ( i.e. dst <- src for time-independent D)
template <int dim>
void WaveEquationBase<dim>::vmult_D_intermediate_transpose(const SparseMatrix<double> &mass_matrix, Vector<double> &dst,
                                                           const Vector<double> &src, double tolerance) const {
  if (rho_time_dependent) {
    Vector<double> tmp(src.size());

    AssertThrow(matrix_D_intermediate.n() > 0, ExcInternalError("matrix_D_intermediate is missing"));
    matrix_D_intermediate.vmult(tmp, src);

    SolverControl solver_control(2000, tolerance * tmp.l2_norm());
    SolverCG<> cg(solver_control);

    // PreconditionIdentity precondition = PreconditionIdentity();
    PreconditionSSOR<SparseMatrix<double>> precondition;
    precondition.initialize(mass_matrix, PreconditionSSOR<SparseMatrix<double>>::AdditionalData(1.0));

    cg.solve(mass_matrix, dst, tmp, precondition);
  } else {
    dst.equ(1.0, src);
  }
}

// before mesh change, let dst <- (D^n)^{-1} C^{n-1} src
// ( i.e. dst <- matrix_C * src for time-independent D)
template <int dim>
void WaveEquationBase<dim>::vmult_C_intermediate(const SparseMatrix<double> &matrix_C, Vector<double> &dst,
                                                 const Vector<double> &src) const {
  if (rho_time_dependent) {
    AssertThrow(matrix_C_intermediate.n() > 0, ExcInternalError("matrix_C_intermediate is missing"));
    matrix_C_intermediate.vmult(dst, src);
  } else {
    matrix_C.vmult(dst, src);
  }
}

template <int dim>
void WaveEquationBase<dim>::fill_A(std::shared_ptr<SpaceTimeMesh<dim>> mesh, size_t time_idx,
                                   SparseMatrix<double> &destination) {
  const double time = mesh->get_time(time_idx);
  auto dof_handler  = mesh->get_dof_handler(time_idx);

  if ((!param_rho_disc && !param_q_disc) || !using_special_assembly(mesh))
    MatrixCreator<dim>::create_A_matrix(dof_handler, mesh->get_quadrature(), destination, param_rho, param_q, time);
  else if (param_rho_disc && !param_q_disc)
    MatrixCreator<dim>::create_A_matrix(dof_handler, mesh->get_quadrature(), destination,
                                        param_rho_disc->get_function_coefficients_by_time(time), param_q, time);
  else if (!param_rho_disc && param_q_disc)
    MatrixCreator<dim>::create_A_matrix(dof_handler, mesh->get_quadrature(), destination, param_rho,
                                        param_q_disc->get_function_coefficients_by_time(time), time);
  else
    // (param_rho_disc && param_q_disc)
    MatrixCreator<dim>::create_A_matrix(dof_handler, mesh->get_quadrature(), destination,
                                        param_rho_disc->get_function_coefficients_by_time(time),
                                        param_q_disc->get_function_coefficients_by_time(time));
}

template <int dim>
void WaveEquationBase<dim>::fill_B(std::shared_ptr<SpaceTimeMesh<dim>> mesh, size_t time_idx,
                                   SparseMatrix<double> &destination) {
  const double time = mesh->get_time(time_idx);
  auto dof_handler  = mesh->get_dof_handler(time_idx);

  if (param_nu_disc && using_special_assembly(mesh))
    MatrixCreator<dim>::create_mass_matrix(dof_handler, mesh->get_quadrature(), destination,
                                           param_nu_disc->get_function_coefficients_by_time(time));
  else
    MatrixCreator<dim>::create_mass_matrix(dof_handler, mesh->get_quadrature(), destination, param_nu, time);
}

template <int dim>
void WaveEquationBase<dim>::fill_C(std::shared_ptr<SpaceTimeMesh<dim>> mesh, size_t time_idx,
                                   SparseMatrix<double> &destination) {
  const double time = mesh->get_time(time_idx);
  auto dof_handler  = mesh->get_dof_handler(time_idx);

  if ((!param_rho_disc && !param_c_disc) || !using_special_assembly(mesh))
    MatrixCreator<dim>::create_C_matrix(dof_handler, mesh->get_quadrature(), destination, param_rho, param_c, time,
                                        time);
  else if (param_rho_disc && !param_c_disc)
    MatrixCreator<dim>::create_C_matrix(dof_handler, mesh->get_quadrature(), destination,
                                        param_rho_disc->get_function_coefficients_by_time(time), param_c, time);
  else if (!param_rho_disc && param_c_disc)
    MatrixCreator<dim>::create_C_matrix(dof_handler, mesh->get_quadrature(), destination, param_rho,
                                        param_c_disc->get_function_coefficients_by_time(time), time);
  else
    // (param_rho_disc && param_c_disc)
    MatrixCreator<dim>::create_C_matrix(dof_handler, mesh->get_quadrature(), destination,
                                        param_rho_disc->get_function_coefficients_by_time(time),
                                        param_c_disc->get_function_coefficients_by_time(time));
}

template <int dim>
void WaveEquationBase<dim>::fill_C_intermediate(std::shared_ptr<SpaceTimeMesh<dim>> mesh, size_t time_idx) {
  // fill with (D^{n+1})^{-1} C^n, n = time_idx on the mesh of time_idx
  // -> needs to be able to change the time in ρ to the next time if continuous ρ is used

  matrix_C_intermediate.reinit(*mesh->get_sparsity_pattern(time_idx));
  double current_time = mesh->get_time(time_idx);
  double next_time    = mesh->get_time(time_idx + 1);  // does range checking in debug mode
  auto dof_handler    = mesh->get_dof_handler(time_idx);

  if ((!param_rho_disc && !param_c_disc) || !using_special_assembly(mesh))
    MatrixCreator<dim>::create_C_matrix(dof_handler, mesh->get_quadrature(), matrix_C_intermediate, param_rho, param_c,
                                        next_time, current_time);
  else if (param_rho_disc && !param_c_disc)
    MatrixCreator<dim>::create_C_matrix(dof_handler, mesh->get_quadrature(), matrix_C_intermediate,
                                        param_rho_disc->get_function_coefficients_by_time(next_time), param_c,
                                        current_time);

  else if (!param_rho_disc && param_c_disc)
    MatrixCreator<dim>::create_C_matrix(dof_handler, mesh->get_quadrature(), matrix_C_intermediate, param_rho,
                                        param_c_disc->get_function_coefficients_by_time(current_time), next_time);
  else
    MatrixCreator<dim>::create_C_matrix(dof_handler, mesh->get_quadrature(), matrix_C_intermediate,
                                        param_rho_disc->get_function_coefficients_by_time(next_time),
                                        param_c_disc->get_function_coefficients_by_time(current_time));
}

template <int dim>
void WaveEquationBase<dim>::fill_D_intermediate(std::shared_ptr<SpaceTimeMesh<dim>> mesh, size_t time_idx) {
  // fill with (D^{n+1})^{-1} D^n, n = time_idx on the mesh of time_idx
  // -> needs to be able to change the time in ρ to the next time if continuous ρ is used

  matrix_D_intermediate.reinit(*mesh->get_sparsity_pattern(time_idx));
  double current_time = mesh->get_time(time_idx);
  double next_time    = mesh->get_time(time_idx + 1);  // does range checking in debug mode
  auto dof_handler    = mesh->get_dof_handler(time_idx);

  if (!param_rho_disc || !using_special_assembly(mesh))
    MatrixCreator<dim>::create_D_intermediate_matrix(dof_handler, mesh->get_quadrature(), matrix_D_intermediate,
                                                     param_rho, current_time, next_time);
  else
    MatrixCreator<dim>::create_D_intermediate_matrix(dof_handler, mesh->get_quadrature(), matrix_D_intermediate,
                                                     param_rho_disc->get_function_coefficients_by_time(current_time),
                                                     param_rho_disc->get_function_coefficients_by_time(next_time));
}

template class WaveEquationBase<1>;
template class WaveEquationBase<2>;
template class WaveEquationBase<3>;

} /* namespace forward */
} /* namespace wavepi */


#include <random>

#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <Eigen/Dense>

#include <geometry/camera.h>

class CameraFixture : public ::testing::Test {
 public:
  CameraFixture() {
    pixels.resize(pixels_count, 2);

    std::mt19937 gen(42);
    std::uniform_real_distribution<> rand(-0.5, 0.5);
    for (int i = 0; i < pixels_count; ++i) {
      const auto x = rand(gen) * max_width;
      const auto y = rand(gen) * max_height;
      pixels.row(i) << x, y;
    }

    distortion.resize(5);
    distortion << -0.1, 0.03, 0.001, 0.001, 0.002;
    principal_point << 0.1, -0.05;

    new_distortion.resize(5);
    new_distortion << 1, 1, 1, 1, 1;
    new_principal_point << 0.02, -0.01;
  }

  double ComputeError(const Eigen::MatrixX2d& other) {
    double error = 0;
    for (int i = 0; i < pixels_count; ++i) {
      error += (other.row(i) - pixels.row(i)).norm();
    }
    return error/pixels_count;
  }

  int pixels_count{10000};
  Eigen::MatrixX2d pixels;

  double max_width{0.90};
  double max_height{0.75};
  double focal{0.4};
  double new_focal{0.8};
  double new_ar{0.9};

  int pixel_width{4000};
  int pixel_height{3000};

  Eigen::VectorXd distortion;
  Eigen::VectorXd new_distortion;
  Eigen::Vector2d principal_point;
  Eigen::Vector2d new_principal_point;
};

TEST_F(CameraFixture, PerspectiveIsConsistent){
  Camera camera = Camera::CreatePerspectiveCamera(focal, distortion[0], distortion[1]);
  const auto projected = camera.ProjectMany(camera.BearingsMany(pixels));
  ASSERT_LT(ComputeError(projected), 2e-7);
}

TEST_F(CameraFixture, BrownIsConsistent){
  Camera camera = Camera::CreateBrownCamera(focal, 1.0, principal_point, distortion);
  const auto projected = camera.ProjectMany(camera.BearingsMany(pixels));
  ASSERT_LT(ComputeError(projected), 2e-7);
}

TEST_F(CameraFixture, FisheyeIsConsistent){
  Camera camera = Camera::CreateFisheyeCamera(focal, distortion[0], distortion[1]);
  const auto projected = camera.ProjectMany(camera.BearingsMany(pixels));
  ASSERT_LT(ComputeError(projected), 2e-7);
}

TEST_F(CameraFixture, DualIsConsistent){
  Camera camera = Camera::CreateDualCamera(0.5, focal, distortion[0], distortion[1]);
  const auto projected = camera.ProjectMany(camera.BearingsMany(pixels));
  ASSERT_LT(ComputeError(projected), 2e-7);
}

TEST_F(CameraFixture, SphericalIsConsistent){
  Camera camera = Camera::CreateSphericalCamera();
  const auto projected = camera.Project(camera.Bearing(Eigen::Vector2d(0.2, 0.2)));
  ASSERT_EQ(projected(0), 0.2);
  ASSERT_EQ(projected(1), 0.2);
}

TEST_F(CameraFixture, PerspectiveCanSetDistortion){
  Camera camera = Camera::CreatePerspectiveCamera(focal, distortion[0], distortion[1]);
  camera.SetDistortion(new_distortion);

  Eigen::VectorXd expected_distortion(5);
  expected_distortion << 1, 1, 0, 0, 0;
  ASSERT_EQ(expected_distortion, camera.GetDistortion());
}

TEST_F(CameraFixture, FisheyeCanSetDistortion){
  Camera camera = Camera::CreateFisheyeCamera(focal, distortion[0], distortion[1]);
  camera.SetDistortion(new_distortion);

  Eigen::VectorXd expected_distortion(5);
  expected_distortion << 1, 1, 0, 0, 0;
  ASSERT_EQ(expected_distortion, camera.GetDistortion());
}

TEST_F(CameraFixture, DualCanSetDistortion){
  Camera camera = Camera::CreateDualCamera(0.5, focal, distortion[0], distortion[1]);
  camera.SetDistortion(new_distortion);

  Eigen::VectorXd expected_distortion(5);
  expected_distortion << 1, 1, 0, 0, 0;
  ASSERT_EQ(expected_distortion, camera.GetDistortion());
}

TEST_F(CameraFixture, BrownCanSetDistortion){
  Camera camera = Camera::CreateBrownCamera(focal, 1.0, principal_point, distortion);
  camera.SetDistortion(new_distortion);

  ASSERT_EQ(new_distortion, camera.GetDistortion());
}

TEST_F(CameraFixture, SphericalCanSetDistortion){
  Camera camera = Camera::CreateSphericalCamera();
  camera.SetDistortion(new_distortion);

  Eigen::VectorXd expected_distortion(5);
  expected_distortion << 0, 0, 0, 0, 0;
  ASSERT_EQ(expected_distortion, camera.GetDistortion());
}

TEST_F(CameraFixture, PerspectiveCanSetPrincipalPoint){
  Camera camera = Camera::CreatePerspectiveCamera(focal, distortion[0], distortion[1]);
  camera.SetPrincipalPoint(new_principal_point);

  const auto expected_principal_point = Eigen::Vector2d::Zero();
  ASSERT_EQ(expected_principal_point, camera.GetPrincipalPoint());
}

TEST_F(CameraFixture, FisheyeCanSetPrincipalPoint){
  Camera camera = Camera::CreateFisheyeCamera(focal, distortion[0], distortion[1]);
  camera.SetPrincipalPoint(new_principal_point);

  const auto expected_principal_point = Eigen::Vector2d::Zero();
  ASSERT_EQ(expected_principal_point, camera.GetPrincipalPoint());
}

TEST_F(CameraFixture, DualCanSetPrincipalPoint){
  Camera camera = Camera::CreateDualCamera(0.5, focal, distortion[0], distortion[1]);
  camera.SetPrincipalPoint(new_principal_point);

  const auto expected_principal_point = Eigen::Vector2d::Zero();
  ASSERT_EQ(expected_principal_point, camera.GetPrincipalPoint());
}

TEST_F(CameraFixture, BrownCanSetPrincipalPoint){
  Camera camera = Camera::CreateBrownCamera(focal, 1.0, principal_point, distortion);
  camera.SetPrincipalPoint(new_principal_point);

  ASSERT_EQ(new_principal_point, camera.GetPrincipalPoint());
}

TEST_F(CameraFixture, SphericalCanSetPrincipalPoint){
  Camera camera = Camera::CreateSphericalCamera();
  camera.SetPrincipalPoint(new_principal_point);

  const auto expected_principal_point = Eigen::Vector2d::Zero();
  ASSERT_EQ(expected_principal_point, camera.GetPrincipalPoint());
}

TEST_F(CameraFixture, PerspectiveCanSetFocalAndAspectRatio){
  Camera camera = Camera::CreatePerspectiveCamera(focal, distortion[0], distortion[1]);
  camera.SetFocal(new_focal);
  camera.SetAspectRatio(new_ar);

  ASSERT_EQ(new_focal, camera.GetFocal());
  ASSERT_EQ(1.0, camera.GetAspectRatio());
}

TEST_F(CameraFixture, FisheyeCanSetFocalAndAspectRatio){
  Camera camera = Camera::CreateFisheyeCamera(focal, distortion[0], distortion[1]);
  camera.SetFocal(new_focal);
  camera.SetAspectRatio(new_ar);

  ASSERT_EQ(new_focal, camera.GetFocal());
  ASSERT_EQ(1.0, camera.GetAspectRatio());
}

TEST_F(CameraFixture, DualCanSetFocalAndAspectRatio){
  Camera camera = Camera::CreateDualCamera(0.5, focal, distortion[0], distortion[1]);
  camera.SetFocal(new_focal);
  camera.SetAspectRatio(new_ar);

  ASSERT_EQ(new_focal, camera.GetFocal());
  ASSERT_EQ(1.0, camera.GetAspectRatio());
}

TEST_F(CameraFixture, BrownCanSetFocalAndAspectRatio){
  Camera camera = Camera::CreateBrownCamera(focal, 1.0, principal_point, distortion);
  camera.SetFocal(new_focal);
  camera.SetAspectRatio(new_ar);

  ASSERT_EQ(new_focal, camera.GetFocal());
  ASSERT_EQ(new_ar, camera.GetAspectRatio());
}

TEST_F(CameraFixture, SphericalCanSetFocalAndAspectRatio){
  Camera camera = Camera::CreateSphericalCamera();
  camera.SetFocal(new_focal);
  camera.SetAspectRatio(new_ar);

  ASSERT_EQ(1.0, camera.GetFocal());
  ASSERT_EQ(1.0, camera.GetAspectRatio());
}

TEST_F(CameraFixture, PerspectiveReturnCorrectK){
  Camera camera = Camera::CreatePerspectiveCamera(focal, distortion[0], distortion[1]);

  Eigen::Matrix3d expected = Eigen::Matrix3d::Identity();
  expected(0, 0) = expected(1, 1) = focal;
  ASSERT_EQ(expected, camera.GetProjectionMatrix());
}

TEST_F(CameraFixture, PerspectiveReturnCorrectKScaled){
  Camera camera = Camera::CreatePerspectiveCamera(focal, distortion[0], distortion[1]);

  Eigen::Matrix3d expected = Eigen::Matrix3d::Identity();
  expected(0, 0) = expected(1, 1) = focal*std::max(pixel_width, pixel_height);
  expected(0, 2) = pixel_width*0.5;
  expected(1, 2) = pixel_height*0.5;
  ASSERT_EQ(expected, camera.GetProjectionMatrixScaled(pixel_width, pixel_height));
}

TEST_F(CameraFixture, BrownReturnCorrectK){
  Camera camera = Camera::CreateBrownCamera(focal, new_ar, principal_point, distortion);

  Eigen::Matrix3d expected = Eigen::Matrix3d::Identity();
  expected(0, 0) = focal;
  expected(1, 1) = new_ar*focal;
  expected(0, 2) = principal_point(0);
  expected(1, 2) = principal_point(1);
  ASSERT_EQ(expected, camera.GetProjectionMatrix());
}

TEST_F(CameraFixture, BrownReturnCorrectKScaled){
  Camera camera = Camera::CreateBrownCamera(focal, new_ar, principal_point, distortion);

  Eigen::Matrix3d expected = Eigen::Matrix3d::Identity();
  const auto normalizer = std::max(pixel_width, pixel_height);
  expected(0, 0) = focal*normalizer;
  expected(1, 1) = (focal*new_ar)*normalizer;
  expected(0, 2) = principal_point(0)*normalizer + pixel_width*0.5;
  expected(1, 2) = principal_point(1)*normalizer + pixel_height*0.5;

  ASSERT_EQ(expected, camera.GetProjectionMatrixScaled(pixel_width, pixel_height));
}


// This file is part of snark, a generic and flexible library for robotics research
// Copyright (c) 2011 The University of Sydney
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. 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.
// 3. Neither the name of the University of Sydney nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
// GRANTED BY THIS LICENSE.  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 OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
// BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
// OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
// IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <cmath>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/foreach.hpp>
#include <Eigen/Core>
#include "region_properties.h"
#include <comma/base/exception.h>

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS( cs::cartesian )

namespace snark{ namespace imaging {

/// compute the area of a polygon and its convex hull
/// @param points polygon points
/// @param area area of the polygon
/// @param convexArea area of the convex hull
void compute_area( const std::vector< cv::Point >& points, double& area, double& convexArea )
{
    boost::geometry::model::polygon< boost::tuple< double, double > > polygon;
    for( unsigned int i = 0; i < points.size(); i++ )
    {
        boost::geometry::append( polygon, boost::make_tuple( points[i].x, points[i].y ) );
    }
    boost::geometry::append( polygon, boost::make_tuple( points[0].x, points[0].y ) ); // close polygon

    area = boost::geometry::area( polygon );

    boost::geometry::model::polygon< boost::tuple<double, double> > hull;
    boost::geometry::convex_hull( polygon, hull );

    convexArea = boost::geometry::area( hull );
}

/// constructor
/// @param image input image, is considered as a binary image ( all non-zero pixels are 1 )
region_properties::region_properties ( const cv::Mat& image, double minArea ):
    m_minArea( minArea )
{
    cv::Mat binary;
    if( image.channels() == 3 )
    {
        cv::cvtColor( image, binary, CV_RGB2GRAY );
    }
    else if( image.channels() == 1 )
    {
        binary = image;
    }
    else
    {
        COMMA_THROW( comma::exception, "incorrect number of channels, should be 1 or 3, not " << image.channels() );
    }
//     cv::Mat closed;
//     cv::morphologyEx( binary, closed, cv::MORPH_CLOSE, cv::Mat::ones( 3, 3, CV_8U) );
    std::vector< std::vector<cv::Point> > contours;
    cv::findContours( binary, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE );

    for( unsigned int i = 0; i < contours.size(); i++ )
    {
        binary = cv::Scalar(0);
        cv::drawContours( binary, contours, i, cv::Scalar(0xFF), CV_FILLED );
        cv::Rect rect = cv::boundingRect( cv::Mat( contours[i]) );
        cv::Moments moments = cv::moments( binary( rect ), true );
        double x = moments.m10/moments.m00;
        double y = moments.m01/moments.m00;
        double area = moments.m00; // cv::countNonZero( binary( rect ) )
        if( area > m_minArea )
        {
            // see wikipedia, image moments
            double diff = moments.nu20 - moments.nu02;
            double a = 0.5 * ( moments.nu20 + moments.nu02 );
            double b = 0.5 * std::sqrt( 4 * moments.nu11 * moments.nu11 + diff * diff );
            double minEigenValue = a - b;
            double maxEigenValue = a + b;
    //         std::cerr << " min " << minEigenValue << " max " << maxEigenValue << std::endl;
            double theta = 0.5 * std::atan2( 2 * moments.nu11, diff );
            double eccentricity = 1;
            if( std::fabs( maxEigenValue ) > 1e-15 )
            {
                eccentricity = std::sqrt( 1 - minEigenValue / maxEigenValue );
            }

            double polygonArea;
            double convexArea;
            compute_area( contours[i], polygonArea, convexArea );
    //         std::cerr << " area " << area << " polygon " << polygonArea << " convex " << convexArea << std::endl;

            blob blob;
            blob.majorAxis = 2 * std::sqrt( moments.m00 * maxEigenValue );
            blob.minorAxis = 2 * std::sqrt( moments.m00 * minEigenValue );
            blob.orientation = theta;
            blob.centroid = cv::Point( x + rect.x, y + rect.y );
            blob.area = area;
            blob.eccentricity = eccentricity;
            blob.solidity = 0;
            if( std::fabs( convexArea ) > 1e-15 )
            {
                blob.solidity = polygonArea / convexArea;
            }
            m_blobs.push_back( blob );
        }
    }
}

/// draw debug information on the image
void region_properties::show( cv::Mat& image, bool text )
{
    for( unsigned int i = 0; i < m_blobs.size(); i++ )
    {
        cv::Point centroid = m_blobs[i].centroid;
        cv::circle( image, centroid, 3, cv::Scalar( 0, 0, 255 ), 2 );
        std::stringstream s;
        s << i;
        if( text )
        {
            cv::putText( image, s.str(), centroid + cv::Point( 2, 2 ), cv::FONT_HERSHEY_PLAIN ,1, cv::Scalar( 0, 0, 255 ) );
        }
        cv::ellipse( image, centroid, cv::Size( m_blobs[i].majorAxis, m_blobs[i].minorAxis ), m_blobs[i].orientation * 180.0 / M_PI, 0, 360, cv::Scalar( 0, 255, 0 ) );
//         std::cerr << i << ": area " << m_blobs[i].area << " eccentricity " << m_blobs[i].eccentricity << " solidity " << m_blobs[i].solidity << std::endl;
    }
}

} }


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

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

 /*!

    @ingroup group-predicates
    Function object implementing is_not_denormal capabilities

    Returns @ref True or @ref False according x is not denormal or is.

    @par Semantic:

    @code
    as_logical_t<T> r = is_not_denormal(x);
    @endcode

    is similar to:

    @code
    as_logical_t<T> r = (abs(x) >=  Smallestposval<T>());
    @endcode

    @see Smallestposval

  **/
  const boost::dispatch::functor<tag::is_not_denormal_> is_not_denormal = {};
} }
#endif

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

#endif


/*
Copyright 2009-2019 Nicolas Colombe

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 <math/polygon.hpp>
#include <math/polygon_def.hpp>
#include <math/segment.hpp>
#include <set>

#include <boost/polygon/polygon.hpp>

namespace boost
{
  namespace polygon
  {

    template <typename Real>
    struct geometry_concept<eXl::Polygon<Real> >{ typedef polygon_with_holes_concept type; };

    template <typename Real>
    struct geometry_concept<eXl::Vector<eXl::Vector2<Real> > >{ typedef polygon_concept type; };

    template <typename Real>
    struct polygon_90_traits < eXl::Polygon<Real>, boost::polygon::gtl_no > {};

    template <typename Real>
    struct polygon_90_traits < eXl::Vector<eXl::Vector2<Real> >, boost::polygon::gtl_no > {};

    template <typename Real>
    struct polygon_traits_general<eXl::Polygon<Real> > {
      typedef Real coordinate_type;
      typedef typename eXl::Vector<eXl::Vector2<Real> >::const_iterator iterator_type;
      typedef eXl::Vector2<Real> point_type;

      static inline iterator_type begin_points(eXl::Polygon<Real> const& t) {
          return t.Border().begin();
      }
      static inline iterator_type end_points(eXl::Polygon<Real> const& t) {
          return t.Border().end();
      }

      // Get the number of sides of the polygon
      static inline std::size_t size(eXl::Polygon<Real> const& t) {
        return t.Border().size();
      }

      // Get the winding direction of the polygon
      static inline winding_direction winding(eXl::Polygon<Real> const& t) {
        return unknown_winding;
      }
    };

    template <typename Real>
    struct polygon_mutable_traits<eXl::Polygon<Real> > {

      template <typename iT>
      static inline eXl::Polygon<Real>& set_points(eXl::Polygon<Real> & t,
                                         iT input_begin, iT input_end) {

        eXl::Vector<eXl::Vector2<Real> > tempStor;
        //iterator_compact_to_points<iT,eXl::Vector2<Real> > iterBegin(input_begin,input_end);
        //iterator_compact_to_points<iT,eXl::Vector2<Real> > iterEnd(input_end,input_end);

        //tempStor.assign(input_begin,input_end);
        for (; input_begin != input_end; ++input_begin)
        {
          eXl::Vector2<Real> temp(boost::polygon::point_traits<typename iT::value_type>::get(*input_begin, boost::polygon::HORIZONTAL),
                                  boost::polygon::point_traits<typename iT::value_type>::get(*input_begin, boost::polygon::VERTICAL)) ;
          tempStor.push_back(temp);
        }

        t = eXl::Polygon<Real>();
        t.m_Ext.swap(tempStor);
        t.ForceClockwise();
        t.UpdateAABB();
        return t;
      }

    };

    template <typename Real>
    struct polygon_traits_general<eXl::Vector<eXl::Vector2<Real> > > {
      typedef Real coordinate_type;
      typedef typename eXl::Vector<eXl::Vector2<Real> >::const_iterator iterator_type;
      typedef eXl::Vector2<Real> point_type;

      static inline iterator_type begin_points(typename eXl::Polygon<Real>::PtList const& t) {
          return t.begin();
      }
      static inline iterator_type end_points(typename eXl::Polygon<Real>::PtList const& t) {
          return t.end();
      }

      // Get the number of sides of the polygon
      static inline std::size_t size(typename eXl::Polygon<Real>::PtList const& t) {
        return t.size();
      }

      // Get the winding direction of the polygon
      static inline winding_direction winding(typename eXl::Polygon<Real>::PtList const& t) {
        return unknown_winding;
      }
    };

    template <typename Real>
    struct polygon_mutable_traits<eXl::Vector<eXl::Vector2<Real> > > {

      template <typename iT>
      static inline typename eXl::Polygon<Real>::PtList& set_points(typename eXl::Polygon<Real>::PtList & t,
                                         iT input_begin, iT input_end) {

        //iterator_compact_to_points<iT,eXl::Vector2<Real> > iterBegin(input_begin,input_end);
        //iterator_compact_to_points<iT,eXl::Vector2<Real> > iterEnd(input_end,input_end);
        //t.assign(input_begin,input_end);
        t.clear();
        for (; input_begin != input_end; ++input_begin)
        {
          eXl::Vector2<Real> temp(boost::polygon::point_traits<typename iT::value_type>::get(*input_begin, boost::polygon::HORIZONTAL),
                                  boost::polygon::point_traits<typename iT::value_type>::get(*input_begin, boost::polygon::VERTICAL)) ;
          t.push_back(temp);
        }
        return t;
      }

    };

    template <typename Real,typename enable>
    struct polygon_with_holes_traits<eXl::Polygon<Real>,enable> {
         typedef typename eXl::Polygon<Real>::PtLists::const_iterator iterator_holes_type;
         typedef typename eXl::Polygon<Real>::PtList hole_type;
         static inline iterator_holes_type begin_holes(const eXl::Polygon<Real>& t) {
              return t.Holes().begin();
         }
         static inline iterator_holes_type end_holes(const eXl::Polygon<Real>& t) {
              return t.Holes().end();
         }
         static inline Real size_holes(const eXl::Polygon<Real>& t) {
              return t.Holes().size();
         }
    };

    template <typename Real, typename enable>
    struct polygon_with_holes_mutable_traits<eXl::Polygon<Real>,enable> {
         template <typename iT>
         static inline eXl::Polygon<Real>& set_holes(eXl::Polygon<Real>& t, iT inputBegin, iT inputEnd) {

              for(;inputBegin != inputEnd;++inputBegin)
              {
                typename eXl::Polygon<Real>::PtList hole;
                boost::polygon::assign(hole,*inputBegin);
                t.HolesRW().push_back(typename eXl::Polygon<Real>::PtList());
                t.HolesRW().back().swap(hole);
              }
              t.ForceCClockwiseHoles();
              return t;
         }
    };

    template <typename T>
    struct is_polygon_set_type<eXl::Vector<T> > {
      typedef typename gtl_or<
        typename is_polygonal_concept<typename geometry_concept<eXl::Vector<T> >::type>::type,
        typename is_polygonal_concept<typename geometry_concept<typename eXl::Vector<T>::value_type>::type>::type>::type type;
    };

    template <typename T>
    struct is_mutable_polygon_set_type<eXl::Vector<T> > {
      typedef typename gtl_or<
        typename gtl_same_type<polygon_set_concept, typename geometry_concept<eXl::Vector<T> >::type>::type,
        typename is_polygonal_concept<typename geometry_concept<typename eXl::Vector<T>::value_type>::type>::type>::type type;
    };

    template <typename T>
    struct polygon_set_mutable_traits<eXl::Vector<T> > {
      template <typename input_iterator_type>
      static inline void set(eXl::Vector<T>& polygon_set, input_iterator_type input_begin, input_iterator_type input_end) {
        polygon_set.clear();
        size_t num_ele = std::distance(input_begin, input_end);
        polygon_set.reserve(num_ele);
        polygon_set_data<typename polygon_set_traits<std::list<T> >::coordinate_type> ps;
        ps.reserve(num_ele);
        ps.insert(input_begin, input_end);
        ps.get(polygon_set);
      }
    };

  }
}

namespace eXl
{
  class Serializer;

  template <typename PolygonType>
  Err Stream_T(PolygonType& iPoly, Serializer iStreamer);

  template <typename Real>
  struct PreciseVector
  {
    typedef eXl::Vector2<typename eXl::PreciseType<Real>::type > type;
  };

  template <typename Real>
  inline typename PreciseVector<Real>::type ToPrecise(Vector2<Real> const& iVec)
  {
    return typename PreciseVector<Real>::type(Math<Real>::ToPrecise(iVec.X()), Math<Real>::ToPrecise(iVec.Y()));
  }

  template <typename Real>
  inline bool VectorNotNull(Vector2<Real> const& iVec)
  {
    return iVec.Length() > Math<Real>::ZERO_TOLERANCE;
  }

  template <>
  inline bool VectorNotNull<int>(Vector2i const& iVec)
  {
    return iVec != Vector2i::ZERO;
  }

  template <typename Real>
  Polygon<Real>::Polygon(){}

  template <typename Real>
  Polygon<Real>::~Polygon(){}

  template <typename Real>
  void Polygon<Real>::Translate(Vector2<Real> const& iTrans)
  {
    for (unsigned int i = 0; i < m_Ext.size(); ++i)
    {
      m_Ext[i] += iTrans;
    }

    for (unsigned int i = 0; i < m_Holes.size(); ++i)
    {
      for (unsigned int j = 0; j < m_Holes[i].size(); ++j)
      {
        m_Holes[i][j] += iTrans;
      }
    }
    //UpdateAABB();
    m_AABB.m_Data[0] += iTrans;
    m_AABB.m_Data[1] += iTrans;
  }

  template <typename Real>
  void Polygon<Real>::Scale(Real iNum, Real iDenom)
  {
    //Polygone valide si edge confondues ??
    for (unsigned int i = 0; i < m_Ext.size(); ++i)
    {
      m_Ext[i] = (m_Ext[i] * iNum) / iDenom;
    }

    for (unsigned int i = 0; i < m_Holes.size(); ++i)
    {
      for (unsigned int j = 0; j < m_Holes[i].size(); ++j)
      {
        m_Holes[i][j] = (m_Holes[i][j] * iNum) / iDenom;
      }
    }
    UpdateAABB();
  }

  template <typename Real>
  void Polygon<Real>::Rotate(typename eXl::PreciseType<Real>::type iAngle)
  {
    if(Math<typename eXl::PreciseType<Real>::type>::Abs(iAngle) > Math<typename eXl::PreciseType<Real>::type>::ZERO_TOLERANCE)
    {
      auto xAxis = Vector2<typename eXl::PreciseType<Real>::type>(Math<typename eXl::PreciseType<Real>::type>::Cos(iAngle), Math<typename eXl::PreciseType<Real>::type>::Sin(iAngle));
      auto yAxis = Vector2<typename eXl::PreciseType<Real>::type>(-xAxis.Y(), xAxis.X());

      for (unsigned int i = 0; i < m_Ext.size(); ++i)
      {
        auto vec = xAxis * m_Ext[i].X() + yAxis * m_Ext[i].Y();
        m_Ext[i] = Vector2<Real>(Math<Real>::Round(vec.X()), Math<Real>::Round(vec.Y()));
      }

      for (unsigned int i = 0; i < m_Holes.size(); ++i)
      {
        for (unsigned int j = 0; j < m_Holes[i].size(); ++j)
        {
          auto vec = xAxis * m_Holes[i][j].X() + yAxis * m_Holes[i][j].Y();
          m_Holes[i][j] = Vector2<Real>(Math<Real>::Round(vec.X()), Math<Real>::Round(vec.Y()));
        }
      }

      UpdateAABB();
    }
  }

  template <typename Real>
  Polygon<Real>::Polygon(Vector<Vector2<Real> > const& iPoints)
  {
    if(iPoints.size() > 2)
    {
      m_Ext = iPoints;
      if(m_Ext.front() != m_Ext.back())
        m_Ext.push_back(m_Ext.front());
      ForceClockwise();
    }
    UpdateAABB();
  }

  template <typename Real>
  void Polygon<Real>::UpdateAABB()
  {
    boost::polygon::extents(m_AABB, *this);
  }

  template <typename Real>
  Polygon<Real>::Polygon(AABB2D<Real> const& iBox)
  {
    m_Ext.clear();

    m_Ext.push_back(iBox.m_Data[0]);
    m_Ext.push_back(Vector2<Real>(iBox.m_Data[0].X(), iBox.m_Data[1].Y()));
    m_Ext.push_back(iBox.m_Data[1]);
    m_Ext.push_back(Vector2<Real>(iBox.m_Data[1].X(), iBox.m_Data[0].Y()));
    m_Ext.push_back(iBox.m_Data[0]);

    ForceClockwise();

    m_AABB = iBox;
  }

  template <typename Real>
  void Polygon<Real>::Swap(Polygon& iOther)
  {
    AABB2D<Real> temp = iOther.m_AABB;
    iOther.m_Ext.swap(m_Ext);
    iOther.m_Holes.swap(m_Holes);
    iOther.m_AABB = m_AABB;
    m_AABB = temp;
  }

  template <typename Real>
  Real Polygon<Real>::Perimeter()const
  {
    return boost::polygon::perimeter(*this);
  }

  template <typename Real>
  Real Polygon<Real>::Area() const
  {
    return boost::polygon::area(*this);
  }

  template <typename Real>
  void Polygon<Real>::ForceClockwise()
  {
    if(boost::polygon::winding(*this) != boost::polygon::CLOCKWISE)
    {
      std::reverse(m_Ext.begin(),m_Ext.end());
    }
  }

  template <typename Real>
  void Polygon<Real>::ForceCClockwiseHoles()
  {
    for(unsigned int i = 0;i<m_Holes.size();++i)
    {
      if(boost::polygon::winding(m_Holes[i]) != boost::polygon::COUNTERCLOCKWISE)
      {
        std::reverse(m_Holes[i].begin(),m_Holes[i].end());
      }
    }
  }

  template <typename Real>
  void Polygon<Real>::InternalSwap(Polygon &iOther)const
  {
    m_Ext.swap(iOther.m_Ext);
    m_Holes.swap(iOther.m_Holes);
  }

  template <typename Real>
  void Polygon<Real>::Clear()
  {
    m_Ext.clear();
    m_Holes.clear();
  }

  template <typename Real>
  bool Polygon<Real>::Empty() const
  {
    return m_Ext.empty();
  }

  template <typename Real>
  bool Polygon<Real>::ContainsPoint(Vector2<Real> const& iPoint) const
  {
    boost::polygon::point_data<Real> point(iPoint.X(), iPoint.Y());
    return boost::polygon::contains(*this, point);
  }

  //template <typename Real>
  //void Polygon<Real>::GetBoxes(Vector<AABB2D<Real> >& oBoxes)const
  //{
  //  Vector<Polygon> meSet;
  //  meSet.push_back(Polygon());
  //  InternalSwap(meSet[0]);
  //
  //  boost::polygon::get_rectangles(oBoxes,meSet);
  //
  //  InternalSwap(meSet[0]);
  //}

  template <typename Real>
  void Polygon<Real>::Merge(Vector<Polygon>& ioPoly)
  {
    Vector<Polygon> res;
    if(ioPoly.size() > 0)
    {
      res.push_back(ioPoly.front());
      for(unsigned int i = 1 ; i<ioPoly.size();++i)
      {
        boost::polygon::operators::operator|=(res,ioPoly[i]);
      }
    }
    ioPoly.swap(res);
  }

  template <typename Real>
  void Polygon<Real>::Union(Polygon const& iOther, Polygon& oPoly)const
  {
    oPoly.Clear();

    Vector<Polygon> meSet;
    Vector<Polygon> otherSet;

    meSet.push_back(Polygon());
    InternalSwap(meSet[0]);

    otherSet.push_back(Polygon());
    iOther.InternalSwap(otherSet[0]);

    Vector<Polygon> result;
    boost::polygon::assign(result,boost::polygon::operators::operator+(meSet,otherSet));

    InternalSwap(meSet[0]);
    iOther.InternalSwap(otherSet[0]);
    if (result.size() == 1)
    {
      oPoly.Swap(result[0]);
    }
  }

  template <typename Real>
  void Polygon<Real>::Difference(Polygon const& iOther, Vector<Polygon>& oPoly)const
  {
    oPoly.clear();

    Vector<Polygon> meSet;
    Vector<Polygon> otherSet;

    meSet.push_back(Polygon());
    InternalSwap(meSet[0]);

    otherSet.push_back(Polygon());
    iOther.InternalSwap(otherSet[0]);

    boost::polygon::assign(oPoly,boost::polygon::operators::operator-(meSet,otherSet));

    InternalSwap(meSet[0]);
    iOther.InternalSwap(otherSet[0]);
  }

  template <typename Real>
  void Polygon<Real>::Intersection(Polygon const& iOther, Vector<Polygon>& oPoly)const
  {
    Vector<Polygon> meSet;
    Vector<Polygon> otherSet;

    meSet.push_back(Polygon());
    InternalSwap(meSet[0]);

    otherSet.push_back(Polygon());
    iOther.InternalSwap(otherSet[0]);

    boost::polygon::assign(oPoly,boost::polygon::operators::operator&(meSet,otherSet));

    InternalSwap(meSet[0]);
    iOther.InternalSwap(otherSet[0]);

  }

  template <typename Real>
  Vector<Polygon<Real> > Polygon<Real>::GetTrapezoids() const
  {
    Vector< Polygon<Real> > result;
    Vector<Polygon> meSet;

    meSet.push_back(Polygon());
    InternalSwap(meSet[0]);

    boost::polygon::get_trapezoids(result, meSet, boost::polygon::VERTICAL);

    InternalSwap(meSet[0]);

    return result;
  }

  template <typename Real>
  void Polygon<Real>::CutConvex(Vector2<Real> const& iOrig, Vector2<Real> const& iDir, Polygon& oLeftPoly, Polygon& oRightPoly) const
  {
    oLeftPoly.Clear();
    oRightPoly.Clear();

    auto lowPt = ToPrecise(m_AABB.m_Data[0]);
    auto highPt = ToPrecise(m_AABB.m_Data[1]);

    typename PreciseVector<Real>::type boxSeg[5] =
    {lowPt,  typename PreciseVector<Real>::type(lowPt.X(), highPt.Y()),
     highPt, typename PreciseVector<Real>::type(highPt.X(), lowPt.Y()),
     lowPt
    };

    unsigned int curPoint = 0;
    Vector2<Real> cutPoints[2];

    auto dOrig = ToPrecise(iOrig);
    auto dDir  = ToPrecise(iDir);

    for (unsigned int i = 0; i < 4 && curPoint < 2; ++i)
    {
      typename PreciseVector<Real>::type res;
      unsigned int result = Segment<typename PreciseType<Real>::type>::Intersect(dOrig, dOrig + dDir, boxSeg[i], boxSeg[i + 1], res);
      unsigned int mask = Segment<typename PreciseType<Real>::type>::PointFound | Segment<typename PreciseType<Real>::type>::PointOnSegment2;
      if((result & mask) == mask)
      {
        Vector2<Real> potPoint(Math<Real>::Round(res.X()), Math<Real>::Round(res.Y()));
        if (curPoint == 0 || VectorNotNull(cutPoints[0] - potPoint))
        {
          //Snap to bbox seg if integer.
          cutPoints[curPoint] = potPoint;
          ++curPoint;
        }
      }
    }

    if (curPoint == 2)
    {
      Vector2<Real> cutVect = cutPoints[0] - cutPoints[1];
      if (VectorNotNull(cutVect))
      {
        int origPoint = cutVect.Dot(iDir) > 0 ? 0 : 1;

        typename Segment<Real>::SortByAngle sortMeth(cutPoints[origPoint]);
        std::set<Vector2<Real>, typename Segment<Real>::SortByAngle > sortedPoints(sortMeth);
        sortedPoints.insert(cutPoints[1 - origPoint]);
        sortedPoints.insert(m_AABB.m_Data[0]);
        sortedPoints.insert(m_AABB.m_Data[1]);
        sortedPoints.insert(Vector2<Real>(m_AABB.m_Data[0].X(), m_AABB.m_Data[1].Y()));
        sortedPoints.insert(Vector2<Real>(m_AABB.m_Data[1].X(), m_AABB.m_Data[0].Y()));

        Vector<Vector2<Real> > cutPoly;

        cutPoly.push_back(cutPoints[origPoint]);

        auto iter = sortedPoints.begin();
        while (*iter != cutPoints[1 - origPoint] && iter != sortedPoints.end())
        {
          cutPoly.push_back(*iter);
          ++iter;
        }
        //eXl_ASSERT(iter != sortedPoints.end(), "Error");
        //eXl_ASSERT(cutPoly.size() > 3, "Error");
        if (iter != sortedPoints.end() && cutPoly.size() > 0)
        {
          cutPoly.push_back(*iter);
          Vector<Polygon<Real> > oPoly1;
          Polygon<Real> leftPoly(cutPoly);
          Intersection(leftPoly, oPoly1);
          //eXl_ASSERT(oPoly1.size() == 0 || oPoly1.size() == 1, "Error");
          if (oPoly1.size() == 1)
          {
            Vector<Polygon<Real> > oPoly2;
            Difference(oPoly1[0], oPoly2);
            //eXl_ASSERT(oPoly2.size() == 0 || oPoly2.size() == 1, "Error");
            if (oPoly2.size() < 2)
            {
              if (oPoly2.size() == 1)
              {
                oRightPoly.Swap(oPoly2[0]);
              }
              oLeftPoly.Swap(oPoly1[0]);
            }
          }
          else if (oPoly1.size() == 0)
          {
            oRightPoly = *this;
          }
        }
      }
    }
  }

  template <typename Real>
  bool Polygon<Real>::IsConvex() const
  {
    if(!m_Holes.empty())
      return false;

    if(m_Ext.size() <= 2)
      return true;

    Vector2<Real> initPoint = m_Ext[0];
    Vector2<Real> lastPoint1 = initPoint;
    Vector2<Real> lastPoint2 = m_Ext[1];
    Vector2<Real> curPoint = m_Ext[2];

    Real initSign = Segment<Real>::IsLeft(lastPoint1, lastPoint2, curPoint);
    initSign = initSign > 0 ? 1 : -1;
    lastPoint1 = lastPoint2;
    lastPoint2 = curPoint;
    //+1 to loop on the last segment.
    for (unsigned int i = 3; i < m_Ext.size() + 1; ++i)
    {
      if (m_Ext.size() == i)
      {
        curPoint = m_Ext[0];
        if(Segment<Real>::IsLeft(lastPoint1, lastPoint2, curPoint) * initSign < 0)
          return false;
      }
      else
      {
        curPoint = m_Ext[i];
        if (curPoint == initPoint)
        {
          if(Segment<Real>::IsLeft(lastPoint1, lastPoint2, curPoint) * initSign < 0)
            return false;
          break;
        }
        if(Segment<Real>::IsLeft(lastPoint1, lastPoint2, curPoint) * initSign < 0)
          return false;

      }
      lastPoint1 = lastPoint2;
      lastPoint2 = curPoint;
    }
    return true;
  }

  template <class Real>
  struct SortPoints
  {
    bool operator()(Vector2<Real> const& iPt1, Vector2<Real> const& iPt2)
    {
      if(iPt1.X() == iPt2.X())
        return iPt1.Y() < iPt2.Y();
      return iPt1.X() < iPt2.X();
    }
  };

  template <class Real>
  void Polygon<Real>::ConvexHull(Polygon<Real>& oHull) const
  {
    return ConvexHull(m_Ext, oHull);
  }

  template <class Real>
  void Polygon<Real>::ConvexHull(Vector<Vector2<Real> >const& iPoints, Polygon<Real>& oHull)
  {
    oHull.Clear();
    if(!iPoints.empty())
    {
      Vector<Vector2<Real> > sorted = iPoints;

      std::sort(sorted.begin(), sorted.end(), SortPoints<Real>());

      int n = sorted.size();
      int k = 0;
	    Vector<Vector2<Real> > H(2*n);

	    for (int i = 0; i < n; ++i)
      {
		    while (k >= 2 && Segment<Real>::IsLeft(H[k-2], H[k-1], sorted[i]) <= 0)
          k--;

		    H[k++] = sorted[i];
	    }

	    // Build upper hull
	    for (int i = n-2, t = k+1; i >= 0; i--)
      {
		    while (k >= t && Segment<Real>::IsLeft(H[k-2], H[k-1], sorted[i]) <= 0)
          k--;

		    H[k++] = sorted[i];
	    }

	    H.resize(k);

      oHull = Polygon<Real>(H);
    }
  }

  template <class Real>
  void Polygon<Real>::RemoveUselessPoints()
  {
    _RemoveUselessPoints(m_Ext);
    for(unsigned int i = 0; i<m_Holes.size(); ++i)
    {
      _RemoveUselessPoints(m_Holes[i]);
    }
  }

  template <class Real>
  void Polygon<Real>::_RemoveUselessPoints(Vector<Vector2<Real> >& ioPoints)
  {
    Vector2<Real> prevPt1 = ioPoints.back();
    Vector2<Real> prevPt2 = ioPoints.back();
    for (unsigned int i = 0; i<ioPoints.size(); ++i)
    {
      Vector2<Real> curPt =  ioPoints[i];
      if (curPt != prevPt1 && prevPt1 != prevPt2)
      {
        typename PreciseVector<Real>::type dir1 = ToPrecise<Real>(prevPt1 - prevPt2);
        typename PreciseVector<Real>::type dir2 = ToPrecise<Real>(curPt - prevPt2);
        typename PreciseType<Real>::type len1 = dir1.Normalize();
        typename PreciseType<Real>::type len2 = dir2.Normalize();
        if (dir1.Dot(dir2) > (1 - eXl::Math<typename PreciseType<Real>::type>::EPSILON) && len1 < len2)
        {
          ioPoints[i - 1] = curPt;
          ioPoints.erase(ioPoints.begin() + i);
          prevPt1 = curPt;
          --i;
          continue;
        }
      }
      prevPt2 = prevPt1;
      prevPt1 = curPt;
    }
  }

  template <typename Real>
  void Polygon<Real>::Shrink(Real iFactor,Vector<Polygon>& oOut)const
  {
    Vector<Polygon> meSet;
    meSet.push_back(*this);

    boost::polygon::shrink(meSet,iFactor);

    if(meSet.size()>0)
      meSet.swap(oOut);
    else
      oOut.clear();
  }

  template <typename Real>
  void Polygon<Real>::Bloat(Real iFactor,Polygon& oOut)const
  {
    Vector<Polygon> meSet;
    meSet.push_back(*this);

    boost::polygon::bloat(meSet,iFactor);

    if(meSet.size()>0)
      meSet[0].Swap(oOut);
    else
      oOut.Clear();
  }

  template <typename Real>
  void Polygon<Real>::RemoveTiny(Real iFactor,Vector<Polygon>& oOut,Vector<Polygon>& oRemoved)const
  {
    oOut.clear();
    oRemoved.clear();
    Vector<Polygon> meSet;
    meSet.push_back(*this);

    /*meSet = */boost::polygon::shrink(meSet,iFactor);

    /*meSet = */boost::polygon::bloat(meSet,iFactor);

    //meSet = boost::polygon::keep(meSet,0,ULLONG_MAX,iFactor,ULLONG_MAX,iFactor,ULLONG_MAX);

    if(meSet.empty())
    {
      oRemoved.push_back(*this);
    }
    else
    {

      Vector<Polygon> good;
      good.push_back(Polygon());
      InternalSwap(good.back());

      boost::polygon::assign(oRemoved,boost::polygon::operators::operator-(good,meSet));
      InternalSwap(good.back());

      //meSet.back().Swap(*this);
      oOut.swap(meSet);
    }
  }
}

#include <core/stream/serializer.hpp>

namespace eXl
{

  template <typename PolygonType>
  Err Stream_T(PolygonType& iPoly, Serializer iStreamer)
  {
    iStreamer.BeginStruct();
    iStreamer.PushKey("Border");
    iStreamer &= iPoly.Border();
    iStreamer.PopKey();
    iStreamer.PushKey("Holes");
    iStreamer &= iPoly.Holes();
    iStreamer.PopKey();
    iStreamer.EndStruct();

    RETURN_SUCCESS;
  }

  template <typename Real>
  Err Polygon<Real>::Stream(Streamer& iStreamer) const
  {
    Serializer serializer(iStreamer);
    return Stream_T(*this, serializer);
  }

  template <typename Real>
  Err Polygon<Real>::Unstream(Unstreamer& iStreamer)
  {
    Serializer serializer(iStreamer);
    return Stream_T(*this, serializer);
  }
}


#ifndef _POSE_ESTIMATION_BODY_STATE_MEASUREMENT_HPP
#define _POSE_ESTIMATION_BODY_STATE_MEASUREMENT_HPP

#include <base/Time.hpp>
#include <base/samples/RigidBodyState.hpp>
#include <Eigen/Core>
#include "PoseWithVelocity.hpp"

namespace pose_estimation
{

struct BodyStateMeasurement
{
    static void fromRigidBodyState(const base::samples::RigidBodyState &body_state, PoseWithVelocity &filter_state, PoseWithVelocityCovariance &filter_state_cov)
    {
        filter_state.position = TranslationType(body_state.position);
        filter_state.orientation = RotationType(MTK::SO3<double>(body_state.orientation));
        filter_state.velocity = VelocityType(body_state.velocity);
        filter_state.angular_velocity = VelocityType(body_state.angular_velocity);

        filter_state_cov.setZero();
        filter_state_cov.block(0, 0, 3, 3) = body_state.cov_position;
        filter_state_cov.block(3, 3, 3, 3) = body_state.cov_orientation;
        filter_state_cov.block(6, 6, 3, 3) = body_state.cov_velocity;
        filter_state_cov.block(9, 9, 3, 3) = body_state.cov_angular_velocity;
    }

    static void toRigidBodyState(const PoseWithVelocity &filter_state, const PoseWithVelocityCovariance &filter_state_cov, base::samples::RigidBodyState &body_state)
    {
        body_state.position = filter_state.position;
        body_state.orientation = filter_state.orientation;
        body_state.velocity = body_state.orientation * filter_state.velocity;
        body_state.angular_velocity = filter_state.angular_velocity;

        body_state.cov_position = filter_state_cov.block(0, 0, 3, 3);
        body_state.cov_orientation = filter_state_cov.block(3, 3, 3, 3);
        body_state.cov_velocity = filter_state_cov.block(6, 6, 3, 3);
        body_state.cov_angular_velocity = filter_state_cov.block(9, 9, 3, 3);
    }
};

}

#endif


//=========================================================================
// This file is part of HMMlib.
//
// HMMlib is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as
// published by the Free Software Foundation, either version 3 of
// the License, or (at your option) any later version.
//
// HMMlib is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with HMMlib. If not, see
// <http://www.gnu.org/licenses/>.
//
// Copyright (C) 2010  Bioinformatics Research Centre, Aarhus University.
// Author: Andreas Sand (asand@birc.au.dk)
//=========================================================================

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN hmm_vector_test

#include <boost/test/included/unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/mpl/list.hpp>
using namespace boost::unit_test;

#include "HMMlib/hmm_vector.hpp"
using namespace hmmlib;

#include <pmmintrin.h>
#include <cmath>

BOOST_AUTO_TEST_SUITE( hmm_vector_test )

typedef boost::mpl::list<HMMVector<double, double>,
			 HMMVector<double, __m128d>,
			 HMMVector<float, float>,
			 HMMVector<float, __m128> > test_types;

BOOST_AUTO_TEST_CASE_TEMPLATE(construction_test, Vector, test_types) {
  Vector vector(5);

  BOOST_CHECK_EQUAL(vector.get_no_rows(), 1);
  BOOST_CHECK_EQUAL(vector.get_no_columns(), 5);

  for(int i = 0; i < 5; ++i) {
    BOOST_CHECK_CLOSE(vector(i), 0.0, 0.001);
  }

  Vector vector2(5, 3.0);

  for(int i = 0; i < 5; ++i) {
    BOOST_CHECK_CLOSE(vector2(i), 3.0, 0.001);
  }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(size_test, Vector, test_types) {
  Vector vector1(5);

  BOOST_CHECK_EQUAL(vector1.get_size(), 5);

  Vector vector2(127);

  BOOST_CHECK_EQUAL(vector2.get_size(), 127);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(log_non_sse_test, Vector, test_types) {
  Vector vector(6);
  for(int i = 0; i < vector.get_size(); ++i)
      vector(i) = i+1;

  Vector log_vector(6);
  vector.log(log_vector);

  for(int i = 0; i < vector.get_size(); ++i)
    BOOST_CHECK_CLOSE(log_vector(i), std::log(i+1), 0.001);
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/graph/johnson_all_pairs_shortest.hpp>


#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/core/utility.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <g2o/core/robust_kernel_impl.h>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/plot.hpp>

#include "data.hpp"
#include "helper.hpp"
#include "stereo_processor/g2o_edges/klt_edge.hpp"

class KLT{
public:
	void compute(const cv::Mat& prev_img, const cv::Mat& next_img, const std::vector<cv::Point3f>& prev_fts_d, std::vector<cv::Point2f>& next_fts,
								const cv::Mat& K, const cv::Mat& R, const cv::Mat& t,
								std::vector<uchar>& status, std::vector<float>& err, int maxLevel=1);

private:
	void compute_pymd(const cv::Mat& prev_img, const cv::Mat& next_img, const std::vector<cv::Point3f>& prev_fts_d, std::vector<cv::Point2f>& next_fts,
									const cv::Mat& K, const cv::Mat& R, const cv::Mat& t,
									std::vector<uchar>& status, std::vector<float>& err);
};


/*
 * Lumi3DReWeighting.cpp
 *
 *  Created on: 5 Dec 2011
 *      Author: kreczko
 */

#include "../interface/Lumi3DReWeighting.h"

#include "TRandom1.h"
#include "TRandom2.h"
#include "TRandom3.h"
#include "TStopwatch.h"
#include "TH1.h"
#include "TH3.h"
#include "TFile.h"
#include <string>
#include <algorithm>
#include <boost/shared_ptr.hpp>
#include <boost/scoped_ptr.hpp>
#include <iostream>

namespace BAT {

Lumi3DReWeighting::Lumi3DReWeighting(std::string generatedFile, std::string dataFile,
		std::string GenHistName = "pileup", std::string DataHistName = "pileup") :
		generatedFileName_(generatedFile), //
		dataFileName_(dataFile), //
		GenHistName_(GenHistName), //
		DataHistName_(DataHistName) {
	generatedFile_ = boost::shared_ptr<TFile>(new TFile(generatedFileName_.c_str())); //MC distribution
	dataFile_ = boost::shared_ptr<TFile>(new TFile(dataFileName_.c_str())); //Data distribution

	boost::scoped_ptr<TH1> Data_temp((static_cast<TH1*>(dataFile_->Get(DataHistName_.c_str())->Clone())));

	boost::scoped_ptr<TH1> MC_temp((static_cast<TH1*>(generatedFile_->Get(GenHistName_.c_str())->Clone())));

	MC_distr_ = boost::shared_ptr<TH1>((static_cast<TH1*>(generatedFile_->Get(GenHistName_.c_str())->Clone())));
	Data_distr_ = boost::shared_ptr<TH1>((static_cast<TH1*>(dataFile_->Get(DataHistName_.c_str())->Clone())));

	// MC * data/MC = data, so the weights are data/MC:

	// normalize both histograms first

	Data_distr_->Scale(1.0 / Data_distr_->Integral());
	MC_distr_->Scale(1.0 / MC_distr_->Integral());

}

Lumi3DReWeighting::Lumi3DReWeighting(std::vector<float> MC_distr, std::vector<float> Lumi_distr) {
	// no histograms for input: use vectors

	// now, make histograms out of them:

	Int_t NMCBins = MC_distr.size();

	MC_distr_ = boost::shared_ptr<TH1>(new TH1F("MC_distr", "MC dist", NMCBins, 0., float(NMCBins)));

	Int_t NDBins = Lumi_distr.size();

	Data_distr_ = boost::shared_ptr<TH1>(new TH1F("Data_distr", "Data dist", NDBins, 0., float(NDBins)));

	for (int ibin = 1; ibin < NMCBins + 1; ++ibin) {
		MC_distr_->SetBinContent(ibin, MC_distr[ibin - 1]);
	}

	for (int ibin = 1; ibin < NDBins + 1; ++ibin) {
		Data_distr_->SetBinContent(ibin, Lumi_distr[ibin - 1]);
	}

	// check integrals, make sure things are normalized

	float deltaH = Data_distr_->Integral();
	if (fabs(1.0 - deltaH) > 0.001) { //*OOPS*...
		Data_distr_->Scale(1.0 / Data_distr_->Integral());
	}
	float deltaMC = MC_distr_->Integral();
	if (fabs(1.0 - deltaMC) > 0.001) {
		MC_distr_->Scale(1.0 / MC_distr_->Integral());
	}

}

double Lumi3DReWeighting::weight3D(int pv1, int pv2, int pv3) {

	using std::min;

	int npm1 = min(pv1, NVERTEX - 1);
	int np0 = min(pv2, NVERTEX - 1);
	int npp1 = min(pv3, NVERTEX - 1);

	return Weight3D_[npm1][np0][npp1];

}

/**
 * @PARAM ScaleFactor: Scale factor is used to shift target distribution (i.e. luminosity scale)  1. = no shift
 */
void Lumi3DReWeighting::weight3D_init(float ScaleFactor) {

	//create histogram to write output weights, save pain of generating them again...

	boost::scoped_ptr<TH3D> WHist(
			new TH3D("WHist", "3D weights", NVERTEX, -.5, 49.5, NVERTEX, -.5, 49.5, NVERTEX, -.5, 49.5));
	boost::scoped_ptr<TH3D> DHist(
			new TH3D("DHist", "3D weights", NVERTEX, -.5, 49.5, NVERTEX, -.5, 49.5, NVERTEX, -.5, 49.5));
	boost::scoped_ptr<TH3D> MHist(
			new TH3D("MHist", "3D weights", NVERTEX, -.5, 49.5, NVERTEX, -.5, 49.5, NVERTEX, -.5, 49.5));

	using std::min;

	if (MC_distr_->GetEntries() == 0) {
		std::cout << " MC and Data distributions are not initialised! You must call the Lumi3DReWeighting constructor. "
				<< std::endl;
	}

	// arrays for storing number of interactions

	double MC_ints[NVERTEX][NVERTEX][NVERTEX];
	double Data_ints[NVERTEX][NVERTEX][NVERTEX];

	for (int i = 0; i < NVERTEX; i++) {
		for (int j = 0; j < NVERTEX; j++) {
			for (int k = 0; k < NVERTEX; k++) {
				MC_ints[i][j][k] = 0.;
				Data_ints[i][j][k] = 0.;
			}
		}
	}

	double factorial[NVERTEX];
	double PowerSer[NVERTEX];
	double base = 1.;

	factorial[0] = 1.;
	PowerSer[0] = 1.;

	for (int i = 1; i < NVERTEX + 1; ++i) {
		base = base * float(i);
		factorial[i] = base;
	}

	double x;
	double xweight;
	double probi, probj, probk;
	double Expval, mean;
	int xi;

	// Get entries for Data, MC, fill arrays:

	int NMCbin = MC_distr_->GetNbinsX();

	for (int jbin = 1; jbin < NMCbin + 1; jbin++) {
		x = MC_distr_->GetBinCenter(jbin);
		xweight = MC_distr_->GetBinContent(jbin); //use as weight for matrix

		//for Summer 11, we have this int feature:
		xi = int(x);

		// Generate Poisson distribution for each value of the mean

		mean = double(xi);

		if (mean < 0.) {
//      throw cms::Exception("BadInputValue") << " Your histogram generates MC luminosity values less than zero!"
//						<< " Please Check.  Terminating." << std::endl;
		}

		if (mean == 0.) {
			Expval = 1.;
		} else {
			Expval = exp(-1. * mean);
		}

		base = 1.;

		for (int i = 1; i < NVERTEX; ++i) {
			base = base * mean;
			PowerSer[i] = base; // PowerSer is mean^i
		}

		// compute poisson probability for each Nvtx in weight matrix

		for (int i = 0; i < NVERTEX; i++) {
			probi = PowerSer[i] / factorial[i] * Expval;
			for (int j = 0; j < NVERTEX; j++) {
				probj = PowerSer[j] / factorial[j] * Expval;
				for (int k = 0; k < NVERTEX; k++) {
					probk = PowerSer[k] / factorial[k] * Expval;
					// joint probability is product of event weights multiplied by weight of input distribution bin
					MC_ints[i][j][k] = MC_ints[i][j][k] + probi * probj * probk * xweight;
				}
			}
		}

	}

	int NDatabin = Data_distr_->GetNbinsX();

	for (int jbin = 1; jbin < NDatabin + 1; jbin++) {
		mean = (Data_distr_->GetBinCenter(jbin)) * ScaleFactor;
		xweight = Data_distr_->GetBinContent(jbin);

		// Generate poisson distribution for each value of the mean

		if (mean < 0.) {
//      throw cms::Exception("BadInputValue") << " Your histogram generates Data luminosity values less than zero!"
//						<< " Please Check.  Terminating." << std::endl;
		}

		if (mean == 0.) {
			Expval = 1.;
		} else {
			Expval = exp(-1. * mean);
		}

		base = 1.;

		for (int i = 1; i < NVERTEX; ++i) {
			base = base * mean;
			PowerSer[i] = base;
		}

		// compute poisson probability for each Nvtx in weight matrix

		for (int i = 0; i < NVERTEX; i++) {
			probi = PowerSer[i] / factorial[i] * Expval;
			for (int j = 0; j < NVERTEX; j++) {
				probj = PowerSer[j] / factorial[j] * Expval;
				for (int k = 0; k < NVERTEX; k++) {
					probk = PowerSer[k] / factorial[k] * Expval;
					// joint probability is product of event weights multiplied by weight of input distribution bin
					Data_ints[i][j][k] = Data_ints[i][j][k] + probi * probj * probk * xweight;
				}
			}
		}

	}

	for (int i = 0; i < NVERTEX; i++) {
		//if(i<5) std::cout << "i = " << i << std::endl;
		for (int j = 0; j < NVERTEX; j++) {
			for (int k = 0; k < NVERTEX; k++) {
				if ((MC_ints[i][j][k]) > 0.) {
					Weight3D_[i][j][k] = Data_ints[i][j][k] / MC_ints[i][j][k];
				} else {
					Weight3D_[i][j][k] = 0.;
				}
				WHist->SetBinContent(i + 1, j + 1, k + 1, Weight3D_[i][j][k]);
				DHist->SetBinContent(i + 1, j + 1, k + 1, Data_ints[i][j][k]);
				MHist->SetBinContent(i + 1, j + 1, k + 1, MC_ints[i][j][k]);
				//	if(i<5 && j<5 && k<5) std::cout << Weight3D_[i][j][k] << " " ;
			}
			//      if(i<5 && j<5) std::cout << std::endl;
		}
	}

	std::cout << " 3D Weight Matrix initialized! " << std::endl;
	std::cout << " Writing weights to file Weight3D.root for re-use...  " << std::endl;

	boost::scoped_ptr<TFile> outfile(new TFile("Weight3D.root", "RECREATE"));
	outfile->cd();
	WHist->Write();
	MHist->Write();
	DHist->Write();
	outfile->Write();
	outfile->Close();
//	outfile->Delete();

	return;

}

void Lumi3DReWeighting::weight3D_init(std::string WeightFileName) {

	boost::scoped_ptr<TFile> infile(new TFile(WeightFileName.c_str()));
	boost::scoped_ptr<TH3D> WHist((TH3D*) infile->Get("WHist"));

	// Check if the histogram exists
	if (!WHist) {
//    throw cms::Exception("HistogramNotFound") << " Could not find the histogram WHist in the file "
//					      << "in the file " << WeightFileName << "." << std::endl;
	}

	for (int i = 0; i < NVERTEX; i++) {
		for (int j = 0; j < NVERTEX; j++) {
			for (int k = 0; k < NVERTEX; k++) {
				Weight3D_[i][j][k] = WHist->GetBinContent(i + 1, j + 1, k + 1);
			}
		}
	}

	std::cout << " 3D Weight Matrix initialized! " << std::endl;

	return;

}

void Lumi3DReWeighting::weight3D_init(std::string MCWeightFileName, std::string DataWeightFileName) {

	boost::scoped_ptr<TFile> infileMC(new TFile(MCWeightFileName.c_str()));
	boost::scoped_ptr<TH3D> MHist((TH3D*) infileMC->Get("MHist"));

	// Check if the histogram exists
	if (!MHist) {
//    throw cms::Exception("HistogramNotFound") << " Could not find the histogram MHist in the file "
//					      << "in the file " << MCWeightFileName << "." << std::endl;
	}

	boost::scoped_ptr<TFile> infileD(new TFile(DataWeightFileName.c_str()));
	boost::scoped_ptr<TH3D> DHist((TH3D*) infileD->Get("DHist"));

	// Check if the histogram exists
	if (!DHist) {
//    throw cms::Exception("HistogramNotFound") << " Could not find the histogram DHist in the file "
//					      << "in the file " << DataWeightFileName << "." << std::endl;
	}

	for (int i = 0; i < NVERTEX; i++) {
		for (int j = 0; j < NVERTEX; j++) {
			for (int k = 0; k < NVERTEX; k++) {
				Weight3D_[i][j][k] = DHist->GetBinContent(i + 1, j + 1, k + 1)
						/ MHist->GetBinContent(i + 1, j + 1, k + 1);
			}
		}
	}

	std::cout << " 3D Weight Matrix initialized! " << std::endl;

	return;

}

} /* namespace BAT */


#ifndef INCLUDED_scheme_util_SimpleArray_HH
#define INCLUDED_scheme_util_SimpleArray_HH

#include "scheme/types.hh"
#include <cmath>
#include <algorithm>
#include <boost/assert.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_arithmetic.hpp>
#include <boost/static_assert.hpp>

#include <limits>

#ifdef CEREAL
// #include <boost/serialization/access.hpp>
#include <cereal/access.hpp>
#endif

namespace scheme {
namespace util {

//TODO: finish this and replace Eigen dependency in NEST
namespace impl {
	struct NoInit {};
}

///@brief minimal fixed size array with element-wise operations
///@note used this instead of Eigen Array in NEST to speed compilation by 20%-50%
template<int N, class F=double,bool init0=false>
struct SimpleArray {
	typedef SimpleArray<N,F> THIS;
	typedef F value_type;
	typedef F Scalar;
	typedef F* iterator;
	typedef F const* const_iterator;
	typedef F & reference;
	typedef F const & const_reference;
	typedef size_t difference_type;
	typedef size_t size_type;
	typedef std::numeric_limits<F> NL;
	F D[N];
	template<class A>
	SimpleArray(A const & a,typename boost::disable_if<boost::is_arithmetic<A> >::type* = 0)
		{ for(int i=0;i<N;++i) D[i] = a[i]; }
	template<int N2>
	SimpleArray(SimpleArray<N2,F> const & a){
		BOOST_STATIC_ASSERT((N2==N));
		for(int i=0;i<N;++i) D[i] = a[i];
	}
	template<int N2>
	SimpleArray(SimpleArray<N2,F> const & a, int ofst){
		for(int i=0;i<N;++i) D[i] = a[i+ofst];
	}
	// SimpleArray() { for(size_t i = 0; i < N; ++i) D[i]=0; }
	SimpleArray(){ if(init0) fill(0); }
	// explicit SimpleArray(F const* fp){ for(size_t i = 0; i < N; ++i) D[i] = fp[i]; }
	SimpleArray(F a){ fill(a); }
	SimpleArray(F a, F b                                        ){ BOOST_STATIC_ASSERT((N== 2)); D[0]=a;D[1]=b; }
	SimpleArray(F a, F b, F c                                   ){ BOOST_STATIC_ASSERT((N== 3)); D[0]=a;D[1]=b;D[2]=c; }
	SimpleArray(F a, F b, F c, F d                              ){ BOOST_STATIC_ASSERT((N== 4)); D[0]=a;D[1]=b;D[2]=c;D[3]=d; }
	SimpleArray(F a, F b, F c, F d, F e                         ){ BOOST_STATIC_ASSERT((N== 5)); D[0]=a;D[1]=b;D[2]=c;D[3]=d;D[4]=e; }
	SimpleArray(F a, F b, F c, F d, F e, F f                    ){ BOOST_STATIC_ASSERT((N== 6)); D[0]=a;D[1]=b;D[2]=c;D[3]=d;D[4]=e;D[5]=f; }
	SimpleArray(F a, F b, F c, F d, F e, F f, F g               ){ BOOST_STATIC_ASSERT((N== 7)); D[0]=a;D[1]=b;D[2]=c;D[3]=d;D[4]=e;D[5]=f;D[6]=g; }
	SimpleArray(F a, F b, F c, F d, F e, F f, F g, F h          ){ BOOST_STATIC_ASSERT((N== 8)); D[0]=a;D[1]=b;D[2]=c;D[3]=d;D[4]=e;D[5]=f;D[6]=g;D[7]=h; }
	SimpleArray(F a, F b, F c, F d, F e, F f, F g, F h, F i     ){ BOOST_STATIC_ASSERT((N== 9)); D[0]=a;D[1]=b;D[2]=c;D[3]=d;D[4]=e;D[5]=f;D[6]=g;D[7]=h;D[8]=i; }
	SimpleArray(F a, F b, F c, F d, F e, F f, F g, F h, F i, F j){ BOOST_STATIC_ASSERT((N==10)); D[0]=a;D[1]=b;D[2]=c;D[3]=d;D[4]=e;D[5]=f;D[6]=g;D[7]=h;D[8]=i;D[9]=j; }
	F       & operator[](size_t i)       { return D[i]; }
	F const & operator[](size_t i) const { return D[i]; }
	F       & at(size_t i)       { BOOST_VERIFY(i < N); return D[i]; }
	F const & at(size_t i) const { BOOST_VERIFY(i < N); return D[i]; }
	template<class OF> SimpleArray<N,OF> cast() const {
		SimpleArray<N,OF> r; for(int i=0;i<N;++i) r[i] = (*this)[i]; return r;
	}
	THIS maxCoeff() const { F r=NL::min(); for(int i=0;i<N;++i) r = std::max(r,D[i]); return r; }
	THIS minCoeff() const { F r=NL::max(); for(int i=0;i<N;++i) r = std::min(r,D[i]); return r; }
	template<class I> THIS maxCoeff(I*arg) const { F r=NL::min(); for(I i=0;i<N;++i){ *arg=r<D[i]?i:*arg; r=r<D[i]?D[i]:r; }; return r; }
	template<class I> THIS minCoeff(I*arg) const { F r=NL::max(); for(I i=0;i<N;++i){ *arg=r>D[i]?i:*arg; r=r>D[i]?D[i]:r; }; return r; }
	THIS max(F            b) const { THIS r(*this); for(int i=0;i<N;++i) r[i] = std::max(r[i],b); return r; }
	THIS min(F            b) const { THIS r(*this); for(int i=0;i<N;++i) r[i] = std::min(r[i],b); return r; }
	THIS max(THIS const & b) const { THIS r(*this); for(int i=0;i<N;++i) r[i] = std::max(r[i],b[i]); return r; }
	THIS min(THIS const & b) const { THIS r(*this); for(int i=0;i<N;++i) r[i] = std::min(r[i],b[i]); return r; }
	F prod() const { F p=1; for(int i=0;i<N;++i) p *= D[i]; return p; }
	F sum () const { F p=0; for(int i=0;i<N;++i) p += D[i]; return p; }
	F prod(size_t l) const { F p=1; for(int i = 0; i < l; ++i) p *= D[i]; return p; }
	F sum (size_t l) const { F p=0; for(int i = 0; i < l; ++i) p += D[i]; return p; }
	bool operator==(THIS const & o) const {	bool r = true; for(int i=0;i<N;++i) r &= D[i]==o.D[i]; return r; }
	bool operator!=(THIS const & o) const { return !(*this==o); }
	F squaredNorm() const { F n=0; for(int i=0;i<N;++i) n+=D[i]*D[i]; return n; }
	F norm() const { return std::sqrt(squaredNorm()); }
	void normalize(){ *this /= norm(); }
	void fill(F v){ for(int i=0;i<N;++i) D[i]=v; }
	iterator begin() { return &D[0]; }
	iterator end  () { return &D[N]; }
	const_iterator begin() const { return &D[0]; }
	const_iterator end  () const { return &D[N]; }
	bool empty() const { return false; }
	size_type size() const { return N; }
	void swap(THIS & o){ for(int i=0;i<N;++i) std::swap(D[i],o.D[i]); }
	// friend class boost::serialization::access;
	#ifdef CEREAL
	friend class cereal::access;
	#endif
    template<class Archive> void serialize(Archive & ar, const unsigned int ){
		for(size_t i = 0; i < N; ++i) ar & D[i];
	}
	THIS operator-() const { THIS r; for(size_t i = 0; i < N; ++i) r[i] = -D[i]; return r; }
	SimpleArray<N,F> sign() const {
		SimpleArray<N,F> r; for(int i=0;i<N;++i) r[i] = D[i]>0 ? 1.0 : -1.0; return r; }
	template<class F2> SimpleArray<N,F> & operator*=(F2 const & o){ for(int i=0;i<N;++i) D[i] *= o; return *this; }
	template<class F2> SimpleArray<N,F> & operator/=(F2 const & o){ for(int i=0;i<N;++i) D[i] /= o; return *this; }
	template<class F2> SimpleArray<N,F> & operator+=(F2 const & o){ for(int i=0;i<N;++i) D[i] += o; return *this; }
	template<class F2> SimpleArray<N,F> & operator-=(F2 const & o){ for(int i=0;i<N;++i) D[i] -= o; return *this; }
	SimpleArray<N,F> & operator*=(THIS const & o){ for(int i=0;i<N;++i) D[i] *= o[i]; return *this; }
	SimpleArray<N,F> & operator/=(THIS const & o){ for(int i=0;i<N;++i) D[i] /= o[i]; return *this; }
	SimpleArray<N,F> & operator+=(THIS const & o){ for(int i=0;i<N;++i) D[i] += o[i]; return *this; }
	SimpleArray<N,F> & operator-=(THIS const & o){ for(int i=0;i<N;++i) D[i] -= o[i]; return *this; }
};
template<int N, class F>
std::ostream & operator<<( std::ostream & out, SimpleArray<N,F> const & a ){
	for(int i=0;i<N;++i) out << a[i] << " ";
	return out;
}

template<int N, class F> SimpleArray<N,F> operator+( SimpleArray<N,F> const & a, SimpleArray<N,F> const & b ){
	SimpleArray<N,F> r(a); for(int i=0;i<N;++i) r[i] += b[i]; return r; }
template<int N, class F> SimpleArray<N,F> operator-( SimpleArray<N,F> const & a, SimpleArray<N,F> const & b ){
	SimpleArray<N,F> r(a); for(int i=0;i<N;++i) r[i] -= b[i]; return r; }
template<int N, class F> SimpleArray<N,F> operator*( SimpleArray<N,F> const & a, SimpleArray<N,F> const & b ){
	SimpleArray<N,F> r(a); for(int i=0;i<N;++i) r[i] *= b[i]; return r; }
template<int N, class F> SimpleArray<N,F> operator/( SimpleArray<N,F> const & a, SimpleArray<N,F> const & b ){
	SimpleArray<N,F> r(a); for(int i=0;i<N;++i) r[i] /= b[i];	return r; }
template<int N, class F, class F2> SimpleArray<N,F> operator+( SimpleArray<N,F> const & a, F2 const & b ){
	SimpleArray<N,F> r(a); for(int i=0;i<N;++i) r[i] += b; return r; }
template<int N, class F, class F2> SimpleArray<N,F> operator-( SimpleArray<N,F> const & a, F2 const & b ){
	SimpleArray<N,F> r(a); for(int i=0;i<N;++i) r[i] -= b; return r; }
template<int N, class F, class F2> SimpleArray<N,F> operator*( SimpleArray<N,F> const & a, F2 const & b ){
	SimpleArray<N,F> r(a); for(int i=0;i<N;++i) r[i] *= b; return r; }
template<int N, class F, class F2> SimpleArray<N,F> operator/( SimpleArray<N,F> const & a, F2 const & b ){
	SimpleArray<N,F> r(a); for(int i=0;i<N;++i) r[i] /= b;	return r; }
template<int N, class F, class F2> SimpleArray<N,F> operator+( F2 const & b, SimpleArray<N,F> const & a ){
	SimpleArray<N,F> r(a); for(int i=0;i<N;++i) r[i] += b; return r; }
template<int N, class F, class F2> SimpleArray<N,F> operator-( F2 const & b, SimpleArray<N,F> const & a ){
	SimpleArray<N,F> r(b); for(int i=0;i<N;++i) r[i] -= a[i]; return r; }
template<int N, class F, class F2> SimpleArray<N,F> operator*( F2 const & b, SimpleArray<N,F> const & a ){
	SimpleArray<N,F> r(a); for(int i=0;i<N;++i) r[i] *= b; return r; }
template<int N, class F, class F2> SimpleArray<N,F> operator/( F2 const & b, SimpleArray<N,F> const & a ){
	SimpleArray<N,F> r(b); for(int i=0;i<N;++i) r[i] /= a[i];	return r; }

template<int N, class F> SimpleArray<N,F> operator%( SimpleArray<N,F> const & a, SimpleArray<N,F> const & b ){
	SimpleArray<N,F> r(a); for(int i=0;i<N;++i) r[i] %= b[i]; return r; }

// this uint64_t assumption seems a little sketchy....
template<int N, class F, class F2> SimpleArray<N,uint64_t> operator<( SimpleArray<N,F> const & a, F2 const & b ){
	SimpleArray<N,uint64_t> r; for(int i=0;i<N;++i) r[i] = a[i]<b; return r; }
template<int N, class F, class F2> SimpleArray<N,uint64_t> operator<( F2 const & a, SimpleArray<N,F> const & b ){
	SimpleArray<N,uint64_t> r; for(int i=0;i<N;++i) r[i] = a<b[i]; return r; }

template<int N, class F> SimpleArray<N,uint64_t> operator<( SimpleArray<N,F> const & a, SimpleArray<N,F> const & b ){
	SimpleArray<N,uint64_t> r; for(int i=0;i<N;++i) r[i] = a[i]<b[i]; return r; }

}
}

#endif


#include <iostream>
#include <set>
#include <map>
#include <GraphMol/PeriodicTable.h>
#include <GraphMol/RDKitBase.h>
#include <GraphMol/SmilesParse/SmilesParse.h>
#include <GraphMol/SmilesParse/SmilesWrite.h>

#include <boost/graph/adjacency_list.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/maximum_weighted_matching.hpp>

#include "Mol.h"
#include "Fileio.h"
#include "AdjMatrix.h"
#include "VecUtils.h"

using namespace boost;

typedef property <edge_weight_t, int, property<edge_index_t, int>> EdgeProperty;
typedef adjacency_list<vecS, vecS, undirectedS, no_property, EdgeProperty> Graph;

ValenceMap atomicValence = {
    {"H", {1}},
    {"B", {3,4}},
    {"C", {4}},
    {"N", {3,4}},
    {"O", {2,1,3}},
    {"F", {1}},
    {"Si", {4}},
    {"P", {5,3}},
    {"S", {6,3,2}},
    {"Cl", {1}},
    {"Ge", {4}},
    {"Br", {1}},
    {"I", {1}}
};

std::unordered_map<int, RDKit::Bond::BondType> bondMap = {
        {1, RDKit::Bond::BondType::SINGLE},
        {2, RDKit::Bond::BondType::DOUBLE},
        {3, RDKit::Bond::BondType::TRIPLE}
    };

// Constructor functions
Mol::Mol(std::vector<Atom> atoms, int charge, bool chargedFragments, bool ignoreChirality, bool useAtomMap)
    : atoms(atoms), charge(charge), chargedFragments(chargedFragments), ignoreChirality(ignoreChirality), useAtomMap(useAtomMap)
{
    SetAC();
    SetBO();
}

Mol Mol::FromXYZ(std::string FileName, int charge, bool chargedFragments, bool ignoreChirality, bool useAtomMap )
{
    return Mol(ReadXYZ(FileName), charge, chargedFragments, ignoreChirality, useAtomMap);
}

// Member functions
void Mol::SetAC(double covalentFactor) {
        AC = AdjacencyMatrixDist(atoms, covalentFactor);
        BO = AC;
}

void Mol::SetBO(double covalentFactor) {
    // 1) Find all possible combinations of allowed valences.
    Eigen::VectorXi ACvalence = AC.colwise().sum();
    std::vector<std::vector<int> > allValences;

    for (int i = 0; i < atoms.size(); i++) {
        std::vector<int> possibleValence;

        // valence can not be smaller than degree.
        for (int val : atomicValence[atoms[i].symbol]) {
            if ( val >= ACvalence[i] ) {
                possibleValence.push_back(val);
            }
        }
        allValences.push_back(possibleValence);
    }

    // 2: Validate the valence possibilities, and identify the best BO.
    std::vector<std::vector<int> > allValenceProduct = IterProduct(allValences);
    for (std::vector<int> valences: allValenceProduct) {
        SetUAaDU(valences, ACvalence);

        // All atoms fully saturated.
        if ( UA.empty() && CheckBO(valences) ) {
            BO = AC;
            break;
        }

        ListofPairs UAPairs = GetUAPairs();
        ComputeBO(valences, UAPairs);
        if ( CheckBO(valences) )  {
            break;
        }
    }
}

bool Mol::CheckBO(std::vector<int> valences) {
    if ( BOValencesToLarge(valences) ) {
        return false;
    }
    bool checkSum = ( (BO - AC).sum() == SumVector(DU) );
    bool checkCharge = ChargeOK(valences);
    if (checkSum && checkCharge) {
        return true;
    }
    return false;
}

void Mol::SetUAaDU(std::vector<int> maxValenceList, Eigen::VectorXi valanceList) {
    // Reset UA and DU.
    UA.clear();
    DU.clear();
    for (int i = 0; i < maxValenceList.size(); i++) {
        if ( valanceList[i] < maxValenceList[i]) {
            UA.push_back(i);
            DU.push_back(maxValenceList[i] - valanceList[i]);
        }
    }
}

bool Mol::BOValencesToLarge(std::vector<int> valences) {
    Eigen::VectorXi numberOfBonds = BO.colwise().sum();
    for ( int i = 0; i < numberOfBonds.size(); i++ ) {
        if (numberOfBonds[i] > valences[i]) {
            return true;
        }
    }
    return false;
}

bool Mol::ChargeOK(std::vector<int> valences) {
    int Q = 0; // total charge

    if ( ! chargedFragments ) {
        return (charge == Q);
    }

    Eigen::VectorXi BOvalences = BO.colwise().sum();
    for (int i = 0; i < atoms.size(); i++) {
        int q = GetAtomicCharge(i,  BOvalences[i]);
        Q += q;

        if ( atoms[i].symbol == "C" ) {
            // count single bonds to C.
            int cSingleBonds = 0;
            for (int bondBO: BO(Eigen::all, i) ) {if (bondBO == 1) {
                cSingleBonds += 1;}
            }
            if ( cSingleBonds == 2 && BOvalences[i] == 2 ) {Q += 1;}
            if ( cSingleBonds == 3 && Q + 1 < charge) {Q += 2;}
        }

    }
    return (charge == Q);
}

ListofPairs Mol::GetUAPairs() {
    ListofPairs UAPairs;
    ListofPairs bonds = {};
    for (int i = 0; i < UA.size(); i++) {
        for (int j =  i + 1; j < UA.size(); j++) {
            if (AC(UA[i], UA[j]) == 1 ) {
                if (UA[i] < UA[j]) {
                    bonds.push_back(std::make_pair(UA[i], UA[j]));
                } else {
                    bonds.push_back(std::make_pair(UA[j], UA[i]));
                }
            }
        }
    }
    if ( bonds.empty() ) {
        return bonds;
    }

    // build graph and run maximum_weighted_matching, and extract pairs.
    std::set<int> s;
    for (std::pair<int,int> bond: bonds) {
        s.insert(bond.first);
        s.insert(bond.second);
    }

    std::map<int,int> molToGraphID;
    std::map<int,int> graphTomolID;
    int graphID = 0;
    for (int molAtomId: s) {
        molToGraphID[molAtomId] = graphID;
        graphTomolID[graphID] = molAtomId;
        graphID++;
    }

    Graph g(s.size());
    for (std::pair<int,int> bond: bonds) {
        boost::add_edge(
            molToGraphID[bond.first], molToGraphID[bond.second], EdgeProperty(1), g);
    }

    // Maximum weighted matching
    std::vector< graph_traits<Graph>::vertex_descriptor > mate1(s.size());
    maximum_weighted_matching(g, &mate1[0]);

    graph_traits< Graph >::vertex_iterator vi, vi_end;
    for ( boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi ) {
        if ( mate1[*vi] != graph_traits< Graph >::null_vertex() && *vi < mate1[*vi] ) {
            UAPairs.push_back(std::make_pair(graphTomolID[*vi], graphTomolID[mate1[*vi]]));
        }
    }
    return UAPairs;
}

void Mol::ComputeBO(std::vector<int> valences, std::vector<std::pair<int, int> > &UAPairs) {

    BO = AC; // Reset BO.
    std::vector<int> DUsave = {};

    while (DUsave != DU) {
        for (std::pair<int, int> p: UAPairs) {
            BO(p.first, p.second) += 1;
            BO(p.second, p.first) += 1;
        }

        Eigen::VectorXi BOvalence = BO.colwise().sum();

        // copy DU vector.
        DUsave = {};
        for (int i: DU) {
            DUsave.push_back(i);
        }
        UAPairs = GetUAPairs();
    }
}

int Mol::GetAtomicCharge(int atomID, int BOvalence) {

    RDKit::PeriodicTable *tbl = RDKit::PeriodicTable::getTable();

    if (atoms[atomID].symbol == "H") {
        return 1 - BOvalence;
    } else if (atoms[atomID].symbol == "B") {
        return 3 - BOvalence;
    } else if (atoms[atomID].symbol == "P" && BOvalence == 5) {
        return 0;
    } else if (atoms[atomID].symbol == "S" && BOvalence == 6) {
        return 0;
    } else {
        return tbl->getNouterElecs(atoms[atomID].symbol) - 8 + BOvalence;
    }
}

std::string Mol::GetSmiles() {

    if (atoms[0].symbol == "H") {
        std::cout << "Don't start with H" << std::endl;
        exit(1);
    }

    RDKit::RWMol m = RDKit::RWMol( *RDKit::SmilesToMol(atoms[0].symbol));
    // Add Atoms
    for (int i = 1; i < atoms.size(); i++) {
        RDKit::Atom a = RDKit::Atom(atoms[i].symbol);
        int idx = m.addAtom(&a);
    }

    // Add Bonds
    for (int i = 0; i < atoms.size(); i++) {
        for (int j = i + 1; j < atoms.size(); j++) {
            if ( BO(i,j) != 0) {
                m.addBond(i, j, bondMap[BO(i,j)]);
            }
        }
    }

    // TODO: Set atomic charge/radical if needed.
    if (chargedFragments) {
        SetRDKitMolAtomicCharges(m);
    } else {
        SetRDKitMolRadicals(m);
    }

    if (! ignoreChirality ) {
        RDKit::MolOps::sanitizeMol(m);
    }

    // Add Conformer - with xyz coords
    RDKit::ROMol *mol = new RDKit::ROMol(m);
    RDKit::Conformer conf = RDKit::Conformer(atoms.size());
    for (int i = 0; i < atoms.size(); i++) {
        conf.setAtomPos(i, RDGeom::Point3D(atoms[i].x, atoms[i].y, atoms[i].z));
    }
    mol->addConformer(&conf);

    if (! ignoreChirality  ) {
        RDKit::MolOps::assignStereochemistryFrom3D(*mol, -1, true);
    }

    if (useAtomMap) {
        for (int i = 0; i < atoms.size(); i++ ) {
            mol->getAtomWithIdx(i)->setAtomMapNum(i + 1);
        }
    }

    // Make Mol Canonical
    RDKit::ROMol *outm = RDKit::SmilesToMol(RDKit::MolToSmiles(*mol));

    // check charge.
    return RDKit::MolToSmiles(*outm);
}

void Mol::SetRDKitMolAtomicCharges(RDKit::RWMol &mol) {

    Eigen::VectorXi  BOvalence = BO.colwise().sum();
    int q = 0;

    for (int i = 0; i < atoms.size(); i++) {
        int charge = GetAtomicCharge(i, BOvalence[i]);
        q += charge;
        if (atoms[i].symbol == "C") {
            int cSingleBonds = 0;
            for (int bondBO: BO(Eigen::all, i) ) {
                if (bondBO == 1) {
                    cSingleBonds += 1;
                }
            }

            if (cSingleBonds == 2 && BOvalence[i] == 2) {
                q += 1;
                charge = 0;
            }

            if ( cSingleBonds == 3 && q + 1 < charge ) {
                q += 2;
                charge = 1;
            }
        }

        if ( std::abs(charge) > 0 ) {
            mol.getAtomWithIdx(i)->setFormalCharge(charge);
        }
    }
}

void Mol::SetRDKitMolRadicals(RDKit::RWMol &mol) {

    Eigen::VectorXi  BOvalence = BO.colwise().sum();

    for (int i = 0; i < atoms.size(); i++) {
        int charge = GetAtomicCharge(i, BOvalence[i]);

        if ( std::abs(charge) > 0 ) {
            mol.getAtomWithIdx(i)->setNumRadicalElectrons(charge);
        }
    }
}