/********************************************************************************
*  This file is part of CinoLib                                                 *
*  Copyright(C) 2016: Marco Livesu                                              *
*                                                                               *
*  The MIT License                                                              *
*                                                                               *
*  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 NON INFRINGEMENT. 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.                                                             *
*                                                                               *
*  Author(s):                                                                   *
*                                                                               *
*     Marco Livesu (marco.livesu@gmail.com)                                     *
*     http://pers.ge.imati.cnr.it/livesu/                                       *
*                                                                               *
*     Daniela Cabiddu                                                           *
*     http://www.imati.cnr.it/index.php/people/8-curricula/119-daniela-cabiddu  *
*                                                                               *
*     Italian National Research Council (CNR)                                   *
*     Institute for Applied Mathematics and Information Technologies (IMATI)    *
*     Via de Marini, 6                                                          *
*     16149 Genoa,                                                              *
*     Italy                                                                     *
*********************************************************************************/
#include <cinolib/polygon_maximum_inscribed_circle.h>
#include <cinolib/geometry/segment.h>
#include <cinolib/min_max_inf.h>

// Most of this is coming from here:
// http://www.boost.org/doc/libs/1_65_1/libs/polygon/doc/voronoi_diagram.htm
// http://www.boost.org/doc/libs/1_65_0/libs/polygon/example/voronoi_basic_tutorial.cpp
//
#ifdef CINOLIB_USES_BOOST
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry.hpp>
#include <boost/polygon/voronoi.hpp>
//
using boost::polygon::voronoi_builder;
using boost::polygon::voronoi_diagram;
using boost::polygon::voronoi_edge;
using boost::polygon::x;
using boost::polygon::y;
using boost::polygon::low;
using boost::polygon::high;
//
struct polygon_point
{
    int x;
    int y;
    polygon_point(int x, int y) : x(x), y(y) {}
};
//
struct polygon_segment
{
    polygon_point p0;
    polygon_point p1;
    polygon_segment(int x1, int y1, int x2, int y2) : p0(x1, y1), p1(x2, y2) {}
};
//
template<>
struct boost::polygon::geometry_concept<polygon_point>
{
    typedef boost::polygon::point_concept type;
};
//
template<>
struct boost::polygon::point_traits<polygon_point>
{
    typedef int coordinate_type;
    static inline coordinate_type get(const polygon_point & point, orientation_2d orient)
    {
        return (orient == HORIZONTAL) ? point.x : point.y;
    }
};
//
template<>
struct boost::polygon::geometry_concept<polygon_segment>
{
    typedef boost::polygon::segment_concept type;
};
//
template<>
struct boost::polygon::segment_traits<polygon_segment>
{
    typedef int coordinate_type;
    typedef polygon_point point_type;

    static inline point_type get(const polygon_segment& segment, direction_1d dir)
    {
        return dir.to_int() ? segment.p1 : segment.p0;
    }
};
//
typedef boost::geometry::model::d2::point_xy<double> BoostPoint;
typedef boost::geometry::model::polygon<BoostPoint>  BoostPolygon;

#endif // CINOLIB_USES_BOOST


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT license.
//
// gDLS*: Generalized Pose-and-Scale Estimation Given Scale and Gravity Priors
//
// Victor Fragoso, Joseph DeGol, Gang Hua.
// Proc. of the IEEE/CVF Conf. on Computer Vision and Pattern Recognition 2020.
//
// Please contact the author of this library if you have any questions.
// Author: Victor Fragoso (victor.fragoso@microsoft.com)

#include <vector>

#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <pybind11/eigen.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include "gdls_star/pinhole_camera.h"
#include "gdls_star/camera_feature_correspondence_2d_3d.h"
#include "gdls_star/estimate_similarity_transformation.h"
#include "gdls_star/gdls_star.h"
#include "gdls_star/gdls_star_robust_estimator.h"
#include "gdls_star/util.h"

namespace py = pybind11;

using msft::PinholeCamera;
using msft::CameraFeatureCorrespondence2D3D;
using msft::GdlsStar;
using msft::GdlsStarRobustEstimator;

struct PySolution {
  Eigen::Vector4d rotation = Eigen::Vector4d(1.0, 0.0, 0.0, 0.0);
  Eigen::Vector3d translation = Eigen::Vector3d::Zero();
  double scale = 1.0;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
};

struct PyRansacSolution {
  PySolution best_solution;
  std::vector<int> inliers;
};

std::vector<PySolution> EstimateSimilarityTransformation(
    const GdlsStar::Priors& priors,
    const std::vector<CameraFeatureCorrespondence2D3D>& correspondences) {
  std::vector<PySolution> final_solutions;
  GdlsStar::Solution solution;
  // Compute input datum.
  const GdlsStar::Input input = msft::ComputeInputDatum(correspondences);
  GdlsStar estimator;
  estimator.EstimateSimilarityTransformation(input, &solution);
  final_solutions.resize(solution.rotations.size());
  for (int i = 0; i < solution.rotations.size(); ++i) {
    const Eigen::Quaterniond& rotation = solution.rotations[i];
    final_solutions[i].rotation = Eigen::Vector4d(rotation.w(),
                                                  rotation.x(),
                                                  rotation.y(),
                                                  rotation.z());
    final_solutions[i].translation = solution.translations[i];
    final_solutions[i].scale = solution.scales[i];
  }
  return final_solutions;
}

// Computes the similarity transformation given 2D-3D correspondences and priors
// using a RANSAC estimator.
PyRansacSolution EstimateSimilarityTransformationViaRansac(
    const GdlsStarRobustEstimator::RansacParameters& params,
    const GdlsStar::Priors& priors,
    const std::vector<CameraFeatureCorrespondence2D3D>& correspondences) {
  PyRansacSolution final_solution;
  GdlsStarRobustEstimator::RansacSummary ransac_summary;
  GdlsStarRobustEstimator estimator(params);
  const GdlsStar::Solution solution =
      estimator.Estimate(priors, correspondences, &ransac_summary);
  const Eigen::Quaterniond& rotation = solution.rotations[0];
  final_solution.best_solution.rotation = Eigen::Vector4d(rotation.w(),
                                                          rotation.x(),
                                                          rotation.y(),
                                                          rotation.z());
  final_solution.best_solution.translation = solution.translations[0];
  final_solution.best_solution.scale = solution.scales[0];
  final_solution.inliers = ransac_summary.inliers;
  return final_solution;
}

PYBIND11_MODULE(pygdls_star, module) {
  module.doc() = "gDLS* Python module"; // Optional module docstring.

  // Ransac parameter class.
  py::class_<GdlsStarRobustEstimator::RansacParameters>(module, "RansacParams")
      .def(py::init<>())
      .def_readwrite("failure_probability",
                     &GdlsStarRobustEstimator::RansacParameters::failure_probability)
      .def_readwrite("reprojection_error_thresh",
                     &GdlsStarRobustEstimator::RansacParameters::reprojection_error_thresh)
      .def_readwrite("min_iterations",
                     &GdlsStarRobustEstimator::RansacParameters::min_iterations)
      .def_readwrite("max_iterations",
                     &GdlsStarRobustEstimator::RansacParameters::max_iterations);

  // Ransac summary class.
  py::class_<GdlsStarRobustEstimator::RansacSummary>(module, "RansacSummary")
      .def(py::init<>())
      .def_readwrite("inliers",
                     &GdlsStarRobustEstimator::RansacSummary::inliers)
      .def_readwrite("num_iterations",
                     &GdlsStarRobustEstimator::RansacSummary::num_iterations)
      .def_readwrite("confidence",
                     &GdlsStarRobustEstimator::RansacSummary::confidence)
      .def_readwrite("num_hypotheses",
                     &GdlsStarRobustEstimator::RansacSummary::num_hypotheses);

  // Pinhole camera.
  py::class_<PinholeCamera>(module, "PinholeCamera")
      .def(py::init<const double,
                    const Eigen::Vector2d,
                    const Eigen::Vector4d,
                    const Eigen::Vector3d>(),
           py::arg("focal_length") = 1.0,
           py::arg("principal_point") = Eigen::Vector2d::Zero(),
           py::arg("world_to_cam_rot") = Eigen::Vector4d(1.0, 0.0, 0.0, 0.0),
           py::arg("world_to_cam_trans") = Eigen::Vector3d::Zero())
      .def("project_point", &PinholeCamera::ProjectPoint)
      .def("pixel_to_unit_ray", &PinholeCamera::PixelToUnitRay)
      .def("get_position", &PinholeCamera::GetPosition);

  // CameraFeatureCorrespondence2D3D.
  py::class_<CameraFeatureCorrespondence2D3D>(module,
                                              "CameraFeatureCorrespondence2D3D")
      .def(py::init<>())
      .def_readwrite("camera", &CameraFeatureCorrespondence2D3D::camera)
      .def_readwrite("observation",
                     &CameraFeatureCorrespondence2D3D::observation)
      .def_readwrite("point", &CameraFeatureCorrespondence2D3D::point);

  // Priors.
  py::class_<GdlsStar::Priors>(module, "Priors")
      .def(py::init<>())
      .def_readwrite("world_down_direction",
                     &GdlsStar::Priors::world_down_direction)
      .def_readwrite("query_down_direction",
                     &GdlsStar::Priors::query_down_direction)
      .def_readwrite("scale_penalty", &GdlsStar::Priors::scale_penalty)
      .def_readwrite("scale_prior", &GdlsStar::Priors::scale_prior)
      .def_readwrite("gravity_penalty", &GdlsStar::Priors::gravity_penalty);

  // PySolution.
  py::class_<PySolution>(module, "Solution")
      .def(py::init<>())
      .def_readwrite("rotation", &PySolution::rotation)
      .def_readwrite("translation", &PySolution::translation)
      .def_readwrite("scale", &PySolution::scale);

  // PyRansacSolution.
  py::class_<PyRansacSolution>(module, "RansacSolution")
      .def(py::init<>())
      .def_readwrite("best_solution", &PyRansacSolution::best_solution)
      .def_readwrite("inliers", &PyRansacSolution::inliers);

  // Estimate similarity transformation using plain gDLS*.
  module.def("estimate", &EstimateSimilarityTransformation);
  module.def("estimate_ransac", &EstimateSimilarityTransformationViaRansac);
}


