#include "PID_3DOF.h"
#include <Eigen/Dense>
using Eigen::Matrix;
//Sets initial errors to zero
void initializePID(PID_3DOF* PID){
	Matrix<float, 3, 1> zeros = Matrix<float, 3, 1>::Zero(3, 1);

	PID->e_prop = zeros;
	PID->e_deriv = zeros;
	PID->e_integ = zeros;
}

//Sets integral error to zero
void resetIntegralErrorPID(PID_3DOF* PID){
	PID->e_integ = Matrix<float, 3, 1>::Zero(3, 1);
}

//Update Kp, Ki and Kd in the PID
void updateControlParamPID(PID_3DOF* PID, Matrix<float, 3, 1> K_p, Matrix<float, 3, 1> K_i, Matrix<float, 3, 1> K_d, Matrix<float, 3, 1> maxInteg){
	PID->K_p = K_p;
	PID->K_d = K_d;
	PID->K_i = K_i;
	PID->maxInteg = maxInteg;
}

//Update all errors
void updateErrorPID(PID_3DOF* PID, Matrix<float, 3, 1> feedForward, Matrix<float, 3, 1> e_prop, Matrix<float, 3, 1> e_deriv, float dt){

	PID->feedForward = feedForward;
	PID->e_prop = e_prop;
	PID->e_deriv = e_deriv;
	PID->e_integ = PID->e_integ+(e_prop*dt); //e_integ = e_integ + e_prop*dt

	//Saturate integral error
	for (int i = 0; i < 3; i++){
		if (PID->e_integ(i) > PID->maxInteg(i)){
			PID->e_integ(i) = PID->maxInteg(i);
		}
		else if (PID->e_integ(i) < -PID->maxInteg(i)){
			PID->e_integ(i) = -PID->maxInteg(i);
		}
	}
}

//Calculate output of PID
Matrix<float, 3, 1> outputPID(PID_3DOF PID){
	Matrix<float, 3, 1> PID_out;
	PID_out =  PID.feedForward +
				PID.e_prop.cwiseProduct(PID.K_p) +
				PID.e_deriv.cwiseProduct(PID.K_d) +
				PID.e_integ.cwiseProduct(PID.K_i);
	return PID_out;
}

//Helper functions to parse the config file
void split(const string &s, char delim, vector<string> &elems) {
    stringstream ss(s);
    string item;
    while (getline(ss, item, delim)) {
        elems.push_back(item);
    }
}

vector<string> split(const string &s, char delim) {
    vector<string> elems;
    split(s, delim, elems);
    return elems;
}

void updatePar(PID_3DOF *PID_att, PID_3DOF *PID_angVel, PID_3DOF *PID_pos,char const *AttFile,char const *PosFile) {
	Matrix<float, 3, 1> Zero_3x1 = Matrix<float, 3, 1>::Zero(3, 1);
	Matrix<float, 3, 1> KP_RPY = Zero_3x1, KD_RPY = Zero_3x1, KI_RPY = Zero_3x1, maxInteg_RPY = Zero_3x1;
	Matrix<float, 3, 1> KP_w = Zero_3x1, KD_w = Zero_3x1, KI_w = Zero_3x1, maxInteg_w = Zero_3x1;
	Matrix<float, 3, 1> KP_Pos = Zero_3x1, KD_Pos = Zero_3x1, KI_Pos = Zero_3x1, maxInteg_Pos = Zero_3x1;
	char AttParamPath[64];
	char PosParamPath[64];
    string line;
    vector<string> line_vec;

    sprintf(AttParamPath,"/home/root/%s",AttFile);
    sprintf(PosParamPath,"/home/root/%s",PosFile);

    // printf("%s\n",AttParamPath);

    //Get parameters for attitude controller
    ifstream myfile (AttParamPath);
    if (myfile.is_open()) {
		while (getline (myfile ,line)) {
		    line_vec = split(line, ' ');
		    if (line_vec[0] == "KP_R") {
	            KP_RPY(0) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KP_P") {
				KP_RPY(1) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KP_Y") {
				KP_RPY(2) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KD_R") {
	            	KD_RPY(0) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KD_P") {
				KD_RPY(1) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KD_Y") {
				KD_RPY(2) = atof(line_vec[2].c_str());
		    }
		    if (line_vec[0] == "KI_R") {
	            KI_RPY(0) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KI_P") {
				KI_RPY(1) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KI_Y") {
				KI_RPY(2) = atof(line_vec[2].c_str());
		    }
		    if (line_vec[0] == "maxInteg_R") {
	            maxInteg_RPY(0) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "maxInteg_P") {
				maxInteg_RPY(1) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "maxInteg_Y") {
				maxInteg_RPY(2) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KP_wx") {
				KP_w(0) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KP_wy") {
				KP_w(1) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KP_wz") {
		        KP_w(2)  = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KD_wx") {
				KD_w(0) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KD_wy") {
				KD_w(1) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KD_wz") {
		        KD_w(2) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KI_wx") {
				KI_w(0) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KI_wy") {
				KI_w(1) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KI_wz") {
		        KI_w(2) = atof(line_vec[2].c_str());
		    }
		    if (line_vec[0] == "maxInteg_wx") {
	            maxInteg_w(0) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "maxInteg_wy") {
				maxInteg_w(1) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "maxInteg_wz") {
				maxInteg_w(2) = atof(line_vec[2].c_str());
		    }
		}
		myfile.close();
		updateControlParamPID(PID_att, KP_RPY, KI_RPY, KD_RPY, maxInteg_RPY);
		updateControlParamPID(PID_angVel, KP_w, KI_w, KD_w, maxInteg_w);
    }
    else {
		printf("Unable to open file %s \n",AttParamPath);
    }

    //Get parameters for position controller
    ifstream myfile2 (PosParamPath);
    if (myfile2.is_open()) {
		while (getline (myfile2 ,line)) {
		    line_vec = split(line, ' ');
		    if (line_vec[0] == "KP_X") {
	            KP_Pos(0) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KP_Y") {
				KP_Pos(1) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KP_Z") {
				KP_Pos(2) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KD_X") {
	            KD_Pos(0) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KD_Y") {
				KD_Pos(1) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KD_Z") {
				KD_Pos(2) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KI_X") {
	            KI_Pos(0) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KI_Y") {
				KI_Pos(1) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KI_Z") {
				KI_Pos(2) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "maxInteg_X") {
	            maxInteg_Pos(0) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "maxInteg_Y") {
				maxInteg_Pos(1) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "maxInteg_Z") {
				maxInteg_Pos(2) = atof(line_vec[2].c_str());
		    }
		}
		myfile2.close();
		updateControlParamPID(PID_pos, KP_Pos, KI_Pos, KD_Pos, maxInteg_Pos);
    }
    else {
		printf("Unable to open file %s \n", PosParamPath);
    }

	printf("Done updating control parameters\n");
}


//==================================================================================================
/*!
  @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_ABS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ABS_HPP_INCLUDED

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

 /*!
  @ingroup group-arithmetic
    Function object implementing abs

    Computes the absolute value of its parameter.

    @par Semantic

    For any value @c x of type @c T,

    @code
    T r = abs(x);
    @endcode

    is similar to:

    @code
    T r = x < T(0) ? -x : x;
    @endcode

    @par Notes:

    - Be aware that for signed integers the absolute value of @ref Valmin is
    @ref Valmin (thus negative!). This is a side effect of the 2-complement
    representation of integers. To avoid this, you may use the
    saturated_ @ref decorator or convert the input parameter to a larger type
    before taking the absolute value.

    - abs is a also a standard library function name and there possibly exists
    a C macro version which may be called instead of the boost simd version.
    To avoid this you may prefix abs using boost::simd::abs notation.

    @par Decorators

     - std_ @ref decorator for floating entries results in a call to std::abs
     - saturated_ @ref decorator garanties that saturated_(abs)(x) will never be strictly less than 0.
       In fact the only change if that for any signed type T saturated_(abs)(Valmin<T>()) will be
       Valmax<T>()) which is already true for boost::simd::abs with floating types but not for integral
       signed types.

    @see sqr_abs, sqr

  **/
  Value abs(Value const& a0);
} }
#endif

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

#endif


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This is an example illustrating the use of the Bayesian Network
    inference utilities found in the dlib C++ library.

    In this example all the nodes in the Bayesian network are
    boolean variables.  That is, they take on either the value
    0 or the value 1.

    The network contains 4 nodes and looks as follows:

        B     C
        \\   //
         \/ \/
           A
           ||
           \/
            D

    The probabilities of each node are summarized below.  (The probability
    of each node being 0 is not listed since it is just P(X=0) = 1-p(X=1) )

        p(B=1) = 0.01

        p(C=1) = 0.001

        p(A=1 | B=0, C=0) = 0.01
        p(A=1 | B=0, C=1) = 0.5
        p(A=1 | B=1, C=0) = 0.9
        p(A=1 | B=1, C=1) = 0.99

        p(D=1 | A=0) = 0.2
        p(D=1 | A=1) = 0.5

*/

#include <dlib/bayes_utils.h>
#include <dlib/graph_utils.h>
#include <dlib/graph.h>
#include <dlib/directed_graph.h>
#include <iostream>

using namespace dlib;
using namespace std;

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

