/* Copyright (c) 2020 C. Pattison
 * 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 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.
 */

#pragma once
#include <cassert>
#include <algorithm>
#include <utility>
#include <complex>
#include <cstdint>
#include <exception>

#include <Eigen/Dense>
#include "syk_types.hpp"
#include "util.hpp"

/** Utilities for working with a Hamiltonian as a sum of Pauli operators
 */
namespace syk {
using namespace std::complex_literals;

/** Pauli group
 */

enum class AbstractPauli : short {I = 0, X = 1, Y = 2, Z = 3};
enum class Sign : short {P = 1, M = -1};
struct Pauli { AbstractPauli m; Sign s; };

Sign operator*(Sign a, Sign b) {
    return (a == b) ? Sign::P : Sign::M;
}

Sign operator!(Sign a) {
    return a * Sign::M;
}

Pauli operator*(Pauli a, Pauli b) {
    auto new_sign = a.s * b.s;
    if( !(static_cast<int>(a.m) <= static_cast<int>(b.m)) ) {
        std::swap(a, b);
        new_sign = (
                a.m == AbstractPauli::I
            ||  b.m == AbstractPauli::I
            ||  a.m == b.m) ? new_sign : !new_sign;
    }

    if (a.m == AbstractPauli::I) { return Pauli {b.m, new_sign}; }
    if (a.m == b.m) { return Pauli {AbstractPauli::I, new_sign}; }
    return Pauli {
        static_cast<AbstractPauli>(3 - (static_cast<int>(a.m) + static_cast<int>(b.m))%3),
        new_sign};
}

/** Utilities for construction Hamiltonian as a sum of products of Paulis
 * This has a hard coded maximum number of qubits (20) for performance
 * If/When increased it should be a multiple of 4 to allow vectorization
 *
 * Constructing the full Hamiltonian is done coefficient-wise by going through each term in the sum
 * For each term we stop when we hit a 0
 * This is branch heavy but was about 2x faster than without the branch
 */
struct PauliRep {
    static constexpr int kmax_qubits = 20;
    using PauliString = std::array<Pauli, kmax_qubits>;
    using PauliMatrix = Eigen::Matrix<std::complex<short>, 2, 2>;

    struct Term {
        PauliString pauli_op;
        double weight;
    };

protected:
    int num_qubits_;

public:
    PauliMatrix m_[4];

    PauliRep(int num_qubits) : num_qubits_(num_qubits) {
        if(num_qubits > kmax_qubits) { throw std::runtime_error("Number of qubits exceeds maximum support. Increase the compile time value"); }

        m_[0] << 1, 0,
                0, 1;

        m_[1] << 0, 1,
                1, 0;

        m_[2] << 0, -1i,
                1i, 0;

        m_[3] << 1, 0,
                0, -1;
    }

    /** Get sign
     */
    short repp(Sign a) const __attribute__((always_inline)) {
        return static_cast<short>(a);
    }

    /** Get matrix for a one qubit Pauli
     */
    const PauliMatrix& repp(AbstractPauli a) const __attribute__((always_inline)) {
        return m_[static_cast<short>(a)];
    }

    auto get_repp_factor(Pauli a) const __attribute__((always_inline)) {
        return repp(a.m) * repp(a.s);
    }

    Pauli PauliI() const { return Pauli {AbstractPauli::I, Sign::P}; }
    Pauli PauliX() const { return Pauli {AbstractPauli::X, Sign::P}; }
    Pauli PauliY() const { return Pauli {AbstractPauli::Y, Sign::P}; }
    Pauli PauliZ() const { return Pauli {AbstractPauli::Z, Sign::P}; }

    /** Single Pauli matrix element
     */
    std::complex<short> get_pauli_element(int i, int j, Pauli p) const __attribute__((always_inline)) {
        return repp(p.m)(i,j) * repp(p.s);
    }

    /** Convert a short taking values 0, +/- 1 to double
     */
    std::complex<double> zero_one_short_to_double(std::complex<short> a) const {
        return std::complex<double>(
            a.real() == 0 ? 0.0 : (a.real() > 0 ? 1.0 : -1.0),
            a.imag() == 0 ? 0.0 : (a.imag() > 0 ? 1.0 : -1.0)
        );
    }

    /** Get matrix element of a string of Paulis
     * shorts everywhere to hopefully let the compiler do its thing
     */
    std::complex<double> get_matrix_element(int i, int j, const PauliString& string) {
        std::complex<short> acc(1,0);
        int max_level = num_qubits_ - 1;

        #pragma GCC unroll 4
        for(int level = 0; level <= max_level; ++level) {
            int mask = 1 << level;
            int sub_i = (i & mask) >> level;
            int sub_j = (j & mask) >> level;
            acc *= get_pauli_element(sub_i, sub_j, string[level]);
            if(acc.real() == 0 && acc.imag() == 0) { break; }
        }
        return zero_one_short_to_double(acc);
    }

    /** Compute the Hamiltonian for some sum of terms
     * Give only the cols/rows specified by subspace
     * Subspace is usually 0 to (2^N - 1) but could be something else where symmetries exist
     */
    MatrixType get_hamiltonian(std::vector<Term> terms, std::vector<std::uint64_t> subspace) {
        MatrixType hamiltonian = MatrixType::Zero(subspace.size(), subspace.size());

        #pragma omp parallel for
        for(int j = 0; j < subspace.size(); ++j) {
            for(int i = 0; i < subspace.size(); ++i) {
                double matrix_element_re = 0.0;
                double matrix_element_im = 0.0;
                auto num_terms = terms.size();

                for(int k = 0; k < num_terms; ++k) {
                    auto single_element = get_matrix_element(subspace[i], subspace[j], terms[k].pauli_op);
                    matrix_element_re += terms[k].weight * single_element.real();
                    matrix_element_im += terms[k].weight * single_element.imag();
                }
                hamiltonian(i,j) = std::complex<double>(matrix_element_re, matrix_element_im);
            }
        }

        return hamiltonian;
    }
};
}


/*!
 * @file
 * Defines `boost::mpl11::detail::logical_or::strict_constexpr_array`.
 *
 *
 * @copyright Louis Dionne 2014
 * Distributed under the Boost Software License, Version 1.0.
 *         (See accompanying file LICENSE.md or copy at
 *             http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_MPL11_DETAIL_LOGICAL_OR_STRICT_CONSTEXPR_ARRAY_HPP
#define BOOST_MPL11_DETAIL_LOGICAL_OR_STRICT_CONSTEXPR_ARRAY_HPP

#include <boost/mpl11/bool.hpp>
#include <boost/mpl11/detail/std_size_t.hpp>

namespace boost { namespace mpl11 { namespace detail { namespace logical_or {
    template <std_size_t N>
    constexpr bool strict_constexpr_array_impl(const bool (&array)[N]) {
        // Note: GCC 4.9 does not like range-based for loop in constexpr.
        for (std_size_t i = 0; i < N; ++i)
            if (array[i])
                return true;
        return false;
    }

    /*!
     * @ingroup details
     *
     * Strict (non short-circuiting) `logical or` implemented with constexpr.
     */
    template <typename ...xs>
    using strict_constexpr_array = bool_<
        strict_constexpr_array_impl<sizeof...(xs)+1>({
            (bool)xs::type::value..., false
        })
    >;
}}}} // end namespace boost::mpl11::detail::logical_or

#endif // !BOOST_MPL11_DETAIL_LOGICAL_OR_STRICT_CONSTEXPR_ARRAY_HPP


#include <dai/alldai.h>
using namespace dai;

#include <boost/lexical_cast.hpp>

#include <cassert>
#include <chrono>
#include <ctime>
#include <iostream>
#include <map>
#include <string>
#include <vector>

#include "linenoise.h"

using namespace std;

static FactorGraph fg;
static PropertySet opts;
static BP bp;
static map<int, bool> clamps;

void initBP() {
    bp = BP(fg, opts);
    for (const auto& clamp : clamps) {
        int varIndex = clamp.first;
        bool varValue = clamp.second;
        bp.clamp(varIndex, varValue ? 1 : 0);
    }
    bp.init();
}

void queryVariable(stringstream &SS) {
  int varIndex;
  SS >> varIndex;
  clog << __LOGSTR__ << "Q " << varIndex << endl;

  // auto ans = bp.belief(fg.var(varIndex)).get(1);
  auto ans = bp.newBelief(varIndex);
  clog << __LOGSTR__ << "Returning " << ans << "." << endl;
  cout << ans << endl;
}

void queryFactor(stringstream &SS) {
  int factorIndex, valueIndex;
  SS >> factorIndex >> valueIndex;
  clog << __LOGSTR__ << "FQ " << factorIndex << " " << valueIndex << endl;

  auto ans = bp.beliefF(factorIndex).get(valueIndex);
  clog << __LOGSTR__ << "Returning " << ans << "." << endl;
  cout << ans << endl;
}

void runBP(stringstream &SS) {
  initBP();
  double tolerance;
  size_t minIters, maxIters, histLength;
  SS >> tolerance >> minIters >> maxIters >> histLength;
  clog << __LOGSTR__ << "BP " << tolerance << " " << minIters << " " << maxIters
       << " " << histLength << endl;

  try {
    double yetToConvergeFraction =
      bp.run(tolerance, minIters, maxIters, histLength);
    cout << yetToConvergeFraction << endl;
  } catch (dai::Exception const &) {
    cout << "LIBDAI EXCEPTION" << endl;
  }
}

void clamp(stringstream &SS) {
  int varIndex;
  string varValueStr;
  SS >> varIndex >> varValueStr;
  clog << __LOGSTR__ << "O " << varIndex << " " << varValueStr << endl;
  if (varValueStr.empty()) {
    clog << "Invalid arguments" << endl;
    return;
  }
  if (varValueStr != "true" && varValueStr != "false") {
    clog << varIndex << "is " << varValueStr << " is an invalid boolean value"
         << endl;
    return;
  }
  bool varValue = (varValueStr == "true");
  clamps[varIndex] = varValue;
  cout << "O " << varIndex << " " << varValueStr << endl;
}

int main(int argc, char *argv[]) {
    if (argc < 2) {
        cerr << __LOGSTR__ << "Insufficient number of arguments." << endl;
        cerr << __LOGSTR__ << "Insufficient number of arguments." << endl;
        return 1;
    }

    bool SaveHistory = true;
    char *factorGraphFileName;
    if (string(argv[1]) == ("--no-history")) {
      SaveHistory = false;
      factorGraphFileName = argv[2];
    } else {
      factorGraphFileName = argv[1];
    }

    clog << __LOGSTR__ << "Hello!" << endl
                       << "Bingo compiled on " << __DATE__ << " at " << __TIME__ << "." << endl;
    fg.ReadFromFile(factorGraphFileName);
    clog << __LOGSTR__ << "Finished reading factor graph." << endl;

    opts.set("maxiter", static_cast<size_t>(10000000));
    opts.set("tol", Real(1e-6));
    opts.set("verb", static_cast<size_t>(1));
    opts.set("updates", string("SEQRND")); // "SEQRND", or "PARALL", or "SEQFIX", or "SEQRNDPAR"
    opts.set("logdomain", true);

    initBP();

    linenoiseHistoryLoad("history.txt");
    char *line;
    string CmdType;
    while ((line = linenoise("bingo> ")) != NULL) {
      if (SaveHistory) {
        linenoiseHistoryAdd(line);
        linenoiseHistorySave("history.txt");
      }
      string Line(line);
      stringstream SS(Line);
      SS >> CmdType;
      clog << __LOGSTR__ << "Read command " << CmdType << endl;
      if (CmdType == "Q") {
        queryVariable(SS);
      } else if (CmdType == "FQ") {
        queryFactor(SS);
      } else if (CmdType == "BP") {
        runBP(SS);
      } else if (CmdType == "O") {
        clamp(SS);
      } else if (CmdType == "exit") {
        return 0;
      } else {
        cout << "Invalid command" << endl;
      }

      free(line);
    }
    clog << __LOGSTR__ << "Bye!" << endl;
    return 0;
}


#ifndef ALM_A_STAR_PLANNER_H
#define ALM_A_STAR_PLANNER_H

#include <iostream>
#include <cmath>
#include <math.h>
#include <stdio.h>
#include <vector>
#include <string>
#include <map>
#include <queue>
#include <algorithm>
#include <set>
#include <stdlib.h>
#include <memory>
#include <cstdlib>
#include <sstream>
#include <Eigen/Dense>
#include <fstream>
#include <utility>
#include <ostream>
#include <list>

#include <random>

#include <chrono>

using namespace std;
#define A_STAR_SET_TYPE_OPEN 0
#define A_STAR_SET_TYPE_CLOSED 1

namespace planner{
	typedef int set_type;

	typedef std::chrono::high_resolution_clock Clock;

	class NodePtr_Compare_Fcost;
	class NodePtr_Compare_key;