/*
Copyright 2020 Standard Cyborg

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/
#include "standard_cyborg/math/Transform.hpp"

#include "standard_cyborg/math/Mat3x4.hpp"
#include "standard_cyborg/math/Mat3x3.hpp"
#include "standard_cyborg/util/DataUtils.hpp"

#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wdocumentation"
#ifndef DEBUG
#define EIGEN_NO_DEBUG  1
#endif
#include <Eigen/Core>
#include <Eigen/Cholesky>
#pragma clang diagnostic pop

namespace standard_cyborg {
namespace math {

// This function is derived from the transforms3d python package and specialized for 3x3 matrices.
// You may find the original source of this function at:
//     https://github.com/matthew-brett/transforms3d/blob/8f81b063686f9b892bdbd4775d02615dce028105/transforms3d/affines.py#L156-L246
//
// **********************
// Copyright and Licenses
// **********************
//
// Retrieved from https://github.com/matthew-brett/transforms3d/blob/master/LICENSE on Dec. 2, 2019
//
// Transforms3d
// ============
//
// The transforms3d package, including all examples, code snippets and attached
// documentation is covered by the 2-clause BSD license.
//
//    Copyright (c) 2009-2017, Matthew Brett and Christoph Gohlke
//    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.
//
//    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
//    IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
//    THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
//    PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
//    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
//    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
//    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
//    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
//    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
//    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
//    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// 3rd party code and data
// =======================
//
// Some code distributed within the transforms3d sources was developed by other
// projects. This code is distributed under its respective licenses that are
// listed below.
//
// Sphinx autosummary extension
// ----------------------------
//
// This extension has been copied from NumPy (Jul 16, 2010) as the one shipped with
// Sphinx 0.6 doesn't work properly.
//
// ::
//
//  Copyright (c) 2007-2009 Stefan van der Walt and Sphinx team
//
//  Redistribution and use in source and binary forms, with or without
//  modification, are permitted provided that the following conditions are met:
//
//    a. Redistributions of source code must retain the above copyright notice,
//       this list of conditions and the following disclaimer.
//    b. 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.
//    c. Neither the name of the Enthought 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 REGENTS 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.

typedef Eigen::Matrix<float, 3, 3> EMat3;
typedef Eigen::Vector3f EVec3;

Transform Transform::fromMat3x4(const math::Mat3x4& A) {
    Transform transform;

    // Start by extracting the translation. This part is very easy.
    transform.translation = {A.m03, A.m13, A.m23};

    // R = rotation
    // Z = zoom (scale)
    // S = shear
    EMat3 RZS;
    RZS << A.m00, A.m01, A.m02,
           A.m10, A.m11, A.m12,
           A.m20, A.m21, A.m22;

    // Use the Cholesky decomposition to perform the separation
    // This part very closely mirrors the original implementation, only specialized for 3x3 matrices
    EMat3 RZSt_RZS (RZS.transpose() * RZS);
    Eigen::LLT<EMat3> cholesky;
    cholesky.compute(RZSt_RZS);
    EMat3 ZS (cholesky.matrixL().transpose());
    EVec3 Z (ZS.diagonal());
    EMat3 R (RZS * ZS.inverse());

    // Detect and fix if the rotation flips parity
    if (R.determinant() < 0.0f) {
        Z(0) = -Z(0);
        ZS.row(0) = -ZS.row(0);
        R = RZS * ZS.inverse();
    }

    // Extract the result into StandardCyborg data types
    transform.scale = toVec3(Z);

    transform.shear = math::Vec3{
        ZS(0, 1) / Z(0),
        ZS(0, 2) / Z(0),
        ZS(1, 2) / Z(1)
    };

    transform.rotation = math::Quaternion::fromMat3x3(toMat3x3(R));

    return transform;
}

Transform Transform::fromMat3x4(const math::Mat3x4& A, std::string srcFrame, std::string destFrame) {
  Transform t (Transform::fromMat3x4(A));
  t.srcFrame = srcFrame;
  t.destFrame = destFrame;
  return t;
}

Transform Transform::inverse() const {
  Mat3x4 m (Mat3x4::fromTransform(*this));
  return Transform::fromMat3x4(m.invert(), destFrame, srcFrame);
}

} // namespace math
} // namespace standard_cyborg


#include <boost/random/binomial_distribution.hpp>


//
//! Copyright © 2018
//! Brandon Kohn
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
#pragma once

#include <geometrix/utility/utilities.hpp>
#include <geometrix/primitive/point_traits.hpp>
#include <boost/functional/hash.hpp>
#include <unordered_map>

namespace geometrix {

	template <typename Point, typename NumberComparisonPolicy>
	class triangle_complex
	{
		struct point_compare
		{
			point_compare(const NumberComparisonPolicy& cmp)
				: cmp(cmp)
			{}

			bool operator()(const Point& a, const Point& b) const
			{
				return lexicographically_less_than(a, b, cmp);
			}

			NumberComparisonPolicy cmp;
		};

		using vertex_handle = std::uint32_t;
		using invalid_handle = std::integral_constant<vertex_handle, static_cast<vertex_handle>(-1)>;
		using vertex_handle_map = std::map<Point, vertex_handle>;
		using vertex_point_map = std::vector<Point>;
		using edge_key = std::tuple<vertex_handle, vertex_handle>;
		using trig_key = std::tuple<vertex_handle, vertex_handle, vertex_handle>;

		struct half_edge
		{
			half_edge(vertex_handle o = invalid_handle::value, bool constrained = false)
				: opposite(o)
				, constrained(constrained)
			{}

			vertex_handle opposite;
			bool constrained;
		};

		using triangle_edge_opposite_vertex_map = std::unordered_map<edge_key, half_edge, boost::hash<edge_key>>;
		using triangle_map = std::unordered_map<trig_key, std::array<Point, 3>, boost::hash<trig_key>>;

	public:

		triangle_complex() = default;
		triangle_complex(const NumberComparisonPolicy& cmp)
			: m_cmp(cmp)
			, m_vertexHandleMap(point_compare(cmp))
		{}

		bool add_triangle(Point a, Point b, Point c)
		{
			sort(a, b, c);
			auto key = get_triangle_key(a, b, c);
			vertex_handle u, v, w;
			std::tie(u, v, w) = key;
			return add_triangle(u, v, w);
		}

		bool set_constraint(const Point& a, const Point& b, bool c)
		{
			auto u = get_vertex_handle(a);
			auto v = get_vertex_handle(b);
			auto it = m_edgeVertexMap.find(edge_key(u, v));
			if (it != m_edgeVertexMap.end()) {
				it->second.constrained = c;

				it = m_edgeVertexMap.find(edge_key(v, u));
				if (it != m_edgeVertexMap.end()) {
					it->second.constrained = c;
					return true;
				}
			}

			return false;
		}

		template <typename EncroachSegmentPredicate, typename TrianglePredicate>
		void refine(const stk::units::angle& minAngle, EncroachSegmentPredicate&& is_encroached, TrianglePredicate&& is_bad)
		{
			auto badSegs = get_encroaching_segments(std::forward<EncroachSegmentPredicate>(is_encroached));
			split_encroaching_segments(badSegs);

			std::deque<trig_key> badTrigs;

			for(auto item : m_triangleMap)
			{
				if (is_bad(item.first, item.second))
					badTrigs.emplace_back(item.first);
			}
		}

		/*
        bool delete_triangle(Point a, Point b, Point c)
        {
            sort(a,b,c);
            auto key = find_triangle_key(a, b, c);

            if(is_valid(key))
            {
                vertex_handle u, v, w;
                std::tie(u,v,w) = key;
                delete_triangle(u, v, w);
                return true;
            }

            return false;
        }
		*/

        void insert_vertex(const Point& p, Point a, Point b, Point c)
        {
            sort(a,b,c);
            auto key = find_triangle_key(a, b, c);

            GEOMETRIX_ASSERT(is_valid(key));
            GEOMETRIX_ASSERT(point_in_circumcircle(p, a, b, c, m_cmp) == point_circle_orientation::inside);

            vertex_handle u = get_vertex_handle(p), v, w, x;
            std::tie(v,w,x) = key;
            insert_vertex(u, v, w, x);
        }

    private:

		template <typename EncroachSegmentPredicate>
		std::deque<edge_key> get_encroaching_segments(EncroachSegmentPredicate&& is_encroached) const
		{
			std::deque<edge_key> badSegs;

			for (auto item : m_edgeVertexMap)
			{
				vertex_handle u, v;
				std::tie(u, v) = item.first;
				auto w = item.second.opposite;

				if (is_encroached(m_points[w], m_points[u], m_points[v]))
					badSegs.push_back(item.first);
			}

			return std::move(badSegs);
		}

		bool in_diametral_circle(Point const& p, Point const& o, Point const& d)
		{
			//! If the angle between OP and DP is obtuse, then P is inside the diametral circle of OD.
			//! two vectors have obtuse angle if dot product is negative.
			auto dp = dot_product(o - p, d - p);
			return dp < constants::zero<decltype(dp)>();
		}

		//bool in_diametral_lens(Angle const& theta, Point const& o, Point const& d, const Point& p)
		bool in_diametral_lens(Point const& p, Point const& o, Point const& d)
		{
			//static const stk::units::angle theta = 0.523599 * units::si::radians;
			constexpr auto cos_theta = 0.86602529158;//cos(theta);
			constexpr auto v2_cos_theta2_1 = 2.0 * cos_theta * cos_theta - 1.0;
            constexpr auto e2 = v2_cos_theta2_1 * v2_cos_theta2_1;

            stk::vector2 op = o - p;
			stk::vector2 dp = d - p;
			auto dt = dot_product(op, dp);
			return (dt * dt) >= (e2 * magnitude_sqrd(op) * magnitude_sqrd(dp));
		}

        bool add_triangle(vertex_handle u, vertex_handle v, vertex_handle w)
        {
            auto key = trig_key(u,v,w);
            auto it = m_triangleMap.find(key);
            if(it == m_triangleMap.end())
            {
                m_edgeVertexMap[edge_key(u,v)] = w;
                m_edgeVertexMap[edge_key(v,w)] = u;
                m_edgeVertexMap[edge_key(w,u)] = v;

                std::array<Point,3> trig = {m_points[u],m_points[v],m_points[w]};
                m_triangleMap.emplace_hint(it, key, trig);
                return true;
            }

            return false;
        }

        void delete_triangle(vertex_handle u, vertex_handle v, vertex_handle w, bool& uvConstrained, bool& vwConstrained, bool& wuConstrained)
        {
            m_triangleMap.erase(trig_key(u,v,w));
            m_edgeVertexMap.erase(edge_key(u,v));
            m_edgeVertexMap.erase(edge_key(v,w));
            m_edgeVertexMap.erase(edge_key(w,u));
            m_triangleMap.erase(trig_key(u,v,w));

			auto it = m_edgeVertexMap.find(edge_key(u,v));
			if(it != m_edgeVertexMap.end())
			{
				uvConstrained = it->second.constrained;
				m_edgeVertexMap.erase(it);
			}

			it = m_edgeVertexMap.find(edge_key(v,w));
			if(it != m_edgeVertexMap.end())
			{
				vwConstrained = it->second.constrained;
				m_edgeVertexMap.erase(it);
			}

			it = m_edgeVertexMap.find(edge_key(w,u));
			if(it != m_edgeVertexMap.end())
			{
				wuConstrained = it->second.constrained;
				m_edgeVertexMap.erase(it);
			}
        }

        void insert_vertex(vertex_handle u, vertex_handle v, vertex_handle w, vertex_handle x)
        {
			bool vwConstrained = false;
			bool wxConstrained = false;
			bool xvConstrained = false;
            delete_triangle(v,w,x, vwConstrained, wxConstrained, xvConstrained);
            dig_cavity(u,v,w, vwConstrained);
            dig_cavity(u,w,x, wxConstrained);
            dig_cavity(u,x,v, xvConstrained);
        }

        half_edge adjacent(vertex_handle u, vertex_handle v) const
        {
            auto key = edge_key(u,v);
            auto it = m_edgeVertexMap.find(key);
            if(it != m_edgeVertexMap.end())
                return it->second;

            return half_edge(invalid_handle::value);
        }

        bool is_sorted(const Point& a, const Point& b, const Point& c) const
        {
            return lexicographically_less_than(a,b,m_cmp) && lexicographically_less_than(a,c,m_cmp) && is_ccw(a,b,c);
        }

        bool is_ccw(const Point& a, const Point& b, const Point& c) const
        {
            return (get_orientation(a, b, c, m_cmp) != oriented_right);
        }

        void sort(Point& a, Point& b, Point& c) const
        {
            using std::swap;

            //! A should always be the lexicographically lowest point. Then all points should order CCW.
            if(lexicographically_less_than(c, a, m_cmp))
                swap(a,c);

            if(lexicographically_less_than(b, a, m_cmp))
                swap(a,b);

            if(!is_ccw(a, b, c))
                swap(b,c);
        }

        bool is_degenerate(const Point& a, const Point& b, const Point& c) const
        {
            return (numeric_sequence_equals(a, b, m_cmp) ||
                    numeric_sequence_equals(b, c, m_cmp) ||
                    numeric_sequence_equals(c, a, m_cmp));
        }

        vertex_handle find_vertex_handle(const Point& p) const
        {
	        auto it = m_vertexHandleMap.lower_bound(p);
        	if( it != m_vertexHandleMap.end() && !m_vertexHandleMap.key_comp()(p, it->first) )
                return it->second;

            return invalid_handle::value;
        }

        vertex_handle get_vertex_handle(const Point& p) const
        {
	        auto it = m_vertexHandleMap.lower_bound(p);
        	if( it != m_vertexHandleMap.end() && !m_vertexHandleMap.key_comp()(p, it->first) )
                return it->second;

		    auto vDescriptor = get_descriptor();
		    m_vertexHandleMap.insert(it, std::make_pair(p, vDescriptor));
            if(vDescriptor >= m_points.size())
                m_points.emplace_back(p);
            else
                m_points[vDescriptor] = p;
            return vDescriptor;
        }

        vertex_handle get_descriptor() const
        {
            if(m_freeDescriptors.empty())
                return m_points.size();

            auto v = m_freeDescriptors.top();
            m_freeDescriptors.pop();
            return v;
        }

        trig_key get_triangle_key(const Point& a, const Point& b, const Point& c) const
        {
            GEOMETRIX_ASSERT(is_sorted(a,b,c));
            auto v0 = get_vertex_handle(a);
            auto v1 = get_vertex_handle(b);
            auto v2 = get_vertex_handle(c);
            return trig_key(v0, v1, v2);
        }

        trig_key find_triangle_key(const Point& a, const Point& b, const Point& c) const
        {
            GEOMETRIX_ASSERT(is_sorted(a,b,c));
            auto v0 = find_vertex_handle(a);
            auto v1 = find_vertex_handle(b);
            auto v2 = find_vertex_handle(c);
            return trig_key(v0, v1, v2);
        }

        bool is_valid(trig_key const& key) const
        {
            return std::get<0>(key) != invalid_handle::value &&
                std::get<1>(key) != invalid_handle::value &&
                std::get<2>(key) != invalid_handle::value;
        }

        void dig_cavity(vertex_handle u, vertex_handle v, vertex_handle w, bool vwConstrained)
        {
            auto x = adjacent(w,v);
            if(x.opposite != invalid_handle::value)
            {
                if(!x.constrained && point_in_circumcircle(m_points[v], m_points[w], m_points[x], u, m_cmp) == point_circle_orientation::inside)
                {
					bool wvConstrained = false, vxConstrained = false, xwConstrained = false;
                    delete_triangle(w,v,x, wvConstrained, vxConstrained, xwConstrained);
                    dig_cavity(u,v,x, vxConstrained);
                    dig_cavity(u,x,w, xwConstrained);
                }
				else
				{
					add_triangle(u, v, w);
					auto it = m_edgeVertexMap.find(edge_key(v, w));
					GEOMETRIX_ASSERT(it != m_edgeVertexMap.end());
					if (it != m_edgeVertexMap.end())
						it->second.constrained = vwConstrained;
				}
            }
        }

        NumberComparisonPolicy            m_cmp;
        mutable vertex_handle_map         m_vertexHandleMap;
        mutable std::stack<vertex_handle> m_freeDescriptors;
        mutable vertex_point_map          m_points;
        triangle_edge_opposite_vertex_map m_edgeVertexMap;
        triangle_map                      m_triangleMap;

    };

}//! namespace geometrix;