int main()
{
    try
    {
        // There are many useful convenience functions in this namespace.  They all
        // perform simple access or modify operations on the nodes of a bayesian network.
        // You don't have to use them but they are convenient and they also will check for
        // various errors in your bayesian network when your application is built with
        // the DEBUG or ENABLE_ASSERTS preprocessor definitions defined.  So their use
        // is recommended.  In fact, most of the global functions used in this example
        // program are from this namespace.
        using namespace bayes_node_utils;

        // This statement declares a bayesian network called bn.  Note that a bayesian network
        // in the dlib world is just a directed_graph object that contains a special kind
        // of node called a bayes_node.
        directed_graph<bayes_node>::kernel_1a_c bn;

        // Use an enum to make some more readable names for our nodes.
        enum nodes
        {
            A = 0,
            B = 1,
            C = 2,
            D = 3
        };

        // The next few blocks of code setup our bayesian network.

        // The first thing we do is tell the bn object how many nodes it has
        // and also add the three edges.  Again, we are using the network
        // shown in ASCII art at the top of this file.
        bn.set_number_of_nodes(4);
        bn.add_edge(A, D);
        bn.add_edge(B, A);
        bn.add_edge(C, A);

        // Now we inform all the nodes in the network that they are binary
        // nodes.  That is, they only have two possible values.
        set_node_num_values(bn, A, 2);
        set_node_num_values(bn, B, 2);
        set_node_num_values(bn, C, 2);
        set_node_num_values(bn, D, 2);

        assignment parent_state;
        // Now we will enter all the conditional probability information for each node.
        // Each node's conditional probability is dependent on the state of its parents.
        // To specify this state we need to use the assignment object.  This assignment
        // object allows us to specify the state of each nodes parents.

        // Here we specify that p(B=1) = 0.01
        // parent_state is empty in this case since B is a root node.
        set_node_probability(bn, B, 1, parent_state, 0.01);
        // Here we specify that p(B=0) = 1-0.01
        set_node_probability(bn, B, 0, parent_state, 1-0.01);

        // Here we specify that p(C=1) = 0.001
        // parent_state is empty in this case since B is a root node.
        set_node_probability(bn, C, 1, parent_state, 0.001);
        // Here we specify that p(C=0) = 1-0.001
        set_node_probability(bn, C, 0, parent_state, 1-0.001);

        // This is our first node that has parents. So we set the parent_state
        // object to reflect that A has both B and C as parents.
        parent_state.add(B, 1);
        parent_state.add(C, 1);
        // Here we specify that p(A=1 | B=1, C=1) = 0.99
        set_node_probability(bn, A, 1, parent_state, 0.99);
        // Here we specify that p(A=0 | B=1, C=1) = 1-0.99
        set_node_probability(bn, A, 0, parent_state, 1-0.99);

        // Here we use the [] notation because B and C have already
        // been added into parent state.
        parent_state[B] = 1;
        parent_state[C] = 0;
        // Here we specify that p(A=1 | B=1, C=0) = 0.9
        set_node_probability(bn, A, 1, parent_state, 0.9);
        set_node_probability(bn, A, 0, parent_state, 1-0.9);

        parent_state[B] = 0;
        parent_state[C] = 1;
        // Here we specify that p(A=1 | B=0, C=1) = 0.5
        set_node_probability(bn, A, 1, parent_state, 0.5);
        set_node_probability(bn, A, 0, parent_state, 1-0.5);

        parent_state[B] = 0;
        parent_state[C] = 0;
        // Here we specify that p(A=1 | B=0, C=0) = 0.01
        set_node_probability(bn, A, 1, parent_state, 0.01);
        set_node_probability(bn, A, 0, parent_state, 1-0.01);

        // Here we set probabilities for node D.
        // First we clear out parent state so that it doesn't have any of
        // the assignments for the B and C nodes used above.
        parent_state.clear();
        parent_state.add(A,1);
        // Here we specify that p(D=1 | A=1) = 0.5
        set_node_probability(bn, D, 1, parent_state, 0.5);
        set_node_probability(bn, D, 0, parent_state, 1-0.5);

        parent_state[A] = 0;
        // Here we specify that p(D=1 | A=0) = 0.2
        set_node_probability(bn, D, 1, parent_state, 0.2);
        set_node_probability(bn, D, 0, parent_state, 1-0.2);

        // We have now finished setting up our bayesian network.  So lets compute some
        // probability values.  The first thing we will do is compute the prior probability
        // of each node in the network.  To do this we will use the join tree algorithm which
        // is an algorithm for performing exact inference in a bayesian network.

        // First we need to create an undirected graph which contains set objects at each node and
        // edge.  This long declaration does the trick.
        typedef dlib::set<unsigned long>::compare_1b_c set_type;
        typedef graph<set_type, set_type>::kernel_1a_c join_tree_type;
        join_tree_type join_tree;

        // Now we need to populate the join_tree with data from our bayesian network.  The next
        // function calls do this.  Explaining exactly what they do is outside the scope of this
        // example.  Just think of them as filling join_tree with information that is useful
        // later on for dealing with our bayesian network.
        create_moral_graph(bn, join_tree);
        create_join_tree(join_tree, join_tree);

        // Now that we have a proper join_tree we can use it to obtain a solution to our
        // bayesian network.  Doing this is as simple as declaring an instance of
        // the bayesian_network_join_tree object as follows:
        bayesian_network_join_tree solution(bn, join_tree);

        // now print out the probabilities for each node
        cout << "Using the join tree algorithm:\n";
        cout << "p(A=1) = " << solution.probability(A)(1) << endl;
        cout << "p(A=0) = " << solution.probability(A)(0) << endl;
        cout << "p(B=1) = " << solution.probability(B)(1) << endl;
        cout << "p(B=0) = " << solution.probability(B)(0) << endl;
        cout << "p(C=1) = " << solution.probability(C)(1) << endl;
        cout << "p(C=0) = " << solution.probability(C)(0) << endl;
        cout << "p(D=1) = " << solution.probability(D)(1) << endl;
        cout << "p(D=0) = " << solution.probability(D)(0) << endl;
        cout << "\n\n\n";

        // Now to make things more interesting lets say that we have discovered that the C
        // node really has a value of 1.  That is to say, we now have evidence that
        // C is 1.  We can represent this in the network using the following two function
        // calls.
        set_node_value(bn, C, 1);
        set_node_as_evidence(bn, C);

        // Now we want to compute the probabilities of all the nodes in the network again
        // given that we now know that C is 1.  We can do this as follows:
        bayesian_network_join_tree solution_with_evidence(bn, join_tree);

        // now print out the probabilities for each node
        cout << "Using the join tree algorithm:\n";
        cout << "p(A=1 | C=1) = " << solution_with_evidence.probability(A)(1) << endl;
        cout << "p(A=0 | C=1) = " << solution_with_evidence.probability(A)(0) << endl;
        cout << "p(B=1 | C=1) = " << solution_with_evidence.probability(B)(1) << endl;
        cout << "p(B=0 | C=1) = " << solution_with_evidence.probability(B)(0) << endl;
        cout << "p(C=1 | C=1) = " << solution_with_evidence.probability(C)(1) << endl;
        cout << "p(C=0 | C=1) = " << solution_with_evidence.probability(C)(0) << endl;
        cout << "p(D=1 | C=1) = " << solution_with_evidence.probability(D)(1) << endl;
        cout << "p(D=0 | C=1) = " << solution_with_evidence.probability(D)(0) << endl;
        cout << "\n\n\n";

        // Note that when we made our solution_with_evidence object we reused our join_tree object.
        // This saves us the time it takes to calculate the join_tree object from scratch.  But
        // it is important to note that we can only reuse the join_tree object if we haven't changed
        // the structure of our bayesian network.  That is, if we have added or removed nodes or
        // edges from our bayesian network then we must recompute our join_tree.  But in this example
        // all we did was change the value of a bayes_node object (we made node C be evidence)
        // so we are ok.

        // Next this example will show you how to use the bayesian_network_gibbs_sampler object
        // to perform approximate inference in a bayesian network.  This is an algorithm
        // that doesn't give you an exact solution but it may be necessary to use in some
        // instances.  For example, the join tree algorithm used above, while fast in many
        // instances, has exponential runtime in some cases.  Moreover, inference in bayesian
        // networks is NP-Hard for general networks so sometimes the best you can do is
        // find an approximation.
        // However, it should be noted that the gibbs sampler does not compute the correct
        // probabilities if the network contains a deterministic node.  That is, if any
        // of the conditional probability tables in the bayesian network have a probability
        // of 1.0 for something the gibbs sampler should not be used.

        // This Gibbs sampler algorithm works by randomly sampling possibles values of the
        // network.  So to use it we should set the network to some initial state.

        set_node_value(bn, A, 0);
        set_node_value(bn, B, 0);
        set_node_value(bn, D, 0);

        // We will leave the C node with a value of 1 and keep it as an evidence node.

        // First create an instance of the gibbs sampler object
        bayesian_network_gibbs_sampler sampler;

        // To use this algorithm all we do is go into a loop for a certain number of times
        // and each time through we sample the bayesian network.  Then we count how
        // many times a node has a certain state.  Then the probability of that node
        // having that state is just its count/total times through the loop.

        // The following code illustrates the general procedure.
        unsigned long A_count = 0;
        unsigned long B_count = 0;
        unsigned long C_count = 0;
        unsigned long D_count = 0;

        // The more times you let the loop run the more accurate the result will be.  Here we loop
        // 2000 times.
        const long rounds = 2000;
        for (long i = 0; i < rounds; ++i)
        {
            sampler.sample_graph(bn);

            if (node_value(bn, A) == 1)
                ++A_count;
            if (node_value(bn, B) == 1)
                ++B_count;
            if (node_value(bn, C) == 1)
                ++C_count;
            if (node_value(bn, D) == 1)
                ++D_count;
        }

        cout << "Using the approximate Gibbs Sampler algorithm:\n";
        cout << "p(A=1 | C=1) = " << (double)A_count/(double)rounds << endl;
        cout << "p(B=1 | C=1) = " << (double)B_count/(double)rounds << endl;
        cout << "p(C=1 | C=1) = " << (double)C_count/(double)rounds << endl;
        cout << "p(D=1 | C=1) = " << (double)D_count/(double)rounds << endl;
    }
    catch (std::exception& e)
    {
        cout << "exception thrown: " << endl;
        cout << e.what() << endl;
        cout << "hit enter to terminate" << endl;
        cin.get();
    }
}


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_basic_types.h>
#include <OpenTissue/core/containers/mesh/polymesh/polymesh.h>
#include <OpenTissue/core/containers/mesh/polymesh/util/polymesh_compute_voronoi.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

BOOST_AUTO_TEST_SUITE(opentissue_mesh_polymesh_voronoi);

BOOST_AUTO_TEST_CASE(simple_test)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;
  typedef math_types::vector3_type                         vector3_type;
  typedef OpenTissue::polymesh::PolyMesh<math_types>       mesh_type;

  mesh_type   mesh;

  std::vector<vector3_type> sites(5);

  sites[0](0) =  1.0; sites[0](1) = -1.0; sites[0](2) =  0.0;
  sites[1](0) =  1.0; sites[1](1) =  1.0; sites[1](2) =  0.0;
  sites[2](0) = -1.0; sites[2](1) =  1.0; sites[2](2) =  0.0;
  sites[3](0) = -1.0; sites[3](1) = -1.0; sites[3](2) =  0.0;
  sites[4](0) =  0.0; sites[4](1) =  0.0; sites[4](2) =  0.0;

  OpenTissue::polymesh::compute_voronoi(sites,mesh);

  BOOST_CHECK(mesh.size_vertices() == 4 );
  BOOST_CHECK(mesh.size_faces() == 1 );
  BOOST_CHECK(mesh.size_edges() == 4 );
}

BOOST_AUTO_TEST_CASE(simple_test2)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;
  typedef math_types::vector3_type                         vector3_type;
  typedef OpenTissue::polymesh::PolyMesh<math_types>       mesh_type;

  mesh_type   mesh;

  std::vector<vector3_type> sites(8);

  sites[0](0) =  1.0; sites[0](1) = -1.0; sites[0](2) =  0.0;
  sites[1](0) =  1.0; sites[1](1) =  1.0; sites[1](2) =  0.0;
  sites[2](0) = -1.0; sites[2](1) =  1.0; sites[2](2) =  0.0;
  sites[3](0) = -1.0; sites[3](1) = -1.0; sites[3](2) =  0.0;
  sites[4](0) =  0.0; sites[4](1) =  0.0; sites[4](2) =  0.0;
  sites[5](0) =  2.0; sites[5](1) =  0.0; sites[5](2) =  0.0;
  sites[6](0) =  3.0; sites[6](1) =  1.0; sites[6](2) =  0.0;
  sites[7](0) =  3.0; sites[7](1) = -1.0; sites[7](2) =  0.0;

  OpenTissue::polymesh::compute_voronoi(sites,mesh);

  BOOST_CHECK(mesh.size_vertices() == 8 );
  BOOST_CHECK(mesh.size_faces() == 2 );
  BOOST_CHECK(mesh.size_edges() == 9 );
}

BOOST_AUTO_TEST_SUITE_END();


#include <boost/numeric/conversion/cast.hpp>
#include <boost/test/unit_test.hpp>

#include "util/coordinate_calculation.hpp"

#include <osrm/coordinate.hpp>

#include <cmath>

using namespace osrm;
using namespace osrm::util;

BOOST_AUTO_TEST_SUITE(coordinate_calculation_tests)