	class Node {
	public:
		Node(); // Constructor
		virtual ~Node(); // Destructor

		double g_score;
		double f_score;
		shared_ptr<Node> parent;
		int step_num;
		string key;

        bool isStartNode = false;

	};

	class NodePtr_Compare_Fcost{
	public:
		NodePtr_Compare_Fcost();
		bool operator() (const shared_ptr<Node> & lhs, const shared_ptr<Node> &rhs) const;
	};

	class NodePtr_Compare_key{
	public:
		NodePtr_Compare_key();
		bool operator() (const shared_ptr<Node> & lhs, const shared_ptr<Node> &rhs) const;
	};

	class A_starPlanner{
	public:
		A_starPlanner();
		virtual ~A_starPlanner();

		virtual bool constructPath();
		bool doAstar();

		virtual void setStartNode(const shared_ptr<Node> begin_input);
		virtual void setGoalNode(const shared_ptr<Node> goal_input);

		virtual	double gScore(const shared_ptr<Node> current, const shared_ptr<Node> neighbor);
		virtual double heuristicCost(const shared_ptr<Node> neighbor,const shared_ptr<Node> goal);
		virtual bool goalReached(shared_ptr<Node> current_node, shared_ptr<Node> goal);
		virtual std::vector< shared_ptr<Node> > getNeighbors(shared_ptr<Node> & current);

		void addToDeleteSet(const std::vector< std::shared_ptr<Node> > & node_list);
		bool checkPathToStartExists(const std::shared_ptr<Node> node, std::vector< std::shared_ptr<Node> > & candidates);
		void checkSetValidity(const std::set< std::shared_ptr<Node>, NodePtr_Compare_key> & current_set,
							  const set_type type);
		void filterValidNodes();

		// Set member variables
        std::vector< std::shared_ptr<Node> > OpenSet;
        std::set< std::shared_ptr<Node>, NodePtr_Compare_key> ClosedSet;
        std::set< std::shared_ptr<Node>, NodePtr_Compare_key> ExploredSet;

        // Extra Sets for handling deleted nodes and checking for set validity
        std::set< std::shared_ptr<Node> > DeletedSet;
        std::set< std::shared_ptr<Node> > validNodes;
        std::set< std::shared_ptr<Node> > invalidNodes;
		std::map<std::shared_ptr<Node>, set_type> vertexAssignment; // open or closed set

	    std::vector< std::shared_ptr<Node> > invalidSequence;

		shared_ptr<Node> begin;
		shared_ptr<Node> goal;
		shared_ptr<Node> achieved_goal;
		std::vector< shared_ptr<Node> > optimal_path;

		double epsilon_greedy = 0.1; // Percentage of time we randomly select a node from the open set

		 // Test Planner speed:
		Clock::time_point start_time;
		Clock::time_point goal_time;
  	  	double goal_time_span = 0;

		Clock::time_point start_path_reconstruction_time;
		Clock::time_point end_path_reconstruction_time;
  	  	double path_reconstruction_time_span = 0;

		Clock::time_point total_plan_time;
  	  	double total_plan_time_span = 0;

	};

	class XYNode: public Node{
	public:
		XYNode(); // Constructor
		virtual ~XYNode(); // Destructor

	// Problem specific:
		XYNode(const double x_in, const double y_in); // Constructor
		double x;
		double y;

		void commonInitialization();

	};

	class XYPlanner: public A_starPlanner{
	public:
		XYPlanner();
		virtual ~XYPlanner();

		virtual	double gScore(const shared_ptr<Node> current, const shared_ptr<Node> neighbor);
		virtual double heuristicCost(const shared_ptr<Node> neighbor,const shared_ptr<Node> goal);
		virtual bool goalReached(shared_ptr<Node> current_node, shared_ptr<Node> goal);

		virtual std::vector< shared_ptr<Node> > getNeighbors(shared_ptr<Node> & current);

		void WriteData(vector< shared_ptr<Node> > optimal_path);
		void printPath();

		int branch;
		double disc;

		std::shared_ptr<XYNode> current_;
		std::shared_ptr<XYNode> goal_;
		std::shared_ptr<XYNode> neighbor_;
		std::shared_ptr<XYNode> opt_node_;

	};

	class FootstepNode: public Node{
	public:
		FootstepNode(); // Constructor
		virtual ~FootstepNode(); // Destructor

	// Problem specific:
		FootstepNode(const double xLF_in, const double yLF_in, const double xRF_in, const double yRF_in, const double thetaLF_in, const double thetaRF_in, string turn_in, double s_in); // Constructor
		double xLF;
		double yLF;
		double xRF;
		double yRF;

		double thetaLF;
		double thetaRF;

		string turn; //true = LF, false = RF

		double s;

		void commonInitializationFootstep();

	};

	class FootstepPlanner: public A_starPlanner{
	public:
		FootstepPlanner();
		virtual ~FootstepPlanner();

		virtual	double gScore(const shared_ptr<Node> current, const shared_ptr<Node> neighbor);
		virtual double heuristicCost(const shared_ptr<Node> neighbor,const shared_ptr<Node> goal);
		virtual bool goalReached(shared_ptr<Node> current_node, shared_ptr<Node> goal);

		virtual std::vector< shared_ptr<Node> > getNeighbors(shared_ptr<Node> & current);

		void WriteData();
		void printPath();

		int branch;
		double disc;

		std::shared_ptr<FootstepNode> current_;
		std::shared_ptr<FootstepNode> goal_;
		std::shared_ptr<FootstepNode> neighbor_;
		std::shared_ptr<FootstepNode> opt_node_;

		//neighbor generation
		double x_df;
		double y_df;
		double theta_df;

		double max_reach;
		double max_width;
		double max_angle;
		double min_angle;

		double mindist_reach;
		double maxdist_reach;
		double mindist_width;
		double maxdist_width;
		double mindist_theta;
		double maxdist_theta;

	};

}

#endif


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program 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 General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include <boost/python.hpp>

#include "graph_tool.hh"
#include "graph_vertex_similarity.hh"
#include "numpy_bind.hh"

using namespace std;
using namespace boost;
using namespace graph_tool;

void get_dice_similarity(GraphInterface& gi, boost::any as, bool self_loop)
{
    gt_dispatch<>()
        ([&](auto& g, auto& s)
         {
             all_pairs_similarity(g, s,
                                  [&](auto u, auto v, auto& mask)
                                  {
                                      return dice(u, v, self_loop, mask, g);
                                  });
         },
         all_graph_views(), vertex_floating_vector_properties())
        (gi.get_graph_view(), as);
}

void get_dice_similarity_pairs(GraphInterface& gi, python::object opairs,
                               python::object osim, bool self_loop)
{
    multi_array_ref<int64_t,2> pairs = get_array<int64_t,2>(opairs);
    multi_array_ref<double,1> sim = get_array<double,1>(osim);

    gt_dispatch<>()
        ([&](auto& g)
         {
             some_pairs_similarity(g, pairs, sim,
                                   [&](auto u, auto v, auto& mask)
                                   {
                                       return dice(u, v, self_loop, mask, g);
                                   });
         },
         all_graph_views())
        (gi.get_graph_view());
}

void get_jaccard_similarity(GraphInterface& gi, boost::any as, bool self_loop)
{
    gt_dispatch<>()
        ([&](auto& g, auto& s)
         {
             all_pairs_similarity(g, s,
                                  [&](auto u, auto v, auto& mask)
                                  {
                                      return jaccard(u, v, self_loop, mask, g);
                                  });
         },
         all_graph_views(), vertex_floating_vector_properties())
        (gi.get_graph_view(), as);
}

void get_jaccard_similarity_pairs(GraphInterface& gi, python::object opairs,
                                  python::object osim, bool self_loop)
{
    multi_array_ref<int64_t,2> pairs = get_array<int64_t,2>(opairs);
    multi_array_ref<double,1> sim = get_array<double,1>(osim);

    gt_dispatch<>()
        ([&](auto& g)
         {
             some_pairs_similarity(g, pairs, sim,
                                   [&](auto u, auto v, auto& mask)
                                   {
                                       return jaccard(u, v, self_loop, mask, g);
                                   });
         },
         all_graph_views())
        (gi.get_graph_view());
}

void get_inv_log_weight_similarity(GraphInterface& gi, boost::any as)
{
    gt_dispatch<>()
        ([&](auto& g, auto& s)
         {
             all_pairs_similarity(g, s,
                                  [&](auto u, auto v, auto& mask)
                                  {
                                      return inv_log_weighted(u, v, mask, g);
                                  });
         },
         all_graph_views(), vertex_floating_vector_properties())
        (gi.get_graph_view(), as);
}

void get_inv_log_weight_similarity_pairs(GraphInterface& gi,
                                         python::object opairs,
                                         python::object osim)
{
    multi_array_ref<int64_t,2> pairs = get_array<int64_t,2>(opairs);
    multi_array_ref<double,1> sim = get_array<double,1>(osim);

    gt_dispatch<>()
        ([&](auto& g)
         {
             some_pairs_similarity(g, pairs, sim,
                                   [&](auto u, auto v, auto& mask)
                                   {
                                       return inv_log_weighted(u, v,  mask, g);
                                   });
         },
         all_graph_views())
        (gi.get_graph_view());
}

void export_vertex_similarity()
{
    python::def("dice_similarity", &get_dice_similarity);
    python::def("dice_similarity_pairs", &get_dice_similarity_pairs);
    python::def("jaccard_similarity", &get_jaccard_similarity);
    python::def("jaccard_similarity_pairs", &get_jaccard_similarity_pairs);
    python::def("inv_log_weight_similarity", &get_inv_log_weight_similarity);
    python::def("inv_log_weight_similarity_pairs",
                &get_inv_log_weight_similarity_pairs);
};


//
// Created by Alex Beccaro on 18/01/18.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/1-50/14/problem14.hpp"

BOOST_AUTO_TEST_SUITE( Problem14 )

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem14::solve();
        BOOST_CHECK_EQUAL(res, 837799);
    }

BOOST_AUTO_TEST_SUITE_END()


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

    @copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-predicates

    This function object returns @ref True or @ref False according x is pure imaginary or not.
    For non complex numbers it is same as to be zero

    @par Header <boost/simd/function/is_imag.hpp>

    @par Note

     Using `is_imag(x)` is similar to: `x == 0` for real entries.

    @par Example:

      @snippet is_imag.cpp is_imag

    @par Possible output:

      @snippet is_imag.txt is_imag

  **/
  as_logical_t<Value> is_imag(Value const& x);
} }
#endif

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

#endif


///////////////////////////////////////////////////////////////////////////////
// view::iterator::columns_iterator.hpp                                      //
//                                                                           //
//  (C) Copyright 2009 Erwann Rogard                                         //
//  Use, modification and distribution are subject to the                    //
//  Boost Software License, Version 1.0. (See accompanying file              //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_VIEW_DETAIL_COLUMNS_ITERATOR_HPP_ER_2009
#define BOOST_VIEW_DETAIL_COLUMNS_ITERATOR_HPP_ER_2009
#include <iterator>
#include <algorithm>
#include <cmath>
#include <iostream>
#include <boost/view//step_count_down_iterator.hpp>
#include <boost/iterator/iterator_traits.hpp>
#include <boost/range.hpp>

namespace boost{
namespace view{
namespace detail{

// I is an iterator to some sequence
//
// *columns_iterator returns a range representing a columns in
// row-major matrix view. Each row is of length step
template< class I
        , class V = use_default
        , class C = use_default
        , class R = use_default
        , class D = use_default
>
class columns_iterator
  : public iterator_adaptor<
        columns_iterator<I, V, C, R, D>,
        I,
        iterator_range<
            step_count_down_iterator<I,V,C,R,D>           //V1
        >,
        C,
        iterator_range<
            step_count_down_iterator<I,V,C,R,D>           // R1
        >,
        D
    >
{
    typedef iterator_adaptor<
        columns_iterator<I, V, C, R, D>,
            I,
            iterator_range<
            step_count_down_iterator<I,V,C,R,D>
        >,
        C,
        iterator_range<
            step_count_down_iterator<I,V,C,R,D>
        >,
        D
  > super_;

    typedef step_count_down_iterator<I,V,C,R,D> iter_;
    typedef iterator_range<iter_> range_;

 public:
    typedef typename super_::difference_type difference_type;
    typedef typename super_::reference reference;

    columns_iterator() : super_(){}

    explicit columns_iterator(
       I b,
       I e,
       difference_type step
    )
    :super_(b),base_e_(e),step_(step){
        BOOST_ASSERT(
            this->step()>=0
        );
    }

    columns_iterator(const columns_iterator& that)
    :super_(that),base_e_(that.base_e_),step_(that.step_){}

    difference_type step()const{ return this->step_; }

 private:

    friend class iterator_core_access;

    // default should work for:
    // void increment();
    // void decrement()
    // void advance(difference_type n)
    // difference_type distance_to(columns_iterator const& other) const{
    // bool equal(columns_iterator const& other) const

    reference dereference() const {
        // TODO may check against end
        iter_ b = make_step_count_down_iterator(
            this->base(),
            this->base_e_,
            this->step()
        );
        iter_ e = make_end_step_count_down_iterator(
            this->base(),
            this->base_e_,
            this->step()
        );
        return range_(
            b,
            e
        );
    }

    void advance(difference_type n)
    {
        std::advance(this->base_reference(), n);
    }
    I base_e_;
    difference_type step_;
};

template<typename It,typename N>
columns_iterator<It>
make_columns_iterator(
    It b,
    It e,
    N stride
){
    typedef columns_iterator<It> it_;
    return it_(
        b,
        e,
        stride
    );
}

// [b,e) is the sequence from which the matrix view is created
template<typename It,typename N>
columns_iterator<It>
make_end_columns_iterator(
    It b,
    It e,
    N step
){
    typedef columns_iterator<It> it_;
    return it_(
        boost::next(b,step),
        e,
        step
    );
}

}// detail
}// view
}// boost