#include <iostream>
#include <armadillo>
#include<complex>
#include<cstdlib>
#include <ctime>
#include <string>
#include "Particle.h"
#include "Spherical.h"
#include "Cubic.h"
#include "Rhombohedral.h"
#include "Shell.h"
using namespace std;
using namespace arma;

int main(){
    // Input
    int sizeCells;  //sizeCells of lattice in +/-ve x/y/z direction
    int pSizeCells;  // size of the particle (radius, edge) in unit cells - carved out of bulk lattice
    double disorderStrength; // Magnitude of disorder potential
    double disorderCoverage; // fraction of disordered states above orderRadius
    char pShape; // Spherical, Cubic or Rhombohedral

    // Parameters and data structures (Don't need to change those)
    const double tHopping = 1;
    const double deltatHopping = 0.4;
    const complex<double> spinOrbitCoupling(0,1);

    // Geometry of the model;
    const rowvec latVec1 ("0 0.5 0.5");
    const rowvec latVec2 ("0.5 0 0.5");
    const rowvec latVec3 ("0.5 0.5 0");
    const rowvec sublatVec ("0.25 0.25 0.25");
    const rowvec comShift ("-0.5 -0.5 -0.5"); // to have centre of mass of sublatOne at the origin
    const double latVecNorm = 1/sqrt(2); // cubic cell size a = 1
    const double sublatVecNorm = sqrt(3)/4;

    const double delta = 0.01; // small number for numerical comparisons

    // Read input from file
    ifstream inputfile;
    inputfile.open("input.txt");
    char buffer [512];
    string line;
    while (getline(inputfile, line)) {
        sscanf(line.c_str(), "sizeCells = %d %s", &sizeCells, buffer);
        sscanf(line.c_str(), "particleSizeCells = %d %s", &pSizeCells, buffer);
        sscanf(line.c_str(), "disorderStrength = %lf %s", &disorderStrength, buffer);
        sscanf(line.c_str(), "disorderCoverage = %lf %s", &disorderCoverage, buffer);
        sscanf(line.c_str(), "particleShape = %c %s", &pShape, buffer);
    }

    Particle * myParticle;

    if (pShape == 'S' || pShape == 's') {
        myParticle = new Spherical(pSizeCells*latVecNorm, pSizeCells*latVecNorm - sublatVecNorm);
    } else if (pShape == 'C' ||  pShape == 'c') {
        myParticle = new Cubic (pSizeCells*latVecNorm, pSizeCells*latVecNorm - sublatVecNorm);
    } else if (pShape == 'R' || pShape == 'r') {
        myParticle = new Rhombohedral(pSizeCells*latVecNorm, pSizeCells*latVecNorm - sublatVecNorm);
	} else if (pShape == 'H' || pShape == 'h') {
        myParticle = new Shell(pSizeCells*latVecNorm, pSizeCells*latVecNorm - 4*sublatVecNorm);
    } else { cout << "Wrong particle type - exiting" << endl; return 1;}
    cout << "Input: "<< sizeCells << " " << pSizeCells << " " <<  disorderStrength << " " << disorderCoverage << " " << myParticle->shape << endl;

    int pCells = 0; // # unit cells belonging to the particle
    int dummy = 0;
    rowvec dummyVec(3);
    cx_mat dummyMat(2,2);
    rowvec pcomShift(3);

    cx_mat unity(2,2), sigma_x(2,2), sigma_y(2,2), sigma_z(2,2); // Pauli matrices
    unity.fill(0), sigma_x.fill(0), sigma_y.fill(0), sigma_z.fill(0);
    mat dummy_unity={{1,0},{0,1}}, dummy_sigma_x={{0,1},{1,0}};
    mat dummy_sigma_y={{0,-1},{1,0}}, dummy_sigma_z={{1,0},{0,-1}};
    unity.set_real(dummy_unity);
    sigma_x.set_real(dummy_sigma_x);
    sigma_y.set_imag(dummy_sigma_y);
    sigma_z.set_real(dummy_sigma_z);

    // Rhombohedral has the shape of the unit cell, so don't need to carve out
    if (myParticle->shape == "Rhombohedral") {
        sizeCells = pSizeCells;
    }

    // Set up lattice and calculate number of cells within particle
    for (int i=-sizeCells; i<sizeCells; i++) {
        for (int j=-sizeCells;  j<sizeCells; j++) {
            for (int k=-sizeCells; k<sizeCells; k++) {
                dummyVec=i*latVec1+j*latVec2+k*latVec3-comShift;
                if (myParticle->WithinParticle(dummyVec)) {
                    pcomShift=pcomShift+2*dummyVec+sublatVec;
                    pCells++;
                }
            }
        }
    }

    // Finding particle's COM
    pcomShift=0.5*pcomShift/pCells;

    // Set up data structures for the particle
    myParticle->SetDataStructures(pCells);

    // Carve out the unit sphere out of lattice
    for (int i=-sizeCells; i<sizeCells; i++) {
        for (int j=-sizeCells;  j<sizeCells; j++) {
            for (int k=-sizeCells; k<sizeCells; k++) {
                dummyVec=i*latVec1+j*latVec2+k*latVec3-comShift;
                if (myParticle->WithinParticle(dummyVec)) {
                    myParticle->sublatOne(dummy,0)=dummy;
                    myParticle->sublatOne(dummy,span(1,3))=dummyVec-pcomShift;
                    myParticle->sublatTwo(dummy,0)=dummy;
                    myParticle->sublatTwo(dummy,span(1,3))=dummyVec+sublatVec-pcomShift;
                    dummy++;
                }
            }
        }
    }

    // Set up the Hamiltonian
    for (int i=0; i<pCells; i++) {
        for (int j=0; j<pCells; j++) {
            // Set up nearest neighbour hoppings (between sublattices)
            dummyVec=myParticle->sublatOne(i,span(1,3))-myParticle->sublatTwo(j,span(1,3));
            if (dot(dummyVec,dummyVec) < sublatVecNorm*sublatVecNorm+delta) {
                if (i==j) {
                    // hoppings within a cell i
                    myParticle->Hamiltonian(span(4*i+0,4*i+1),span(4*i+2,4*i+3))+=(tHopping+deltatHopping)*unity;
                    myParticle->Hamiltonian(span(4*i+2,4*i+3),span(4*i+0,4*i+1))+=(tHopping+deltatHopping)*unity;
                } else {
                    // hoppings from sublatTwo of cell j to sublatOne of cell i
                    myParticle->Hamiltonian(span(4*i+0,4*i+1),span(4*j+2,4*j+3))+=(tHopping)*unity;
                    // hoppings from sublatOne of cell i to sublatTwo of cell j
                    myParticle->Hamiltonian(span(4*j+2,4*j+3),span(4*i+0,4*i+1))+=(tHopping)*unity;
                }
            }

            // Set up next nearest neighbour hoppings (within each sublattice)
            dummyVec=myParticle->sublatOne(i,span(1,3))-myParticle->sublatOne(j,span(1,3));
            if ( (dot(dummyVec,dummyVec) < latVecNorm*latVecNorm+delta)  &&
                    (dot(dummyVec,dummyVec) > sublatVecNorm*sublatVecNorm+delta) ) {

                // Matrix elements are direction-dependent
                if ( (dummyVec(0) > 0 && dummyVec(1) > 0) || (dummyVec(0) > 0 &&
                            dummyVec(2) > 0) || (dummyVec(1) > 0 && dummyVec(2) > 0) ) {
                    dummyVec = cross(sublatVec, dummyVec-sublatVec);
                } else if ( (dummyVec(1) < 0 && dummyVec(2) < 0) || (dummyVec(0) > 0 &&
                            dummyVec(1) < 0) || (dummyVec(0) > 0 && dummyVec(2) < 0) ) {
                    dummyVec = cross(-latVec1+sublatVec, dummyVec+latVec1-sublatVec);
                } else if ( (dummyVec(0) < 0 && dummyVec(1) > 0) || (dummyVec(0) < 0 &&
                            dummyVec(2) < 0) || (dummyVec(1) > 0 && dummyVec(2) < 0) ) {
                    dummyVec = cross(-latVec2+sublatVec, dummyVec+latVec2-sublatVec);
                } else if ( (dummyVec(0) < 0 && dummyVec(2) > 0) || (dummyVec(1) < 0 &&
                            dummyVec(2) > 0) || (dummyVec(0) < 0 && dummyVec(1) < 0) ) {
                    dummyVec = cross(-latVec3+sublatVec, dummyVec+latVec3-sublatVec);
                } else {}
                dummyMat=dummyVec(0)*sigma_x+dummyVec(1)*sigma_y+dummyVec(2)*sigma_z;
                // See if both atoms are connected to the same linking atom
                // hoppings from cell j to cell i within sublattice One
                myParticle->Hamiltonian(span(4*i,4*i+1),span(4*j,4*j+1))=spinOrbitCoupling*dummyMat;
                // hoppings from cell i to cell j within sublattice One
                // Minus sign for hopping in opposite direction
                myParticle->Hamiltonian(span(4*j,4*j+1),span(4*i,4*i+1))=-spinOrbitCoupling*dummyMat;
                // hoppings from cell j to cell i within sublattice Two
                // Minus sign for sublattice
                myParticle->Hamiltonian(span(4*i+2,4*i+3),span(4*j+2,4*j+3))=-spinOrbitCoupling*dummyMat;
                // hoppings from cell i to cell j within sublattice Two
                myParticle->Hamiltonian(span(4*j+2,4*j+3),span(4*i+2,4*i+3))=spinOrbitCoupling*dummyMat;
            }
        }
    }

    myParticle->AddDisorder(disorderStrength, disorderCoverage, delta);

    // abs returns a matrix, max returns a vector, 2nd max - largest value
    if (max(max(abs(myParticle->Hamiltonian-myParticle->Hamiltonian.t()))) > delta ) {
        cout << "Error, Hamiltonian is not Hermitian by at least " << delta << endl;
    }

    // Get the eigenvectors and eigevalues
    eig_sym(myParticle->eigvals, myParticle->eigvecs, myParticle->Hamiltonian);

    myParticle->PrintInfo(disorderStrength);

    // Find the middle of the spectrum (Dirac point, E=0, for clean spectrum)
    for (int i=0; i<4*pCells-1;i++) {
        if (myParticle->eigvals(i) > 0 && myParticle->eigvals(i-1) < 0) {
            dummy = i ;
        }
    }
    cout << "First state above E=0 is #: " << dummy << " with E= " << myParticle->eigvals(dummy) << endl;
    dummy=2*pCells;
    cout << "Picking state #: " << dummy << " with E= " << myParticle->eigvals(dummy) << endl;

    mat probDensity(2*pCells,4+100);
    probDensity(span(0,pCells-1),span(0,2))=myParticle->sublatOne.cols(1,3);
    probDensity(span(pCells,2*pCells-1),span(0,2))=myParticle->sublatTwo.cols(1,3);

    // Calculate probability density of probDensity labelled dummy at each lattice site
    for (int i=0; i<pCells; i++) {
		for (int j=0; j<100; j++) {
        probDensity(i,3+j)=norm(myParticle->eigvecs(span(4*i,4*i+1),dummy+j))*norm(myParticle->eigvecs(span(4*i,4*i+1),dummy+j));
        probDensity(pCells+i,3+j)=norm(myParticle->eigvecs(span(4*i+2,4*i+3),dummy+j))*norm(myParticle->eigvecs(span(4*i+2,4*i+3),dummy+j));
		}
    }
    // Save all eigenvalues and a particular eigenvector
    myParticle->eigvals.save("output_eigvals.txt",raw_ascii);
    probDensity.save("output_state.txt", raw_ascii);
    return 0;
}