BOOST_AUTO_TEST_CASE(compute_angle)
{
    // Simple cases
    // North-South straight line
    Coordinate first(FloatLongitude{1}, FloatLatitude{-1});
    Coordinate middle(FloatLongitude{1}, FloatLatitude{0});
    Coordinate end(FloatLongitude{1}, FloatLatitude{1});
    auto angle = coordinate_calculation::computeAngle(first, middle, end);
    BOOST_CHECK_EQUAL(angle, 180);

    // North-South-North u-turn
    first = Coordinate(FloatLongitude{1}, FloatLatitude{0});
    middle = Coordinate(FloatLongitude{1}, FloatLatitude{1});
    end = Coordinate(FloatLongitude{1}, FloatLatitude{0});
    angle = coordinate_calculation::computeAngle(first, middle, end);
    BOOST_CHECK_EQUAL(angle, 0);

    // East-west straight lines are harder, *simple* coordinates only
    // work at the equator.  For other locations, we need to follow
    // a rhumb line.
    first = Coordinate(FloatLongitude{1}, FloatLatitude{0});
    middle = Coordinate(FloatLongitude{2}, FloatLatitude{0});
    end = Coordinate(FloatLongitude{3}, FloatLatitude{0});
    angle = coordinate_calculation::computeAngle(first, middle, end);
    BOOST_CHECK_EQUAL(angle, 180);

    // East-West-East u-turn
    first = Coordinate(FloatLongitude{1}, FloatLatitude{0});
    middle = Coordinate(FloatLongitude{2}, FloatLatitude{0});
    end = Coordinate(FloatLongitude{1}, FloatLatitude{0});
    angle = coordinate_calculation::computeAngle(first, middle, end);
    BOOST_CHECK_EQUAL(angle, 0);

    // 90 degree left turn
    first = Coordinate(FloatLongitude{1}, FloatLatitude{1});
    middle = Coordinate(FloatLongitude{0}, FloatLatitude{1});
    end = Coordinate(FloatLongitude{0}, FloatLatitude{2});
    angle = coordinate_calculation::computeAngle(first, middle, end);
    BOOST_CHECK_EQUAL(angle, 90);

    // 90 degree right turn
    first = Coordinate(FloatLongitude{1}, FloatLatitude{1});
    middle = Coordinate(FloatLongitude{0}, FloatLatitude{1});
    end = Coordinate(FloatLongitude{0}, FloatLatitude{0});
    angle = coordinate_calculation::computeAngle(first, middle, end);
    BOOST_CHECK_EQUAL(angle, 270);

    // Weird cases
    // Crossing both the meridians
    first = Coordinate(FloatLongitude{-1}, FloatLatitude{-1});
    middle = Coordinate(FloatLongitude{0}, FloatLatitude{1});
    end = Coordinate(FloatLongitude{1}, FloatLatitude{-1});
    angle = coordinate_calculation::computeAngle(first, middle, end);
    BOOST_CHECK_CLOSE(angle, 53.1, 0.2);

    // All coords in the same spot
    first = Coordinate(FloatLongitude{-1}, FloatLatitude{-1});
    middle = Coordinate(FloatLongitude{-1}, FloatLatitude{-1});
    end = Coordinate(FloatLongitude{-1}, FloatLatitude{-1});
    angle = coordinate_calculation::computeAngle(first, middle, end);
    BOOST_CHECK_EQUAL(angle, 180);

    // First two coords in the same spot, then heading north-east
    first = Coordinate(FloatLongitude{-1}, FloatLatitude{-1});
    middle = Coordinate(FloatLongitude{-1}, FloatLatitude{-1});
    end = Coordinate(FloatLongitude{1}, FloatLatitude{1});
    angle = coordinate_calculation::computeAngle(first, middle, end);
    BOOST_CHECK_EQUAL(angle, 180);

    // First two coords in the same spot, then heading west
    first = Coordinate(FloatLongitude{1}, FloatLatitude{1});
    middle = Coordinate(FloatLongitude{1}, FloatLatitude{1});
    end = Coordinate(FloatLongitude{2}, FloatLatitude{1});
    angle = coordinate_calculation::computeAngle(first, middle, end);
    BOOST_CHECK_EQUAL(angle, 180);

    // First two coords in the same spot then heading north
    first = Coordinate(FloatLongitude{1}, FloatLatitude{1});
    middle = Coordinate(FloatLongitude{1}, FloatLatitude{1});
    end = Coordinate(FloatLongitude{1}, FloatLatitude{2});
    angle = coordinate_calculation::computeAngle(first, middle, end);
    BOOST_CHECK_EQUAL(angle, 180);

    // Second two coords in the same spot
    first = Coordinate(FloatLongitude{1}, FloatLatitude{1});
    middle = Coordinate(FloatLongitude{-1}, FloatLatitude{-1});
    end = Coordinate(FloatLongitude{-1}, FloatLatitude{-1});
    angle = coordinate_calculation::computeAngle(first, middle, end);
    BOOST_CHECK_EQUAL(angle, 180);

    // First and last coords on the same spot
    first = Coordinate(FloatLongitude{1}, FloatLatitude{1});
    middle = Coordinate(FloatLongitude{-1}, FloatLatitude{-1});
    end = Coordinate(FloatLongitude{1}, FloatLatitude{1});
    angle = coordinate_calculation::computeAngle(first, middle, end);
    BOOST_CHECK_EQUAL(angle, 0);

    // Check the antimeridian
    first = Coordinate(FloatLongitude{180}, FloatLatitude{90});
    middle = Coordinate(FloatLongitude{180}, FloatLatitude{0});
    end = Coordinate(FloatLongitude{180}, FloatLatitude{-90});
    angle = coordinate_calculation::computeAngle(first, middle, end);
    BOOST_CHECK_EQUAL(angle, 180);

    // Tiny changes below our calculation resolution
    // This should be equivalent to having two points on the same
    // spot.
    first = Coordinate{FloatLongitude{0}, FloatLatitude{0}};
    middle = Coordinate{FloatLongitude{1}, FloatLatitude{0}};
    end = Coordinate{FloatLongitude{1 + std::numeric_limits<double>::epsilon()}, FloatLatitude{0}};
    angle = coordinate_calculation::computeAngle(first, middle, end);
    BOOST_CHECK_EQUAL(angle, 180);

    // Invalid values
    /* TODO: Enable this when I figure out how to use BOOST_CHECK_THROW
     *       and not have the whole test case fail...
    first = Coordinate(FloatLongitude{0}, FloatLatitude{0});
    middle = Coordinate(FloatLongitude{1}, FloatLatitude{0});
    end = Coordinate(FloatLongitude(std::numeric_limits<double>::max()), FloatLatitude{0});
    BOOST_CHECK_THROW( coordinate_calculation::computeAngle(first,middle,end),
                       boost::numeric::positive_overflow);
                       */
}

// Regression test for bug captured in #1347
BOOST_AUTO_TEST_CASE(regression_test_1347)
{
    Coordinate u(FloatLongitude{-100}, FloatLatitude{10});
    Coordinate v(FloatLongitude{-100.002}, FloatLatitude{10.001});
    Coordinate q(FloatLongitude{-100.001}, FloatLatitude{10.002});

    double d1 = coordinate_calculation::perpendicularDistance(u, v, q);

    double ratio;
    Coordinate nearest_location;
    double d2 = coordinate_calculation::perpendicularDistance(u, v, q, nearest_location, ratio);

    BOOST_CHECK_LE(std::abs(d1 - d2), 0.01);
}

BOOST_AUTO_TEST_CASE(regression_point_on_segment)
{
    //  ^
    //  |               t
    //  |
    //  |                 i
    //  |
    //  |---|---|---|---|---|---|---|--->
    //  |
    //  |
    //  |
    //  |
    //  |
    //  |
    //  |
    //  |
    //  |                           s
    FloatCoordinate input{FloatLongitude{55.995715}, FloatLatitude{48.332711}};
    FloatCoordinate start{FloatLongitude{74.140427}, FloatLatitude{-180}};
    FloatCoordinate target{FloatLongitude{53.041084}, FloatLatitude{77.21011}};

    FloatCoordinate nearest;
    double ratio;
    std::tie(ratio, nearest) = coordinate_calculation::projectPointOnSegment(start, target, input);

    FloatCoordinate diff{target.lon - start.lon, target.lat - start.lat};

    BOOST_CHECK_CLOSE(static_cast<double>(start.lon + FloatLongitude{ratio} * diff.lon),
                      static_cast<double>(nearest.lon),
                      0.1);
    BOOST_CHECK_CLOSE(static_cast<double>(start.lat + FloatLatitude{ratio} * diff.lat),
                      static_cast<double>(nearest.lat),
                      0.1);
}

BOOST_AUTO_TEST_CASE(point_on_segment)
{
    //  t
    //  |
    //  |---- i
    //  |
    //  s
    auto result_1 =
        coordinate_calculation::projectPointOnSegment({FloatLongitude{0}, FloatLatitude{0}},
                                                      {FloatLongitude{0}, FloatLatitude{2}},
                                                      {FloatLongitude{2}, FloatLatitude{1}});
    auto reference_ratio_1 = 0.5;
    auto reference_point_1 = FloatCoordinate{FloatLongitude{0}, FloatLatitude{1}};
    BOOST_CHECK_EQUAL(result_1.first, reference_ratio_1);
    BOOST_CHECK_EQUAL(result_1.second.lon, reference_point_1.lon);
    BOOST_CHECK_EQUAL(result_1.second.lat, reference_point_1.lat);

    //  i
    //  :
    //  t
    //  |
    //  |
    //  |
    //  s
    auto result_2 =
        coordinate_calculation::projectPointOnSegment({FloatLongitude{0.}, FloatLatitude{0.}},
                                                      {FloatLongitude{0}, FloatLatitude{2}},
                                                      {FloatLongitude{0}, FloatLatitude{3}});
    auto reference_ratio_2 = 1.;
    auto reference_point_2 = FloatCoordinate{FloatLongitude{0}, FloatLatitude{2}};
    BOOST_CHECK_EQUAL(result_2.first, reference_ratio_2);
    BOOST_CHECK_EQUAL(result_2.second.lon, reference_point_2.lon);
    BOOST_CHECK_EQUAL(result_2.second.lat, reference_point_2.lat);

    //  t
    //  |
    //  |
    //  |
    //  s
    //  :
    //  i
    auto result_3 =
        coordinate_calculation::projectPointOnSegment({FloatLongitude{0.}, FloatLatitude{0.}},
                                                      {FloatLongitude{0}, FloatLatitude{2}},
                                                      {FloatLongitude{0}, FloatLatitude{-1}});
    auto reference_ratio_3 = 0.;
    auto reference_point_3 = FloatCoordinate{FloatLongitude{0}, FloatLatitude{0}};
    BOOST_CHECK_EQUAL(result_3.first, reference_ratio_3);
    BOOST_CHECK_EQUAL(result_3.second.lon, reference_point_3.lon);
    BOOST_CHECK_EQUAL(result_3.second.lat, reference_point_3.lat);

    //     t
    //    /
    //   /.
    //  /  i
    // s
    //
    auto result_4 = coordinate_calculation::projectPointOnSegment(
        {FloatLongitude{0}, FloatLatitude{0}},
        {FloatLongitude{1}, FloatLatitude{1}},
        {FloatLongitude{0.5 + 0.1}, FloatLatitude{0.5 - 0.1}});
    auto reference_ratio_4 = 0.5;
    auto reference_point_4 = FloatCoordinate{FloatLongitude{0.5}, FloatLatitude{0.5}};
    BOOST_CHECK_EQUAL(result_4.first, reference_ratio_4);
    BOOST_CHECK_EQUAL(result_4.second.lon, reference_point_4.lon);
    BOOST_CHECK_EQUAL(result_4.second.lat, reference_point_4.lat);
}

BOOST_AUTO_TEST_CASE(circleCenter)
{
    Coordinate a(FloatLongitude{-100.}, FloatLatitude{10.});
    Coordinate b(FloatLongitude{-100.002}, FloatLatitude{10.001});
    Coordinate c(FloatLongitude{-100.001}, FloatLatitude{10.002});

    auto result = coordinate_calculation::circleCenter(a, b, c);
    BOOST_CHECK(result);
    BOOST_CHECK_EQUAL(*result, Coordinate(FloatLongitude{-100.000833}, FloatLatitude{10.000833}));

    // Co-linear longitude
    a = Coordinate(FloatLongitude{-100.}, FloatLatitude{10.});
    b = Coordinate(FloatLongitude{-100.001}, FloatLatitude{10.001});
    c = Coordinate(FloatLongitude{-100.001}, FloatLatitude{10.002});
    result = coordinate_calculation::circleCenter(a, b, c);
    BOOST_CHECK(result);
    BOOST_CHECK_EQUAL(*result, Coordinate(FloatLongitude{-99.9995}, FloatLatitude{10.0015}));

    // Co-linear longitude, impossible to calculate
    a = Coordinate(FloatLongitude{-100.001}, FloatLatitude{10.});
    b = Coordinate(FloatLongitude{-100.001}, FloatLatitude{10.001});
    c = Coordinate(FloatLongitude{-100.001}, FloatLatitude{10.002});
    result = coordinate_calculation::circleCenter(a, b, c);
    BOOST_CHECK(!result);

    // Co-linear latitude, this is a real case that failed
    a = Coordinate(FloatLongitude{-112.096234}, FloatLatitude{41.147101});
    b = Coordinate(FloatLongitude{-112.096606}, FloatLatitude{41.147101});
    c = Coordinate(FloatLongitude{-112.096419}, FloatLatitude{41.147259});
    result = coordinate_calculation::circleCenter(a, b, c);
    BOOST_CHECK(result);
    BOOST_CHECK_EQUAL(*result, Coordinate(FloatLongitude{-112.09642}, FloatLatitude{41.14707}));

    // Co-linear latitude, variation
    a = Coordinate(FloatLongitude{-112.096234}, FloatLatitude{41.147101});
    b = Coordinate(FloatLongitude{-112.096606}, FloatLatitude{41.147259});
    c = Coordinate(FloatLongitude{-112.096419}, FloatLatitude{41.147259});
    result = coordinate_calculation::circleCenter(a, b, c);
    BOOST_CHECK(result);
    BOOST_CHECK_EQUAL(*result, Coordinate(FloatLongitude{-112.096512}, FloatLatitude{41.146962}));

    // Co-linear latitude, impossible to calculate
    a = Coordinate(FloatLongitude{-112.096234}, FloatLatitude{41.147259});
    b = Coordinate(FloatLongitude{-112.096606}, FloatLatitude{41.147259});
    c = Coordinate(FloatLongitude{-112.096419}, FloatLatitude{41.147259});
    result = coordinate_calculation::circleCenter(a, b, c);
    BOOST_CHECK(!result);

    // Out of bounds
    a = Coordinate(FloatLongitude{-112.096234}, FloatLatitude{41.147258});
    b = Coordinate(FloatLongitude{-112.106606}, FloatLatitude{41.147259});
    c = Coordinate(FloatLongitude{-113.096419}, FloatLatitude{41.147258});
    result = coordinate_calculation::circleCenter(a, b, c);
    BOOST_CHECK(!result);
}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <fstream>
#include <sstream>
#include <stdexcept>
#include <string>
#include <vector>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/adapted/boost_array.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/vector.hpp>
BOOST_GEOMETRY_REGISTER_BOOST_ARRAY_CS(cs::cartesian)

#include <enne-stage/constants.hpp>

#include"/usr/local/pykep/src/lambert_problem.cpp"