#endif


// Copyright (C) 2019 T. Zachary Laine
//
// 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)
//[ random_access_iterator
#include <boost/stl_interfaces/iterator_interface.hpp>

#include <algorithm>
#include <array>

#include <cassert>

// This is a minimal random access iterator.  It uses default template
// parameters for most stl_interfaces template parameters.
struct simple_random_access_iterator
    : boost::stl_interfaces::iterator_interface<
          simple_random_access_iterator,
          std::random_access_iterator_tag,
          int>
{
    // This default constructor does not initialize it_, since that's how int *
    // works as well.  This allows optimum performance in code paths where
    // initializing a single pointer may be measurable.  It is also a
    // reasonable choice to initialize with nullptr.
    simple_random_access_iterator() noexcept {}
    simple_random_access_iterator(int * it) noexcept : it_(it) {}

    int & operator*() const noexcept { return *it_; }
    simple_random_access_iterator & operator+=(std::ptrdiff_t i) noexcept
    {
        it_ += i;
        return *this;
    }
    auto operator-(simple_random_access_iterator other) const noexcept
    {
        return it_ - other.it_;
    }

private:
    int * it_;
};

int main()
{
    std::array<int, 10> ints = {{0, 2, 1, 3, 4, 5, 7, 6, 8, 9}};

    simple_random_access_iterator first(ints.data());
    simple_random_access_iterator last(ints.data() + ints.size());
    std::sort(first, last);
    assert(ints == (std::array<int, 10>{{0, 1, 2, 3, 4, 5, 6, 7, 8, 9}}));
}
//]


#include <boost/test/unit_test.hpp>
#include "algorithms/search/max_area_of_island.hpp"

BOOST_AUTO_TEST_SUITE(TestMaxAreaOfIsland)

BOOST_AUTO_TEST_CASE(empty_matrix)
{
    std::vector<std::vector<int>> matrix = {};
    BOOST_CHECK(0 == Algo::Search::MaxAreaOfIsland::find(matrix));
}

BOOST_AUTO_TEST_CASE(matrix_one_elem_no_islands)
{
    std::vector<std::vector<int>> matrix = { { 0 } };
    BOOST_CHECK(0 == Algo::Search::MaxAreaOfIsland::find(matrix));
}

BOOST_AUTO_TEST_CASE(matrix_one_elem_one_island)
{
    std::vector<std::vector<int>> matrix = { { 1 } };
    BOOST_CHECK(1 == Algo::Search::MaxAreaOfIsland::find(matrix));
}

BOOST_AUTO_TEST_CASE(valid_matrix_all_land)
{
    std::vector<std::vector<int>> matrix =
        { {1, 1, 1},
          {1, 1, 1},
          {1, 1, 1} };

    BOOST_CHECK(9 == Algo::Search::MaxAreaOfIsland::find(matrix));
}

BOOST_AUTO_TEST_CASE(valid_matrix_2x4_island_on_border)
{
    std::vector<std::vector<int>> matrix =
        { {1, 1, 1, 0},
          {1, 1, 1, 0} };

    BOOST_CHECK(6 == Algo::Search::MaxAreaOfIsland::find(matrix));
}

BOOST_AUTO_TEST_CASE(valid_matrix_4x4_two_equal_islands)
{
    std::vector<std::vector<int>> matrix =
        { {0, 0, 1, 1},
          {0, 0, 1, 1},
          {1, 1, 0, 1},
          {1, 1, 1, 0}};

    BOOST_CHECK(5 == Algo::Search::MaxAreaOfIsland::find(matrix));
}

BOOST_AUTO_TEST_CASE(valid_matrix_4x3_one_island)
{
    std::vector<std::vector<int>> matrix =
        {{0,0,1,0,0},
         {0,1,0,1,0},
         {0,1,1,1,0}};

    BOOST_CHECK(5 == Algo::Search::MaxAreaOfIsland::find(matrix));
}

BOOST_AUTO_TEST_CASE(valid_matrix_7x7_have_two_islands)
{
    std::vector<std::vector<int>> matrix =
        {{1,1,1,1,1,1,1},
         {1,0,0,0,0,0,1},
         {1,0,1,1,1,0,1},
         {1,0,1,0,1,0,1},
         {1,0,1,1,1,0,1},
         {1,0,0,0,0,0,1},
         {1,1,1,1,1,1,1}};

    BOOST_CHECK(24 == Algo::Search::MaxAreaOfIsland::find(matrix));
}

BOOST_AUTO_TEST_SUITE_END()


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_POLYNOM_FUNCTIONS_EXPR_POLYVAL_HPP_INCLUDED
#define NT2_POLYNOM_FUNCTIONS_EXPR_POLYVAL_HPP_INCLUDED
#include <nt2/polynom/functions/polyval.hpp>
#include <nt2/include/functions/fma.hpp>
#include <nt2/include/functions/isempty.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <nt2/include/functions/repnum.hpp>
#include <nt2/include/functions/vandermonde.hpp>
#include <nt2/include/functions/sqrt.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/functions/sum.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/functions/inf.hpp>
#include <nt2/include/functions/trsolve.hpp>
#include <nt2/polynom/category.hpp>
#include <nt2/sdk/meta/fusion.hpp>
#include <boost/fusion/adapted/array.hpp>
#include <nt2/include/functions/tie.hpp>
#include <nt2/include/functions/transpose.hpp>
#include <nt2/sdk/error/warning.hpp>
#include <nt2/core/container/table/table.hpp>
#include <nt2/core/container/colon/colon.hpp>