#pragma once
#include "vector.hpp"
#include <boost/operators.hpp>
#include <iostream>

namespace dmc
{
	template <class Scalar, int Dimension>
	class dual
		: boost::addable<dual<Scalar, Dimension>
		, boost::addable<dual<Scalar, Dimension>, Scalar
		, boost::subtractable<dual<Scalar, Dimension>
		, boost::subtractable<dual<Scalar, Dimension>, Scalar
		, boost::multipliable<dual<Scalar, Dimension>
		, boost::multipliable<dual<Scalar, Dimension>, Scalar
		, boost::dividable<dual<Scalar, Dimension>
		, boost::dividable<dual<Scalar, Dimension>, Scalar
		, boost::equality_comparable<dual<Scalar, Dimension>
		, boost::equality_comparable<dual<Scalar, Dimension>, Scalar
		, boost::less_than_comparable<dual<Scalar, Dimension>
		, boost::less_than_comparable<dual<Scalar, Dimension>, Scalar
		>>>>>>>>>>>>
	{
	public:
		typedef Scalar scalar_type;
		static const int dimension = Dimension;
		typedef vector<scalar_type, dimension> vector_type;

		dual() = default;

		dual(scalar_type value)
			: value_(value)
		{
		}

		dual(scalar_type value, const vector_type& grad)
			: value_(value)
			, grad_(grad)
		{
		}

		scalar_type& value()
		{
			return value_;
		}

		const scalar_type& value() const
		{
			return value_;
		}

		vector_type& grad()
		{
			return grad_;
		}

		const vector_type& grad() const
		{
			return grad_;
		}

		const dual& operator+() const
		{
			return *this;
		}

		dual operator-() const
		{
			return dual(-value_, -grad_);
		}

		dual& operator+=(const dual& rhs)
		{
			value_ += rhs.value_;
			grad_ += rhs.grad_;
			return *this;
		}

		dual& operator-=(const dual& rhs)
		{
			value_ -= rhs.value_;
			grad_ -= rhs.grad_;
			return *this;
		}

		dual& operator*=(const dual& rhs)
		{
			grad_ = value_ * rhs.grad_ + rhs.value_ * grad_;
			value_ *= rhs.value_;
			return *this;
		}

		dual& operator/=(const dual& rhs)
		{
			grad_ = (grad_ * rhs.value_ - value_ * rhs.grad_) / (rhs.value_ * rhs.value_);
			value_ /= rhs.value_;
			return *this;
		}

		friend bool operator==(const dual& lhs, const dual& rhs)
		{
			return lhs.value_ == rhs.value_;
		}

		friend bool operator<(const dual& lhs, const dual& rhs)
		{
			return lhs.value_ < rhs.value_;
		}

		friend dual abs(const dual& d)
		{
			return d.value() < static_cast<scalar_type>(0.0) ? -d : d;
		}

		friend dual sqrt(const dual& d)
		{
			auto value = std::sqrt(d.value());
			auto grad = value * static_cast<scalar_type>(0.5) * d.grad() / d.value();
			return dual(value, grad);
		}

		friend dual sin(const dual& d)
		{
			auto value = std::sin(d.value());
			auto grad = d.grad() * std::cos(d.value());
			return dual(value, grad);
		}

		friend dual cos(const dual& d)
		{
			auto value = std::cos(d.value());
			auto grad = -d.grad() * std::sin(d.value());
			return dual(value, grad);
		}

	private:
		scalar_type value_{};
		vector_type grad_;
	};