int main()
{
double MU_EARTH = enne_stage::ASTRO_ENNE_GM_EARTH;// m^3/s^2

// 200 km, 30deg, quarter orbit
boost::array< double, 3> r1 = {6778136,0,0};
boost::array< double, 3> r2 = {0,6043243.047,3489068};
boost::array< double, 3> v0 = {0,7668.558733,0};
boost::array< double, 3> v3 = {-7557.86574,0,0};

// // Hohmann transfer initial conditions.
// boost::array< double, 3> r1 = {6778136,0,0};
// boost::array< double, 3> r2 = {-6978136,0.00007,0};
// boost::array< double, 3> v0 = {0,7668.558733,0};
// boost::array< double, 3> v3 = {0.000000008,-7557.86574,0};

double r1a = std::sqrt(std::pow(r1[0],2.0)+std::pow(r1[1],2.0)+std::pow(r1[2],2.0));
double dt_orbit =  2*enne_stage::kPI * std::sqrt(std::pow((r1a),3.0)/MU_EARTH);

double start = 500;
const int amount = 100000;

boost::array< double, amount> toff;

std::ofstream myfile;
myfile.open ("../data/i.txt");
for (int i=start; i<start+amount; i++) myfile << i / 10000.0 << std::endl;
myfile.close();

for (int j = 0; j < 5; ++j)
{
	boost::array< double, amount> dvtotarray;
	boost::array< double, amount> toff;

	for (int i = start; i < start + amount; ++i)
	{
		const double a = i / 10000.0;
		double dt = dt_orbit * a;
		kep_toolbox::lambert_problem test( r1,r2, dt, MU_EARTH,0,j);
		int	nmax = test.lambert_problem::get_Nmax();

		if (nmax > 0 && j>0)
		{
		std::vector<boost::array< double, 3> > v1;
		v1 = test.lambert_problem::get_v1();
	    double dv1 = boost::geometry::distance(v1[nmax*2-1], v0);

		std::vector<boost::array< double, 3> > v2;
		v2 = test.lambert_problem::get_v2();
	    double dv2 = boost::geometry::distance(v3, v2[nmax*2-1]);

	    double dvtot = dv1+dv2;

		toff[i-start] = a;
		dvtotarray[i-start] = dvtot;
		}

		if (nmax == 0 && j==0)
		{
		std::vector<boost::array< double, 3> > v1;
		v1 = test.lambert_problem::get_v1();
	    double dv1 = boost::geometry::distance(v1[0], v0);

		std::vector<boost::array< double, 3> > v2;
		v2 = test.lambert_problem::get_v2();
	    double dv2 = boost::geometry::distance(v3, v2[0]);

	    double dvtot = dv1+dv2;

		toff[i-start] = a;
		dvtotarray[i-start] = dvtot;
		}

		if (nmax == 0 && j>0)
		{
		toff[i-start] = a;
		dvtotarray[i-start] = 100000;
		}
		if (nmax == 1 && j>1)
		{
		toff[i-start] = a;
		dvtotarray[i-start] = 100000;
		}
		if (nmax == 2 && j>2)
		{
		toff[i-start] = a;
		dvtotarray[i-start] = 100000;
		}
		if (nmax == 3 && j>3)
		{
		toff[i-start] = a;
		dvtotarray[i-start] = 100000;
		}
	}
	std::string filename;
	std::ostringstream convert;
	convert << j;
	filename = "../data/dv_" + convert.str();
	filename += ".txt";
  	std::ofstream myfile1;
    myfile1.open (filename);
	for (int i=0; i<amount; i++) myfile1 << dvtotarray[i] << std::endl;
  	myfile1.close();

}

  return 0;
}


#include <hpp/fcl/math/sampling.h>
#include <boost/random/lagged_fibonacci.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/math/constants/constants.hpp>

namespace fcl
{

/// The seed the user asked for (cannot be 0)
static boost::uint32_t userSetSeed = 0;

/// Flag indicating whether the first seed has already been generated or not
static bool firstSeedGenerated = false;

/// The value of the first seed
static boost::uint32_t firstSeedValue = 0;

/// Compute the first seed to be used; this function should be called only once
static boost::uint32_t firstSeed()
{
  static boost::mutex fsLock;
  boost::mutex::scoped_lock slock(fsLock);

  if(firstSeedGenerated)
    return firstSeedValue;

  if(userSetSeed != 0)
    firstSeedValue = userSetSeed;
  else firstSeedValue = (boost::uint32_t)(boost::posix_time::microsec_clock::universal_time() - boost::posix_time::ptime(boost::date_time::min_date_time)).total_microseconds();
  firstSeedGenerated = true;

  return firstSeedValue;
}

/// We use a different random number generator for the seeds of the
/// Other random generators. The root seed is from the number of
/// nano-seconds in the current time.
static boost::uint32_t nextSeed()
{
  static boost::mutex rngMutex;
  boost::mutex::scoped_lock slock(rngMutex);
  static boost::lagged_fibonacci607 sGen(firstSeed());
  static boost::uniform_int<> sDist(1, 1000000000);
  static boost::variate_generator<boost::lagged_fibonacci607&, boost::uniform_int<> > s(sGen, sDist);
  return s();
}

boost::uint32_t RNG::getSeed()
{
  return firstSeed();
}

void RNG::setSeed(boost::uint32_t seed)
{
  if(firstSeedGenerated)
  {
    std::cerr << "Random number generation already started. Changing seed now will not lead to deterministic sampling." << std::endl;
  }
  if(seed == 0)
  {
    std::cerr << "Random generator seed cannot be 0. Using 1 instead." << std::endl;
    userSetSeed = 1;
  }
  else
    userSetSeed = seed;
}

RNG::RNG() : generator_(nextSeed()),
                 uniDist_(0, 1),
                 normalDist_(0, 1),
                 uni_(generator_, uniDist_),
                 normal_(generator_, normalDist_)
{
}

double RNG::halfNormalReal(double r_min, double r_max, double focus)
{
  assert(r_min <= r_max);

  const double mean = r_max - r_min;
  double v = gaussian(mean, mean / focus);

  if(v > mean) v = 2.0 * mean - v;
  double r = v >= 0.0 ? v + r_min : r_min;
  return r > r_max ? r_max : r;
}

int RNG::halfNormalInt(int r_min, int r_max, double focus)
{
  int r = (int)floor(halfNormalReal((double)r_min, (double)(r_max) + 1.0, focus));
  return (r > r_max) ? r_max : r;
}

// From: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III,
//       pg. 124-132
void RNG::quaternion(double value[4])
{
  double x0 = uni_();
  double r1 = sqrt(1.0 - x0), r2 = sqrt(x0);
  double t1 = 2.0 * boost::math::constants::pi<double>() * uni_(), t2 = 2.0 * boost::math::constants::pi<double>() * uni_();
  double c1 = cos(t1), s1 = sin(t1);
  double c2 = cos(t2), s2 = sin(t2);
  value[0] = s1 * r1;
  value[1] = c1 * r1;
  value[2] = s2 * r2;
  value[3] = c2 * r2;
}

// From Effective Sampling and Distance Metrics for 3D Rigid Body Path Planning, by James Kuffner, ICRA 2004
void RNG::eulerRPY(double value[3])
{
  value[0] = boost::math::constants::pi<double>() * (2.0 * uni_() - 1.0);
  value[1] = acos(1.0 - 2.0 * uni_()) - boost::math::constants::pi<double>() / 2.0;
  value[2] = boost::math::constants::pi<double>() * (2.0 * uni_() - 1.0);
}

void RNG::disk(double r_min, double r_max, double& x, double& y)
{
  double a = uniform01();
  double b = uniform01();
  double r = std::sqrt(a * r_max * r_max + (1 - a) * r_min * r_min);
  double theta = 2 * boost::math::constants::pi<double>() * b;
  x = r * std::cos(theta);
  y = r * std::sin(theta);
}

void RNG::ball(double r_min, double r_max, double& x, double& y, double& z)
{
  double a = uniform01();
  double b = uniform01();
  double c = uniform01();
  double r = std::pow(a * r_max * r_max * r_max + (1 - a) * r_min * r_min * r_min, 1 / 3.0);
  double theta = std::acos(1 - 2 * b);
  double phi = 2 * boost::math::constants::pi<double>() * c;

  double costheta = std::cos(theta);
  double sintheta = std::sin(theta);
  double cosphi = std::cos(phi);
  double sinphi = std::sin(phi);
  x = r * costheta;
  y = r * sintheta * cosphi;
  z = r * sintheta * sinphi;
}

}


#include <Eigen/Core>

USING_PART_OF_NAMESPACE_EIGEN

namespace Eigen {

/* Echelon a matrix in-place:
 *
 * Meta-Unrolled version, for small fixed-size matrices
 */
template<typename Derived, int Step>
struct unroll_echelon
{
  enum { k = Step - 1,
         Rows = Derived::RowsAtCompileTime,
         Cols = Derived::ColsAtCompileTime,
         CornerRows = Rows - k,
         CornerCols = Cols - k
  };
  static void run(MatrixBase<Derived>& m)
  {
    unroll_echelon<Derived, Step-1>::run(m);
    int rowOfBiggest, colOfBiggest;
    m.template corner<CornerRows, CornerCols>(BottomRight)
     .cwise().abs()
     .maxCoeff(&rowOfBiggest, &colOfBiggest);
    m.row(k).swap(m.row(k+rowOfBiggest));
    m.col(k).swap(m.col(k+colOfBiggest));
    m.template corner<CornerRows-1, CornerCols>(BottomRight)
      -= m.col(k).template end<CornerRows-1>()
       * (m.row(k).template end<CornerCols>() / m(k,k));
  }
};

template<typename Derived>
struct unroll_echelon<Derived, 0>
{
  static void run(MatrixBase<Derived>& m) {}
};

/* Echelon a matrix in-place:
 *
 * Non-unrolled version, for dynamic-size matrices.
 * (this version works for all matrices, but in the fixed-size case the other
 * version is faster).
 */
template<typename Derived>
struct unroll_echelon<Derived, Dynamic>
{
  static void run(MatrixBase<Derived>& m)
  {
    for(int k = 0; k < m.diagonal().size() - 1; k++)
    {
      int rowOfBiggest, colOfBiggest;
      int cornerRows = m.rows()-k, cornerCols = m.cols()-k;
      m.corner(BottomRight, cornerRows, cornerCols)
      .cwise().abs()
      .maxCoeff(&rowOfBiggest, &colOfBiggest);
      m.row(k).swap(m.row(k+rowOfBiggest));
      m.col(k).swap(m.col(k+colOfBiggest));
      m.corner(BottomRight, cornerRows-1, cornerCols)
        -= m.col(k).end(cornerRows-1) * (m.row(k).end(cornerCols) / m(k,k));
    }
  }
};

using namespace std;
template<typename Derived>
void echelon(MatrixBase<Derived>& m)
{
  const int size = DiagonalCoeffs<Derived>::SizeAtCompileTime;
  const bool unroll = size <= 4;
  unroll_echelon<Derived, unroll ? size-1 : Dynamic>::run(m);
}

template<typename Derived>
void doSomeRankPreservingOperations(MatrixBase<Derived>& m)
{
  for(int a = 0; a < 3*(m.rows()+m.cols()); a++)
  {
    double d = ei_random<double>(-1,1);
    int i = ei_random<int>(0,m.rows()-1); // i is a random row number
    int j;
    do {
      j = ei_random<int>(0,m.rows()-1);
    } while (i==j); // j is another one (must be different)
    m.row(i) += d * m.row(j);

    i = ei_random<int>(0,m.cols()-1); // i is a random column number
    do {
      j = ei_random<int>(0,m.cols()-1);
    } while (i==j); // j is another one (must be different)
    m.col(i) += d * m.col(j);
  }
}

} // namespace Eigen

using namespace std;

int main(int, char **)
{
  srand((unsigned int)time(0));
  const int Rows = 6, Cols = 4;
  typedef Matrix<double, Rows, Cols> Mat;
  const int N = Rows < Cols ? Rows : Cols;

  // start with a matrix m that's obviously of rank N-1
  Mat m = Mat::identity(Rows, Cols); // args just in case of dyn. size
  m.row(0) = m.row(1) = m.row(0) + m.row(1);

  doSomeRankPreservingOperations(m);

  // now m is still a matrix of rank N-1
  cout << "Here's the matrix m:" << endl << m << endl;

  cout << "Now let's echelon m (repeating many times for benchmarking purposes):" << endl;
  for(int i = 0; i < 1000000; i++) echelon(m);

  cout << "Now m is:" << endl << m << endl;
}


// Copyright (c) 2018-2019, The Arqma Network
// Copyright (c) 2017-2018, The Monero Project
//
// 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.
//
// Adapted from Java code by Sarang Noether