namespace nt2 { namespace ext
{
   BOOST_DISPATCH_IMPLEMENT  ( polyval_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_<unspecified_<A0> > )(scalar_<unspecified_<A1> > )
                            )
  {

    typedef A1 result_type;
    NT2_FUNCTOR_CALL(2)
    {
      return a0*a1;
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( polyval_, tag::cpu_
                            , (A0)(A1)
                            , (unspecified_<A0>)(scalar_<unspecified_<A1> > )
                            )
  {

    typedef typename A0::value_type value_type;
    typedef A1 result_type;
    NT2_FUNCTOR_CALL(2)
    {
      if (isempty(a0)) return Zero<A1>();
      value_type ans = a0(1);
      for(size_t i = 2; i <= numel(a0); ++i)
      {
//       ans = fma(ans, a1, a0(i));
        ans *= a1;
        ans += a0(i);
      }
      return ans;
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( polyval_, tag::cpu_
                              , (A0)(N0)(A1)(N1)
                              , ((node_<A0, nt2::tag::polyval_, N0, nt2::container::domain>))
                              ((node_<A1, nt2::tag::tie_ , N1, nt2::container::domain>))
                            )
  {
    typedef void                                                    result_type;
    typedef typename boost::proto::result_of::child_c<A1&,0>::type       v_type;
    typedef typename boost::proto::result_of::child_c<A0&,0>::type       p_type;
    typedef typename boost::proto::result_of::child_c<A0&,1>::type       x_type;
    typedef typename A0::value_type                                  value_type;
    BOOST_FORCEINLINE result_type operator()( A0& a0, A1& a1 ) const
    {
      // Copy data in output first
      v_type& v = boost::proto::child_c<0>(a1);
      v.resize(a0.extent());
      val(a0, a1, N1(), N0());
    }

  private:
    template < class S,  class T, class U> BOOST_FORCEINLINE
      void compute_val(S p, T x, U v)const
    {
      if (isempty(p))
      {
        v =  nt2::zeros(extent(x), meta::as_<value_type>());
      }
      else
      {
        v = repnum(p(1), size(x));
        for(size_t i = 2; i <= numel(p); ++i)
        {
            //          v = fma(v, x, p(i));
          v*= x;
          v+= p(i);
        }
      }
    }

    template < class S,  class T, class U,  class V> BOOST_FORCEINLINE
      void compute_delta(const S& x, const T& r,
                         const U& df,  const V& normr,
                         const size_t & nc,
                         container::table<value_type>& delta)const
    {
      NT2_WARNING(nt2::is_eqz(value_type(df)), "zero degree of freedom implies infinite error bounds.");
      BOOST_AUTO_TPL(vnd, nt2::vandermonde(x, nc));
      BOOST_AUTO_TPL(err, nt2::trsolve(nt2::trans(r), nt2::trans(vnd),'L') );
      value_type fact =   (normr/nt2::sqrt(value_type(df)));
      delta(nt2::_) =nt2::sqrt(oneplus(sum(sqr(err),1)))*fact;
    }

    template < class T > BOOST_FORCEINLINE
      void val(A0& a0, A1& a1,
               boost::mpl::long_<1> const &, const T&) const
    {
      BOOST_AUTO_TPL(p,  boost::proto::child_c<0>(a0));
      BOOST_AUTO_TPL(x,  boost::proto::child_c<1>(a0));
      BOOST_AUTO_TPL(v,  boost::proto::child_c<0>(a1));
      compute_val(p, x, v);
    }

    BOOST_FORCEINLINE
    void val(A0& a0, A1& a1,
             boost::mpl::long_<1> const &, boost::mpl::long_<3> const &) const
    {
      BOOST_AUTO_TPL(p,  boost::proto::child_c<0>(a0));
      BOOST_AUTO_TPL(x,  boost::proto::child_c<1>(a0));
      BOOST_AUTO_TPL(v,  boost::proto::child_c<0>(a1));
      BOOST_AUTO_TPL(mu,  boost::proto::child_c<2>(a0));
      compute_val(p, (x-mu(1))/mu(2), v);
    }
    BOOST_FORCEINLINE
    void val(A0& a0, A1& a1,
             boost::mpl::long_<1> const &, boost::mpl::long_<6> const &) const
    {
      BOOST_AUTO_TPL(p,  boost::proto::child_c<0>(a0));
      BOOST_AUTO_TPL(x,  boost::proto::child_c<1>(a0));
      BOOST_AUTO_TPL(v,  boost::proto::child_c<0>(a1));
      BOOST_AUTO_TPL(mu,  boost::proto::child_c<5>(a0));
      compute_val(p, (x-mu(1))/mu(2), v);
    }
    BOOST_FORCEINLINE
    void val(A0& a0, A1& a1,
             boost::mpl::long_<2> const &, boost::mpl::long_<5> const &) const
    {
      BOOST_AUTO_TPL(p,  boost::proto::child_c<0>(a0));
      BOOST_AUTO_TPL(x,  boost::proto::child_c<1>(a0));
      BOOST_AUTO_TPL(r,  boost::proto::child_c<2>(a0));
      BOOST_AUTO_TPL(df, boost::proto::child_c<3>(a0));
      BOOST_AUTO_TPL(nr, boost::proto::child_c<4>(a0));
      BOOST_AUTO_TPL(v,  boost::proto::child_c<0>(a1));
      compute_val(p, x, v);
      container::table<value_type> delta(extent(x));
      compute_delta(x, r, df, nr, nt2::numel(p), delta);
      boost::proto::child_c<1>(a1) = delta;
    }
    BOOST_FORCEINLINE
    void val(A0& a0, A1& a1,
             boost::mpl::long_<2> const &, boost::mpl::long_<6> const &) const
    {
      BOOST_AUTO_TPL(p,  boost::proto::child_c<0>(a0));
      BOOST_AUTO_TPL(x,  boost::proto::child_c<1>(a0));
      BOOST_AUTO_TPL(r,  boost::proto::child_c<2>(a0));
      BOOST_AUTO_TPL(df, boost::proto::child_c<3>(a0));
      BOOST_AUTO_TPL(nr, boost::proto::child_c<4>(a0));
      BOOST_AUTO_TPL(v,  boost::proto::child_c<0>(a1));
      BOOST_AUTO_TPL(mu,  boost::proto::child_c<5>(a0));
      BOOST_AUTO_TPL(xred, (x-mu(1))/mu(2));
      compute_val(p, xred, v);
      container::table<value_type> delta(of_size(extent(x)));
      delta.resize(extent(x));
      compute_delta(xred, r, df, nr, nt2::numel(p), delta);
      boost::proto::child_c<1>(a1) = delta;
    }

  };
} }

#endif


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/type.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/integral.hpp>
using namespace boost::hana;

struct X { };
BOOST_HANA_CONSTANT_ASSERT(sizeof_(type<int>) == size_t<sizeof(int)>);
BOOST_HANA_CONSTANT_ASSERT(sizeof_(type<X>) == size_t<sizeof(X)>);
BOOST_HANA_CONSTANT_ASSERT(sizeof_(type<X>) == size_t<sizeof(X{})>);

int main() { }


#include <boost/graph/find_flow_cost.hpp>


#include "sandBox.h"
#include "igl/edge_flaps.h"
#include "igl/collapse_edge.h"
#include "Eigen/dense"
#include <functional>
#include <Eigen/Core>
#include "igl/opengl/ViewerCore.h"
#include "igl/opengl/glfw/renderer.h"
#include "igl/decimate.h"
#include "igl/writeOBJ.h"
#include <igl/circulation.h>
#include <igl/shortest_edge_and_midpoint.h>
#include <igl/parallel_for.h>
#include <igl/opengl/glfw/Viewer.h>
#include <set>
#include "simplifier.h"
#include "igl/swept_volume_bounding_box.h"

SandBox::SandBox()
	:
	objectsData{ new std::vector<ObjectData*>{} },
	linksCounter{ 0 }, destIndex{ 0 },
	firstLinkIndex{ 1 },
	lastLinkIndex{ 1 },
	isCCD{ false },
	xVelocity{-0.005},
	yVelocity{ 0 }
{

}

SandBox::~SandBox()
{
    delete objectsData;
}

void SandBox::OnNewMeshLoad()
{
	if (linksCounter > 0)
	{
		data().MyTranslate(Eigen::Vector3d(0, 1.6, 0), true);
		parents.push_back(linksCounter);
	}
	else
	{
		parents.push_back(-1);
	}

	data().add_points(Eigen::RowVector3d(0, 0, 0), Eigen::RowVector3d(0, 0, 1));
	data().show_overlay_depth = false;
	data().point_size = 10;
	data().line_width = 2;
	data().set_visible(true, 2);
	data().show_faces = 3;
	data().show_texture = 2;

	data().dirty = 157; //this line prevents texture coordinates
	data().MyScale(Eigen::Vector3d(1, 1, 1));
	data().SetCenterOfRotation(Eigen::Vector3d(0, -0.8, 0));

	Eigen::MatrixXd axis(7, 3);
	axis << 0, -0.8, 0, 0, -0.8, 1.6, 0, -0.8, -1.6, 0, 0.8, 0, 0, -2.4, 0, -1.6, -0.8, 0, 1.6, -0.8, 0;
	data().add_points(axis, Eigen::RowVector3d(0, 0, 1));
 	data().add_edges(axis.row(0), axis.row(1), Eigen::RowVector3d(0, 0, 1));
	data().add_edges(axis.row(0), axis.row(2), Eigen::RowVector3d(0, 0, 1));
	data().add_edges(axis.row(0), axis.row(3), Eigen::RowVector3d(0, 1, 0));
	data().add_edges(axis.row(0), axis.row(4), Eigen::RowVector3d(0, 1, 0));
	data().add_edges(axis.row(0), axis.row(5), Eigen::RowVector3d(1, 0, 0));
	data().add_edges(axis.row(0), axis.row(6), Eigen::RowVector3d(1, 0, 0));

	ObjectData* od = new ObjectData();
	InitObjectData(*od, data().V, data().F);
	objectsData->push_back(od);
	data().AddBoundingBox(od->tree->m_box, Eigen::RowVector3d(0, 1, 0));

 	linksCounter++;
	lastLinkIndex = linksCounter;
}

void SandBox::Init(const std::string &config)
{
	std::string item_name;
	std::ifstream nameFileout;
	nameFileout.open(config);

	if (!nameFileout.is_open())
	{
		std::cout << "Can't open file " << config << std::endl;
	}
	else
	{
		while (nameFileout >> item_name)
		{
			std::cout << "openning " << item_name << std::endl;
			load_mesh_from_file(item_name);

			if (item_name.find("sphere") != std::string::npos)
			{
				parents.push_back(-1);
				data().add_points(Eigen::RowVector3d(0, 0, 0), Eigen::RowVector3d(0, 0, 1));
				data().show_overlay_depth = false;
				data().point_size = 10;
				data().line_width = 2;
				data().set_visible(false, 1);
			}
			else
			{
				if (linksCounter > 0)
				{
					data().MyTranslate(Eigen::Vector3d(0, 1.6, 0), true);
					parents.push_back(linksCounter);
				}
				else
				{
					parents.push_back(-1);
				}

				data().add_points(Eigen::RowVector3d(0, 0, 0), Eigen::RowVector3d(0, 0, 1));
				data().show_overlay_depth = false;
				data().point_size = 10;
				data().line_width = 2;
				data().set_visible(false, 1);

				data().dirty = 157; //this line prevents texture coordinates
				data().MyScale(Eigen::Vector3d(1, 1, 1));
				data().SetCenterOfRotation(Eigen::Vector3d(0, -0.8, 0));

				Eigen::MatrixXd axis(7, 3);
				axis <<
					   0, -0.8,    0,
					   0, -0.8,  1.6,
					   0, -0.8,	-1.6,
					   0,  0.8,	   0,
					   0, -2.4,    0,
					-1.6, -0.8,    0,
					 1.6, -0.8,	   0;

				data().add_points(axis, Eigen::RowVector3d(0, 0, 1));
				data().add_edges(axis.row(0), axis.row(1), Eigen::RowVector3d(0, 0, 1));
				data().add_edges(axis.row(0), axis.row(2), Eigen::RowVector3d(0, 0, 1));
				data().add_edges(axis.row(0), axis.row(3), Eigen::RowVector3d(0, 1, 0));
				data().add_edges(axis.row(0), axis.row(4), Eigen::RowVector3d(0, 1, 0));
				data().add_edges(axis.row(0), axis.row(5), Eigen::RowVector3d(1, 0, 0));
				data().add_edges(axis.row(0), axis.row(6), Eigen::RowVector3d(1, 0, 0));

				ObjectData* od = new ObjectData();
				InitObjectData(*od, data().V, data().F);
				objectsData->push_back(od);
				data().AddBoundingBox(od->tree->m_box, Eigen::RowVector3d(0, 1, 0));

				linksCounter++;
			}
		}
		lastLinkIndex = linksCounter;
		nameFileout.close();
	}
	data().set_colors(Eigen::RowVector3d(0.9, 0.1, 0.1));
	isActive = false;
}

void SandBox::InitObjectData(ObjectData& od, Eigen::MatrixXd& V, Eigen::MatrixXi& F)
{

    od.V = new Eigen::MatrixXd(V);
    od.F = new Eigen::MatrixXi(F);
    od.E = new Eigen::MatrixXi();
    od.EF = new Eigen::MatrixXi();
    od.EI = new Eigen::MatrixXi();
    od.EMAP = new Eigen::VectorXi();
    od.Q = new PriorityQueue();
    od.num_collapsed = 0;
    od.tree = new igl::AABB<Eigen::MatrixXd, 3>{};

    igl::edge_flaps(*od.F, *od.E, *od.EMAP, *od.EF, *od.EI);

    od.tree->init(*od.V, *od.F);

    od.C = new Eigen::MatrixXd(od.E->rows(), od.V->cols());

    ComputeNormals(od, data());

    ComputeQMatrices(od);

    ComputePriorityQueue(od);
}

void SandBox::ReInitObjectData(ObjectData& od)
{
    Eigen::MatrixXd V = *od.V;
    Eigen::MatrixXi F = *od.F;

    // Remove old object data
    ObjectData* odToRemove = objectsData->at(selected_data_index);
    ClearObjectData(*odToRemove);
    delete odToRemove;

    // Add new object data after collapsing edges
    ObjectData* odToAdd = new ObjectData();
    InitObjectData(*odToAdd, V, F);
    objectsData->at(selected_data_index) = odToAdd;
}

void SandBox::ClearObjectData(ObjectData& od)
{
    delete od.V;
    delete od.F;
    delete od.EMAP;
    delete od.E;
    delete od.EF;
    delete od.EI;
    delete od.Q;
    delete od.C;
    delete od.F_NORMALS;

    std::for_each(od.QMATRICES.begin(), od.QMATRICES.end(), [](Eigen::Matrix4d* m) -> void { delete m; });
    od.QMATRICES.clear();
}

void SandBox::Simplify()
{
    ObjectData* objectToSimplify = objectsData->at(selected_data_index);
    int num_to_collapse = std::ceil(0.05 * objectToSimplify->Q->size());
    Simplify(num_to_collapse, *objectToSimplify);
}

void SandBox::Simplify(int num_to_collapse, ObjectData& od)
{

    if (!od.Q->empty())
    {
        bool something_collapsed = false;
        for (int j = 0; j < num_to_collapse; j++)
        {
              if (!collapse_edge(od))
            {
                break;
            }
            something_collapsed = true;
            od.num_collapsed++;
        }

        if (something_collapsed)
        {
            data().clear();
            data().set_mesh(*od.V, *od.F);
            data().set_face_based(true);
            data().dirty = 157; //this line prevents texture coordinates
            ReInitObjectData(od);
        }
    }
}

void SandBox::MoveTo(double x, double y)
{
    data().TranslateInSystem(GetRotation(), Eigen::Vector3d(x, 0, 0));
    data().TranslateInSystem(GetRotation(), Eigen::Vector3d(0, y, 0));
    WhenTranslate();
}

void SandBox::Animate()
{
	if (isActive)
	{

		if (isCCD)
		{
			IK_CCD();
		}
		else
		{
			IK_FABRIK();
		}
	}
}

bool SandBox::ObjectsCollide(igl::AABB<Eigen::MatrixXd, 3>* firstTree, igl::AABB<Eigen::MatrixXd, 3>* secondTree)
{
	if (firstTree == nullptr || secondTree == nullptr)
	{
		return false;
	}
 	if (BoxesIntersect(firstTree->m_box, secondTree->m_box)) {
		if (firstTree->is_leaf() && secondTree->is_leaf())
		{
			data_list.at(0).AddBoundingBox(firstTree->m_box, Eigen::RowVector3d(0, 1, 0));
			data_list.at(1).AddBoundingBox(secondTree->m_box, Eigen::RowVector3d(0, 1, 0));
			return true;
		}
		else if (!firstTree->is_leaf() && secondTree->is_leaf())
		{
			return	ObjectsCollide(firstTree->m_left, secondTree) ||
					ObjectsCollide(firstTree->m_right, secondTree);
		}
		else if (firstTree->is_leaf() && !secondTree->is_leaf())
		{

			return  ObjectsCollide(firstTree, secondTree->m_left) ||
					ObjectsCollide(firstTree, secondTree->m_right);
		}
		else if (!firstTree->is_leaf() && !secondTree->is_leaf())
		{
			return  ObjectsCollide(firstTree->m_left, secondTree->m_left) ||
					ObjectsCollide(firstTree->m_left, secondTree->m_right) ||
					ObjectsCollide(firstTree->m_right, secondTree->m_left) ||
					ObjectsCollide(firstTree->m_right, secondTree->m_right);
		}
	}
	return false;
}

bool SandBox::BoxesIntersect(Eigen::AlignedBox <double, 3>& firstBox, Eigen::AlignedBox <double, 3>& secondBox)
{
	Eigen::Matrix4d firstTrans = data_list.at(0).MakeTransd();
	Eigen::Matrix4d secondTrans = data_list.at(1).MakeTransd();

	Eigen::Vector3d firstBoxCenter = firstBox.center();
	Eigen::Vector3d secondBoxCenter = secondBox.center();

	Eigen::Vector4d firstCenter = firstTrans * Eigen::Vector4d(firstBoxCenter(0), firstBoxCenter(1), firstBoxCenter(2), 1);
	Eigen::Vector4d secondCenter = secondTrans * Eigen::Vector4d(secondBoxCenter(0), secondBoxCenter(1), secondBoxCenter(2), 1);

	Eigen::Vector3d C0(firstCenter(0), firstCenter(1), firstCenter(2));
	Eigen::Vector3d C1(secondCenter(0), secondCenter(1), secondCenter(2));

	Eigen::Vector3d D = C1 - C0;

	Eigen::Matrix3d A = data_list.at(0).GetRotation();
	Eigen::Matrix3d B = data_list.at(1).GetRotation();

	Eigen::Matrix3d A_matrix;
	A_matrix << A(0, 0), A(1, 0), A(2, 0),
				A(0, 1), A(1, 1), A(2, 1),
				A(0, 2), A(1, 2), A(2, 2);

	Eigen::Matrix3d B_matrix;
	B_matrix << B(0, 0), B(1, 0), B(2, 0),
				B(0, 1), B(1, 1), B(2, 1),
				B(0, 2), B(1, 2), B(2, 2);

	Eigen::RowVector3d a;
	a << firstBox.sizes()(0) / 2, firstBox.sizes()(1) / 2, firstBox.sizes()(2) / 2;

	Eigen::RowVector3d b;
	b << secondBox.sizes()(0) / 2, secondBox.sizes()(1) / 2, secondBox.sizes()(2) / 2;

	Eigen::Matrix3d C = A.transpose() * B;

	double R0, R1, R;

	// L = A0
	R0 = a(0);
	R1 = b(0) * abs(C(0, 0)) + b(1) * abs(C(0, 1)) + b(2) * abs(C(0, 2));
	R = (A_matrix.row(0) * D).norm();
	if (R > R0 + R1) return false;

	// L = A1
	R0 = a(1);
	R1 = b(0) * abs(C(1, 0)) + b(1) * abs(C(1, 1)) + b(2) * abs(C(1, 2));
	R = (A_matrix.row(1) * D).norm();
	if (R > R0 + R1) return false;

	// L = A2
	R0 = a(2);
	R1 = b(0) * abs(C(2, 0)) + b(1) * abs(C(2, 1)) + b(2) * abs(C(2, 2));
	R = (A_matrix.row(2) * D).norm();
	if (R > R0 + R1) return false;

	// L = B0
	R0 = a(0) * abs(C(0, 0)) + a(1) * abs(C(1, 0)) + a(2) * abs(C(2, 0));
	R1 = b(0);
	R = (B_matrix.row(0) * D).norm();
	if (R > R0 + R1) return false;

	// L = B1
	R0 = a(0) * abs(C(0, 1)) + a(1) * abs(C(1, 1)) + a(2) * abs(C(2, 1));
	R1 = b(1);
	R = (B_matrix.row(1) * D).norm();
	if (R > R0 + R1) return false;

	// L = B2
	R0 = a(0) * abs(C(0, 2)) + a(1) * abs(C(1, 2)) + a(2) * abs(C(2, 2));
	R1 = b(2);
	R = (B_matrix.row(2) * D).norm();
	if (R > R0 + R1) return false;

	// L = A0 x B0
	R0 = a(1) * abs(C(2, 0)) + a(2) * abs(C(1, 0));
	R1 = b(1) * abs(C(0, 2)) + b(2) * abs(C(0, 1));
	R = (C(1, 0) * A_matrix.row(2) * D - C(2, 0) * A_matrix.row(1) * D).norm();
	if (R > R0 + R1) return false;

	// L = A0 x B1
	R0 = a(1) * abs(C(2, 1)) + (a(2))*abs(C(1, 1));
	R1 = b(0) * abs(C(0, 2)) + b(2) * abs(C(0, 0));
	R = (C(1, 1) * A_matrix.row(2) * D - C(2, 1) * A_matrix.row(1) * D).norm();
	if (R > R0 + R1) return false;

	// L = A0 x B2
	R0 = a(1) * abs(C(2, 2)) + a(2) * abs(C(1, 2));
	R1 = b(0) * abs(C(0, 1)) + b(1) * abs(C(0, 0));
	R = (C(1, 2) * A_matrix.row(2) * D - C(2, 2) * A_matrix.row(1) * D).norm();
	if (R > R0 + R1) return false;

	// L = A1 x B0
	R0 = a(0) * abs(C(2, 0)) + a(2) * abs(C(0, 0));
	R1 = b(1) * abs(C(1, 2)) + b(2) * abs(C(1, 1));
	R = (C(2, 0) * A_matrix.row(0) * D - C(0, 0) * A_matrix.row(2) * D).norm();
	if (R > R0 + R1) return false;

	// L = A1 x B1
	R0 = a(0) * abs(C(2, 1)) + a(2) * abs(C(0, 1));
	R1 = b(0) * abs(C(1, 2)) + b(2) * abs(C(1, 0));
	R = (C(2, 1) * A_matrix.row(0) * D - C(0, 1) * A_matrix.row(2) * D).norm();
	if (R > R0 + R1) return false;

	/// L = A1 x B2
	R0 = a(0) * abs(C(2, 2)) + a(2) * abs(C(0, 2));
	R1 = b(0) * abs(C(1, 1)) + b(1) * abs(C(1, 0));
	R = (C(2, 2) * A_matrix.row(0) * D - C(0, 2) * A_matrix.row(2) * D).norm();
	if (R > R0 + R1) return false;

	// L = A2 x B0
	R0 = a(0) * abs(C(1, 0)) + a(1) * abs(C(0, 0));
	R1 = b(1) * abs(C(2, 2)) + b(2) * abs(C(2, 1));
	R = (C(0, 0) * A_matrix.row(1) * D - C(1, 0) * A_matrix.row(0) * D).norm();
	if (R > R0 + R1) return false;

	// L = A2 x B1
	R0 = a(0) * abs(C(1, 1)) + a(1) * abs(C(0, 1));
	R1 = b(0) * abs(C(2, 2)) + b(2) * abs(C(2, 0));
	R = (C(0, 1) * A_matrix.row(1) * D - C(1, 1) * A_matrix.row(0) * D).norm();
	if (R > R0 + R1) return false;

	// L = A2 x B2
	R0 = a(0) * abs(C(1, 2)) + a(1) * abs(C(0, 2));
	R1 = b(0) * abs(C(2, 1)) + b(1) * abs(C(2, 0));
	R = (C(0, 2) * A_matrix.row(1) * D - C(1, 2) * A_matrix.row(0) * D).norm();
	if (R > R0 + R1) return false;
	return true;
}

void SandBox::SetVelocity(double x, double y)
{
	xVelocity = x;
	yVelocity = y;
}

void  SandBox::IK_CCD()
{
	Eigen::Matrix4d destTrans = data_list[destIndex].MakeTransd();
	Eigen::Matrix4d firstTrans = data_list[firstLinkIndex].MakeTransd();
	Eigen::Matrix4d lastTrans = data_list[lastLinkIndex].MakeTransd();
	Eigen::Matrix4d lastParentsTrans = CalcParentsTrans(lastLinkIndex);

	Eigen::Vector4d lowerTip(0, -0.8, 0, 1);
	Eigen::Vector4d upperTip(0, 0.8, 0, 1);

	Eigen::Vector3d dst = ExtractPosition(destTrans * Eigen::Vector4d(0, 0, 0, 1));
	Eigen::Vector3d src = ExtractPosition(firstTrans * lowerTip);
	Eigen::Vector3d tip = ExtractPosition(lastParentsTrans * lastTrans * upperTip).transpose();

	double distToDest = (dst - tip).norm();
	double absDistance = (dst - src).norm();
	double maxReach = linksCounter * 1.6;
	double threshold = 0.1;

	std::cout << distToDest << std::endl;

	if (maxReach < absDistance)
	{
		std::cout << "Cant reach" << std::endl;
		isActive = false;
		return;
	}

	if (distToDest < threshold)
	{
		std::cout << "Reached" << std::endl;
		isActive = false;
		return;
	}

	for (int i = firstLinkIndex + lastLinkIndex - 1; i >= firstLinkIndex; i--)
	{
		Eigen::Matrix4d iParentsTrans = CalcParentsTrans(i);
		Eigen::Matrix4d iTrans = data_list[i].MakeTransd();
		Eigen::Vector3d D = dst;
		Eigen::Vector3d R = ExtractPosition(iParentsTrans * iTrans * lowerTip);
		Eigen::Vector3d E = ExtractPosition(lastParentsTrans * lastTrans * upperTip);
		Eigen::Vector3d RD = D - R;
		Eigen::Vector3d RE = E - R;
		float RD_dot_RE = RD.normalized().dot(RE.normalized());
		if (abs(RD_dot_RE) > 1)
		{
			std::cout << "Dot product greater then |1|, rounding value" << std::endl;
			RD_dot_RE = RD_dot_RE > 0 ? 1 : -1;
		}
		double theta = acosf(RD_dot_RE);
		Eigen::Matrix3d m = iParentsTrans.inverse().block(0, 0, 3, 3);
		Eigen::Vector3d rotAxis = m * RD.cross(RE);
		theta = theta / 10; // Smooth the rotation
		data_list[i].MyRotate(rotAxis, -theta);
	}
}

void  SandBox::IK_FABRIK()
{
	int n = linksCounter + 1;
	float di = 1.6;

	Eigen::Matrix4d destTrans = data_list[destIndex].MakeTransd();
	Eigen::Matrix4d firstTrans = data_list[firstLinkIndex].MakeTransd();
	Eigen::Matrix4d lastTrans = data_list[lastLinkIndex].MakeTransd();
	Eigen::Matrix4d lastParentsTrans = CalcParentsTrans(lastLinkIndex);

	Eigen::Vector4d lowerTip(0, -0.8, 0, 1);
	Eigen::Vector4d upperTip(0, 0.8, 0, 1);

	Eigen::Vector3d t = ExtractPosition(destTrans * Eigen::Vector4d(0, 0, 0, 1));
	Eigen::Vector3d p1 = ExtractPosition(firstTrans * lowerTip);
	Eigen::Vector3d pn = ExtractPosition(lastParentsTrans * lastTrans * upperTip).transpose();

	float dist = (p1 - t).norm();

	if (dist > di * (n - 1))
	{
		std::cout << "The target is unreachable" << std::endl;
		isActive = false;
		return;
	}

	float difA = (pn - t).norm();
	float tol = 0.1;

	std::cout << difA << std::endl;

	if (difA <= tol)
	{
		std::cout << "Reached target" << std::endl;
		isActive = false;
		return;
	}

	std::vector<Eigen::Vector3d> p;
	p.push_back(Eigen::Vector3d::Identity()); // Dummy
	p.push_back(p1);
	for (int i = 1; i <= linksCounter; i++)
	{
		p.push_back(ExtractPosition(data_list.at(i).MakeTransd() * CalcParentsTrans(i) * upperTip));
	}

	Eigen::Vector3d b = p.at(1);

	// FORWARD REACHING
	p.at(n) = t;

	for (int i = n - 1; i >= 1; i--)
	{
		Eigen::Vector3d pi = p.at(i);
		Eigen::Vector3d pi1 = p.at(i + 1);
		float ri = (pi1 - pi).norm();
		float li = di / ri;
		p.at(i) = ((1 - li) * pi1) + (li * pi);
	}

	// BACKWARD REACHING
	p.at(1) = b;

	for (int i = 1; i <= n - 1; i++)
	{
		Eigen::Vector3d pi = p.at(i);
		Eigen::Vector3d pi1 = p.at(i + 1);
		float ri = (pi1 - pi).norm();
		float li = di / ri;
		p.at(i + 1) = ((1 - li) * pi) + (li * pi1);
	}

	// UPDATE JOINTS POSITIONS
	for (int i = 1; i <= linksCounter; i++)
	{
		Eigen::Matrix4d iParentsTrans = CalcParentsTrans(i);
		Eigen::Matrix4d iTrans = data_list.at(i).MakeTransd();
		Eigen::Vector3d D = p.at(i + 1);
		Eigen::Vector3d R = ExtractPosition(iParentsTrans * iTrans * lowerTip);
		Eigen::Vector3d E = ExtractPosition(iParentsTrans * iTrans * upperTip);
		Eigen::Vector3d RD = D - R;
		Eigen::Vector3d RE = E - R;

		Eigen::Vector3d axis = RD.cross(RE).normalized();
		float RD_dot_RE = RD.normalized().dot(RE.normalized());
		if (abs(RD_dot_RE) > 1)
		{
			std::cout << "Dot product greater then |1|, rounding value" << std::endl;
			RD_dot_RE = RD_dot_RE > 0 ? 1 : -1;
		}
		float theta = acosf(RD_dot_RE);
		Eigen::Matrix4d m = iParentsTrans.inverse();
		Eigen::Matrix3d m1;
		m1 <<
			m(0, 0), m(0, 1), m(0, 2),
			m(1, 0), m(1, 1), m(1, 2),
			m(2, 0), m(2, 1), m(2, 2);

		Eigen::Vector3d rotAxis = m1 * RD.cross(RE);
		theta = theta / 10; // Smooth the rotation
		data_list.at(i).MyRotate(rotAxis, -theta);
	}
}

Eigen::Vector3d SandBox::ExtractPosition(Eigen::Vector4d m)
{
	return Eigen::Vector3d(m(0), m(1), m(2));
}


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

// BSD 3-Clause License

// Copyright (c) 2020, Chenyu
// 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 copyright holder nor the names of its
//    contributors may be used to endorse or promote products derived from
//    this software without specific prior written permission.

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

#include "math/sparse_cholesky_llt.h"

#include <cholmod.h>
#include <glog/logging.h>

#include <Eigen/Core>
#include <Eigen/SparseCore>

// UF_long is deprecated but SuiteSparse_long is only available in
// newer versions of SuiteSparse. So for older versions of
// SuiteSparse, we define SuiteSparse_long to be the same as UF_long,
// which is what recent versions of SuiteSparse do anyways.
#ifndef SuiteSparse_long
#define SuiteSparse_long UF_long
#endif

namespace DAGSfM {
namespace {

cholmod_sparse ViewAsCholmod(const Eigen::SparseMatrix<double>& const_mat) {
  Eigen::SparseMatrix<double>& mat =
      const_cast<Eigen::SparseMatrix<double>&>(const_mat);
  cholmod_sparse res;
  res.nzmax = mat.nonZeros();
  res.nrow = mat.rows();
  res.ncol = mat.cols();
  res.p = mat.outerIndexPtr();
  res.i = mat.innerIndexPtr();
  res.x = mat.valuePtr();
  res.sorted = 1;
  if (mat.isCompressed()) {
    res.packed = 1;
  } else {
    res.packed = 0;
    res.nz = mat.innerNonZeroPtr();
  }

  // Set to 0 if the matrix is not symmetric.
  res.stype = 1;
  res.itype = CHOLMOD_INT;
  res.xtype = CHOLMOD_REAL;
  res.dtype = CHOLMOD_DOUBLE;
  return res;
}

cholmod_dense ViewAsCholmod(const Eigen::VectorXd& const_vec) {
  Eigen::VectorXd& vec = const_cast<Eigen::VectorXd&>(const_vec);
  cholmod_dense res;
  res.nrow = vec.rows();
  res.ncol = vec.cols();
  res.nzmax = res.nrow * res.ncol;
  res.d = vec.size();
  res.x = reinterpret_cast<void*>(vec.data());
  res.z = 0;
  res.xtype = CHOLMOD_REAL;
  return res;
}
}  // namespace

// A class for performing the choleksy decomposition of a sparse matrix using
// CHOLMOD from SuiteSparse. This allows us to utilize the supernodal algorithms
// which are not included with Eigen. The interface is meant to mimic the Eigen
// linear solver interface except that it is not templated and requires sparse
// matrices.
SparseCholeskyLLt::SparseCholeskyLLt(const Eigen::SparseMatrix<double>& mat)
    : cholmod_factor_(nullptr),
      is_factorization_ok_(false),
      is_analysis_ok_(false),
      info_(Eigen::Success) {
  cholmod_start(&cc_);
  Compute(mat);
}

SparseCholeskyLLt::SparseCholeskyLLt()
    : cholmod_factor_(nullptr),
      is_factorization_ok_(false),
      is_analysis_ok_(false),
      info_(Eigen::Success) {
  cholmod_start(&cc_);
}

SparseCholeskyLLt::~SparseCholeskyLLt() {
  if (cholmod_factor_ != nullptr) {
    cholmod_free_factor(&cholmod_factor_, &cc_);
  }
  cholmod_finish(&cc_);
}

void SparseCholeskyLLt::AnalyzePattern(const Eigen::SparseMatrix<double>& mat) {
  // Release the current decomposition if there is one.
  if (cholmod_factor_ != nullptr) {
    cholmod_free_factor(&cholmod_factor_, &cc_);
  }

  // Get the cholmod view of the sparse matrix.
  cholmod_sparse A = ViewAsCholmod(mat);

  // Cholmod can try multiple re-ordering strategies to find a fill
  // reducing ordering. Here we just tell it use AMD with automatic
  // matrix dependence choice of supernodal versus simplicial
  // factorization.
  cc_.nmethods = 1;
  cc_.method[0].ordering = CHOLMOD_AMD;
  cc_.supernodal = CHOLMOD_AUTO;

  // Perform symbolic analysis of the matrix.
  cholmod_factor_ = cholmod_analyze(&A, &cc_);
  if (VLOG_IS_ON(2)) {
    cholmod_print_common(const_cast<char*>("Symbolic Analysis"), &cc_);
  }

  if (cc_.status != CHOLMOD_OK) {
    VLOG(2) << "cholmod_analyze failed. error code: %d" << cc_.status;
    info_ = Eigen::NumericalIssue;
    return;
  }

  is_analysis_ok_ = true;
  is_factorization_ok_ = false;
  info_ = Eigen::Success;
}

void SparseCholeskyLLt::Factorize(const Eigen::SparseMatrix<double>& mat) {
  CHECK_NOTNULL(cholmod_factor_);
  CHECK(is_analysis_ok_) << "Cannot call Factorize() because symbolic analysis "
                            "of the matrix (i.e. AnalyzePattern()) failed!";
  cholmod_sparse A = ViewAsCholmod(mat);

  // Save the current print level and silence CHOLMOD, otherwise
  // CHOLMOD is prone to dumping stuff to stderr, which can be
  // distracting when the error (matrix is indefinite) is not a fatal
  // failure.
  const int old_print_level = cc_.print;
  cc_.print = 0;

  cc_.quick_return_if_not_posdef = 1;
  int cholmod_status = cholmod_factorize(&A, cholmod_factor_, &cc_);
  cc_.print = old_print_level;

  // TODO(sameeragarwal): This switch statement is not consistent. It
  // treats all kinds of CHOLMOD failures as warnings. Some of these
  // like out of memory are definitely not warnings. The problem is
  // that the return value Cholesky is two valued, but the state of
  // the linear solver is really three valued. SUCCESS,
  // NON_FATAL_FAILURE (e.g., indefinite matrix) and FATAL_FAILURE
  // (e.g. out of memory).
  switch (cc_.status) {
    case CHOLMOD_NOT_INSTALLED:
      LOG(ERROR) << "CHOLMOD failure: Method not installed.";
      info_ = Eigen::InvalidInput;
      return;
    case CHOLMOD_OUT_OF_MEMORY:
      LOG(ERROR) << "CHOLMOD failure: Out of memory.";
      info_ = Eigen::NumericalIssue;
      return;
    case CHOLMOD_TOO_LARGE:
      LOG(ERROR) << "CHOLMOD failure: Integer overflow occured.";
      info_ = Eigen::NumericalIssue;
      return;
    case CHOLMOD_INVALID:
      LOG(ERROR) << "CHOLMOD failure: Invalid input.";
      info_ = Eigen::InvalidInput;
      return;
    case CHOLMOD_NOT_POSDEF:
      LOG(ERROR) << "CHOLMOD warning: Matrix not positive definite.";
      info_ = Eigen::NumericalIssue;
      return;
    case CHOLMOD_DSMALL:
      LOG(ERROR) << "CHOLMOD warning: D for LDL' or diag(L) or "
                    "LL' has tiny absolute value.";
      info_ = Eigen::NumericalIssue;
      return;
    case CHOLMOD_OK:
      // If everything is done successfully, set the appropriate flags to
      // success and exit.
      if (cholmod_status != 0) {
        info_ = Eigen::Success;
        is_factorization_ok_ = true;
        return;
      }
      LOG(ERROR) << "CHOLMOD failure: cholmod_factorize returned false "
                    "but cholmod_common::status is CHOLMOD_OK.";
      info_ = Eigen::NumericalIssue;
      return;
    default:
      LOG(ERROR) << "Unknown cholmod return code: " << cc_.status;
      info_ = Eigen::InvalidInput;
      return;
  }
}

void SparseCholeskyLLt::Compute(const Eigen::SparseMatrix<double>& mat) {
  AnalyzePattern(mat);
  Factorize(mat);
}

Eigen::ComputationInfo SparseCholeskyLLt::Info() { return info_; }

// Using the cholesky decomposition, solve for x that minimizes
//    lhs * x = rhs
// where lhs is the factorized matrix.
Eigen::VectorXd SparseCholeskyLLt::Solve(const Eigen::VectorXd& rhs) {
  CHECK_NOTNULL(cholmod_factor_);
  CHECK(is_analysis_ok_) << "Cannot call Solve() because symbolic analysis "
                            "of the matrix (i.e. AnalyzePattern()) failed!";
  CHECK(is_factorization_ok_)
      << "Cannot call Solve() because numeric factorization "
         "of the matrix (i.e. Factorize()) failed!";

  Eigen::VectorXd solution;
  if (cc_.status != CHOLMOD_OK) {
    LOG(ERROR) << "cholmod_solve failed. CHOLMOD status is not CHOLMOD_OK";
    return solution;
  }

  // returns a cholmod_dense*
  cholmod_dense b = ViewAsCholmod(rhs);
  cholmod_dense* x = cholmod_solve(CHOLMOD_A, cholmod_factor_, &b, &cc_);
  if (x == nullptr) {
    info_ = Eigen::NumericalIssue;
    return solution;
  }
  solution = Eigen::Map<Eigen::VectorXd>(reinterpret_cast<double*>(x->x),
                                         x->nrow, x->ncol);
  return solution;
}

}  // namespace DAGSfM


//////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Alejandro Cabrera 2011.
// 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)
//
// See http://www.boost.org/libs/bloom_filter for documentation.
//
//////////////////////////////////////////////////////////////////////////////

#ifndef BOOST_BLOOM_FILTER_APPLY_HASH_HPP
#define BOOST_BLOOM_FILTER_APPLY_HASH_HPP

#include <boost/mpl/at.hpp>

namespace boost {
  namespace bloom_filters {
    namespace detail {

      template <size_t N,
		typename Container>
      struct apply_hash
      {
	typedef typename Container::value_type value_type;
	typedef typename Container::bitset_type bitset_type;
	typedef typename Container::hash_function_type hash_function_type;

        static void insert(const value_type& t,
			   bitset_type& _bits)
	{
	  typedef typename boost::mpl::at_c<hash_function_type, N>::type Hash;
	  static Hash hasher;

	  _bits[hasher(t) % _bits.size()] = true;
	  apply_hash<N-1, Container>::insert(t, _bits);
        }

        static bool contains(const value_type& t,
			     const bitset_type& _bits)
	{
	  typedef typename boost::mpl::at_c<hash_function_type, N>::type Hash;
	  static Hash hasher;

	  return (_bits[hasher(t) % _bits.size()] &&
		  apply_hash<N-1, Container>::contains(t, _bits));
        }
      };

      template <typename Container>
      struct apply_hash<0, Container>
      {
	typedef typename Container::value_type value_type;
	typedef typename Container::bitset_type bitset_type;
	typedef typename Container::hash_function_type hash_function_type;

        static void insert(const value_type& t,
			   bitset_type& _bits)
	{
	  typedef typename boost::mpl::at_c<hash_function_type, 0>::type Hash;
	  static Hash hasher;

	  _bits[hasher(t) % _bits.size()] = true;
        }

        static bool contains(const value_type& t,
			     const bitset_type& _bits)
	{
	  typedef typename boost::mpl::at_c<hash_function_type, 0>::type Hash;
	  static Hash hasher;

	  return (_bits[hasher(t) % _bits.size()]);
        }
      };

    } // namespace detail
  } // namespace bloom_filter
} // namespace boost
#endif


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/include/functions/acos.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <complex>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <boost/math/complex/acos.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/include/constants/four.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/pio_2.hpp>
#include <nt2/include/constants/three.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/i.hpp>

NT2_TEST_CASE_TPL ( acos,  NT2_REAL_TYPES)
{
  using nt2::acos;
  using nt2::tag::acos_;
  typedef typename std::complex<T> cT;
  typedef typename nt2::meta::call<acos_(cT)>::type r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, cT);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(nt2::Nan <T>(),  nt2::Zero<T>())), cT(nt2::Nan <T>(), nt2::Nan<T>()), 1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(nt2::Nan  <T>(), nt2::One<T>())),  cT(nt2::Nan <T>(), nt2::Nan<T>()), 1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(nt2::Nan  <T>(), nt2::Inf<T>())),  cT(nt2::Nan <T>(), -nt2::Inf<T>()), 1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(nt2::Nan  <T>(), nt2::Nan<T>())),  cT(nt2::Nan <T>(), nt2::Nan<T>()), 1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(nt2::Zero <T>(), nt2::Nan <T>())), cT(nt2::Pio_2<T>(), nt2::Nan<T>()), 1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(nt2::One  <T>(), nt2::Nan <T>())), cT(nt2::Nan<T>(),  nt2::Nan<T>()), 1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(nt2::Inf  <T>(), nt2::Nan<T>())),  cT(nt2::Nan <T>(), nt2::Minf<T>()), 1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(nt2::One  <T>(), nt2::Inf <T>())), cT(nt2::Pio_2<T>(), -nt2::Inf<T>()), 1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(nt2::Inf  <T>(), nt2::One <T>())), cT(nt2::Zero<T>(), nt2::Minf <T>()), 1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(nt2::Inf  <T>(), nt2::Inf<T>())),  cT(nt2::Pi<T>()/nt2::Four<T>(), nt2::Minf <T>()), 1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(-nt2::Nan  <T>(), nt2::One<T>())),  cT(nt2::Nan <T>(), nt2::Nan<T>()), 1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(-nt2::Nan  <T>(), nt2::Inf<T>())),  cT(nt2::Nan <T>(), -nt2::Inf<T>()), 1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(-nt2::Nan  <T>(), nt2::Nan<T>())),  cT(nt2::Nan <T>(), nt2::Nan<T>()), 1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(-nt2::Zero <T>(), nt2::Nan <T>())), cT(nt2::Pio_2<T>(), nt2::Nan<T>()), 1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(-nt2::One  <T>(), nt2::Nan <T>())), cT(nt2::Nan<T>(),  nt2::Nan<T>()), 1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(-nt2::Inf  <T>(), nt2::Nan<T>())),  cT(nt2::Nan <T>(), nt2::Minf<T>()), 1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(-nt2::One  <T>(), nt2::Inf <T>())), cT(nt2::Pio_2<T>(), nt2::Minf<T>()), 1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(-nt2::Inf  <T>(), nt2::One <T>())), cT(nt2::Pi<T>(), -nt2::Inf <T>()), 1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(-nt2::Inf  <T>(), nt2::Inf<T>())),  cT(nt2::Three<T>()*nt2::Pi<T>()/nt2::Four<T>(), nt2::Minf <T>()), 1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(nt2::Nan  <T>(), nt2::One<T>())), boost::math::acos(cT(nt2::Nan  <T>(), nt2::One<T>())),  1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(nt2::Nan  <T>(), nt2::Inf<T>())), boost::math::acos(cT(nt2::Nan  <T>(), nt2::Inf<T>())),  1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(nt2::Nan  <T>(), nt2::Nan<T>())), boost::math::acos(cT(nt2::Nan  <T>(), nt2::Nan<T>())),  1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(nt2::Zero <T>(), nt2::Nan <T>())),boost::math::acos(cT(nt2::Zero <T>(), nt2::Nan <T>())), 1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(nt2::One  <T>(), nt2::Nan <T>())),boost::math::acos(cT(nt2::One  <T>(), nt2::Nan <T>())), 1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(nt2::Inf  <T>(), nt2::Nan<T>())), boost::math::acos(cT(nt2::Inf  <T>(), nt2::Nan<T>())),  1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(nt2::One  <T>(), nt2::Inf <T>())),boost::math::acos(cT(nt2::One  <T>(), nt2::Inf <T>())), 1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(nt2::Inf  <T>(), nt2::One <T>())),boost::math::acos(cT(nt2::Inf  <T>(), nt2::One <T>())), 1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(nt2::Inf  <T>(), nt2::Inf<T>())), boost::math::acos(cT(nt2::Inf  <T>(), nt2::Inf<T>())),  1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(-nt2::Nan  <T>(), nt2::One<T>())), boost::math::acos(cT(-nt2::Nan  <T>(), nt2::One<T>())),    1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(-nt2::Nan  <T>(), nt2::Inf<T>())), boost::math::acos(cT(-nt2::Nan  <T>(), nt2::Inf<T>())),    1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(-nt2::Nan  <T>(), nt2::Nan<T>())), boost::math::acos(cT(-nt2::Nan  <T>(), nt2::Nan<T>())),    1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(-nt2::Zero <T>(), nt2::Nan <T>())),boost::math::acos(cT(-nt2::Zero <T>(), nt2::Nan <T>())),   1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(-nt2::One  <T>(), nt2::Nan <T>())),boost::math::acos(cT(-nt2::One  <T>(), nt2::Nan <T>())),   1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(-nt2::Inf  <T>(), nt2::Nan<T>())), boost::math::acos(cT(-nt2::Inf  <T>(), nt2::Nan<T>())),    1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(-nt2::One  <T>(), nt2::Inf <T>())),boost::math::acos(cT(-nt2::One  <T>(), nt2::Inf <T>())),   1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(-nt2::Inf  <T>(), nt2::One <T>())),boost::math::acos(cT(-nt2::Inf  <T>(), nt2::One <T>())),   1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(-nt2::Inf  <T>(), nt2::Inf<T>())), boost::math::acos(cT(-nt2::Inf  <T>(), nt2::Inf<T>())),    1.0);