	template <class Scalar, int Dimension>
	Scalar value(const dual<Scalar, Dimension>& d)
	{
		return d.value();
	}

	template <class Scalar>
	Scalar value(const Scalar& value)
	{
		return value;
	}

	template <class Scalar, int Dimension>
	Scalar grad(const dual<Scalar, Dimension>& d)
	{
		return d.grad();
	}

	template <class Scalar, int Dimension>
	std::ostream& operator<<(std::ostream& os, const dual<Scalar, Dimension>& d)
	{
		os << "{ " << d.value() << ", " << d.grad().transpose() << " }";
		return os;
	}
}


#ifndef OOQPRECOURSEINTERFACE_HPP
#define OOQPRECOURSEINTERFACE_HPP

#include "stochasticInput.hpp"
#include "QpGenVars.h"
#include "QpGenResiduals.h"
#include "SimpleVector.h"
#include "Status.h"
#include "QpGenData.h"
#include "OoqpVersion.h"
#include <boost/scoped_ptr.hpp>
#include "SparseGenMatrix.h"
#include "SparseSymMatrix.h"

using boost::scoped_ptr;

template<typename SOLVER, typename FORMULATION>
class OOQPRecourseInterface {
public:
  OOQPRecourseInterface(stochasticInput &, int scenarioNumber, const std::vector<double> &firstStageSolution);

  void go();
  double getObjective() const;

  std::vector<double> getPrimalColSolution() const;
  std::vector<double> getDualRowSolution() const;

protected:
  scoped_ptr<FORMULATION> qp;
  scoped_ptr<QpGenData> prob;
  scoped_ptr<QpGenVars> vars;
  scoped_ptr<Residuals> resid;
  scoped_ptr<SOLVER> s;
  vector<int> primalOffsets;
  vector<int> rowToEqIneqIdx;
  vector<int> rowOffsets;
};

namespace{

  void scanRows(CoinPackedMatrix& Wrow,
		std::vector<double>& lb, std::vector<double>& ub,
		int &my, int &mz, int &nnzA, int &nnzC,
		vector<int> &rowToEqIneqIdx)
  {

    my = mz = nnzA = nnzC = 0;

    int nrow = lb.size(), rowi=0;
    for (int i = 0; i < nrow; i++) {
      if (lb[i] == ub[i]) {
	my++;
	nnzA += Wrow.getVectorSize(i);
	rowToEqIneqIdx[rowi++]=my;
      } else {
	mz++;
	nnzC += Wrow.getVectorSize(i);
	rowToEqIneqIdx[rowi++]=-mz-1;
      }
    }
  }

  void separateRows(CoinPackedMatrix& Wrow,
		    std::vector<double>& lb, std::vector<double>& ub,
		    SparseGenMatrix &A, SparseGenMatrix &C)
  {
    int *rowptrA = A.krowM(), *rowptrC = C.krowM();
    int *colidxA = A.jcolM(), *colidxC = C.jcolM();
    double *eltA = A.M(), *eltC = C.M();

    int nnzA = 0, nnzC = 0;
    int nrowA = 0, nrowC = 0;

    int *rowptr, *colidx, *nnz, *nr;
    double *elt;

    int nrow = lb.size();
    for (int i = 0; i < nrow; i++) {
      if (lb[i] == ub[i]) {
	rowptr = rowptrA;
	colidx = colidxA;
	elt = eltA;
	nr = &nrowA;
	nnz = &nnzA;
      } else {
	rowptr = rowptrC;
	colidx = colidxC;
	elt = eltC;
	nr = &nrowC;
	nnz = &nnzC;
      }

      rowptr[(*nr)++] = *nnz;
      for (CoinBigIndex k = Wrow.getVectorFirst(i); k < Wrow.getVectorLast(i); k++) {
	colidx[*nnz] = Wrow.getIndices()[k];
	elt[(*nnz)++] = Wrow.getElements()[k];
      }
    }
    rowptrA[nrowA] = nnzA;
    rowptrC[nrowC] = nnzC;

  }

  //we do not deal with cross terms here
  void formQ(stochasticInput &input, int scenNumber,
	     const CoinPackedMatrix& Qrow,
	     SparseSymMatrix &Q)
  {
    int *rowQ = Q.krowM(), *colidx = Q.jcolM();
    double *elt = Q.M();

    int nnzQ = 0;
    int nrowQ = 0;
    //int offset = 0;

    int nrow = input.nSecondStageVars(scenNumber);
    for (int i = 0; i < nrow; i++) {
      rowQ[nrowQ++] = nnzQ;
      for (CoinBigIndex k = Qrow.getVectorFirst(i); k < Qrow.getVectorLast(i); k++) {
	//colidx[nnzQ] = Qrow.getIndices()[k]+offset;
	colidx[nnzQ] = Qrow.getIndices()[k];
	elt[nnzQ++] = Qrow.getElements()[k];
      }
    }
    rowQ[nrowQ] = nnzQ;
    //offset += nrow;
  }
}

template<typename SOLVER, typename FORMULATION>
OOQPRecourseInterface<SOLVER,FORMULATION>::OOQPRecourseInterface(stochasticInput &input, int scenNumber, const std::vector<double> &firstStageSolution)
{
  int nvar2 = input.nSecondStageVars(scenNumber);
  int ncons2 = input.nSecondStageCons(scenNumber);

  CoinPackedMatrix Wrow, Trow;
  Wrow.reverseOrderedCopyOf(input.getSecondStageConstraints(scenNumber));
  Trow.reverseOrderedCopyOf(input.getLinkingConstraints(scenNumber));

  vector<double> Tx(ncons2);
  Trow.times(&firstStageSolution[0],&Tx[0]);

  vector<double> rowlb = input.getSecondStageRowLB(scenNumber),
    rowub = input.getSecondStageRowUB(scenNumber);

  for (int k = 0; k < ncons2; k++) {
    if (rowub[k] < 1e20) {
      rowub[k] -= Tx[k];
    }
    if (rowlb[k] >-1e20) {
      rowlb[k] -= Tx[k];
    }
  }

  rowToEqIneqIdx.resize(ncons2);

  int my, mz, nnzA, nnzC;
  scanRows(Wrow, rowlb, rowub, my, mz, nnzA, nnzC, rowToEqIneqIdx);

  int nnzQ = input.getSecondStageHessian(scenNumber).getNumElements();
  CoinPackedMatrix Qrow;
  Qrow.reverseOrderedCopyOf(input.getSecondStageHessian(scenNumber));

  qp.reset(new FORMULATION(nvar2,my,mz,nnzQ,nnzA,nnzC));
  prob.reset(dynamic_cast<QpGenData*>(qp->makeData()));

  separateRows(Wrow, rowlb, rowub,
	       dynamic_cast<SparseGenMatrix&>(*prob->A),
	       dynamic_cast<SparseGenMatrix&>(*prob->C));

  formQ(input, scenNumber, Qrow,
	dynamic_cast<SparseSymMatrix&>(*prob->Q));

  // the beauty of OOP...
  double *bA = dynamic_cast<SimpleVector&>(*prob->bA).elements();
  double *bl = dynamic_cast<SimpleVector&>(*prob->bl).elements();
  double *bu = dynamic_cast<SimpleVector&>(*prob->bu).elements();
  double *iclow = dynamic_cast<SimpleVector&>(*prob->iclow).elements();
  double *icupp = dynamic_cast<SimpleVector&>(*prob->icupp).elements();
  double *g = dynamic_cast<SimpleVector&>(*prob->g).elements();
  double *blx = dynamic_cast<SimpleVector&>(*prob->blx).elements();
  double *bux = dynamic_cast<SimpleVector&>(*prob->bux).elements();
  double *ixupp = dynamic_cast<SimpleVector&>(*prob->ixupp).elements();
  double *ixlow = dynamic_cast<SimpleVector&>(*prob->ixlow).elements();

  int eq_idx = 0, ineq_idx = 0;
  //std::vector<double> const &l = input.getSecondStageRowLB(scenNumber);
  //std::vector<double> const &u = input.getSecondStageRowUB(scenNumber);
  int nrow = input.nSecondStageCons(scenNumber);
  for (int k = 0; k < nrow; k++) {
    if (rowlb[k] == rowub[k]) {
      bA[eq_idx++] = rowlb[k];
    } else {
      if (rowlb[k] > -1e20) {
	bl[ineq_idx] = rowlb[k];
	iclow[ineq_idx] = 1.;
      } else {
	bl[ineq_idx] = 0.;
	iclow[ineq_idx] = 0.;
      }
      if (rowub[k] < 1e20) {
	bu[ineq_idx] = rowub[k];
	icupp[ineq_idx] = 1.;
      } else {
	bu[ineq_idx] = 0.;
	icupp[ineq_idx] = 0.;
      }
      ineq_idx++;
    }
  }

  //double RESCALE=1.0;
  int idx = 0;
  std::vector<double> const &l = input.getSecondStageColLB(scenNumber);
  std::vector<double> const &u = input.getSecondStageColUB(scenNumber);
  std::vector<double> const &c = input.getSecondStageObj(scenNumber);
  int ncol = input.nSecondStageVars(scenNumber);
  for (int k = 0; k < ncol; k++) {
    //g[idx] = RESCALE*c[k];
    g[idx] = c[k];
    if (l[k] > -1e20) {
      blx[idx] = l[k];
      ixlow[idx] = 1.;
    } else {
      blx[idx] = 0.;
      ixlow[idx] = 0.;
    }
    if (u[k] < 1e20) {
      bux[idx] = u[k];
      ixupp[idx] = 1.;
    } else {
      bux[idx] = 0.;
      ixupp[idx] = 0.;
    }
    idx++;
  }

  vars.reset(dynamic_cast<QpGenVars*>(qp->makeVariables(prob.get())));
  resid.reset(qp->makeResiduals(prob.get()));
  s.reset(new SOLVER(qp.get(),prob.get()));

}