#include <stdlib.h>
#include <openssl/ssl.h>
#include <boost/thread/mutex.hpp>
#include <boost/thread/lock_guard.hpp>
#include "misc_log_ex.h"
#include "common/perf_timer.h"
extern "C"
{
#include "crypto/crypto-ops.h"
}
#include "rctOps.h"
#include "bulletproofs.h"

#undef ARQMA_DEFAULT_LOG_CATEGORY
#define ARQMA_DEFAULT_LOG_CATEGORY "bulletproofs"

//#define DEBUG_BP

#define PERF_TIMER_START_BP(x) PERF_TIMER_START_UNIT(x, 1000000)

namespace rct
{

static rct::key vector_exponent(const rct::keyV &a, const rct::keyV &b);
static rct::keyV vector_powers(rct::key x, size_t n);
static rct::key inner_product(const rct::keyV &a, const rct::keyV &b);

static constexpr size_t maxN = 64;
static rct::key Hi[maxN], Gi[maxN];
static ge_dsmp Gprecomp[64], Hprecomp[64];
static const rct::key TWO = { {0x02, 0x00, 0x00,0x00 , 0x00, 0x00, 0x00,0x00 , 0x00, 0x00, 0x00,0x00 , 0x00, 0x00, 0x00,0x00 , 0x00, 0x00, 0x00,0x00 , 0x00, 0x00, 0x00,0x00 , 0x00, 0x00, 0x00,0x00 , 0x00, 0x00, 0x00,0x00  } };
static const rct::keyV oneN = vector_powers(rct::identity(), maxN);
static const rct::keyV twoN = vector_powers(TWO, maxN);
static const rct::key ip12 = inner_product(oneN, twoN);
static boost::mutex init_mutex;

static rct::key get_exponent(const rct::key &base, size_t idx)
{
  static const std::string salt("bulletproof");
  std::string hashed = std::string((const char*)base.bytes, sizeof(base)) + salt + tools::get_varint_data(idx);
  return rct::hashToPoint(rct::hash2rct(crypto::cn_fast_hash(hashed.data(), hashed.size())));
}

static void init_exponents()
{
  boost::lock_guard<boost::mutex> lock(init_mutex);

  static bool init_done = false;
  if (init_done)
    return;
  for (size_t i = 0; i < maxN; ++i)
  {
    Hi[i] = get_exponent(rct::H, i * 2);
    rct::precomp(Hprecomp[i], Hi[i]);
    Gi[i] = get_exponent(rct::H, i * 2 + 1);
    rct::precomp(Gprecomp[i], Gi[i]);
  }
  init_done = true;
}

static bool is_reduced(const rct::key &scalar)
{
  rct::key reduced = scalar;
  sc_reduce32(reduced.bytes);
  return scalar == reduced;
}

/* Given two scalar arrays, construct a vector commitment */
static rct::key vector_exponent(const rct::keyV &a, const rct::keyV &b)
{
  CHECK_AND_ASSERT_THROW_MES(a.size() == b.size(), "Incompatible sizes of a and b");
  CHECK_AND_ASSERT_THROW_MES(a.size() <= maxN, "Incompatible sizes of a and maxN");
  rct::key res = rct::identity();
  for (size_t i = 0; i < a.size(); ++i)
  {
    rct::key term;
    rct::addKeys3(term, a[i], Gprecomp[i], b[i], Hprecomp[i]);
    rct::addKeys(res, res, term);
  }
  return res;
}

/* Compute a custom vector-scalar commitment */
static rct::key vector_exponent_custom(const rct::keyV &A, const rct::keyV &B, const rct::keyV &a, const rct::keyV &b)
{
  CHECK_AND_ASSERT_THROW_MES(A.size() == B.size(), "Incompatible sizes of A and B");
  CHECK_AND_ASSERT_THROW_MES(a.size() == b.size(), "Incompatible sizes of a and b");
  CHECK_AND_ASSERT_THROW_MES(a.size() == A.size(), "Incompatible sizes of a and A");
  CHECK_AND_ASSERT_THROW_MES(a.size() <= maxN, "Incompatible sizes of a and maxN");
  rct::key res = rct::identity();
  for (size_t i = 0; i < a.size(); ++i)
  {
    rct::key term;
#if 0
    // we happen to know where A and B might fall, so don't bother checking the rest
    ge_dsmp *Acache = NULL, *Bcache = NULL;
    ge_dsmp Acache_custom[1], Bcache_custom[1];
    if (Gi[i] == A[i])
      Acache = Gprecomp + i;
    else if (i<32 && Gi[i+32] == A[i])
      Acache = Gprecomp + i + 32;
    else
    {
      rct::precomp(Acache_custom[0], A[i]);
      Acache = Acache_custom;
    }
    if (i == 0 && B[i] == Hi[0])
      Bcache = Hprecomp;
    else
    {
      rct::precomp(Bcache_custom[0], B[i]);
      Bcache = Bcache_custom;
    }
    rct::addKeys3(term, a[i], *Acache, b[i], *Bcache);
#else
    ge_dsmp Acache, Bcache;
    rct::precomp(Bcache, B[i]);
    rct::addKeys3(term, a[i], A[i], b[i], Bcache);
#endif
    rct::addKeys(res, res, term);
  }
  return res;
}

/* Given a scalar, construct a vector of powers */
static rct::keyV vector_powers(rct::key x, size_t n)
{
  rct::keyV res(n);
  if (n == 0)
    return res;
  res[0] = rct::identity();
  if (n == 1)
    return res;
  res[1] = x;
  for (size_t i = 2; i < n; ++i)
  {
    sc_mul(res[i].bytes, res[i-1].bytes, x.bytes);
  }
  return res;
}

/* Given two scalar arrays, construct the inner product */
static rct::key inner_product(const rct::keyV &a, const rct::keyV &b)
{
  CHECK_AND_ASSERT_THROW_MES(a.size() == b.size(), "Incompatible sizes of a and b");
  rct::key res = rct::zero();
  for (size_t i = 0; i < a.size(); ++i)
  {
    sc_muladd(res.bytes, a[i].bytes, b[i].bytes, res.bytes);
  }
  return res;
}

/* Given two scalar arrays, construct the Hadamard product */
static rct::keyV hadamard(const rct::keyV &a, const rct::keyV &b)
{
  CHECK_AND_ASSERT_THROW_MES(a.size() == b.size(), "Incompatible sizes of a and b");
  rct::keyV res(a.size());
  for (size_t i = 0; i < a.size(); ++i)
  {
    sc_mul(res[i].bytes, a[i].bytes, b[i].bytes);
  }
  return res;
}

/* Given two curvepoint arrays, construct the Hadamard product */
static rct::keyV hadamard2(const rct::keyV &a, const rct::keyV &b)
{
  CHECK_AND_ASSERT_THROW_MES(a.size() == b.size(), "Incompatible sizes of a and b");
  rct::keyV res(a.size());
  for (size_t i = 0; i < a.size(); ++i)
  {
    rct::addKeys(res[i], a[i], b[i]);
  }
  return res;
}

/* Add two vectors */
static rct::keyV vector_add(const rct::keyV &a, const rct::keyV &b)
{
  CHECK_AND_ASSERT_THROW_MES(a.size() == b.size(), "Incompatible sizes of a and b");
  rct::keyV res(a.size());
  for (size_t i = 0; i < a.size(); ++i)
  {
    sc_add(res[i].bytes, a[i].bytes, b[i].bytes);
  }
  return res;
}

/* Subtract two vectors */
static rct::keyV vector_subtract(const rct::keyV &a, const rct::keyV &b)
{
  CHECK_AND_ASSERT_THROW_MES(a.size() == b.size(), "Incompatible sizes of a and b");
  rct::keyV res(a.size());
  for (size_t i = 0; i < a.size(); ++i)
  {
    sc_sub(res[i].bytes, a[i].bytes, b[i].bytes);
  }
  return res;
}

/* Multiply a scalar and a vector */
static rct::keyV vector_scalar(const rct::keyV &a, const rct::key &x)
{
  rct::keyV res(a.size());
  for (size_t i = 0; i < a.size(); ++i)
  {
    sc_mul(res[i].bytes, a[i].bytes, x.bytes);
  }
  return res;
}

/* Exponentiate a curve vector by a scalar */
static rct::keyV vector_scalar2(const rct::keyV &a, const rct::key &x)
{
  rct::keyV res(a.size());
  for (size_t i = 0; i < a.size(); ++i)
  {
    rct::scalarmultKey(res[i], a[i], x);
  }
  return res;
}

static rct::key switch_endianness(rct::key k)
{
  std::reverse(k.bytes, k.bytes + sizeof(k));
  return k;
}

/* Compute the inverse of a scalar, the stupid way */
static rct::key invert(const rct::key &x)
{
  rct::key inv;

  BN_CTX *ctx = BN_CTX_new();
  BIGNUM *X = BN_new();
  BIGNUM *L = BN_new();
  BIGNUM *I = BN_new();

  BN_bin2bn(switch_endianness(x).bytes, sizeof(rct::key), X);
  BN_bin2bn(switch_endianness(rct::curveOrder()).bytes, sizeof(rct::key), L);

  CHECK_AND_ASSERT_THROW_MES(BN_mod_inverse(I, X, L, ctx), "Failed to invert");

  const int len = BN_num_bytes(I);
  CHECK_AND_ASSERT_THROW_MES((size_t)len <= sizeof(rct::key), "Invalid number length");
  inv = rct::zero();
  BN_bn2bin(I, inv.bytes);
  std::reverse(inv.bytes, inv.bytes + len);

  BN_free(I);
  BN_free(L);
  BN_free(X);
  BN_CTX_free(ctx);

#ifdef DEBUG_BP
  rct::key tmp;
  sc_mul(tmp.bytes, inv.bytes, x.bytes);
  CHECK_AND_ASSERT_THROW_MES(tmp == rct::identity(), "invert failed");
#endif
  return inv;
}

/* Compute the slice of a vector */
static rct::keyV slice(const rct::keyV &a, size_t start, size_t stop)
{
  CHECK_AND_ASSERT_THROW_MES(start < a.size(), "Invalid start index");
  CHECK_AND_ASSERT_THROW_MES(stop <= a.size(), "Invalid stop index");
  CHECK_AND_ASSERT_THROW_MES(start < stop, "Invalid start/stop indices");
  rct::keyV res(stop - start);
  for (size_t i = start; i < stop; ++i)
  {
    res[i - start] = a[i];
  }
  return res;
}

static rct::key hash_cache_mash(rct::key &hash_cache, const rct::key &mash0, const rct::key &mash1)
{
  rct::keyV data;
  data.reserve(3);
  data.push_back(hash_cache);
  data.push_back(mash0);
  data.push_back(mash1);
  return hash_cache = rct::hash_to_scalar(data);
}

static rct::key hash_cache_mash(rct::key &hash_cache, const rct::key &mash0, const rct::key &mash1, const rct::key &mash2)
{
  rct::keyV data;
  data.reserve(4);
  data.push_back(hash_cache);
  data.push_back(mash0);
  data.push_back(mash1);
  data.push_back(mash2);
  return hash_cache = rct::hash_to_scalar(data);
}

static rct::key hash_cache_mash(rct::key &hash_cache, const rct::key &mash0, const rct::key &mash1, const rct::key &mash2, const rct::key &mash3)
{
  rct::keyV data;
  data.reserve(5);
  data.push_back(hash_cache);
  data.push_back(mash0);
  data.push_back(mash1);
  data.push_back(mash2);
  data.push_back(mash3);
  return hash_cache = rct::hash_to_scalar(data);
}

/* Given a value v (0..2^N-1) and a mask gamma, construct a range proof */
Bulletproof bulletproof_PROVE(const rct::key &sv, const rct::key &gamma)
{
  CHECK_AND_ASSERT_THROW_MES(is_reduced(sv), "Invalid sv input");
  CHECK_AND_ASSERT_THROW_MES(is_reduced(gamma), "Invalid gamma input");

  init_exponents();

  PERF_TIMER_UNIT(PROVE, 1000000);

  constexpr size_t logN = 6; // log2(64)
  constexpr size_t N = 1<<logN;

  rct::key V;
  rct::keyV aL(N), aR(N);

  PERF_TIMER_START_BP(PROVE_v);
  rct::addKeys2(V, gamma, sv, rct::H);
  PERF_TIMER_STOP(PROVE_v);

  PERF_TIMER_START_BP(PROVE_aLaR);
  for (size_t i = N; i-- > 0; )
  {
    if (sv[i/8] & (((uint64_t)1)<<(i%8)))
    {
      aL[i] = rct::identity();
    }
    else
    {
      aL[i] = rct::zero();
    }
    sc_sub(aR[i].bytes, aL[i].bytes, rct::identity().bytes);
  }
  PERF_TIMER_STOP(PROVE_aLaR);

  rct::key hash_cache = rct::hash_to_scalar(V);

  // DEBUG: Test to ensure this recovers the value
#ifdef DEBUG_BP
  uint64_t test_aL = 0, test_aR = 0;
  for (size_t i = 0; i < N; ++i)
  {
    if (aL[i] == rct::identity())
      test_aL += ((uint64_t)1)<<i;
    if (aR[i] == rct::zero())
      test_aR += ((uint64_t)1)<<i;
  }
  uint64_t v_test = 0;
  for (int n = 0; n < 8; ++n) v_test |= (((uint64_t)sv[n]) << (8*n));
  CHECK_AND_ASSERT_THROW_MES(test_aL == v_test, "test_aL failed");
  CHECK_AND_ASSERT_THROW_MES(test_aR == v_test, "test_aR failed");
#endif

  PERF_TIMER_START_BP(PROVE_step1);
  // PAPER LINES 38-39
  rct::key alpha = rct::skGen();
  rct::key ve = vector_exponent(aL, aR);
  rct::key A;
  rct::addKeys(A, ve, rct::scalarmultBase(alpha));

  // PAPER LINES 40-42
  rct::keyV sL = rct::skvGen(N), sR = rct::skvGen(N);
  rct::key rho = rct::skGen();
  ve = vector_exponent(sL, sR);
  rct::key S;
  rct::addKeys(S, ve, rct::scalarmultBase(rho));

  // PAPER LINES 43-45
  rct::key y = hash_cache_mash(hash_cache, A, S);
  rct::key z = hash_cache = rct::hash_to_scalar(y);

  // Polynomial construction before PAPER LINE 46
  rct::key t0 = rct::zero();
  rct::key t1 = rct::zero();
  rct::key t2 = rct::zero();

  const auto yN = vector_powers(y, N);

  rct::key ip1y = inner_product(oneN, yN);
  rct::key tmp;
  sc_muladd(t0.bytes, z.bytes, ip1y.bytes, t0.bytes);

  rct::key zsq;
  sc_mul(zsq.bytes, z.bytes, z.bytes);
  sc_muladd(t0.bytes, zsq.bytes, sv.bytes, t0.bytes);

  rct::key k = rct::zero();
  sc_mulsub(k.bytes, zsq.bytes, ip1y.bytes, k.bytes);

  rct::key zcu;
  sc_mul(zcu.bytes, zsq.bytes, z.bytes);
  sc_mulsub(k.bytes, zcu.bytes, ip12.bytes, k.bytes);
  sc_add(t0.bytes, t0.bytes, k.bytes);

  // DEBUG: Test the value of t0 has the correct form
#ifdef DEBUG_BP
  rct::key test_t0 = rct::zero();
  rct::key iph = inner_product(aL, hadamard(aR, yN));
  sc_add(test_t0.bytes, test_t0.bytes, iph.bytes);
  rct::key ips = inner_product(vector_subtract(aL, aR), yN);
  sc_muladd(test_t0.bytes, z.bytes, ips.bytes, test_t0.bytes);
  rct::key ipt = inner_product(twoN, aL);
  sc_muladd(test_t0.bytes, zsq.bytes, ipt.bytes, test_t0.bytes);
  sc_add(test_t0.bytes, test_t0.bytes, k.bytes);
  CHECK_AND_ASSERT_THROW_MES(t0 == test_t0, "t0 check failed");
#endif
  PERF_TIMER_STOP(PROVE_step1);

  PERF_TIMER_START_BP(PROVE_step2);
  const auto HyNsR = hadamard(yN, sR);
  const auto vpIz = vector_scalar(oneN, z);
  const auto vp2zsq = vector_scalar(twoN, zsq);
  const auto aL_vpIz = vector_subtract(aL, vpIz);
  const auto aR_vpIz = vector_add(aR, vpIz);

  rct::key ip1 = inner_product(aL_vpIz, HyNsR);
  sc_add(t1.bytes, t1.bytes, ip1.bytes);

  rct::key ip2 = inner_product(sL, vector_add(hadamard(yN, aR_vpIz), vp2zsq));
  sc_add(t1.bytes, t1.bytes, ip2.bytes);

  rct::key ip3 = inner_product(sL, HyNsR);
  sc_add(t2.bytes, t2.bytes, ip3.bytes);

  // PAPER LINES 47-48
  rct::key tau1 = rct::skGen(), tau2 = rct::skGen();

  rct::key T1 = rct::addKeys(rct::scalarmultKey(rct::H, t1), rct::scalarmultBase(tau1));
  rct::key T2 = rct::addKeys(rct::scalarmultKey(rct::H, t2), rct::scalarmultBase(tau2));

  // PAPER LINES 49-51
  rct::key x = hash_cache_mash(hash_cache, z, T1, T2);

  // PAPER LINES 52-53
  rct::key taux = rct::zero();
  sc_mul(taux.bytes, tau1.bytes, x.bytes);
  rct::key xsq;
  sc_mul(xsq.bytes, x.bytes, x.bytes);
  sc_muladd(taux.bytes, tau2.bytes, xsq.bytes, taux.bytes);
  sc_muladd(taux.bytes, gamma.bytes, zsq.bytes, taux.bytes);
  rct::key mu;
  sc_muladd(mu.bytes, x.bytes, rho.bytes, alpha.bytes);

  // PAPER LINES 54-57
  rct::keyV l = vector_add(aL_vpIz, vector_scalar(sL, x));
  rct::keyV r = vector_add(hadamard(yN, vector_add(aR_vpIz, vector_scalar(sR, x))), vp2zsq);
  PERF_TIMER_STOP(PROVE_step2);

  PERF_TIMER_START_BP(PROVE_step3);
  rct::key t = inner_product(l, r);

  // DEBUG: Test if the l and r vectors match the polynomial forms
#ifdef DEBUG_BP
  rct::key test_t;
  sc_muladd(test_t.bytes, t1.bytes, x.bytes, t0.bytes);
  sc_muladd(test_t.bytes, t2.bytes, xsq.bytes, test_t.bytes);
  CHECK_AND_ASSERT_THROW_MES(test_t == t, "test_t check failed");
#endif

  // PAPER LINES 32-33
  rct::key x_ip = hash_cache_mash(hash_cache, x, taux, mu, t);

  // These are used in the inner product rounds
  size_t nprime = N;
  rct::keyV Gprime(N);
  rct::keyV Hprime(N);
  rct::keyV aprime(N);
  rct::keyV bprime(N);
  const rct::key yinv = invert(y);
  rct::key yinvpow = rct::identity();
  for (size_t i = 0; i < N; ++i)
  {
    Gprime[i] = Gi[i];
    Hprime[i] = scalarmultKey(Hi[i], yinvpow);
    sc_mul(yinvpow.bytes, yinvpow.bytes, yinv.bytes);
    aprime[i] = l[i];
    bprime[i] = r[i];
  }
  rct::keyV L(logN);
  rct::keyV R(logN);
  int round = 0;
  rct::keyV w(logN); // this is the challenge x in the inner product protocol
  PERF_TIMER_STOP(PROVE_step3);

  PERF_TIMER_START_BP(PROVE_step4);
  // PAPER LINE 13
  while (nprime > 1)
  {
    // PAPER LINE 15
    nprime /= 2;

    // PAPER LINES 16-17
    rct::key cL = inner_product(slice(aprime, 0, nprime), slice(bprime, nprime, bprime.size()));
    rct::key cR = inner_product(slice(aprime, nprime, aprime.size()), slice(bprime, 0, nprime));

    // PAPER LINES 18-19
    L[round] = vector_exponent_custom(slice(Gprime, nprime, Gprime.size()), slice(Hprime, 0, nprime), slice(aprime, 0, nprime), slice(bprime, nprime, bprime.size()));
    sc_mul(tmp.bytes, cL.bytes, x_ip.bytes);
    rct::addKeys(L[round], L[round], rct::scalarmultKey(rct::H, tmp));
    R[round] = vector_exponent_custom(slice(Gprime, 0, nprime), slice(Hprime, nprime, Hprime.size()), slice(aprime, nprime, aprime.size()), slice(bprime, 0, nprime));
    sc_mul(tmp.bytes, cR.bytes, x_ip.bytes);
    rct::addKeys(R[round], R[round], rct::scalarmultKey(rct::H, tmp));

    // PAPER LINES 21-22
    w[round] = hash_cache_mash(hash_cache, L[round], R[round]);

    // PAPER LINES 24-25
    const rct::key winv = invert(w[round]);
    Gprime = hadamard2(vector_scalar2(slice(Gprime, 0, nprime), winv), vector_scalar2(slice(Gprime, nprime, Gprime.size()), w[round]));
    Hprime = hadamard2(vector_scalar2(slice(Hprime, 0, nprime), w[round]), vector_scalar2(slice(Hprime, nprime, Hprime.size()), winv));

    // PAPER LINES 28-29
    aprime = vector_add(vector_scalar(slice(aprime, 0, nprime), w[round]), vector_scalar(slice(aprime, nprime, aprime.size()), winv));
    bprime = vector_add(vector_scalar(slice(bprime, 0, nprime), winv), vector_scalar(slice(bprime, nprime, bprime.size()), w[round]));

    ++round;
  }
  PERF_TIMER_STOP(PROVE_step4);

  // PAPER LINE 58 (with inclusions from PAPER LINE 8 and PAPER LINE 20)
  return Bulletproof(V, A, S, T1, T2, taux, mu, L, R, aprime[0], bprime[0], t);
}

Bulletproof bulletproof_PROVE(uint64_t v, const rct::key &gamma)
{
  // vG + gammaH
  PERF_TIMER_START_BP(PROVE_v);
  rct::key sv = rct::zero();
  sv.bytes[0] = v & 255;
  sv.bytes[1] = (v >> 8) & 255;
  sv.bytes[2] = (v >> 16) & 255;
  sv.bytes[3] = (v >> 24) & 255;
  sv.bytes[4] = (v >> 32) & 255;
  sv.bytes[5] = (v >> 40) & 255;
  sv.bytes[6] = (v >> 48) & 255;
  sv.bytes[7] = (v >> 56) & 255;
  PERF_TIMER_STOP(PROVE_v);
  return bulletproof_PROVE(sv, gamma);
}

/* Given a range proof, determine if it is valid */
bool bulletproof_VERIFY(const Bulletproof &proof)
{
  init_exponents();

  CHECK_AND_ASSERT_MES(proof.V.size() == 1, false, "V does not have exactly one element");
  CHECK_AND_ASSERT_MES(proof.L.size() == proof.R.size(), false, "Mismatched L and R sizes");
  CHECK_AND_ASSERT_MES(proof.L.size() > 0, false, "Empty proof");
  CHECK_AND_ASSERT_MES(proof.L.size() == 6, false, "Proof is not for 64 bits");

  for (const rct::key &k: proof.V)
    CHECK_AND_ASSERT_MES(rct::isInMainSubgroup(k), false, "Input point not in subgroup");
  for (const rct::key &k: proof.L)
    CHECK_AND_ASSERT_MES(rct::isInMainSubgroup(k), false, "Input point not in subgroup");
  for (const rct::key &k: proof.R)
    CHECK_AND_ASSERT_MES(rct::isInMainSubgroup(k), false, "Input point not in subgroup");

  CHECK_AND_ASSERT_MES(rct::isInMainSubgroup(proof.A), false, "Input point not in subgroup");
  CHECK_AND_ASSERT_MES(rct::isInMainSubgroup(proof.S), false, "Input point not in subgroup");
  CHECK_AND_ASSERT_MES(rct::isInMainSubgroup(proof.T1), false, "Input point not in subgroup");
  CHECK_AND_ASSERT_MES(rct::isInMainSubgroup(proof.T2), false, "Input point not in subgroup");

  // check scalar range
  CHECK_AND_ASSERT_MES(is_reduced(proof.taux), false, "Input scalar not in range");
  CHECK_AND_ASSERT_MES(is_reduced(proof.mu), false, "Input scalar not in range");
  CHECK_AND_ASSERT_MES(is_reduced(proof.a), false, "Input scalar not in range");
  CHECK_AND_ASSERT_MES(is_reduced(proof.b), false, "Input scalar not in range");
  CHECK_AND_ASSERT_MES(is_reduced(proof.t), false, "Input scalar not in range");

  const size_t logN = proof.L.size();
  const size_t N = 1 << logN;

  // Reconstruct the challenges
  PERF_TIMER_START_BP(VERIFY);
  PERF_TIMER_START_BP(VERIFY_start);
  rct::key hash_cache = rct::hash_to_scalar(proof.V[0]);
  rct::key y = hash_cache_mash(hash_cache, proof.A, proof.S);
  rct::key z = hash_cache = rct::hash_to_scalar(y);
  rct::key x = hash_cache_mash(hash_cache, z, proof.T1, proof.T2);
  PERF_TIMER_STOP(VERIFY_start);

  PERF_TIMER_START_BP(VERIFY_line_60);
  // Reconstruct the challenges
  rct::key x_ip = hash_cache_mash(hash_cache, x, proof.taux, proof.mu, proof.t);
  PERF_TIMER_STOP(VERIFY_line_60);

  PERF_TIMER_START_BP(VERIFY_line_61);
  // PAPER LINE 61
  rct::key L61Left = rct::addKeys(rct::scalarmultBase(proof.taux), rct::scalarmultKey(rct::H, proof.t));

  rct::key k = rct::zero();
  const auto yN = vector_powers(y, N);
  rct::key ip1y = inner_product(oneN, yN);
  rct::key zsq;
  sc_mul(zsq.bytes, z.bytes, z.bytes);
  rct::key tmp, tmp2;
  sc_mulsub(k.bytes, zsq.bytes, ip1y.bytes, k.bytes);
  rct::key zcu;
  sc_mul(zcu.bytes, zsq.bytes, z.bytes);
  sc_mulsub(k.bytes, zcu.bytes, ip12.bytes, k.bytes);
  PERF_TIMER_STOP(VERIFY_line_61);

  PERF_TIMER_START_BP(VERIFY_line_61rl);
  sc_muladd(tmp.bytes, z.bytes, ip1y.bytes, k.bytes);
  rct::key L61Right = rct::scalarmultKey(rct::H, tmp);

  CHECK_AND_ASSERT_MES(proof.V.size() == 1, false, "proof.V does not have exactly one element");
  tmp = rct::scalarmultKey(proof.V[0], zsq);
  rct::addKeys(L61Right, L61Right, tmp);

  tmp = rct::scalarmultKey(proof.T1, x);
  rct::addKeys(L61Right, L61Right, tmp);

  rct::key xsq;
  sc_mul(xsq.bytes, x.bytes, x.bytes);
  tmp = rct::scalarmultKey(proof.T2, xsq);
  rct::addKeys(L61Right, L61Right, tmp);
  PERF_TIMER_STOP(VERIFY_line_61rl);

  if (!(L61Right == L61Left))
  {
    MERROR("Verification failure at step 1");
    return false;
  }

  PERF_TIMER_START_BP(VERIFY_line_62);
  // PAPER LINE 62
  rct::key P = rct::addKeys(proof.A, rct::scalarmultKey(proof.S, x));
  PERF_TIMER_STOP(VERIFY_line_62);

  // Compute the number of rounds for the inner product
  const size_t rounds = proof.L.size();
  CHECK_AND_ASSERT_MES(rounds > 0, false, "Zero rounds");

  PERF_TIMER_START_BP(VERIFY_line_21_22);
  // PAPER LINES 21-22
  // The inner product challenges are computed per round
  rct::keyV w(rounds);
  for (size_t i = 0; i < rounds; ++i)
  {
    w[i] = hash_cache_mash(hash_cache, proof.L[i], proof.R[i]);
  }
  PERF_TIMER_STOP(VERIFY_line_21_22);

  PERF_TIMER_START_BP(VERIFY_line_24_25);
  // Basically PAPER LINES 24-25
  // Compute the curvepoints from G[i] and H[i]
  rct::key inner_prod = rct::identity();
  rct::key yinvpow = rct::identity();
  rct::key ypow = rct::identity();

  PERF_TIMER_START_BP(VERIFY_line_24_25_invert);
  const rct::key yinv = invert(y);
  rct::keyV winv(rounds);
  for (size_t i = 0; i < rounds; ++i)
    winv[i] = invert(w[i]);
  PERF_TIMER_STOP(VERIFY_line_24_25_invert);

  for (size_t i = 0; i < N; ++i)
  {
    // Convert the index to binary IN REVERSE and construct the scalar exponent
    rct::key g_scalar = proof.a;
    rct::key h_scalar;
    sc_mul(h_scalar.bytes, proof.b.bytes, yinvpow.bytes);

    for (size_t j = rounds; j-- > 0; )
    {
      size_t J = w.size() - j - 1;

      if ((i & (((size_t)1)<<j)) == 0)
      {
        sc_mul(g_scalar.bytes, g_scalar.bytes, winv[J].bytes);
        sc_mul(h_scalar.bytes, h_scalar.bytes, w[J].bytes);
      }
      else
      {
        sc_mul(g_scalar.bytes, g_scalar.bytes, w[J].bytes);
        sc_mul(h_scalar.bytes, h_scalar.bytes, winv[J].bytes);
      }
    }

    // Adjust the scalars using the exponents from PAPER LINE 62
    sc_add(g_scalar.bytes, g_scalar.bytes, z.bytes);
    sc_mul(tmp.bytes, zsq.bytes, twoN[i].bytes);
    sc_muladd(tmp.bytes, z.bytes, ypow.bytes, tmp.bytes);
    sc_mulsub(h_scalar.bytes, tmp.bytes, yinvpow.bytes, h_scalar.bytes);

    // Now compute the basepoint's scalar multiplication
    // Each of these could be written as a multiexp operation instead
    rct::addKeys3(tmp, g_scalar, Gprecomp[i], h_scalar, Hprecomp[i]);
    rct::addKeys(inner_prod, inner_prod, tmp);

    if (i != N-1)
    {
      sc_mul(yinvpow.bytes, yinvpow.bytes, yinv.bytes);
      sc_mul(ypow.bytes, ypow.bytes, y.bytes);
    }
  }
  PERF_TIMER_STOP(VERIFY_line_24_25);

  PERF_TIMER_START_BP(VERIFY_line_26);
  // PAPER LINE 26
  rct::key pprime;
  sc_sub(tmp.bytes, rct::zero().bytes, proof.mu.bytes);
  rct::addKeys(pprime, P, rct::scalarmultBase(tmp));

  for (size_t i = 0; i < rounds; ++i)
  {
    sc_mul(tmp.bytes, w[i].bytes, w[i].bytes);
    sc_mul(tmp2.bytes, winv[i].bytes, winv[i].bytes);
#if 1
    ge_dsmp cacheL, cacheR;
    rct::precomp(cacheL, proof.L[i]);
    rct::precomp(cacheR, proof.R[i]);
    rct::addKeys3(tmp, tmp, cacheL, tmp2, cacheR);
    rct::addKeys(pprime, pprime, tmp);
#else
    rct::addKeys(pprime, pprime, rct::scalarmultKey(proof.L[i], tmp));
    rct::addKeys(pprime, pprime, rct::scalarmultKey(proof.R[i], tmp2));
#endif
  }
  sc_mul(tmp.bytes, proof.t.bytes, x_ip.bytes);
  rct::addKeys(pprime, pprime, rct::scalarmultKey(rct::H, tmp));
  PERF_TIMER_STOP(VERIFY_line_26);

  PERF_TIMER_START_BP(VERIFY_step2_check);
  sc_mul(tmp.bytes, proof.a.bytes, proof.b.bytes);
  sc_mul(tmp.bytes, tmp.bytes, x_ip.bytes);
  tmp = rct::scalarmultKey(rct::H, tmp);
  rct::addKeys(tmp, tmp, inner_prod);
  PERF_TIMER_STOP(VERIFY_step2_check);
  if (!(pprime == tmp))
  {
    MERROR("Verification failure at step 2");
    return false;
  }

  PERF_TIMER_STOP(VERIFY);
  return true;
}

}