#endif
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(nt2::Zero<T>(),  nt2::Zero<T>())), cT(nt2::Pio_2<T>(), nt2::Zero<T>()), 1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(nt2::One  <T>(), nt2::One <T>())),   cT(0.904556894302381, -1.061275061905036), 2);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(nt2::One  <T>(), nt2::Mone <T>())),  cT(0.904556894302381,  1.061275061905036), 2);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(nt2::Mone  <T>(), nt2::One <T>())),  cT(2.237035759287412, -1.061275061905036), 2);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(nt2::Mone  <T>(), nt2::Mone <T>())), cT(2.237035759287412,  1.061275061905036), 2);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(0.5, -0.5)),  boost::math::acos(cT(0.5, -0.5)), 1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(1, 1)),  boost::math::acos(cT(1, 1)), 1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(1, 10)),  boost::math::acos(cT(1, 10)), 1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(10, 10)),  boost::math::acos(cT(10, 10)), 1.0);
  NT2_TEST_ULP_EQUAL(nt2::acos(cT(10, 1)),  boost::math::acos(cT(10, 1)), 1.0);
}


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/include/functions/exp.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <complex>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/mzero.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/exp_1.hpp>
#include <nt2/include/constants/i.hpp>

NT2_TEST_CASE_TPL ( exp_real,  NT2_REAL_TYPES)
{
  using nt2::exp;
  using nt2::tag::exp_;
  typedef typename std::complex<T> cT;
  NT2_TEST_TYPE_IS( typename nt2::meta::call<exp_(cT)>::type, cT );

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(nt2::exp(cT(nt2::Inf<T>())), cT(nt2::Inf<T>()));
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::Minf<T>())), cT(nt2::Zero<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::Nan<T>())), cT(nt2::Nan<T>(), nt2::Zero<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::Inf  <T>(), nt2::Zero<T>())), cT(nt2::Inf<T>(), nt2::Zero<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::Nan  <T>(), nt2::Zero<T>())), cT(nt2::Nan <T>(), nt2::Zero<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::One  <T>(), nt2::Inf <T>())), cT(nt2::Nan<T>(), nt2::Nan<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::One  <T>(), nt2::Nan <T>())), cT(nt2::Nan<T>(), nt2::Nan<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::Minf <T>(), nt2::One <T>())), cT(nt2::Zero<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::Minf <T>(), nt2::Mone<T>())), cT(nt2::Zero<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::Inf  <T>(), nt2::One <T>())), cT(nt2::Inf <T>()*exp(cT(nt2::Zero<T>(), nt2::One <T>()))), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::Inf  <T>(), nt2::Mone<T>())), cT(nt2::Inf <T>()*exp(cT(nt2::Zero<T>(), nt2::Mone<T>()))), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::Minf <T>(), nt2::Inf<T>())),  cT(nt2::Zero<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::Inf  <T>(), nt2::Inf<T>())),  cT(nt2::Nan <T>(), nt2::Nan<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::Minf <T>(), nt2::Nan<T>())),  cT(nt2::Zero<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::Inf  <T>(), nt2::Nan<T>())),  cT(nt2::Nan <T>(), nt2::Nan<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::Nan  <T>(), nt2::One<T>())),  cT(nt2::Nan <T>(), nt2::Nan<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::Nan  <T>(), nt2::Nan<T>())),  cT(nt2::Nan <T>(), nt2::Nan<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::One  <T>(), -nt2::Inf <T>())), cT(nt2::Nan<T>(), nt2::Nan<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::One  <T>(), -nt2::Nan <T>())), cT(nt2::Nan<T>(), nt2::Nan<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::Minf <T>(), -nt2::One <T>())), cT(nt2::Zero<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::Minf <T>(), -nt2::Mone<T>())), cT(nt2::Zero<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::Inf  <T>(), -nt2::One <T>())), cT(nt2::Inf <T>()*exp(cT(nt2::Zero<T>(), -nt2::One <T>()))), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::Inf  <T>(), -nt2::Mone<T>())), cT(nt2::Inf <T>()*exp(cT(nt2::Zero<T>(), -nt2::Mone<T>()))), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::Minf <T>(), -nt2::Inf<T>())),  cT(nt2::Zero<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::Inf  <T>(), -nt2::Inf<T>())),  cT(nt2::Nan <T>(), -nt2::Nan<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::Minf <T>(), -nt2::Nan<T>())),  cT(nt2::Zero<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::Inf  <T>(), -nt2::Nan<T>())),  cT(nt2::Nan <T>(), -nt2::Nan<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::Nan  <T>(), -nt2::One<T>())),  cT(nt2::Nan <T>(), -nt2::Nan<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::Nan  <T>(), -nt2::Nan<T>())),  cT(nt2::Nan <T>(), -nt2::Nan<T>()), 0.75);
#endif
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::Mone<T>())), cT(nt2::One<T>()/nt2::Exp_1<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::One<T>())), cT(nt2::Exp_1<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::Zero<T>())), cT(nt2::One<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(0, nt2::Pi<T>())),  cT(nt2::Mone<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::Zero<T>(),  nt2::Zero<T>())), cT(nt2::One<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::exp(cT(nt2::Mzero<T>(), nt2::Zero<T>())), cT(nt2::One<T>()), 0.75);
}