template<typename S, typename F>
void OOQPRecourseInterface<S,F>::go()
{
  //s->monitorSelf();
#ifdef TIMING
  int result = s->solve(prob.get(),vars.get(),resid.get());

  if ( 0 == result ) {
    double objective = prob->objectiveValue(vars.get());

    cout << " " << prob->nx << " variables, "
   	 << prob->my  << " equality constraints, "
   	 << prob->mz  << " inequality constraints.\n";

    cout << " Iterates: " << s->iter
   	 <<",    Optimal Solution:  " << objective << endl;
  }
#else
  s->solve(prob.get(),vars.get(),resid.get());
#endif
}

template<typename S, typename F>
double OOQPRecourseInterface<S,F>::getObjective() const {
	return prob->objectiveValue(vars.get());
}

template<typename S, typename F>
std::vector<double> OOQPRecourseInterface<S,F>::getPrimalColSolution() const
{
  double const *sol = &dynamic_cast<SimpleVector const&>(*vars->x)[0];
  return std::vector<double>(sol, sol+vars->x->length());
}

template<typename S, typename F>
std::vector<double> OOQPRecourseInterface<S,F>::getDualRowSolution() const
{
  std::vector<double> out;
  out.reserve(vars->y->length()+vars->z->length());

  double const *eqsol = &dynamic_cast<SimpleVector const&>(*vars->y)[0];
  double const *ineqsol = &dynamic_cast<SimpleVector const&>(*vars->z)[0];

  for(int idx=0; idx<vars->y->length()+vars->z->length(); idx++) {
    int r=rowToEqIneqIdx[idx];
    if(r>=0) out.push_back(eqsol[r]);
    else     out.push_back(ineqsol[-r-1]);
  }
  return out;
}

#endif


#include <gtest/gtest.h>
#include <boost/math/distributions.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <stan/math/prim/mat.hpp>
#include <math/prim/mat/prob/vector_rng_test_helper.hpp>
#include <math/prim/mat/prob/VectorIntRNGTestRig.hpp>
#include <limits>
#include <vector>

class NegativeBinomial2TestRig : public VectorIntRNGTestRig {
 public:
  NegativeBinomial2TestRig()
      : VectorIntRNGTestRig(10000, 10, {0, 1, 2, 3, 4, 5, 6}, {0.1, 1.7, 3.99},
                            {1, 2, 3}, {-2.1, -0.5, 0.0}, {-3, -1, 0},
                            {0.1, 1.1, 4.99}, {1, 2, 3}, {-3.0, -2.0, 0.0},
                            {-3, -1, 0}) {}

  template <typename T1, typename T2, typename T3, typename T_rng>
  auto generate_samples(const T1& mu, const T2& phi, const T3&,
                        T_rng& rng) const {
    return stan::math::neg_binomial_2_rng(mu, phi, rng);
  }

  template <typename T1>
  double pmf(int y, T1 mu, double phi, double) const {
    return std::exp(stan::math::neg_binomial_2_lpmf(y, mu, phi));
  }
};

TEST(ProbDistributionsNegativeBinomial2, errorCheck) {
  check_dist_throws_all_types(NegativeBinomial2TestRig());
}

TEST(ProbDistributionsNegativeBinomial2, distributionCheck) {
  check_counts_real_real(NegativeBinomial2TestRig());
}


//
// Created by mateusz
//
#include <boost/test/unit_test.hpp>
#include <cpuclassifier/CPULearnColumn.hpp>
#include <numeric>

BOOST_AUTO_TEST_SUITE(nbc4cpu_TestSuite)

BOOST_AUTO_TEST_SUITE(LearnColumn_TestSuite)

BOOST_AUTO_TEST_CASE(PrecalulatedTest_Size6) {

  std::vector<float> vec = {1, 1, 1, 2, 2, 2};

  nbc4cpu::CPULearnColumn<float> learner =
      nbc4cpu::CPULearnColumn<float>(vec);
  const auto res = learner();

  BOOST_CHECK_CLOSE(res.first, 1.5, 0.1);
  BOOST_CHECK_CLOSE(res.second, 0.3, 0.1);
}

BOOST_AUTO_TEST_CASE(PrecalulatedTest_Size12) {

  std::vector<float> vec = {91, 11, 1111, 32, 26, 2, 22, 894, 345};

  nbc4cpu::CPULearnColumn<float> learner =
      nbc4cpu::CPULearnColumn<float>(vec);
  const auto res = learner();

  BOOST_CHECK_CLOSE(res.first, 281.55555555556, 0.1);
  BOOST_CHECK_CLOSE(res.second, 181213.77777778, 0.1);
}

BOOST_AUTO_TEST_CASE(CompareWithCpu_LargeVectorTest) {

  srand(1234); // set rand seed to make test repeatable

  std::vector<float> vec(500000);
  std::generate(vec.begin(), vec.end(), []() { return rand() % 250; });

  nbc4cpu::CPULearnColumn<float> learner =
      nbc4cpu::CPULearnColumn<float>(vec);
  const auto res = learner();

  float sum = 0.0;
  sum       = std::accumulate(vec.begin(), vec.end(), sum);
  float avg = sum / static_cast<float>(vec.size());

  BOOST_CHECK_CLOSE(res.first, avg, 1);

  float variance = 0;
  for (float i : vec) {
    float dif = i - avg;
    variance += dif * dif;
  }
  variance = variance / static_cast<float>(vec.size() - 1);

  BOOST_CHECK_CLOSE(res.second, variance, 1);
}

BOOST_AUTO_TEST_CASE(PrecalulatedTest_Size6_double) {

  std::vector<double> vec = {1, 1, 1, 2, 2, 2};

  nbc4cpu::CPULearnColumn<double> learner =
      nbc4cpu::CPULearnColumn<double>(vec);
  const auto res = learner();

  BOOST_CHECK_CLOSE(res.first, 1.5, 0.1);
  BOOST_CHECK_CLOSE(res.second, 0.3, 0.1);
}

BOOST_AUTO_TEST_CASE(PrecalulatedTest_Size12_double) {

  std::vector<double> vec = {91, 11, 1111, 32, 26, 2, 22, 894, 345};

  nbc4cpu::CPULearnColumn<double> learner =
      nbc4cpu::CPULearnColumn<double>(vec);
  const auto res = learner();

  BOOST_CHECK_CLOSE(res.first, 281.55555555556, 0.1);
  BOOST_CHECK_CLOSE(res.second, 181213.77777778, 0.1);
}