#define CATCH_CONFIG_MAIN

#include "CALPHADConcSolverBinary3Ph2Sl.h"
#include "CALPHADConcSolverBinaryThreePhase.h"
#include "CALPHADFreeEnergyFunctionsBinary.h"
#include "CALPHADFreeEnergyFunctionsBinary3Ph2Sl.h"
#include "CALPHADFreeEnergyFunctionsBinaryThreePhase.h"

#include "InterpolationType.h"
#include "PhysicalConstants.h"

#include "catch.hpp"

#include <boost/optional/optional.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>

#include <fstream>
#include <iostream>
#include <string>

namespace pt = boost::property_tree;

TEST_CASE("CALPHAD conc solver binary 3 phase, 2 sublattice KKS, "
          "single-sublattice consistency",
    "[conc solver binary 3 phase, 2 sublattice kks, single-sublattice "
    "consistency]")
{
    // Calculate the inputs and the reference solution
    Thermo4PFM::EnergyInterpolationType energy_interp_func_type
        = Thermo4PFM::EnergyInterpolationType::PBG;
    Thermo4PFM::ConcInterpolationType conc_interp_func_type
        = Thermo4PFM::ConcInterpolationType::PBG;

    double temperature = 900.;

    std::cout << " Read CALPHAD database..." << std::endl;
    pt::ptree calphad_db;
    try
    {
        pt::read_json(
            "../thermodynamic_data/calphadAlCuLFccBcc.json", calphad_db);
    }
    catch (std::exception& e)
    {
        std::cerr << "exception caught: " << e.what() << std::endl;
    }

    boost::optional<pt::ptree&> newton_db;

    // First calculate the reference solution

    Thermo4PFM::CALPHADFreeEnergyFunctionsBinaryThreePhase cafe(
        calphad_db, newton_db, energy_interp_func_type, conc_interp_func_type);

    // Get the CALPHAD parameters
    CalphadDataType fA[3];
    CalphadDataType fB[3];
    CalphadDataType Lmix_L[4];
    CalphadDataType Lmix_A[4];
    CalphadDataType Lmix_B[4];

    cafe.computeTdependentParameters(
        temperature, Lmix_L, Lmix_A, Lmix_B, fA, fB);

    // initial guesses
    double c_init0 = 0.9;
    double c_init1 = 0.9;
    double c_init2 = 0.9;

    // compute concentrations satisfying KKS equations
    const double conc = 0.9;

    // Inputs to the solver
    const double RT    = Thermo4PFM::gas_constant_R_JpKpmol * temperature;
    const double RTinv = 1.0 / RT;

    double hphi0 = interp_func(conc_interp_func_type, 0.5);
    double hphi1 = interp_func(conc_interp_func_type, 0.4);
    double hphi2 = interp_func(conc_interp_func_type, 0.1);

    const double tol    = 1.e-8;
    const double alpha  = 0.1; // Using alpha=1 can lead to convergence issues
    const int max_iters = 10000;

    // Create and set up the solver

    Thermo4PFM::CALPHADConcSolverBinaryThreePhase solver_ref;
    solver_ref.setup(
        conc, hphi0, hphi1, hphi2, RT, Lmix_L, Lmix_A, Lmix_B, fA, fB);

    // Run the solver
    double sol_ref[3] = { c_init0, c_init1, c_init2 };
    int ret = solver_ref.ComputeConcentration(sol_ref, tol, max_iters, alpha);
    REQUIRE(ret >= 0);

    // Now do the same calculation with the two-sublattice solver
    int p[3];
    int q[3];
    for (int i = 0; i < 3; ++i)
    {
        p[i] = 0;
        q[i] = 1;
    }
    Thermo4PFM::CALPHADConcSolverBinary3Ph2Sl solver;
    solver.setup(
        conc, hphi0, hphi1, hphi2, RTinv, Lmix_L, Lmix_A, Lmix_B, fA, fB, p, q);

    // Run the solver
    double sol_test[3] = { c_init0, c_init1, c_init2 };
    ret = solver.ComputeConcentration(sol_test, tol, max_iters, alpha);
    REQUIRE(ret >= 0);

    // Check consistency
    REQUIRE(sol_test[0] == Approx(sol_ref[0]).margin(1.e-5));
    REQUIRE(sol_test[1] == Approx(sol_ref[1]).margin(1.e-5));
    REQUIRE(sol_test[2] == Approx(sol_ref[2]).margin(1.e-5));
}