#include <cstddef>
#include <array>
#include <fstream>
#include <armadillo>

template <std::size_t SIZE>
using column = std::array<arma::vec, SIZE>;

template <std::size_t WIDTH, std::size_t HEIGHT>
class image {
	column<HEIGHT> *color;

	public:
	image () {
		color = new column<HEIGHT>[WIDTH];
	}

	~image () {
		delete [] color;
	}

	column<HEIGHT>& operator[](std::size_t pos) {
		return color[pos];
	}

	const column<HEIGHT>& operator[](std::size_t pos) const {
		return color[pos];
	}
};

template <std::size_t WIDTH, std::size_t HEIGHT>
void write_pgm (const image<WIDTH, HEIGHT> &img, std::ostream &file) {
	file << "P3" << std::endl;
	file << WIDTH << " " << HEIGHT << std::endl;
	file << "255" << std::endl;

	std::size_t i, j;
	for (j=0; j<HEIGHT; j++) {
		for (i=0; i<WIDTH; i++) {
			arma::vec curr = img[i][j];
			file << curr(0) << " " << curr(1) << " "
					<< curr(2) << " ";
		}
		file << std::endl;
	}
}

template <std::size_t WIDTH, std::size_t HEIGHT>
void render (image<WIDTH, HEIGHT> &img) {
	std::size_t i, j;
	for (i=0; i<WIDTH; i++) {
		for (j=0; j<HEIGHT; j++) {
			if (i < WIDTH / 3) {
				img[i][j] << 0 << 0 << 255;
			} else if (i < 2 * WIDTH / 3) {
				img[i][j] << 255 << 255 << 255;
			} else {
				img[i][j] << 255 << 0 << 0;
			}
		}
	}
}