BOOST_AUTO_TEST_CASE(CompareWithCpu_LargeVectorTest_double) {

  srand(1234); // set rand seed to make test repeatable

  std::vector<double> vec(5000000);
  std::generate(vec.begin(), vec.end(), []() { return rand() % 250; });

  nbc4cpu::CPULearnColumn<double> learner =
      nbc4cpu::CPULearnColumn<double>(vec);
  const auto res = learner();

  double sum = 0.0;
  sum        = std::accumulate(vec.begin(), vec.end(), sum);
  double avg = sum / static_cast<double>(vec.size());

  BOOST_CHECK_CLOSE(res.first, avg, 1);

  double variance = 0;
  for (double i : vec) {
    double dif = i - avg;
    variance += dif * dif;
  }
  variance = variance / static_cast<double>(vec.size() - 1);

  BOOST_CHECK_CLOSE(res.second, variance, 1);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/*
*
*   Tutorial: Algebraic multigrid preconditioner (only available with the OpenCL backend, experimental)
*
*/

#ifndef NDEBUG     //without NDEBUG the performance of sparse ublas matrices is poor.
 #define NDEBUG
#endif

#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>

#define VIENNACL_WITH_UBLAS 1

#define SOLVER_ITERS 2500
//#define SCALAR float
#define SCALAR double

//#define SOLVER_TOLERANCE 1e-5
#define SOLVER_TOLERANCE 1e-9

#include "viennacl/vector.hpp"
#include "viennacl/coordinate_matrix.hpp"
#include "viennacl/compressed_matrix.hpp"
#include "viennacl/linalg/ilu.hpp"
#include "viennacl/linalg/cg.hpp"
#include "viennacl/linalg/bicgstab.hpp"
#include "viennacl/io/matrix_market.hpp"
#include "viennacl/linalg/norm_2.hpp"

#include "viennacl/linalg/amg.hpp"

#include <iostream>
#include <vector>
#include <ctime>
#include "vector-io.hpp"

template<typename MatrixType, typename VectorType, typename SolverTag, typename PrecondTag>
void run_solver(MatrixType const & matrix, VectorType const & rhs, VectorType const & ref_result, SolverTag const & solver, PrecondTag const & precond)
{
  VectorType result(rhs);
  VectorType residual(rhs);

  result = viennacl::linalg::solve(matrix, rhs, solver, precond);
  residual -= viennacl::linalg::prod(matrix, result);
  std::cout << "  > Relative residual: " << viennacl::linalg::norm_2(residual) / viennacl::linalg::norm_2(rhs) << std::endl;
  std::cout << "  > Iterations: " << solver.iters() << std::endl;
  result -= ref_result;
  std::cout << "  > Relative deviation from result: " << viennacl::linalg::norm_2(result) / viennacl::linalg::norm_2(ref_result) << std::endl;
}

template<typename ScalarType>
void run_amg(viennacl::linalg::cg_tag & cg_solver,
             boost::numeric::ublas::vector<ScalarType> & /*ublas_vec*/,
             boost::numeric::ublas::vector<ScalarType> & /*ublas_result*/,
             boost::numeric::ublas::compressed_matrix<ScalarType> & ublas_matrix,
             viennacl::vector<ScalarType> & vcl_vec,
             viennacl::vector<ScalarType> & vcl_result,
             viennacl::compressed_matrix<ScalarType> & vcl_compressed_matrix,
             std::string info,
             viennacl::linalg::amg_tag & amg_tag)
{

  viennacl::linalg::amg_precond<boost::numeric::ublas::compressed_matrix<ScalarType> > ublas_amg = viennacl::linalg::amg_precond<boost::numeric::ublas::compressed_matrix<ScalarType> > (ublas_matrix, amg_tag);
  boost::numeric::ublas::vector<ScalarType> avgstencil;
  unsigned int coarselevels = amg_tag.get_coarselevels();

  std::cout << "-- CG with AMG preconditioner, " << info << " --" << std::endl;

  std::cout << " * Setup phase (ublas types)..." << std::endl;

  // Coarse level measure might have been changed during setup. Reload!
  ublas_amg.tag().set_coarselevels(coarselevels);
  ublas_amg.setup();

  std::cout << " * Operator complexity: " << ublas_amg.calc_complexity(avgstencil) << std::endl;

  amg_tag.set_coarselevels(coarselevels);
  viennacl::linalg::amg_precond<viennacl::compressed_matrix<ScalarType> > vcl_amg = viennacl::linalg::amg_precond<viennacl::compressed_matrix<ScalarType> > (vcl_compressed_matrix, amg_tag);
  std::cout << " * Setup phase (ViennaCL types)..." << std::endl;
  vcl_amg.tag().set_coarselevels(coarselevels);
  vcl_amg.setup();

  std::cout << " * CG solver (ublas types)..." << std::endl;
  //run_solver(ublas_matrix, ublas_vec, ublas_result, cg_solver, ublas_amg);

  std::cout << " * CG solver (ViennaCL types)..." << std::endl;
  run_solver(vcl_compressed_matrix, vcl_vec, vcl_result, cg_solver, vcl_amg);

}

int main()
{
  //
  // Print some device info
  //
  std::cout << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << "               Device Info" << std::endl;
  std::cout << "----------------------------------------------" << std::endl;

#ifdef VIENNACL_WITH_OPENCL
  // Optional: Customize OpenCL backend
  viennacl::ocl::platform pf = viennacl::ocl::get_platforms()[0];
  std::vector<viennacl::ocl::device> const & devices = pf.devices();

  // Optional: Set first device to first context:
  viennacl::ocl::setup_context(0, devices[0]);

  // Optional: Set second device for second context (use the same device for the second context if only one device available):
  if (devices.size() > 1)
    viennacl::ocl::setup_context(1, devices[1]);
  else
    viennacl::ocl::setup_context(1, devices[0]);

  std::cout << viennacl::ocl::current_device().info() << std::endl;
  viennacl::context ctx(viennacl::ocl::get_context(1));
#else
  viennacl::context ctx;
#endif

  typedef float    ScalarType;  // feel free to change this to double if supported by your device

  //
  // Set up the matrices and vectors for the iterative solvers (cf. iterative.cpp)
  //
  boost::numeric::ublas::vector<ScalarType> ublas_vec, ublas_result;
  boost::numeric::ublas::compressed_matrix<ScalarType> ublas_matrix;

  viennacl::linalg::cg_tag cg_solver;
  viennacl::linalg::amg_tag amg_tag;
  viennacl::linalg::amg_precond<boost::numeric::ublas::compressed_matrix<ScalarType> > ublas_amg;

  // Read matrix
  if (!viennacl::io::read_matrix_market_file(ublas_matrix, "../examples/testdata/mat65k.mtx"))
  {
    std::cout << "Error reading Matrix file" << std::endl;
    return EXIT_FAILURE;
  }

  // Set up rhs and result vector
  if (!readVectorFromFile("../examples/testdata/rhs65025.txt", ublas_vec))
  {
    std::cout << "Error reading RHS file" << std::endl;
    return 0;
  }

  if (!readVectorFromFile("../examples/testdata/result65025.txt", ublas_result))
  {
    std::cout << "Error reading Result file" << std::endl;
    return 0;
  }

  viennacl::vector<ScalarType> vcl_vec(ublas_vec.size(), ctx);
  viennacl::vector<ScalarType> vcl_result(ublas_vec.size(), ctx);
  viennacl::compressed_matrix<ScalarType> vcl_compressed_matrix(ublas_vec.size(), ublas_vec.size(), ctx);

  // Copy to GPU
  viennacl::copy(ublas_matrix, vcl_compressed_matrix);
  viennacl::copy(ublas_vec, vcl_vec);
  viennacl::copy(ublas_result, vcl_result);

  //
  // Run solver without preconditioner
  //
  std::cout << "-- CG solver (CPU, no preconditioner) --" << std::endl;
  run_solver(ublas_matrix, ublas_vec, ublas_result, cg_solver, viennacl::linalg::no_precond());

  std::cout << "-- CG solver (GPU, no preconditioner) --" << std::endl;
  run_solver(vcl_compressed_matrix, vcl_vec, vcl_result, cg_solver, viennacl::linalg::no_precond());

  //
  // With AMG Preconditioner RS+DIRECT
  //
  amg_tag = viennacl::linalg::amg_tag(VIENNACL_AMG_COARSE_RS,       // coarsening strategy
                                      VIENNACL_AMG_INTERPOL_DIRECT, // interpolation strategy
                                      0.25, // strength of dependence threshold
                                      0.2,  // interpolation weight
                                      0.67, // jacobi smoother weight
                                      3,    // presmoothing steps
                                      3,    // postsmoothing steps
                                      0);   // number of coarse levels to be used (0: automatically use as many as reasonable)
  run_amg (cg_solver, ublas_vec, ublas_result, ublas_matrix, vcl_vec, vcl_result, vcl_compressed_matrix, "RS COARSENING, DIRECT INTERPOLATION", amg_tag);

  //
  // With AMG Preconditioner RS+CLASSIC
  //
  amg_tag = viennacl::linalg::amg_tag(VIENNACL_AMG_COARSE_RS, VIENNACL_AMG_INTERPOL_CLASSIC, 0.25, 0.2, 0.67, 3, 3, 0);
  run_amg ( cg_solver, ublas_vec, ublas_result, ublas_matrix, vcl_vec, vcl_result, vcl_compressed_matrix, "RS COARSENING, CLASSIC INTERPOLATION", amg_tag);

  //
  // With AMG Preconditioner ONEPASS+DIRECT
  //
  amg_tag = viennacl::linalg::amg_tag(VIENNACL_AMG_COARSE_ONEPASS, VIENNACL_AMG_INTERPOL_DIRECT,0.25, 0.2, 0.67, 3, 3, 0);
  run_amg (cg_solver, ublas_vec, ublas_result, ublas_matrix, vcl_vec, vcl_result, vcl_compressed_matrix, "ONEPASS COARSENING, DIRECT INTERPOLATION", amg_tag);

  //
  // With AMG Preconditioner RS0+DIRECT
  //
  amg_tag = viennacl::linalg::amg_tag(VIENNACL_AMG_COARSE_RS0, VIENNACL_AMG_INTERPOL_DIRECT, 0.25, 0.2, 0.67, 3, 3, 0);
  run_amg (cg_solver, ublas_vec, ublas_result, ublas_matrix, vcl_vec, vcl_result, vcl_compressed_matrix, "RS0 COARSENING, DIRECT INTERPOLATION", amg_tag);

  //
  // With AMG Preconditioner RS3+DIRECT
  //
  amg_tag = viennacl::linalg::amg_tag(VIENNACL_AMG_COARSE_RS3, VIENNACL_AMG_INTERPOL_DIRECT, 0.25, 0.2, 0.67, 3, 3, 0);
  run_amg (cg_solver, ublas_vec, ublas_result, ublas_matrix, vcl_vec, vcl_result, vcl_compressed_matrix, "RS3 COARSENING, DIRECT INTERPOLATION", amg_tag);

  //
  // With AMG Preconditioner AG
  //
  amg_tag = viennacl::linalg::amg_tag(VIENNACL_AMG_COARSE_AG, VIENNACL_AMG_INTERPOL_AG, 0.08, 0, 0.67, 3, 3, 0);
  run_amg (cg_solver, ublas_vec, ublas_result, ublas_matrix, vcl_vec, vcl_result, vcl_compressed_matrix, "AG COARSENING, AG INTERPOLATION", amg_tag);

  //
  // With AMG Preconditioner SA
  //
  amg_tag = viennacl::linalg::amg_tag(VIENNACL_AMG_COARSE_AG, VIENNACL_AMG_INTERPOL_SA, 0.08, 0.67, 0.67, 3, 3, 0);
  run_amg (cg_solver, ublas_vec, ublas_result, ublas_matrix, vcl_vec, vcl_result, vcl_compressed_matrix, "AG COARSENING, SA INTERPOLATION",amg_tag);

  //
  //  That's it.
  //
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;

  return EXIT_SUCCESS;
}


#include <boost/compute/random/normal_distribution.hpp>


#define BOOST_TEST_MODULE "test_PWMcos_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/forcefield/PWMcos/PWMcosPotential.hpp>

BOOST_AUTO_TEST_CASE(PWMcos_Potential_f)
{
    mjolnir::LoggerManager::set_default_logger("test_pwmcos_potential.log");

    using real_type = double;
    using potential_type = mjolnir::PWMcosPotential<real_type>;

    constexpr std::size_t N = 1000;
    constexpr real_type   h = 1e-6;

    potential_type potential;

    const real_type r_0   = 7.0;
    const real_type r_min = 1.0;
    const real_type r_max = 15.0;
    const real_type dr    = (r_max - r_min) / N;

    const real_type rsigma = 1.0;

    for(std::size_t i=0; i<N; ++i)
    {
        const real_type r   = r_min + i * dr;
        const real_type f1  = potential.f(r_0, r + h, rsigma);
        const real_type f2  = potential.f(r_0, r - h, rsigma);
        const real_type df_numeric  = (f1 - f2) / (2 * h);
        const real_type df_analytic = potential.f_df(r_0, r, rsigma).second;

        if((f1 == 0.0 || f2 == 0.0) && !(f1 == 0.0 && f2 == 0.0))
        {
            // the numeric differentiation becomes unstable here.
        }
        else
        {
            BOOST_TEST(df_numeric == df_analytic, boost::test_tools::tolerance(h));
        }

        const real_type f1_ = potential.f_df(r_0, r+h, rsigma).first;
        const real_type f2_ = potential.f_df(r_0, r-h, rsigma).first;
        BOOST_TEST(f1 == f1_, boost::test_tools::tolerance(h));
        BOOST_TEST(f2 == f2_, boost::test_tools::tolerance(h));
    }
}

BOOST_AUTO_TEST_CASE(PWMcos_Potential_g)
{
    mjolnir::LoggerManager::set_default_logger("test_pwmcos_potential.log");

    using real_type = double;
    using potential_type = mjolnir::PWMcosPotential<real_type>;

    constexpr std::size_t N = 1000;
    constexpr real_type   h = 1e-6;
    constexpr real_type  pi = mjolnir::math::constants<real_type>::pi();

    potential_type potential;

    const real_type theta_0   = pi * 0.5;
    const real_type theta_min = pi * 0.3;
    const real_type theta_max = pi * 0.7;
    const real_type dtheta    = (theta_max - theta_min) / N;

    const real_type delta = pi / 18.0;
    const real_type delta2 = delta * 2;
    const real_type pi_over_2delta = pi / delta2;

    for(std::size_t i=0; i<N; ++i)
    {
        const real_type theta = theta_min + i * dtheta;
        const real_type g1  = potential.g(theta_0, theta + h, delta, delta2, pi_over_2delta);
        const real_type g2  = potential.g(theta_0, theta - h, delta, delta2, pi_over_2delta);
        const real_type dg_numeric  = (g1 - g2) / (2 * h);
        const real_type dg_analytic = potential.g_dg(theta_0, theta, delta, delta2, pi_over_2delta).second;

        if((g1 == 0.0 || g2 == 0.0) && !(g1 == 0.0 && g2 == 0.0))
        {
            // the numeric differentiation becomes unstable here.
        }
        else
        {
            BOOST_TEST(dg_numeric == dg_analytic, boost::test_tools::tolerance(h));
        }
        const real_type g1_ = potential.g_dg(theta_0, theta+h, delta, delta2, pi_over_2delta).first;
        const real_type g2_ = potential.g_dg(theta_0, theta-h, delta, delta2, pi_over_2delta).first;
        BOOST_TEST(g1 == g1_, boost::test_tools::tolerance(h));
        BOOST_TEST(g2 == g2_, boost::test_tools::tolerance(h));
    }
}


#include <geometry.h>
#include <tiny.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

typedef tiny::MathTypes<float> MT;
typedef MT::quaternion_type    Q;
typedef MT::vector3_type       V;
typedef MT::real_type          T;
typedef MT::value_traits       VT;

BOOST_AUTO_TEST_SUITE(geometry);

BOOST_AUTO_TEST_CASE(overlap_obb_obb_test)
{
  // touching bottom-top faces
  {
    V const centerA   = V::make(0.0, 0.0, 0.0);
    V const half_extA = V::make(1.0, 1.0, 1.0);
    Q const qA        = Q::identity();

    geometry::OBB<MT> obbA = geometry::make_obb<MT>(centerA, qA, half_extA);

    V const centerB   = V::make(0.0, -2.0, 0.0);
    V const half_extB = V::make(1.0, 1.0, 1.0);
    Q const qB        = Q::Ry(VT::pi_quarter());

    geometry::OBB<MT> obbB = geometry::make_obb<MT>(centerB, qB, half_extB);

    bool const test1 = geometry::overlap_obb_obb(obbA,obbB);
    bool const test2 = geometry::overlap_obb_obb(obbB,obbA);

    BOOST_CHECK(test1);
    BOOST_CHECK(test2);
  }
  // touching left-right faces
  {
    V const centerA   = V::make(0.0, 0.0, 0.0);
    V const half_extA = V::make(1.0, 1.0, 1.0);
    Q const qA        = Q::identity();

    geometry::OBB<MT> obbA = geometry::make_obb<MT>(centerA, qA, half_extA);

    V const centerB   = V::make(2.0, 0.0, 0.0);
    V const half_extB = V::make(1.0, 1.0, 1.0);
    Q const qB        = Q::Rx(VT::pi_quarter());

    geometry::OBB<MT> obbB = geometry::make_obb<MT>(centerB, qB, half_extB);

    bool const test1 = geometry::overlap_obb_obb(obbA,obbB);
    bool const test2 = geometry::overlap_obb_obb(obbB,obbA);

    BOOST_CHECK(test1);
    BOOST_CHECK(test2);
  }
  // touching front-back faces
  {
    V const centerA   = V::make(0.0, 0.0, 0.0);
    V const half_extA = V::make(1.0, 1.0, 1.0);
    Q const qA        = Q::identity();

    geometry::OBB<MT> obbA = geometry::make_obb<MT>(centerA, qA, half_extA);

    V const centerB   = V::make(0.0, 0.0, 2.0);
    V const half_extB = V::make(1.0, 1.0, 1.0);
    Q const qB        = Q::Rz(VT::pi_quarter());

    geometry::OBB<MT> obbB = geometry::make_obb<MT>(centerB, qB, half_extB);

    bool const test1 = geometry::overlap_obb_obb(obbA,obbB);
    bool const test2 = geometry::overlap_obb_obb(obbB,obbA);

    BOOST_CHECK(test1);
    BOOST_CHECK(test2);
  }
  // separating bottom-top faces
  {
    V const centerA   = V::make(0.0, 0.0, 0.0);
    V const half_extA = V::make(1.0, 1.0, 1.0);
    Q const qA        = Q::identity();

    geometry::OBB<MT> obbA = geometry::make_obb<MT>(centerA, qA, half_extA);

    V const centerB   = V::make(0.0, -2.1, 0.0);
    V const half_extB = V::make(1.0, 1.0, 1.0);
    Q const qB        = Q::Ry(VT::pi_quarter());

    geometry::OBB<MT> obbB = geometry::make_obb<MT>(centerB, qB, half_extB);

    bool const test1 = geometry::overlap_obb_obb(obbA,obbB);
    bool const test2 = geometry::overlap_obb_obb(obbB,obbA);

    BOOST_CHECK(!test1);
    BOOST_CHECK(!test2);
  }
  // separating left-right faces
  {
    V const centerA   = V::make(0.0, 0.0, 0.0);
    V const half_extA = V::make(1.0, 1.0, 1.0);
    Q const qA        = Q::identity();

    geometry::OBB<MT> obbA = geometry::make_obb<MT>(centerA, qA, half_extA);

    V const centerB   = V::make(2.1, 0.0, 0.0);
    V const half_extB = V::make(1.0, 1.0, 1.0);
    Q const qB        = Q::Rx(VT::pi_quarter());

    geometry::OBB<MT> obbB = geometry::make_obb<MT>(centerB, qB, half_extB);

    bool const test1 = geometry::overlap_obb_obb(obbA,obbB);
    bool const test2 = geometry::overlap_obb_obb(obbB,obbA);

    BOOST_CHECK(!test1);
    BOOST_CHECK(!test2);
  }
  // separating front-back faces
  {
    V const centerA   = V::make(0.0, 0.0, 0.0);
    V const half_extA = V::make(1.0, 1.0, 1.0);
    Q const qA        = Q::identity();

    geometry::OBB<MT> obbA = geometry::make_obb<MT>(centerA, qA, half_extA);

    V const centerB   = V::make(0.0, 0.0, 2.1);
    V const half_extB = V::make(1.0, 1.0, 1.0);
    Q const qB        = Q::Rz(VT::pi_quarter());

    geometry::OBB<MT> obbB = geometry::make_obb<MT>(centerB, qB, half_extB);

    bool const test1 = geometry::overlap_obb_obb(obbA,obbB);
    bool const test2 = geometry::overlap_obb_obb(obbB,obbA);

    BOOST_CHECK(!test1);
    BOOST_CHECK(!test2);
  }
  // A inside of B
  {
    V const centerA   = V::make(0.0, 0.0, 0.0);
    V const half_extA = V::make(0.5, 0.5, 0.5);
    Q const qA        = Q::identity();

    geometry::OBB<MT> obbA = geometry::make_obb<MT>(centerA, qA, half_extA);

    V const centerB   = V::make(0.0, 0.0, 0.0);
    V const half_extB = V::make(1.0, 1.0, 1.0);
    Q const qB        = Q::Ry(VT::pi_quarter());

    geometry::OBB<MT> obbB = geometry::make_obb<MT>(centerB, qB, half_extB);

    bool const test1 = geometry::overlap_obb_obb(obbA,obbB);
    bool const test2 = geometry::overlap_obb_obb(obbB,obbA);

    BOOST_CHECK(test1);
    BOOST_CHECK(test2);
  }
  // B inside of A
  {
    V const centerA   = V::make(0.0, 0.0, 0.0);
    V const half_extA = V::make(1.0, 1.0, 1.0);
    Q const qA        = Q::identity();

    geometry::OBB<MT> obbA = geometry::make_obb<MT>(centerA, qA, half_extA);

    V const centerB   = V::make(0.0, 0.0, 0.0);
    V const half_extB = V::make(0.5, 0.5, 0.5);
    Q const qB        = Q::Ry(VT::pi_quarter());

    geometry::OBB<MT> obbB = geometry::make_obb<MT>(centerB, qB, half_extB);

    bool const test1 = geometry::overlap_obb_obb(obbA,obbB);
    bool const test2 = geometry::overlap_obb_obb(obbB,obbA);

    BOOST_CHECK(test1);
    BOOST_CHECK(test2);
  }
  // touching edge-edge-case
  {
    V const centerA   = V::make(0.0, 0.0, 0.0);
    V const half_extA = V::make(1.0, 1.0, 1.0);
    Q const qA        = Q::identity();

    geometry::OBB<MT> obbA = geometry::make_obb<MT>(centerA, qA, half_extA);

    V const centerB   = V::make(2.0, 2.0, 0.0);
    V const half_extB = V::make(1.0, 1.0, 1.0);
    Q const qB        = Q::Ru(VT::pi_half(), centerB );

    geometry::OBB<MT> obbB = geometry::make_obb<MT>(centerB, qB, half_extB);

    bool const test1 = geometry::overlap_obb_obb(obbA,obbB);
    bool const test2 = geometry::overlap_obb_obb(obbB,obbA);

    BOOST_CHECK(test1);
    BOOST_CHECK(test2);
  }
  // separating edge-edge-case
  {
    V const centerA   = V::make(0.0, 0.0, 0.0);
    V const half_extA = V::make(1.0, 1.0, 1.0);
    Q const qA        = Q::identity();

    geometry::OBB<MT> obbA = geometry::make_obb<MT>(centerA, qA, half_extA);

    V const centerB   = V::make(2.01, 2.01, 0.0);
    V const half_extB = V::make(1.0, 1.0, 1.0);
    Q const qB        = Q::Ru(VT::pi_half(), centerB );

    geometry::OBB<MT> obbB = geometry::make_obb<MT>(centerB, qB, half_extB);

    bool const test1 = geometry::overlap_obb_obb(obbA,obbB);
    bool const test2 = geometry::overlap_obb_obb(obbB,obbA);

    BOOST_CHECK(!test1);
    BOOST_CHECK(!test2);
  }

}

BOOST_AUTO_TEST_SUITE_END();