TEST_CASE("CALPHAD conc solver binary 3 phase, 2 sublattice KKS, "
          "convergence",
    "[conc solver binary 3 phase, 2 sublattice kks, convergence]")
{
    // Calculate the inputs and the reference solution
    Thermo4PFM::EnergyInterpolationType energy_interp_func_type
        = Thermo4PFM::EnergyInterpolationType::PBG;
    Thermo4PFM::ConcInterpolationType conc_interp_func_type
        = Thermo4PFM::ConcInterpolationType::PBG;

    double temperature = 820.;

    std::cout << " Read CALPHAD database..." << std::endl;
    pt::ptree calphad_db;
    try
    {
        pt::read_json(
            "../thermodynamic_data/calphadAlCuLFccTheta.json", calphad_db);
    }
    catch (std::exception& e)
    {
        std::cerr << "exception caught: " << e.what() << std::endl;
    }

    boost::optional<pt::ptree&> newton_db;

    // Test 1

    Thermo4PFM::CALPHADFreeEnergyFunctionsBinary3Ph2Sl cafe(
        calphad_db, newton_db, energy_interp_func_type, conc_interp_func_type);

    // Get the CALPHAD parameters
    CalphadDataType fA[3];
    CalphadDataType fB[3];
    CalphadDataType Lmix_L[4];
    CalphadDataType Lmix_A[4];
    CalphadDataType Lmix_B[4];

    cafe.computeTdependentParameters(
        temperature, Lmix_L, Lmix_A, Lmix_B, fA, fB);

    // initial guesses
    double c_init0 = 0.8;
    double c_init1 = 0.6;
    double c_init2 = 0.67;

    // compute concentrations satisfying KKS equations

    // I think the system at 820K is best behaved in the conc = 0.67-0.8 range
    double conc = 0.7;

    // Inputs to the solver
    const double RTinv
        = 1.0 / (Thermo4PFM::gas_constant_R_JpKpmol * temperature);

    // NOTE: The sum of hphi should equal one, which is not necessarily true for
    // phi
    double hphi0 = 0.1;
    double hphi1 = 0.4;
    double hphi2 = 0.5;

    double tol    = 1.e-8;
    double alpha  = 0.5; // Using alpha=1 can lead to convergence issues
    int max_iters = 100;

    int p[3];
    int q[3];
    for (int i = 0; i < 3; ++i)
    {
        p[i] = 0;
        q[i] = 1;
    }

    p[2] = 2;

    Thermo4PFM::CALPHADConcSolverBinary3Ph2Sl solver;
    solver.setup(
        conc, hphi0, hphi1, hphi2, RTinv, Lmix_L, Lmix_A, Lmix_B, fA, fB, p, q);

    // Run the solver
    double sol_test[3] = { c_init0, c_init1, c_init2 };
    int ret = solver.ComputeConcentration(sol_test, tol, max_iters, alpha);

    std::cout << "-------------------------------" << std::endl;
    std::cout << "Temperature = " << temperature << std::endl;
    std::cout << "Result for c = " << conc << std::endl;
    std::cout << "   cL = " << sol_test[0] << std::endl;
    std::cout << "   cA = " << sol_test[1] << std::endl;
    std::cout << "   cB = " << sol_test[2] << std::endl;

    REQUIRE(ret >= 0);

    // Test 2
    max_iters = 20000;
    alpha     = 1.0;

    sol_test[0] = 0.79267;
    sol_test[1] = 0.79267;
    sol_test[2] = 0.79267;

    hphi0 = 0.989276;
    hphi1 = 1.03871e-28;
    hphi2 = 0.0107243;

    conc = 0.79267;

    solver.setup(
        conc, hphi0, hphi1, hphi2, RTinv, Lmix_L, Lmix_A, Lmix_B, fA, fB, p, q);

    // Run the solver
    ret = solver.ComputeConcentration(sol_test, tol, max_iters, alpha);

    std::cout << "-------------------------------" << std::endl;
    std::cout << "Temperature = " << temperature << std::endl;
    std::cout << "Result for c = " << conc << std::endl;
    std::cout << "   cL = " << sol_test[0] << std::endl;
    std::cout << "   cA = " << sol_test[1] << std::endl;
    std::cout << "   cB = " << sol_test[2] << std::endl;
    std::cout << "Newton iterations = " << ret << std::endl;
    REQUIRE(ret >= 0);

    // Test 3

    max_iters = 2000;
    alpha     = 1.0;

    sol_test[0] = 0.7;
    sol_test[1] = 0.9;
    sol_test[2] = 0.794081;

    hphi0 = 0.982551;
    hphi1 = 0.0174491;
    hphi2 = 4.79499e-25;

    conc = 0.794081;

    solver.setup(
        conc, hphi0, hphi1, hphi2, RTinv, Lmix_L, Lmix_A, Lmix_B, fA, fB, p, q);

    // Run the solver
    ret = solver.ComputeConcentration(sol_test, tol, max_iters, alpha);

    std::cout << "-------------------------------" << std::endl;
    std::cout << "Temperature = " << temperature << std::endl;
    std::cout << "Result for c = " << conc << std::endl;
    std::cout << "   cL = " << sol_test[0] << std::endl;
    std::cout << "   cA = " << sol_test[1] << std::endl;
    std::cout << "   cB = " << sol_test[2] << std::endl;
    std::cout << "Newton iterations = " << ret << std::endl;
    REQUIRE(ret >= 0);
}