int main (int argc, char **argv) {
	image<800, 600> img;
	render (img);

	std::ofstream file("out0.pgm");
	write_pgm (img, file);
	file.close();

	return 0;
}


/**
* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
*
* Copyright (c) 2016 LIBSCAPI (http://crypto.biu.ac.il/SCAPI)
* This file is part of the SCAPI project.
* DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
*
* 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.
*
* We request that any publication and/or code referring to and/or based on SCAPI contain an appropriate citation to SCAPI, including a reference to
* http://crypto.biu.ac.il/SCAPI.
*
* Libscapi uses several open source libraries. Please see these projects for any further licensing issues.
* For more information , See https://github.com/cryptobiu/libscapi/blob/master/LICENSE.MD
*
* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
*
*/

#pragma once

#include <boost/thread/thread.hpp>
#include "../../include/comm/Comm.hpp"
#include "../../include/interactive_mid_protocols/SigmaProtocolDlog.hpp"
#include "../../include/interactive_mid_protocols/ZeroKnowledge.hpp"
#include "../../include/primitives/DlogOpenSSL.hpp"
#include "../../include/infra/Scanner.hpp"
#include "../../include/infra/ConfigFile.hpp"

struct SigmaDlogParams {
	biginteger w;
	biginteger p;
	biginteger q;
	biginteger g;
	int t;
	IpAddress proverIp;
	IpAddress verifierIp;
	int proverPort;
	int verifierPort;
	string protocolName;