//==============================================================================
//         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 BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SCALAR_TWO_PROD_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SCALAR_TWO_PROD_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/two_prod.hpp>
#include <boost/dispatch/meta/adapted_traits.hpp>
#include <boost/fusion/tuple.hpp>
#include <boost/simd/include/functions/scalar/is_invalid.hpp>
#include <boost/simd/include/functions/scalar/two_split.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::two_prod_, tag::cpu_,
                          (A0),
                          ((scalar_<floating_<A0> >))
                          ((scalar_<floating_<A0> >))
                          ((scalar_<floating_<A0> >))
                          ((scalar_<floating_<A0> >))
                         )
  {
    typedef int result_type;
    inline result_type operator()(A0 const& a,A0 const& b,
                                  A0 & r0,A0 & r1) const
    {
      r0  = a*b;
      if (is_invalid(r0))
      {
        r1 = Zero<A0>();
      }
      else
      {
        A0 a1, a2, b1, b2;
        two_split(a, a1, a2);
        two_split(b, b1, b2);
        r1 = a2*b2 -(((r0-a1*b1)-a2*b1)-a1*b2);
      }
      return 0;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::two_prod_, tag::cpu_,
                          (A0),
                          ((scalar_<floating_<A0> >))
                          ((scalar_<floating_<A0> >))
                          ((scalar_<floating_<A0> >))
                         )
  {
    typedef A0 result_type;
    inline result_type operator()(A0 const& a0,A0 const& a1,A0 & a3) const
    {
      A0 a2;
      two_prod(a0, a1, a2, a3);
      return a2;
    }
  };
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::two_prod_, tag::cpu_,
                           (A0),
                           ((scalar_<floating_<A0> >))
                           ((scalar_<floating_<A0> >))
                          )
  {
    typedef typename boost::fusion::tuple<A0,A0> result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      result_type res;
      two_prod(a0,a1, boost::fusion::at_c<0>(res),boost::fusion::at_c<1>(res));
      return res;
    }
  };
} } }

#endif


/*
 * Copyright 2009-2020 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE populationanalysis_test

// Standard includes
#include <iostream>

// Third party includes
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

// VOTCA includes
#include <votca/tools/eigenio_matrixmarket.h>

// Local VOTCA includes
#include "votca/xtp/populationanalysis.h"

using namespace std;
using namespace votca::xtp;
using namespace votca;
BOOST_AUTO_TEST_SUITE(populationanalysis_test)

BOOST_AUTO_TEST_CASE(atompop) {

  Orbitals orb;
  orb.QMAtoms().LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) +
                             "/populationanalysis/molecule.xyz");
  orb.setDFTbasisName(std::string(XTP_TEST_DATA_FOLDER) +
                      "/populationanalysis/3-21G.xml");
  orb.setBasisSetSize(17);
  orb.setNumberOfOccupiedLevels(5);

  Eigen::MatrixXd& MOs = orb.MOs().eigenvectors();
  orb.MOs().eigenvalues() = Eigen::VectorXd::Ones(17);
  MOs = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/populationanalysis/MOs.mm");

  Orbitals orb2 = orb;
  QMState s("n");
  Lowdin low;
  StaticSegment result = low.CalcChargeperAtom(orb, s);

  Eigen::VectorXd charge = Eigen::VectorXd::Zero(result.size());
  for (votca::Index i = 0; i < votca::Index(result.size()); i++) {
    charge(i) = result[i].getCharge();
  }

  Eigen::VectorXd charge_ref = Eigen::VectorXd::Zero(5);
  charge_ref << 0.68862, -0.172155, -0.172154, -0.172154, -0.172155;

  bool check_lowdin = charge_ref.isApprox(charge, 1e-5);
  BOOST_CHECK_EQUAL(check_lowdin, true);
  if (!check_lowdin) {
    cout << "charge" << endl;
    cout << charge << endl;
    cout << "chargeref" << endl;
    cout << charge_ref << endl;
  }

  Mulliken mul;
  StaticSegment result2 = mul.CalcChargeperAtom(orb2, s);
  Eigen::VectorXd charge2 = Eigen::VectorXd::Zero(result2.size());
  for (votca::Index i = 0; i < votca::Index(result2.size()); i++) {
    charge2(i) = result2[i].getCharge();
  }

  Eigen::VectorXd charge_ref2 = Eigen::VectorXd::Zero(5);
  charge_ref2 << 1.21228, -0.303069, -0.303067, -0.303068, -0.303068;

  bool check_mulliken = charge_ref2.isApprox(charge2, 1e-5);
  BOOST_CHECK_EQUAL(check_mulliken, true);
  if (!check_mulliken) {
    cout << "charge" << endl;
    cout << charge2 << endl;
    cout << "chargeref" << endl;
    cout << charge_ref2 << endl;
  }
}

BOOST_AUTO_TEST_CASE(fragment_pop) {

  Orbitals orb;
  orb.QMAtoms().LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) +
                             "/populationanalysis/molecule.xyz");
  orb.setDFTbasisName(std::string(XTP_TEST_DATA_FOLDER) +
                      "/populationanalysis/3-21G.xml");
  orb.setBasisSetSize(17);
  orb.setNumberOfOccupiedLevels(5);
  orb.MOs().eigenvalues() = Eigen::VectorXd::Ones(17);

  Eigen::MatrixXd MOs2 = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/populationanalysis/MOs2.mm");

  orb.MOs().eigenvectors() = MOs2;
  Eigen::VectorXd se_ref = Eigen::VectorXd::Zero(3);
  se_ref << 0.107455, 0.107455, 0.107455;
  orb.BSESinglets().eigenvalues() = se_ref;

  // reference coefficients
  Eigen::MatrixXd spsi_ref = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/populationanalysis/spsi_ref.mm");

  orb.BSESinglets().eigenvectors() = spsi_ref;
  orb.setBSEindices(0, 16);
  orb.setTDAApprox(true);

  std::vector<QMFragment<BSE_Population> > frags;
  QMFragment<BSE_Population> f1(0, "0 1");
  QMFragment<BSE_Population> f2(1, "2 3 4");
  frags.push_back(f1);
  frags.push_back(f2);

  orb.DensityMatrixGroundState();
  QMState n("n");
  Lowdin low;
  BOOST_REQUIRE_THROW(low.CalcChargeperFragment(frags, orb, n.Type()),
                      std::runtime_error);
  QMState s1("s1");
  low.CalcChargeperFragment(frags, orb, s1.Type());

  BOOST_CHECK_CLOSE(frags[0].value().Gs, 0.5164649, 1e-5);
  BOOST_CHECK_CLOSE(frags[1].value().Gs, -0.5164628, 1e-5);

  Eigen::VectorXd f1E_ref = Eigen::VectorXd::Zero(3);
  f1E_ref << -0.384176, -0.812396, -0.414518;

  Eigen::VectorXd f1H_ref = Eigen::VectorXd::Zero(3);
  f1H_ref << 0.657215, 0.622434, 0.654751;

  Eigen::VectorXd f2E_ref = Eigen::VectorXd::Zero(3);
  f2E_ref << -0.615827, -0.187602, -0.58548;

  Eigen::VectorXd f2H_ref = Eigen::VectorXd::Zero(3);
  f2H_ref << 0.342785, 0.377565, 0.34525;

  bool check_f1e = frags[0].value().E.isApprox(f1E_ref, 1e-5);
  BOOST_CHECK_EQUAL(check_f1e, true);
  if (!check_f1e) {
    cout << "charge" << endl;
    cout << frags[0].value().E << endl;
    cout << "chargeref" << endl;
    cout << f1E_ref << endl;
  }

  bool check_f1h = frags[0].value().H.isApprox(f1H_ref, 1e-5);
  BOOST_CHECK_EQUAL(check_f1h, true);
  if (!check_f1h) {
    cout << "charge" << endl;
    cout << frags[0].value().H << endl;
    cout << "chargeref" << endl;
    cout << f1H_ref << endl;
  }
  bool check_f2e = frags[1].value().E.isApprox(f2E_ref, 1e-5);
  BOOST_CHECK_EQUAL(check_f2e, true);
  if (!check_f2e) {
    cout << "charge" << endl;
    cout << frags[1].value().E << endl;
    cout << "chargeref" << endl;
    cout << f2E_ref << endl;
  }
  bool check_f2h = frags[1].value().H.isApprox(f2H_ref, 1e-5);
  BOOST_CHECK_EQUAL(check_f2h, true);
  if (!check_f2h) {
    cout << "charge" << endl;
    cout << frags[1].value().H << endl;
    cout << "chargeref" << endl;
    cout << f2H_ref << endl;
  }
}

BOOST_AUTO_TEST_SUITE_END()