	SigmaDlogParams(biginteger w, biginteger p, biginteger q, biginteger g, int t,
		IpAddress proverIp, IpAddress verifierIp, int proverPort, int verifierPort,
		string protocolName) {
		this->w = w; // witness
		this->p = p; // group order - must be prime
		this->q = q; // sub group order - prime such that p=2q+1
		this->g = g; // generator of Zq
		this->t = t; // soundness param must be: 2^t<q
		this->proverIp = proverIp;
		this->verifierIp = verifierIp;
		this->proverPort = proverPort;
		this->verifierPort = verifierPort;
		this->protocolName = protocolName;
	};
};

SigmaDlogParams readSigmaConfig(string config_file) {
	ConfigFile cf(config_file);
	string input_section = cf.Value("", "input_section");
	biginteger p = biginteger(cf.Value(input_section, "p"));
	biginteger q = biginteger(cf.Value(input_section, "q"));
	biginteger g = biginteger(cf.Value(input_section, "g"));
	biginteger w = biginteger(cf.Value(input_section, "w"));
	int t = stoi(cf.Value(input_section, "t"));
	string proverIpStr = cf.Value("", "proverIp");
	string verifierIpStr = cf.Value("", "verifierIp");
	int proverPort = stoi(cf.Value("", "proverPort"));
	int verifierPort = stoi(cf.Value("", "verifierPort"));
	auto proverIp = IpAddress::from_string(proverIpStr);
	auto verifierIp = IpAddress::from_string(verifierIpStr);
	string protocolName = cf.Value("", "protocolName");
	return SigmaDlogParams(w, p, q, g, t, proverIp, verifierIp, proverPort, verifierPort, protocolName);
};

void SigmaUsage() {
	std::cerr << "Usage: ./libscapi_examples <1(=prover)|2(=verifier)> config_file_path" << std::endl;
}

class ProverVerifierExample {
public:
	virtual void prove(shared_ptr<CommParty> server,
		shared_ptr<SigmaDlogProverComputation> proverComputation,
		shared_ptr<DlogGroup> dg,
		shared_ptr<SigmaDlogProverInput> proverinput) = 0;
	virtual bool verify(shared_ptr<CommParty> server,
		shared_ptr<SigmaDlogVerifierComputation> verifierComputation,
		shared_ptr<SigmaGroupElementMsg> msgA,
		shared_ptr<SigmaBIMsg> msgZ,
		shared_ptr<SigmaDlogCommonInput> commonInput,
		shared_ptr<DlogGroup> dg) = 0;
};

class SimpleDlogSigma : public ProverVerifierExample {
public:
	virtual void prove(shared_ptr<CommParty> server,
		shared_ptr<SigmaDlogProverComputation> proverComputation,
		shared_ptr<DlogGroup> dg,
		shared_ptr<SigmaDlogProverInput> proverinput) override {
		auto sp = new SigmaProtocolProver(server, proverComputation);
		cout << "--> running simple sigma dlog prover" << endl;
		sp->prove(proverinput);
	}
	virtual bool verify(shared_ptr<CommParty> server,
		shared_ptr<SigmaDlogVerifierComputation> verifierComputation,
		shared_ptr<SigmaGroupElementMsg> msgA,
		shared_ptr<SigmaBIMsg> msgZ,
		shared_ptr<SigmaDlogCommonInput> commonInput,
		shared_ptr<DlogGroup> dg) override{
		auto v = new SigmaProtocolVerifier(server, verifierComputation, msgA, msgZ);
		cout << "--> running simple sigma dlog verify" << endl;
		bool verificationPassed = v->verify(commonInput.get());
		delete v;
		return verificationPassed;
	}
};

class ZKFromSigma : public ProverVerifierExample {
public:
	virtual void prove(shared_ptr<CommParty> server,
		shared_ptr<SigmaDlogProverComputation> proverComputation,
		shared_ptr<DlogGroup> dg,
		shared_ptr<SigmaDlogProverInput> proverinput) {
		cout << "before creating ZK prover" << endl;
		auto receiver = make_shared<CmtPedersenReceiver>(server, dg);
		auto sp = new ZKFromSigmaProver(server, proverComputation, receiver);
		cout << "--> running ZK prover" << endl;
		sp->prove(proverinput);
	}
	virtual bool verify(shared_ptr<CommParty> server,
		shared_ptr<SigmaDlogVerifierComputation> verifierComputation,
		shared_ptr<SigmaGroupElementMsg> msgA,
		shared_ptr<SigmaBIMsg> msgZ,
		shared_ptr<SigmaDlogCommonInput> commonInput,
		shared_ptr<DlogGroup> dg) override {
		cout << "before creating ZK verifier" << endl;
		auto emptyTrap = make_shared<CmtRTrapdoorCommitPhaseOutput>();
		auto committer = make_shared<CmtPedersenCommitter>(server, dg);
		auto v = new ZKFromSigmaVerifier(server, verifierComputation, committer);
		cout << "--> running ZK verify" << endl;
		bool verificationPassed = v->verify(commonInput.get(), msgA, msgZ);
		delete v;
		return verificationPassed;
	}
};

class PedersenZKSigma : public ProverVerifierExample {
public:
	virtual void prove(shared_ptr<CommParty> server,
		shared_ptr<SigmaDlogProverComputation> proverComputation,
		shared_ptr<DlogGroup> dg,
		shared_ptr<SigmaDlogProverInput> proverinput) {
		auto sp = new ZKPOKFromSigmaCmtPedersenProver(server, proverComputation, dg);
		cout << "--> running pedersen prover" << endl;
		sp->prove(proverinput);
	}
	virtual bool verify(shared_ptr<CommParty> server,
		shared_ptr<SigmaDlogVerifierComputation> verifierComputation,
		shared_ptr<SigmaGroupElementMsg> msgA,
		shared_ptr<SigmaBIMsg> msgZ,
		shared_ptr<SigmaDlogCommonInput> commonInput,
		shared_ptr<DlogGroup> dg) override {
		auto emptyTrap = make_shared<CmtRTrapdoorCommitPhaseOutput>();
		auto v = new ZKPOKFromSigmaCmtPedersenVerifier(server, verifierComputation, emptyTrap, dg);
		cout << "--> running pedersen verify" << endl;
		bool verificationPassed = v->verify(commonInput.get(), msgA, msgZ);
		delete v;
		return verificationPassed;
	}
};

class ZKPOKFiatShamir : public ProverVerifierExample {
public:
	virtual void prove(shared_ptr<CommParty> server,
		shared_ptr<SigmaDlogProverComputation> proverComputation,
		shared_ptr<DlogGroup> dg,
		shared_ptr<SigmaDlogProverInput> proverinput) {
		auto sp = new ZKPOKFiatShamirFromSigmaProver(server, proverComputation);
		cout << "--> running Fiat Shamir prover" << endl;
		vector<byte> cont;
		auto input = make_shared<ZKPOKFiatShamirProverInput>(proverinput, cont);
		sp->prove(input);
	}
	virtual bool verify(shared_ptr<CommParty> server,
		shared_ptr<SigmaDlogVerifierComputation> verifierComputation,
		shared_ptr<SigmaGroupElementMsg> msgA,
		shared_ptr<SigmaBIMsg> msgZ,
		shared_ptr<SigmaDlogCommonInput> commonInput,
		shared_ptr<DlogGroup> dg) override {
		auto emptyTrap = make_shared<CmtRTrapdoorCommitPhaseOutput>();
		auto v = new ZKPOKFiatShamirFromSigmaVerifier(server, verifierComputation);
		cout << "--> running Fiat Shamir verify" << endl;
		vector<byte> cont;
		auto input = make_shared<ZKPOKFiatShamirCommonInput>(commonInput.get(), cont);
		bool verificationPassed = v->verify(input.get(), msgA, msgZ);
		delete v;
		return verificationPassed;
	}
};

shared_ptr<ProverVerifierExample> getProverVerifier(SigmaDlogParams sdp)
{
	shared_ptr<ProverVerifierExample> sds;
	if(sdp.protocolName=="Simple")
		sds = make_shared<SimpleDlogSigma>();
	else if (sdp.protocolName == "SimpleZK")
		sds = make_shared<ZKFromSigma>();
	else if(sdp.protocolName=="ZKPedersen")
		sds = make_shared<PedersenZKSigma>();
	else if (sdp.protocolName == "ZKFiatShamir")
		sds = make_shared<ZKPOKFiatShamir>();
	return sds;
}


// Copyright 2005 The Trustees of Indiana University.

// Use, modification and distribution is subject to the Boost Software
// License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#include <boost/graph/bellman_ford_shortest_paths.hpp>
#include "graph_types.hpp"
#include <boost/python.hpp>
#include <boost/graph/iteration_macros.hpp>

namespace boost { namespace graph { namespace python {

#define BGL_PYTHON_VISITOR maybe_bellman_ford_visitor
#define BGL_PYTHON_EVENTS_HEADER <boost/graph/python/bellman_ford_events.hpp>
#include <boost/graph/python/visitor.hpp>
#undef BGL_PYTHON_EVENTS_HEADER
#undef BGL_PYTHON_VISITOR

template<typename Graph>
void
bellman_ford_shortest_paths
  (const Graph& g,
   typename graph_traits<Graph>::vertex_descriptor s,
   vector_property_map<
     typename graph_traits<Graph>::vertex_descriptor,
     typename property_map<Graph, vertex_index_t>::const_type>* in_predecessor,
   vector_property_map<
     float,
     typename property_map<Graph, vertex_index_t>::const_type>* in_distance,
   vector_property_map<
     float,
     typename property_map<Graph, edge_index_t>::const_type>* in_weight,
   boost::python::object in_visitor)
{
  using boost::python::object;

  typedef typename property_map<Graph, vertex_index_t>::const_type
    VertexIndexMap;
  typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
  typedef vector_property_map<Vertex, VertexIndexMap> PredecessorMap;
  typedef vector_property_map<float, VertexIndexMap> DistanceMap;
  typedef typename property_map<Graph, edge_index_t>::const_type
    EdgeIndexMap;
  typedef vector_property_map<float, EdgeIndexMap> WeightMap;

  PredecessorMap predecessor =
    in_predecessor? *in_predecessor
    : PredecessorMap(num_vertices(g), get(vertex_index, g));

  DistanceMap distance =
    in_distance? *in_distance
    : DistanceMap(num_vertices(g), get(vertex_index, g));

  WeightMap weight =
    in_weight? *in_weight
    : WeightMap(num_edges(g), get(edge_index, g));

  // If no weight map was provided, initialize every weight with 1.0
  if (!in_weight) {
    BGL_FORALL_EDGES_T(e, g, Graph)
      put(weight, e, 1.0f);
  }

  if (in_visitor != object()) {
    boost::bellman_ford_shortest_paths
      (g,
       root_vertex(s).
       visitor(maybe_bellman_ford_visitor(in_visitor)).
       predecessor_map(predecessor).
       distance_map(distance).
       weight_map(weight));
  } else {
    boost::bellman_ford_shortest_paths
      (g,
       root_vertex(s).
       predecessor_map(predecessor).
       distance_map(distance).
       weight_map(weight));
  }
}

void export_bellman_ford_shortest_paths()
{
  using boost::python::arg;
  using boost::python::def;
  using boost::python::object;

#define UNDIRECTED_GRAPH(Name,Type)                                     \
  {                                                                     \
    typedef graph_traits<Type>::vertex_descriptor vertex_descriptor;    \
    typedef property_map<Type, vertex_index_t>::const_type VertexIndexMap; \
    typedef property_map<Type, edge_index_t>::const_type EdgeIndexMap;  \
    typedef vector_property_map<vertex_descriptor, VertexIndexMap>      \
      VertexPredecessorMap;                                             \
    typedef vector_property_map<float, VertexIndexMap>                  \
      VertexDistanceMap;                                                \
    typedef vector_property_map<default_color_type, VertexIndexMap>     \
      VertexColorMap;                                                   \
    typedef vector_property_map<float, EdgeIndexMap>                    \
      EdgeWeightMap;                                                    \
                                                                        \
    def("bellman_ford_shortest_paths",                                  \
        &boost::graph::python::bellman_ford_shortest_paths<Type>,       \
        (arg("graph"),                                                  \
         arg("root_vertex"),                                            \
         arg("predecessor_map") = static_cast<VertexPredecessorMap*>(0), \
         arg("distance_map") = static_cast<VertexDistanceMap*>(0),      \
         arg("weight_map") = static_cast<EdgeWeightMap*>(0),            \
         arg("visitor") = object()));                                   \
  }
#include "graphs.hpp"
}

} } } // end namespace boost::graph::python


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

    @copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-predicates

    This function object returns @ref True if x is not less than @ref Zero else returns @ref False.

    @par Header <boost/simd/function/is_nltz.hpp>

    @par Note

     Using `is_nltz(x)` is similar to: `!(x < 0)`

    @par Example:

      @snippet is_nltz.cpp is_nltz

    @par Possible output:

      @snippet is_nltz.txt is_nltz

  **/
  as_logical_t<Value> is_nltz(Value const& x);
} }
#endif

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

#endif