// Copyright (C) 2018  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include "opaque_types.h"
#include <dlib/python.h>
#include <dlib/matrix.h>
#include <dlib/geometry/vector.h>
#include <pybind11/stl_bind.h>
#include "indexing.h"

using namespace dlib;
using namespace std;

string line__repr__ (const line& p)
{
    std::ostringstream sout;
    sout << "line(" << p.p1() << ", " << p.p2() << ")";
    return sout.str();
}

string line__str__(const line& p)
{
    std::ostringstream sout;
    sout << "(" << p.p1() << ", " << p.p2() << ")";
    return sout.str();
}

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

void bind_line(py::module& m)
{

    const char* class_docs =
"This object represents a line in the 2D plane.  The line is defined by two points \n\
running through it, p1 and p2.  This object also includes a unit normal vector that \n\
is perpendicular to the line.";

    py::class_<line>(m, "line", class_docs)
        .def(py::init<>(), "p1, p2, and normal are all the 0 vector.")
        .def(py::init<dpoint,dpoint>(), py::arg("a"), py::arg("b"),
"ensures \n\
    - #p1 == a \n\
    - #p2 == b \n\
    - #normal == A vector normal to the line passing through points a and b. \n\
      Therefore, the normal vector is the vector (a-b) but unit normalized and rotated clockwise 90 degrees."
        /*!
            ensures
                - #p1 == a
                - #p2 == b
                - #normal == A vector normal to the line passing through points a and b.
                  Therefore, the normal vector is the vector (a-b) but unit normalized and rotated clockwise 90 degrees.
        !*/
            )
        .def(py::init<point,point>(), py::arg("a"), py::arg("b"),
"ensures \n\
    - #p1 == a \n\
    - #p2 == b \n\
    - #normal == A vector normal to the line passing through points a and b. \n\
      Therefore, the normal vector is the vector (a-b) but unit normalized and rotated clockwise 90 degrees."
        /*!
            ensures
                - #p1 == a
                - #p2 == b
                - #normal == A vector normal to the line passing through points a and b.
                  Therefore, the normal vector is the vector (a-b) but unit normalized and rotated clockwise 90 degrees.
        !*/
            )
        .def_property_readonly("normal", &line::normal, "returns a unit vector that is normal to the line passing through p1 and p2.")
        .def("__repr__", &line__repr__)
        .def("__str__", &line__str__)
        .def_property_readonly("p1", &line::p1, "returns the first endpoint of the line.")
        .def_property_readonly("p2", &line::p2, "returns the second endpoint of the line.");

    m.def("signed_distance_to_line", &signed_distance_to_line<long>, py::arg("l"), py::arg("p"));
    m.def("signed_distance_to_line", &signed_distance_to_line<double>, py::arg("l"), py::arg("p"),
"ensures \n\
    - returns how far p is from the line l.  This is a signed distance.  The sign \n\
      indicates which side of the line the point is on and the magnitude is the \n\
      distance. Moreover, the direction of positive sign is pointed to by the \n\
      vector l.normal. \n\
    - To be specific, this routine returns dot(p-l.p1, l.normal)"
    /*!
        ensures
            - returns how far p is from the line l.  This is a signed distance.  The sign
              indicates which side of the line the point is on and the magnitude is the
              distance. Moreover, the direction of positive sign is pointed to by the
              vector l.normal.
            - To be specific, this routine returns dot(p-l.p1, l.normal)
    !*/
        );

    m.def("distance_to_line", &distance_to_line<long>, py::arg("l"), py::arg("p"));
    m.def("distance_to_line", &distance_to_line<double>, py::arg("l"), py::arg("p"),
       "returns abs(signed_distance_to_line(l,p))" );

    m.def("reverse", [](const line& a){return reverse(a);}, py::arg("l"),
"ensures \n\
    - returns line(l.p2, l.p1) \n\
      (i.e. returns a line object that represents the same line as l but with the \n\
      endpoints, and therefore, the normal vector flipped.  This means that the \n\
      signed distance of operator() is also flipped)."
    /*!
        ensures
            - returns line(l.p2, l.p1)
              (i.e. returns a line object that represents the same line as l but with the
              endpoints, and therefore, the normal vector flipped.  This means that the
              signed distance of operator() is also flipped).
    !*/
        );

    m.def("intersect", [](const line& a, const line& b){return intersect(a,b);}, py::arg("a"), py::arg("b"),
"ensures \n\
    - returns the point of intersection between lines a and b.  If no such point \n\
      exists then this function returns a point with Inf values in it."
    /*!
        ensures
            - returns the point of intersection between lines a and b.  If no such point
              exists then this function returns a point with Inf values in it.
    !*/
        );

    m.def("angle_between_lines", [](const line& a, const line& b){return angle_between_lines(a,b);}, py::arg("a"), py::arg("b"),
"ensures \n\
    - returns the angle, in degrees, between the given lines.  This is a number in \n\
      the range [0 90]."
    /*!
        ensures
            - returns the angle, in degrees, between the given lines.  This is a number in
              the range [0 90].
    !*/
        );

    m.def("count_points_on_side_of_line", &count_points_on_side_of_line<long>, py::arg("l"), py::arg("reference_point"), py::arg("pts"), py::arg("dist_thresh"));
    m.def("count_points_on_side_of_line", &count_points_on_side_of_line<double>, py::arg("l"), py::arg("reference_point"), py::arg("pts"), py::arg("dist_thresh"),
"ensures \n\
    - Returns a count of how many points in pts are on the same side of l as \n\
      reference_point, but also no more than dist_thresh distance from the line."
    /*!
        ensures
            - Returns a count of how many points in pts are on the same side of l as
              reference_point, but also no more than dist_thresh distance from the line.
    !*/
        );

    m.def("count_points_between_lines", &count_points_between_lines<long>, py::arg("l1"), py::arg("l2"), py::arg("reference_point"), py::arg("pts"));
    m.def("count_points_between_lines", &count_points_between_lines<double>, py::arg("l1"), py::arg("l2"), py::arg("reference_point"), py::arg("pts"),
"ensures \n\
    - Counts and returns the number of points in pts that are between lines l1 and \n\
      l2.  Since a pair of lines will, in the general case, divide the plane into 4 \n\
      regions, we identify the region of interest as the one that contains the \n\
      reference_point.  Therefore, this function counts the number of points in pts \n\
      that appear in the same region as reference_point."
    /*!
        ensures
            - Counts and returns the number of points in pts that are between lines l1 and
              l2.  Since a pair of lines will, in the general case, divide the plane into 4
              regions, we identify the region of interest as the one that contains the
              reference_point.  Therefore, this function counts the number of points in pts
              that appear in the same region as reference_point.
    !*/
        );

}


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

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/51-100/52/problem52.hpp"

BOOST_AUTO_TEST_SUITE( Problem52 )

    BOOST_AUTO_TEST_CASE( Example ) {
        auto res = problems::problem52::solve(2);
        BOOST_CHECK_EQUAL(res, 125874);
    }

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem52::solve();
        BOOST_CHECK_EQUAL(res, 142857);
    }

BOOST_AUTO_TEST_SUITE_END()


#include <igl/decimate.h>
#include <Eigen/Core>
#include <iostream>
#include <set>

#ifdef MEX
#  include <mex.h>
#  include <igl/C_STR.h>
#  include <igl/matlab/mexErrMsgTxt.h>
#  undef assert
#  define assert( isOK ) ( (isOK) ? (void)0 : (void) mexErrMsgTxt(C_STR(__FILE__<<":"<<__LINE__<<": failed assertion `"<<#isOK<<"'"<<std::endl) ) )
#endif

#include <igl/matlab/MexStream.h>
#include <igl/matlab/parse_rhs.h>
#include <igl/matlab/prepare_lhs.h>

void mexFunction(
         int          nlhs,
         mxArray      *plhs[],
         int          nrhs,
         const mxArray *prhs[]
         )
{
  using namespace std;
  using namespace igl;
  using namespace igl::matlab;
  using namespace Eigen;
  MatrixXd V,W;
  MatrixXi F,G;
  VectorXi J;

  igl::matlab::MexStream mout;
  std::streambuf *outbuf = std::cout.rdbuf(&mout);

  mexErrMsgTxt(nrhs>=3,"nrhs should be >= 3");
  parse_rhs_double(prhs,V);
  parse_rhs_index(prhs+1,F);
  mexErrMsgTxt(V.cols()==3,"V must be #V by 3");
  mexErrMsgTxt(F.cols()==3,"F must be #F by 3");
  mexErrMsgTxt(
    mxIsDouble(prhs[2]) && mxGetM(prhs[2])==1 && mxGetN(prhs[2])==1,
    "fraction to decimate should be scalar");
  double ratio = * mxGetPr(prhs[2]);
  mexErrMsgTxt((ratio>0 && ratio<1) || (ratio>0 && ratio<F.rows()) ,"Ratio should be in (0,1) or [1,#F)");
  const size_t max_m = ratio<1 ? ratio*F.rows() : ratio;
  decimate(V,F,max_m,W,G,J);

  switch(nlhs)
  {
    case 3:
      prepare_lhs_index(J,plhs+2);
    case 2:
      prepare_lhs_index(G,plhs+1);
    case 1:
      prepare_lhs_double(W,plhs+0);
    default:break;
  }

  // Restore the std stream buffer Important!
  std::cout.rdbuf(outbuf);
  return;
}


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/COPYING
//
// 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)
//
/// \file normalize_sorts_test.cpp
/// \brief Test for normalizing sorts.

#include "mcrl2/data/data_specification.h"
#include "mcrl2/data/normalize_sorts.h"
#include "mcrl2/data/parse.h"
#include <algorithm>
#include <boost/test/minimal.hpp>
#include <iterator>
#include <set>
#include <vector>

using namespace mcrl2;
using namespace mcrl2::data;

void test_normalize_sorts()
{
  std::string DATASPEC =
    "sort Bit = struct e0 | e1;      \n"
    "     AbsBit = struct arbitrary; \n"
    "                                \n"
    "map  inv: Bit -> Bit;           \n"
    "     h: Bit -> AbsBit;          \n"
    "                                \n"
    "eqn  inv(e0)  =  e1;            \n"
    "     inv(e1)  =  e0;            \n"
    ;

  data_specification dataspec = parse_data_specification(DATASPEC);

  data::function_symbol f;
  f = parse_function_symbol("abseq : AbsBit # AbsBit -> Set(Bool)", DATASPEC);
  dataspec.add_mapping(f);
  f = parse_function_symbol("absinv : AbsBit -> Set(AbsBit)", DATASPEC);
  dataspec.add_mapping(f);

  data_equation_vector equations = dataspec.user_defined_equations();
  data::normalize_sorts(equations, dataspec);
}

int test_main(int argc, char* argv[])
{
  test_normalize_sorts();

  return 0;
}


#include <moja/floatcmp.h>

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(FloatCmpTests);

BOOST_AUTO_TEST_CASE(core_floatcmp_EqualTo_Exact) {
   double a = 10.32;
   double b = a;

   BOOST_CHECK(moja::FloatCmp::equalTo(a, b));
}

BOOST_AUTO_TEST_CASE(core_floatcmp_EqualTo_Tolerance) {
   const double Epsilon = 0.000000000000001;

   double a = 6.572;
   double b = a + Epsilon;

   BOOST_CHECK_NE(a, b);
   BOOST_CHECK(moja::FloatCmp::equalTo(a, b));
}

BOOST_AUTO_TEST_CASE(core_floatcmp_EqualTo_NotEqual) {
   double a = 3.1243;
   double b = 5.123;

   BOOST_CHECK(!moja::FloatCmp::equalTo(a, b));
}

BOOST_AUTO_TEST_CASE(core_floatcmp_GreaterThan_True) {
   double a = 5;
   double b = 1;

   BOOST_CHECK(moja::FloatCmp::greaterThan(a, b));
}

BOOST_AUTO_TEST_CASE(core_floatcmp_GreaterThan_False) {
   double a = 1;
   double b = 10;

   BOOST_CHECK(!moja::FloatCmp::greaterThan(a, b));
}

BOOST_AUTO_TEST_CASE(core_floatcmp_GreaterThan_NotEqualTo) {
   double a = 1;
   double b = a;

   BOOST_CHECK(!moja::FloatCmp::greaterThan(a, b));
}

BOOST_AUTO_TEST_CASE(core_floatcmp_GreaterThanOrEqualTo_True) {
   double a = 5;
   double b = 1;

   BOOST_CHECK(moja::FloatCmp::greaterThanOrEqualTo(a, b));
}

BOOST_AUTO_TEST_CASE(core_floatcmp_GreaterThanOrEqualTo_False) {
   double a = 1;
   double b = 10;

   BOOST_CHECK(!moja::FloatCmp::greaterThanOrEqualTo(a, b));
}

BOOST_AUTO_TEST_CASE(core_floatcmp_GreaterThanOrEqualTo_EqualTo_Exact) {
   double a = 1;
   double b = a;

   BOOST_CHECK(moja::FloatCmp::greaterThanOrEqualTo(a, b));
}

BOOST_AUTO_TEST_CASE(core_floatcmp_GreaterThanOrEqualTo_EqualTo_Tolerance) {
   const double Epsilon = 0.000000000000001;

   double a = 5.123;
   double b = a + Epsilon;

   BOOST_CHECK(moja::FloatCmp::greaterThanOrEqualTo(a, b));
}

BOOST_AUTO_TEST_CASE(core_floatcmp_LessThan_True) {
   double a = 1;
   double b = 5;

   BOOST_CHECK(moja::FloatCmp::lessThan(a, b));
}

BOOST_AUTO_TEST_CASE(core_floatcmp_LessThan_False) {
   double a = 10;
   double b = 1;

   BOOST_CHECK(!moja::FloatCmp::lessThan(a, b));
}

BOOST_AUTO_TEST_CASE(core_floatcmp_LessThan_NotEqualTo) {
   double a = 1;
   double b = a;

   BOOST_CHECK(!moja::FloatCmp::lessThan(a, b));
}

BOOST_AUTO_TEST_CASE(core_floatcmp_LessThanOrEqualTo_True) {
   double a = 1;
   double b = 5;

   BOOST_CHECK(moja::FloatCmp::lessThanOrEqualTo(a, b));
}

BOOST_AUTO_TEST_CASE(core_floatcmp_LessThanOrEqualTo_False) {
   double a = 10;
   double b = 1;

   BOOST_CHECK(!moja::FloatCmp::lessThanOrEqualTo(a, b));
}

BOOST_AUTO_TEST_CASE(core_floatcmp_LessThanOrEqualTo_EqualTo_Exact) {
   double a = 1;
   double b = a;

   BOOST_CHECK(moja::FloatCmp::lessThanOrEqualTo(a, b));
}

BOOST_AUTO_TEST_CASE(core_floatcmp_LessThanOrEqualTo_EqualTo_Tolerance) {
   const double Epsilon = 0.000000000000001;

   double a = 5.123;
   double b = a - Epsilon;

   BOOST_CHECK(moja::FloatCmp::lessThanOrEqualTo(a, b));
}

BOOST_AUTO_TEST_SUITE_END();


/*=============================================================================
    Copyright (c) 2001-2003 Joel de Guzman
    http://spirit.sourceforge.net/

    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)
=============================================================================*/
///////////////////////////////////////////////////////////////////////////////
//
//  A complex number micro parser (using subrules)
//
//  [ JDG 5/10/2002 ]
//
///////////////////////////////////////////////////////////////////////////////
#include <boost/spirit/include/classic_core.hpp>
#include <iostream>
#include <complex>
#include <string>

///////////////////////////////////////////////////////////////////////////////
using namespace std;
using namespace BOOST_SPIRIT_CLASSIC_NS;

///////////////////////////////////////////////////////////////////////////////
//
//  Our complex number micro parser
//
///////////////////////////////////////////////////////////////////////////////
bool
parse_complex(char const* str, complex<double>& c)
{
    double rN = 0.0;
    double iN = 0.0;

    subrule<0> first;
    subrule<1> r;
    subrule<2> i;

    if (parse(str,

        //  Begin grammar
        (
            first = '(' >> r >> !(',' >> i) >> ')' | r,
            r = real_p[assign(rN)],
            i = real_p[assign(iN)]
        )
        ,
        //  End grammar

        space_p).full)
    {
        c = complex<double>(rN, iN);
        return true;
    }
    else
    {
        return false;
    }
}

////////////////////////////////////////////////////////////////////////////
//
//  Main program
//
////////////////////////////////////////////////////////////////////////////
int
main()
{
    cout << "/////////////////////////////////////////////////////////\n\n";
    cout << "\t\tA complex number micro parser for Spirit...\n\n";
    cout << "/////////////////////////////////////////////////////////\n\n";

    cout << "Give me a complex number of the form r or (r) or (r,i) \n";
    cout << "Type [q or Q] to quit\n\n";

    string str;
    while (getline(cin, str))
    {
        if (str.empty() || str[0] == 'q' || str[0] == 'Q')
            break;

        complex<double> c;
        if (parse_complex(str.c_str(), c))
        {
            cout << "-------------------------\n";
            cout << "Parsing succeeded\n";
            cout << str << " Parses OK: " << c << endl;
            cout << "-------------------------\n";
        }
        else
        {
            cout << "-------------------------\n";
            cout << "Parsing failed\n";
            cout << "-------------------------\n";
        }
    }

    cout << "Bye... :-) \n\n";
    return 0;
}


/*
 *                   GridGain Community Edition Licensing
 *                   Copyright 2019 GridGain Systems, Inc.
 *
 * Licensed under the Apache License, Version 2.0 (the "License") modified with Commons Clause
 * Restriction; you may not use this file except in compliance with the License. You may obtain a
 * copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software distributed under the
 * License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
 * KIND, either express or implied. See the License for the specific language governing permissions
 * and limitations under the License.
 *
 * Commons Clause Restriction
 *
 * The Software is provided to you by the Licensor under the License, as defined below, subject to
 * the following condition.
 *
 * Without limiting other conditions in the License, the grant of rights under the License will not
 * include, and the License does not grant to you, the right to Sell the Software.
 * For purposes of the foregoing, “Sell” means practicing any or all of the rights granted to you
 * under the License to provide to third parties, for a fee or other consideration (including without
 * limitation fees for hosting or consulting/ support services related to the Software), a product or
 * service whose value derives, entirely or substantially, from the functionality of the Software.
 * Any license notice or attribution required by the License must also include this Commons Clause
 * License Condition notice.
 *
 * For purposes of the clause above, the “Licensor” is Copyright 2019 GridGain Systems, Inc.,
 * the “License” is the Apache License, Version 2.0, and the Software is the GridGain Community
 * Edition software provided with this notice.
 */

#ifndef _MSC_VER
#   define BOOST_TEST_DYN_LINK
#endif

#include <boost/test/unit_test.hpp>

#include <ignite/common/bits.h>

using namespace ignite;
using namespace ignite::common::bits;

BOOST_AUTO_TEST_SUITE(BitsTestSuite)

BOOST_AUTO_TEST_CASE(TestNumberOfLeadingZeroes)
{
    BOOST_CHECK_EQUAL(32, NumberOfLeadingZerosI32(0));

    BOOST_CHECK_EQUAL(31, NumberOfLeadingZerosI32(1));

    BOOST_CHECK_EQUAL(30, NumberOfLeadingZerosI32(2));
    BOOST_CHECK_EQUAL(30, NumberOfLeadingZerosI32(3));

    BOOST_CHECK_EQUAL(29, NumberOfLeadingZerosI32(4));
    BOOST_CHECK_EQUAL(29, NumberOfLeadingZerosI32(5));
    BOOST_CHECK_EQUAL(29, NumberOfLeadingZerosI32(7));

    BOOST_CHECK_EQUAL(28, NumberOfLeadingZerosI32(8));
    BOOST_CHECK_EQUAL(28, NumberOfLeadingZerosI32(12));
    BOOST_CHECK_EQUAL(28, NumberOfLeadingZerosI32(15));

    BOOST_CHECK_EQUAL(0, NumberOfLeadingZerosI32(0xFFFFFFFF));
    BOOST_CHECK_EQUAL(0, NumberOfLeadingZerosI32(0x80000000));

    BOOST_CHECK_EQUAL(1, NumberOfLeadingZerosI32(0x7FFFFFFF));
    BOOST_CHECK_EQUAL(1, NumberOfLeadingZerosI32(0x40000000));

    BOOST_CHECK_EQUAL(8, NumberOfLeadingZerosI32(0x00FFFFFF));
    BOOST_CHECK_EQUAL(8, NumberOfLeadingZerosI32(0x00F00000));
    BOOST_CHECK_EQUAL(8, NumberOfLeadingZerosI32(0x00800000));

    BOOST_CHECK_EQUAL(9, NumberOfLeadingZerosI32(0x00700000));
    BOOST_CHECK_EQUAL(9, NumberOfLeadingZerosI32(0x00400000));
    BOOST_CHECK_EQUAL(9, NumberOfLeadingZerosI32(0x006C0395));
}

BOOST_AUTO_TEST_CASE(TestNumberOfTrailingZeroes)
{
    BOOST_CHECK_EQUAL(0, NumberOfTrailingZerosI32(1));
    BOOST_CHECK_EQUAL(0, NumberOfTrailingZerosI32(3));
    BOOST_CHECK_EQUAL(0, NumberOfTrailingZerosI32(5));
    BOOST_CHECK_EQUAL(0, NumberOfTrailingZerosI32(7));
    BOOST_CHECK_EQUAL(0, NumberOfTrailingZerosI32(15));
    BOOST_CHECK_EQUAL(0, NumberOfTrailingZerosI32(0xFFFFFFFF));
    BOOST_CHECK_EQUAL(0, NumberOfTrailingZerosI32(0x7FFFFFFF));
    BOOST_CHECK_EQUAL(0, NumberOfTrailingZerosI32(0x00FFFFFF));
    BOOST_CHECK_EQUAL(0, NumberOfTrailingZerosI32(0x006C0395));

    BOOST_CHECK_EQUAL(1, NumberOfTrailingZerosI32(2));

    BOOST_CHECK_EQUAL(2, NumberOfTrailingZerosI32(4));
    BOOST_CHECK_EQUAL(2, NumberOfTrailingZerosI32(12));

    BOOST_CHECK_EQUAL(3, NumberOfTrailingZerosI32(8));

    BOOST_CHECK_EQUAL(20, NumberOfTrailingZerosI32(0xFFF00000));
    BOOST_CHECK_EQUAL(20, NumberOfTrailingZerosI32(0x00F00000));
    BOOST_CHECK_EQUAL(20, NumberOfTrailingZerosI32(0x00700000));
    BOOST_CHECK_EQUAL(20, NumberOfTrailingZerosI32(0x80700000));

    BOOST_CHECK_EQUAL(22, NumberOfTrailingZerosI32(0x00400000));
    BOOST_CHECK_EQUAL(22, NumberOfTrailingZerosI32(0x80400000));
    BOOST_CHECK_EQUAL(22, NumberOfTrailingZerosI32(0x10400000));

    BOOST_CHECK_EQUAL(23, NumberOfTrailingZerosI32(0x00800000));
    BOOST_CHECK_EQUAL(23, NumberOfTrailingZerosI32(0x80800000));
    BOOST_CHECK_EQUAL(23, NumberOfTrailingZerosI32(0xFF800000));

    BOOST_CHECK_EQUAL(30, NumberOfTrailingZerosI32(0x40000000));
    BOOST_CHECK_EQUAL(30, NumberOfTrailingZerosI32(0xC0000000));

    BOOST_CHECK_EQUAL(31, NumberOfTrailingZerosI32(0x80000000));

    BOOST_CHECK_EQUAL(32, NumberOfTrailingZerosI32(0));
}

BOOST_AUTO_TEST_CASE(TestBitCount)
{
    BOOST_CHECK_EQUAL(0, BitCountI32(0));

    for (int j = 0; j < 32; ++j)
    {
        const int32_t testNum = 0xFFFFFFFFUL >> (31 - j);

        for (int i = 0; i < (32 - j); ++i)
            BOOST_CHECK_EQUAL(j + 1, BitCountI32(testNum));
    }

    for (int j = 0; j < 32; j += 2)
    {
        const int32_t testNum = 0xAAAAAAAAUL >> (31 - j);

        for (int i = 0; i < (32 - j); ++i)
            BOOST_CHECK_EQUAL((j / 2) + 1, BitCountI32(testNum));
    }
}

BOOST_AUTO_TEST_SUITE_END()


//          Copyright Maksym V. Bilinets 2015 - 2019.
// 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 "tools/indexing_tree_invariant.h"
#include "tools/trees_generator.h"

#include <dst/allocator/global_counter_allocator.h>
#include <dst/binary_tree/mixin/indexing.h>
#include <dst/binary_tree/tree.h>

#include <boost/test/unit_test.hpp>

#include <cstddef> // std::size_t
#include <iostream>
#include <iterator> // std::advance
#include <numeric>  // std::iota

namespace dst_test
{

using indexed_tree =
  dst::binary_tree::tree<int, std::allocator<int>, dst::binary_tree::Indexing>;

BOOST_AUTO_TEST_SUITE(test_binary_tree_indexing)

BOOST_AUTO_TEST_CASE(test_indexed_tree_rotations)
{
  const auto tree_height = 10;

  indexed_tree t = generate_fibonacci_tree(tree_height);

  BOOST_TEST((std::distance(t.cbegin().base(), t.croot()) + 1) == tree_height);

  std::iota(t.begin(), t.end(), 0);

  BOOST_TEST(indexing_invariant_holds(t));

  auto it = t.croot();
  while (!!it)
  {
    it = t.rotate_right(it);

    while (!!it && !left(it))
    {
      it = right(it);
    }

    BOOST_TEST(indexing_invariant_holds(t));

    for (indexed_tree::size_type idx = 0; idx < t.size(); ++idx)
    {
      auto it = t.element_at(idx);
      BOOST_CHECK_EQUAL(*it, idx);
      BOOST_CHECK_EQUAL(t.index(it), idx);
      BOOST_CHECK_EQUAL(t.at(idx), idx);
      BOOST_CHECK_EQUAL(t[idx], idx);
    }
  }

  for (auto it = t.croot(); !!it && !!right(it); it = t.rotate_left(it))
  {
    BOOST_TEST(indexing_invariant_holds(t));

    for (indexed_tree::size_type idx = 0; idx < t.size(); ++idx)
    {
      auto it = t.element_at(idx);
      BOOST_CHECK_EQUAL(*it, idx);
      BOOST_CHECK_EQUAL(t.index(it), idx);
      BOOST_CHECK_EQUAL(t.at(idx), idx);
      BOOST_CHECK_EQUAL(t[idx], idx);
    }
  }
}

BOOST_AUTO_TEST_SUITE_END()
}


#include "CZZX.h"

#include <NTL/ZZ.h>
#include <sstream>

CZZX CZZX::operator+(const CZZX& o) {
	ZZX resr = rx + o.rx;
	ZZX resi = ix + o.ix;
	CZZX res(resr, resi);
	return res;
}

void CZZX::operator+=(const CZZX& o) {
	rx += o.rx;
	ix += o.ix;
}

CZZX CZZX::operator-(const CZZX& o) {
	ZZX resr = rx - o.rx;
	ZZX resi = ix - o.ix;
	CZZX res(resr, resi);
	return res;
}

void CZZX::operator-=(const CZZX& o) {
	rx -= o.rx;
	ix -= o.ix;
}

CZZX CZZX::operator -() {
	return CZZX(-rx, -ix);
}

CZZX CZZX::operator *(const CZZX& o) {
	ZZX tmpProd = (rx + ix) * o.rx;

	ZZX resr = tmpProd - (o.rx + o.ix) * ix;
	ZZX resi = tmpProd + (o.ix - o.rx) * rx;
	CZZX res(resr, resi);
	return res;
}

void CZZX::operator*=(const CZZX& o) {
	ZZX tmpProd = (rx + ix) * o.rx;

	ZZX tr = tmpProd - (o.rx + o.ix) * ix;
	ix = tmpProd + (o.ix - o.rx) * rx;
	rx = tr;
}

CZZX CZZX::operator >>(const long& s) {
	ZZX resr = rx >> s;
	ZZX resi = ix >> s;
	return CZZX(resr, resi);
}

void CZZX::operator >>=(const long& s) {
	rx >>= s;
	ix >>= s;
}

CZZX CZZX::operator <<(const long& s) {
	ZZX resr = rx << s;
	ZZX resi = ix << s;
	return CZZX(resr, resi);
}

void CZZX::operator <<=(const long& s) {
	rx <<= s;
	ix <<= s;
}

CZZX CZZX::sqr() {
	ZZX resr = (rx + ix) * (rx - ix);
	ZZX resi = (rx << 1) * ix;
	CZZX res(resr, resi);
	return res;
}

void CZZX::sqrThis() {
	ZZX tr = (rx + ix) * (rx - ix);
	ix *= (rx << 1);
	rx = tr;
}

void CZZX::SetMaxLength(long d) {
	rx.SetMaxLength(d);
	ix.SetMaxLength(d);
}

void CZZX::SetLength(long d) {
	rx.SetLength(d);
	ix.SetLength(d);
}

void CZZX::normalize() {
	rx.normalize();
	ix.normalize();
}

CZZ coeff(CZZX& cx, long s) {
	ZZ r = NTL::coeff(cx.rx, s);
	ZZ i = NTL::coeff(cx.ix, s);
	CZZ res(r, i);
	return res;
}

void SetCoeff(CZZX& cx, long s, CZZ& c) {
	NTL::SetCoeff(cx.rx, s, c.r);
	NTL::SetCoeff(cx.ix, s, c.i);
}

void GetCoeff(CZZ& c, CZZX& cx, long s) {
	GetCoeff(c.r, cx.rx, s);
	GetCoeff(c.i, cx.ix, s);
}

long deg(CZZX& cx) {
	return deg(cx.rx);
}

string CZZX::toString() {
	stringstream ss;
	ss << " [rx = " << rx << "\n";
	ss << " ,ix = " << ix;
	ss << " ] ";
	return ss.str();
}


/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

// includes
// std
#include <iostream>

// boost
#define BOOST_TEST_MODULE Jacobian
#include <boost/math/constants/constants.hpp>
#include <boost/test/unit_test.hpp>

// SpaceVecAlg
#include <SpaceVecAlg/SpaceVecAlg>

// RBDyn
#include "RBDyn/Body.h"
#include "RBDyn/EulerIntegration.h"
#include "RBDyn/FA.h"
#include "RBDyn/FK.h"
#include "RBDyn/FV.h"
#include "RBDyn/Jacobian.h"
#include "RBDyn/Joint.h"
#include "RBDyn/MultiBody.h"
#include "RBDyn/MultiBodyConfig.h"
#include "RBDyn/MultiBodyGraph.h"

// Arm
#include "SSSarm.h"
#include "XYZSarm.h"

const double TOL = 0.0000001;

void checkMultiBodyEq(const rbd::MultiBody & mb,
                      std::vector<rbd::Body> bodies,
                      std::vector<rbd::Joint> joints,
                      std::vector<int> pred,
                      std::vector<int> succ,
                      std::vector<int> parent,
                      std::vector<sva::PTransformd> Xt)
{
  // bodies
  BOOST_CHECK_EQUAL_COLLECTIONS(mb.bodies().begin(), mb.bodies().end(), bodies.begin(), bodies.end());
  // joints
  BOOST_CHECK_EQUAL_COLLECTIONS(mb.joints().begin(), mb.joints().end(), joints.begin(), joints.end());
  // pred
  BOOST_CHECK_EQUAL_COLLECTIONS(mb.predecessors().begin(), mb.predecessors().end(), pred.begin(), pred.end());
  // succ
  BOOST_CHECK_EQUAL_COLLECTIONS(mb.successors().begin(), mb.successors().end(), succ.begin(), succ.end());
  // parent
  BOOST_CHECK_EQUAL_COLLECTIONS(mb.parents().begin(), mb.parents().end(), parent.begin(), parent.end());

  // Xt
  BOOST_CHECK_EQUAL_COLLECTIONS(mb.transforms().begin(), mb.transforms().end(), Xt.begin(), Xt.end());

  // nrBodies
  BOOST_CHECK_EQUAL(mb.nrBodies(), bodies.size());
  // nrJoints
  BOOST_CHECK_EQUAL(mb.nrJoints(), bodies.size());

  int params = 0, dof = 0;
  for(std::size_t i = 0; i < joints.size(); ++i)
  {
    params += joints[i].params();
    dof += joints[i].dof();
  }

  BOOST_CHECK_EQUAL(params, mb.nrParams());
  BOOST_CHECK_EQUAL(dof, mb.nrDof());
}

BOOST_AUTO_TEST_CASE(JacobianConstructTest)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  namespace cst = boost::math::constants;

  MultiBody mb;
  MultiBodyConfig mbc;
  MultiBodyGraph mbg;
  std::tie(mb, mbc, mbg) = makeXYZSarm();

  Jacobian jac1(mb, "b3");
  Jacobian jac2(mb, "b4");

  // test jointsPath
  std::vector<int> jointPath1 = {0, 1, 2, 3};
  std::vector<int> jointPath2 = {0, 1, 4};

  BOOST_CHECK_EQUAL_COLLECTIONS(jointPath1.begin(), jointPath1.end(), jac1.jointsPath().begin(),
                                jac1.jointsPath().end());
  BOOST_CHECK_EQUAL_COLLECTIONS(jointPath2.begin(), jointPath2.end(), jac2.jointsPath().begin(),
                                jac2.jointsPath().end());

  // test subMultibody
  MultiBody chain1 = jac1.subMultiBody(mb);
  MultiBody chain2 = jac2.subMultiBody(mb);

  // chain 1
  std::vector<Body> bodies = {mb.body(0), mb.body(1), mb.body(2), mb.body(3)};

  std::vector<Joint> joints = {Joint(Joint::Fixed, true, "Root"), mb.joint(1), mb.joint(2), mb.joint(3)};

  std::vector<int> pred = {-1, 0, 1, 2};
  std::vector<int> succ = {0, 1, 2, 3};
  std::vector<int> parent = {-1, 0, 1, 2};

  PTransformd I(PTransformd::Identity());
  PTransformd to(Vector3d(0., 0.5, 0.));
  PTransformd unitY(Vector3d(0., 1., 0.));
  std::vector<PTransformd> Xt = {I, to, unitY, unitY};

  checkMultiBodyEq(chain1, bodies, joints, pred, succ, parent, Xt);

  // chain 2
  bodies = {mb.body(0), mb.body(1), mb.body(4)};

  joints = {Joint(Joint::Fixed, true, "Root"), mb.joint(1), mb.joint(4)};

  pred = {-1, 0, 1};
  succ = {0, 1, 2};
  parent = {-1, 0, 1};

  Xt = {I, to, PTransformd(Vector3d(0.5, 0.5, 0.))};

  checkMultiBodyEq(chain2, bodies, joints, pred, succ, parent, Xt);

  // test subMultiBody safe version
  BOOST_CHECK_THROW(jac1.sSubMultiBody(chain2), std::domain_error);
}

void checkJacobianMatrixFromVelocity(const rbd::MultiBody & subMb,
                                     rbd::MultiBodyConfig & subMbc,
                                     const std::vector<sva::MotionVecd> & velVec,
                                     const Eigen::MatrixXd & jacMat)
{
  int col = 0;
  for(int i = 0; i < subMb.nrJoints(); ++i)
  {
    for(int j = 0; j < subMb.joint(i).dof(); ++j)
    {
      subMbc.alpha[i][j] = 1.;

      forwardVelocity(subMb, subMbc);

      Eigen::Vector6d mv = velVec.back().vector();
      BOOST_CHECK_SMALL((mv - jacMat.col(col)).norm(), TOL);

      subMbc.alpha[i][j] = 0.;
      ++col;
    }
  }
}

void checkJacobianMatrixSize(const rbd::MultiBody & subMb, const Eigen::MatrixXd & jacMat)
{
  // test jacobian size
  BOOST_CHECK_EQUAL(jacMat.rows(), 6);
  BOOST_CHECK_EQUAL(jacMat.cols(), subMb.nrDof());
}

void checkFullJacobianMatrix(const rbd::MultiBody & mb,
                             const rbd::MultiBody & subMb,
                             const rbd::Jacobian & jac,
                             const Eigen::MatrixXd & jacMat)
{
  using namespace Eigen;

  MatrixXd fakeFull1(5, mb.nrDof());
  MatrixXd fakeFull2(6, mb.nrDof() + 1);
  MatrixXd fullJacMat(6, mb.nrDof());
  BOOST_CHECK_THROW(jac.sFullJacobian(mb, jacMat, fakeFull1), std::domain_error);
  BOOST_CHECK_THROW(jac.sFullJacobian(mb, jacMat, fakeFull2), std::domain_error);
  BOOST_CHECK_NO_THROW(jac.sFullJacobian(mb, jacMat, fullJacMat));

  for(int i = 0; i < subMb.nrJoints(); ++i)
  {
    int joint = jac.jointsPath()[i];
    int dof = mb.joint(i).dof();
    BOOST_CHECK_EQUAL(jacMat.block(0, subMb.jointPosInDof(i), 6, dof),
                      fullJacMat.block(0, mb.jointPosInDof(joint), 6, dof));
  }
}

void checkJacobian(const rbd::MultiBody & mb, const rbd::MultiBodyConfig & mbc, rbd::Jacobian & jac)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;

  const MatrixXd & jac_mat = jac.jacobian(mb, mbc);
  MultiBody subMb = jac.subMultiBody(mb);

  // fill subMbc
  MultiBodyConfig subMbc(subMb);
  for(int i = 0; i < subMb.nrJoints(); ++i)
  {
    subMbc.bodyPosW[i] = mbc.bodyPosW[jac.jointsPath()[i]];
    subMbc.jointConfig[i] = mbc.jointConfig[jac.jointsPath()[i]];
    subMbc.parentToSon[i] = mbc.parentToSon[jac.jointsPath()[i]];
  }

  // test fullJacobian
  checkFullJacobianMatrix(mb, subMb, jac, jac_mat);

  // test jacobian
  const MatrixXd & jac_mat_w = jac.jacobian(mb, mbc);
  checkJacobianMatrixSize(subMb, jac_mat_w);
  checkJacobianMatrixFromVelocity(subMb, subMbc, subMbc.bodyVelW, jac_mat_w);

  // test bodyJacobian
  const MatrixXd & jac_mat_b = jac.bodyJacobian(mb, mbc);
  checkJacobianMatrixSize(subMb, jac_mat_b);
  checkJacobianMatrixFromVelocity(subMb, subMbc, subMbc.bodyVelB, jac_mat_w);
}

BOOST_AUTO_TEST_CASE(JacobianComputeTest)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  namespace cst = boost::math::constants;

  MultiBody mb;
  MultiBodyConfig mbc;
  MultiBodyGraph mbg;
  std::tie(mb, mbc, mbg) = makeXYZSarm();

  Jacobian jac1(mb, "b3");
  Jacobian jac2(mb, "b4");

  mbc.q = {{}, {0.}, {0.}, {0.}, {1., 0., 0., 0.}};
  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);

  checkJacobian(mb, mbc, jac1);
  checkJacobian(mb, mbc, jac2);

  mbc.q = {{}, {cst::pi<double>() / 2.}, {0.}, {0.}, {1., 0., 0., 0.}};
  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);

  checkJacobian(mb, mbc, jac1);
  checkJacobian(mb, mbc, jac2);

  // test jacobian safe version
  MultiBodyConfig mbcBadNrBodyPos(mbc);
  mbcBadNrBodyPos.bodyPosW.resize(1);
  BOOST_CHECK_THROW(jac1.sJacobian(mb, mbcBadNrBodyPos), std::domain_error);

  MultiBodyConfig mbcBadNrJointConf(mbc);
  mbcBadNrJointConf.motionSubspace.resize(1);
  BOOST_CHECK_THROW(jac1.sJacobian(mb, mbcBadNrJointConf), std::domain_error);

  MultiBody mbErr = jac2.subMultiBody(mb);
  MultiBodyConfig mbcErr(mbErr);
  BOOST_CHECK_THROW(jac1.sJacobian(mbErr, mbcErr), std::domain_error);
}

BOOST_AUTO_TEST_CASE(JacobianComputeTestFreeFlyer)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  namespace cst = boost::math::constants;

  MultiBody mb;
  MultiBodyConfig mbc;
  MultiBodyGraph mbg;
  std::tie(mb, mbc, mbg) = makeXYZSarm(false);

  Jacobian jac1(mb, "b3");
  Jacobian jac2(mb, "b4");

  Quaterniond quat(AngleAxisd(cst::pi<double>() / 2., Vector3d::UnitX())
                   * AngleAxisd(cst::pi<double>() / 8., Vector3d::UnitZ()));
  Vector3d tran = Vector3d::Random() * 10.;

  mbc.q = {{quat.w(), quat.x(), quat.y(), quat.z(), tran.x(), tran.y(), tran.z()}, {0.}, {0.}, {0.}, {1., 0., 0., 0.}};
  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);

  checkJacobian(mb, mbc, jac1);
  checkJacobian(mb, mbc, jac2);

  quat =
      (AngleAxisd(cst::pi<double>() / 8., Vector3d::UnitX()) * AngleAxisd(cst::pi<double>() / 2., Vector3d::UnitZ()));
  tran = Vector3d::Random() * 10.;
  mbc.q = {{quat.w(), quat.x(), quat.y(), quat.z(), tran.x(), tran.y(), tran.z()},
           {cst::pi<double>() / 2.},
           {0.},
           {0.},
           {1., 0., 0., 0.}};
  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);

  checkJacobian(mb, mbc, jac1);
  checkJacobian(mb, mbc, jac2);

  // test jacobian safe version
  MultiBodyConfig mbcBadNrBodyPos(mbc);
  mbcBadNrBodyPos.bodyPosW.resize(1);
  BOOST_CHECK_THROW(jac1.sJacobian(mb, mbcBadNrBodyPos), std::domain_error);

  MultiBodyConfig mbcBadNrJointConf(mbc);
  mbcBadNrJointConf.motionSubspace.resize(1);
  BOOST_CHECK_THROW(jac1.sJacobian(mb, mbcBadNrJointConf), std::domain_error);

  MultiBody mbErr = jac2.subMultiBody(mb);
  MultiBodyConfig mbcErr(mbErr);
  BOOST_CHECK_THROW(jac1.sJacobian(mbErr, mbcErr), std::domain_error);
}

BOOST_AUTO_TEST_CASE(JacobianComputeTest2)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  namespace cst = boost::math::constants;

  MultiBody mb;
  MultiBodyConfig mbc;
  MultiBodyGraph mbg;
  std::tie(mb, mbc, mbg) = makeSSSarm(true);

  Jacobian jac1(mb, "b3");

  mbc.q = {{}, {1., 0., 0., 0.}, {1., 0., 0., 0.}, {1., 0., 0., 0.}};
  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);

  checkJacobian(mb, mbc, jac1);

  Quaterniond q1(AngleAxisd(cst::pi<double>() / 2., Vector3d::UnitX()));
  Quaterniond q2 = Quaterniond::Identity();
  Quaterniond q3 = Quaterniond::Identity();
  mbc.q = {{}, {q1.w(), q1.x(), q1.y(), q1.z()}, {q2.w(), q2.x(), q2.y(), q2.z()}, {q3.w(), q3.x(), q3.y(), q3.z()}};
  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);

  checkJacobian(mb, mbc, jac1);

  q1 = AngleAxisd(cst::pi<double>() / 2., Vector3d::UnitX()) * AngleAxisd(cst::pi<double>() / 4., Vector3d::UnitY());
  mbc.q = {{}, {q1.w(), q1.x(), q1.y(), q1.z()}, {q2.w(), q2.x(), q2.y(), q2.z()}, {q3.w(), q3.x(), q3.y(), q3.z()}};
  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);

  checkJacobian(mb, mbc, jac1);

  q2 = AngleAxisd(cst::pi<double>() / 4., Vector3d::UnitX());
  mbc.q = {{}, {q1.w(), q1.x(), q1.y(), q1.z()}, {q2.w(), q2.x(), q2.y(), q2.z()}, {q3.w(), q3.x(), q3.y(), q3.z()}};
  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);

  checkJacobian(mb, mbc, jac1);

  q3 = AngleAxisd(cst::pi<double>() / 8., Vector3d::UnitZ());
  mbc.q = {{}, {q1.w(), q1.x(), q1.y(), q1.z()}, {q2.w(), q2.x(), q2.y(), q2.z()}, {q3.w(), q3.x(), q3.y(), q3.z()}};
  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);

  checkJacobian(mb, mbc, jac1);
}

typedef std::function<const Eigen::MatrixXd &(const rbd::MultiBody &, const rbd::MultiBodyConfig &)> jacobianComputeFunc;

Eigen::MatrixXd makeJDotFromStep(const rbd::MultiBody & mb,
                                 const rbd::MultiBodyConfig & mbc,
                                 jacobianComputeFunc jacComp)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;

  double step = 1e-8;

  MultiBodyConfig mbcTmp(mbc);

  MatrixXd oJ = jacComp(mb, mbcTmp);
  eulerIntegration(mb, mbcTmp, step);
  forwardKinematics(mb, mbcTmp);
  forwardVelocity(mb, mbcTmp);
  MatrixXd nJ = jacComp(mb, mbcTmp);

  return (nJ - oJ) / step;
}

typedef const Eigen::MatrixXd & (rbd::Jacobian::*worldJacobian_t)(const rbd::MultiBody &, const rbd::MultiBodyConfig &);
typedef sva::MotionVecd (rbd::Jacobian::*worldVelocity_t)(const rbd::MultiBody &, const rbd::MultiBodyConfig &) const;

void testJacobianDot(const rbd::MultiBody & mb, const rbd::MultiBodyConfig & mbc, rbd::Jacobian & jac)
{
  using namespace std::placeholders; // bind
  using namespace Eigen;

  MatrixXd JD_diff = makeJDotFromStep(mb, mbc, std::bind(worldJacobian_t(&rbd::Jacobian::jacobian), jac, _1, _2));
  MatrixXd JD = jac.jacobianDot(mb, mbc);

  BOOST_CHECK_SMALL((JD_diff - JD).norm(), 2e-5);

  MatrixXd JD_diff_b = makeJDotFromStep(mb, mbc, std::bind(&rbd::Jacobian::bodyJacobian, jac, _1, _2));
  MatrixXd JD_b = jac.bodyJacobianDot(mb, mbc);

  BOOST_CHECK_SMALL((JD_diff_b - JD_b).norm(), 2e-5);
}

BOOST_AUTO_TEST_CASE(JacobianDotComputeTest)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  namespace cst = boost::math::constants;

  MultiBody mb;
  MultiBodyConfig mbc;
  MultiBodyGraph mbg;
  std::tie(mb, mbc, mbg) = makeSSSarm(true);

  Jacobian jac1(mb, "b3");

  mbc.q = {{}, {1., 0., 0., 0.}, {1., 0., 0., 0.}, {1., 0., 0., 0.}};
  mbc.alpha = {{}, {0., 0., 0.}, {0., 0., 0.}, {0., 0., 0.}};
  mbc.alphaD = {{}, {0., 0., 0.}, {0., 0., 0.}, {0., 0., 0.}};
  forwardKinematics(mb, mbc);
  for(int i = 0; i < mb.nrJoints(); ++i)
  {
    for(int j = 0; j < mb.joint(i).dof(); ++j)
    {
      mbc.alpha[i][j] = 1.;
      forwardVelocity(mb, mbc);

      testJacobianDot(mb, mbc, jac1);

      mbc.alpha[i][j] = 0.;
    }
  }

  Quaterniond q1 =
      AngleAxisd(cst::pi<double>() / 2., Vector3d::UnitX()) * AngleAxisd(cst::pi<double>() / 4., Vector3d::UnitY());
  Quaterniond q2 = Quaterniond::Identity();
  Quaterniond q3 = Quaterniond::Identity();
  mbc.q = {{}, {q1.w(), q1.x(), q1.y(), q1.z()}, {q2.w(), q2.x(), q2.y(), q2.z()}, {q3.w(), q3.x(), q3.y(), q3.z()}};
  forwardKinematics(mb, mbc);
  for(int i = 0; i < mb.nrJoints(); ++i)
  {
    for(int j = 0; j < mb.joint(i).dof(); ++j)
    {
      mbc.alpha[i][j] = 1.;
      forwardVelocity(mb, mbc);

      testJacobianDot(mb, mbc, jac1);

      mbc.alpha[i][j] = 0.;
    }
  }

  q2 = AngleAxisd(cst::pi<double>() / 4., Vector3d::UnitX());
  mbc.q = {{}, {q1.w(), q1.x(), q1.y(), q1.z()}, {q2.w(), q2.x(), q2.y(), q2.z()}, {q3.w(), q3.x(), q3.y(), q3.z()}};
  forwardKinematics(mb, mbc);
  for(int i = 0; i < mb.nrJoints(); ++i)
  {
    for(int j = 0; j < mb.joint(i).dof(); ++j)
    {
      mbc.alpha[i][j] = 1.;
      forwardVelocity(mb, mbc);

      testJacobianDot(mb, mbc, jac1);

      mbc.alpha[i][j] = 0.;
    }
  }

  q3 = AngleAxisd(cst::pi<double>() / 8., Vector3d::UnitZ());
  mbc.q = {{}, {q1.w(), q1.x(), q1.y(), q1.z()}, {q2.w(), q2.x(), q2.y(), q2.z()}, {q3.w(), q3.x(), q3.y(), q3.z()}};
  forwardKinematics(mb, mbc);
  for(int i = 0; i < mb.nrJoints(); ++i)
  {
    for(int j = 0; j < mb.joint(i).dof(); ++j)
    {
      mbc.alpha[i][j] = 1.;
      forwardVelocity(mb, mbc);

      testJacobianDot(mb, mbc, jac1);

      mbc.alpha[i][j] = 0.;
    }
  }

  // test with all joint velocity
  for(int i = 0; i < mb.nrJoints(); ++i)
  {
    for(int j = 0; j < mb.joint(i).dof(); ++j)
    {
      mbc.alpha[i][j] = 1.;
      forwardVelocity(mb, mbc);

      testJacobianDot(mb, mbc, jac1);
    }
  }
  mbc.alpha = {{}, {0., 0., 0.}, {0., 0., 0.}, {0., 0., 0.}};

  // test with a point
  Jacobian jacP(mb, "b3", Vector3d::Random() * 10.);
  for(int i = 0; i < mb.nrJoints(); ++i)
  {
    for(int j = 0; j < mb.joint(i).dof(); ++j)
    {
      mbc.alpha[i][j] = 1.;
      forwardVelocity(mb, mbc);

      testJacobianDot(mb, mbc, jacP);
    }
  }
  mbc.alpha = {{}, {0., 0., 0.}, {0., 0., 0.}, {0., 0., 0.}};

  // test with free flyier

  MultiBody mbF = mbg.makeMultiBody("b0", false);

  MultiBodyConfig mbcF(mbF);

  Jacobian jacF(mbF, "b3");

  mbcF.q = {{1., 0., 0., 0., 0., 0., 0.}, {1., 0., 0., 0.}, {1., 0., 0., 0.}, {1., 0., 0., 0.}};
  mbcF.alpha = {{0., 0., 0., 0., 0., 0.}, {0., 0., 0.}, {0., 0., 0.}, {0., 0., 0.}};
  mbcF.alphaD = {{0., 0., 0., 0., 0., 0.}, {0., 0., 0.}, {0., 0., 0.}, {0., 0., 0.}};
  forwardKinematics(mbF, mbcF);

  for(int i = 0; i < mbF.nrJoints(); ++i)
  {
    for(int j = 0; j < mbF.joint(i).dof(); ++j)
    {
      mbcF.alpha[i][j] = 1.;
      forwardVelocity(mbF, mbcF);

      testJacobianDot(mbF, mbcF, jacF);
    }
  }
  mbcF.alpha = {{0., 0., 0., 0., 0., 0.}, {0., 0., 0.}, {0., 0., 0.}, {0., 0., 0.}};

  Quaterniond qF = AngleAxisd(1.2, Vector3d::UnitX()) * AngleAxisd(-0.4, Vector3d::UnitZ());
  mbc.q = {{qF.w(), qF.x(), qF.y(), qF.z(), 1., 2., 3.},
           {q1.w(), q1.x(), q1.y(), q1.z()},
           {q2.w(), q2.x(), q2.y(), q2.z()},
           {q3.w(), q3.x(), q3.y(), q3.z()}};
  forwardKinematics(mbF, mbcF);

  for(int i = 0; i < mbF.nrJoints(); ++i)
  {
    for(int j = 0; j < mbF.joint(i).dof(); ++j)
    {
      mbcF.alpha[i][j] = 1.;
      forwardVelocity(mbF, mbcF);

      testJacobianDot(mbF, mbcF, jacF);
    }
  }
  mbcF.alpha = {{0., 0., 0., 0., 0., 0.}, {0., 0., 0.}, {0., 0., 0.}, {0., 0., 0.}};
}

void testTranslateJacobian(const rbd::MultiBody & mb,
                           const rbd::MultiBodyConfig & mbc,
                           const Eigen::Vector3d & P,
                           rbd::Jacobian & jacO,
                           rbd::Jacobian & jacP)
{
  using namespace Eigen;

  MatrixXd JO_w = jacO.jacobian(mb, mbc);
  MatrixXd JP_w = jacP.jacobian(mb, mbc);

  MatrixXd JO_P_w = JO_w;
  jacO.translateJacobian(JO_w, mbc, P, JO_P_w);

  BOOST_CHECK_SMALL((JO_P_w - JP_w).norm(), TOL);

  MatrixXd JO_b = jacO.jacobian(mb, mbc);
  MatrixXd JP_b = jacP.jacobian(mb, mbc);

  MatrixXd JO_P_b = JO_b;
  jacO.translateJacobian(JO_b, mbc, P, JO_P_b);

  BOOST_CHECK_SMALL((JO_P_b - JP_b).norm(), TOL);
}

BOOST_AUTO_TEST_CASE(JacobianTranslateTest)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  namespace cst = boost::math::constants;

  MultiBody mb;
  MultiBodyConfig mbc;
  MultiBodyGraph mbg;
  std::tie(mb, mbc, mbg) = makeSSSarm(true);

  Vector3d point = Vector3d::Random() * 10.;

  Jacobian jacO(mb, "b3");
  Jacobian jacP(mb, "b3", point);

  mbc.q = {{}, {1., 0., 0., 0.}, {1., 0., 0., 0.}, {1., 0., 0., 0.}};
  mbc.alpha = {{}, {0., 0., 0.}, {0., 0., 0.}, {0., 0., 0.}};
  mbc.alphaD = {{}, {0., 0., 0.}, {0., 0., 0.}, {0., 0., 0.}};
  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);

  testTranslateJacobian(mb, mbc, point, jacO, jacP);

  Quaterniond q1 =
      AngleAxisd(cst::pi<double>() / 2., Vector3d::UnitX()) * AngleAxisd(cst::pi<double>() / 4., Vector3d::UnitY());
  Quaterniond q2(AngleAxisd(cst::pi<double>() / 4., Vector3d::UnitX()));
  Quaterniond q3(AngleAxisd(cst::pi<double>() / 8., Vector3d::UnitZ()));
  mbc.q = {{}, {q1.w(), q1.x(), q1.y(), q1.z()}, {q2.w(), q2.x(), q2.y(), q2.z()}, {q3.w(), q3.x(), q3.y(), q3.z()}};

  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);

  testTranslateJacobian(mb, mbc, point, jacO, jacP);
}

Eigen::MatrixXd jacobianVecBodyFromJac(const rbd::MultiBody & mb,
                                       const rbd::MultiBodyConfig & mbc,
                                       rbd::Jacobian & jac,
                                       const Eigen::Vector3d & vec)
{
  Eigen::MatrixXd jacO = jac.bodyJacobian(mb, mbc);
  Eigen::MatrixXd jacV(jacO.rows(), jacO.cols());
  jac.translateBodyJacobian(jacO, mbc, vec, jacV);

  return jacV - jacO;
}

Eigen::MatrixXd jacobianVecFromJac(const rbd::MultiBody & mb,
                                   const rbd::MultiBodyConfig & mbc,
                                   rbd::Jacobian & jac,
                                   const Eigen::Vector3d & vec)
{
  Eigen::MatrixXd jacO = jac.jacobian(mb, mbc);
  Eigen::MatrixXd jacV(jacO.rows(), jacO.cols());
  jac.translateJacobian(jacO, mbc, vec, jacV);

  return jacV - jacO;
}

BOOST_AUTO_TEST_CASE(JacobianVectorTest)
{
  rbd::MultiBody mb;
  rbd::MultiBodyConfig mbc;
  rbd::MultiBodyGraph mbg;
  std::tie(mb, mbc, mbg) = makeXYZSarm(false);

  rbd::forwardKinematics(mb, mbc);
  rbd::forwardVelocity(mb, mbc);

  rbd::Jacobian jac1(mb, "b3");
  rbd::Jacobian jac2(mb, "b4", Eigen::Vector3d(0.1, -0.1, 0.2));

  for(int i = 0; i < 50; ++i)
  {
    Eigen::VectorXd q(mb.nrParams());
    q.setRandom();
    // normalize free flyier and spherical joint
    q.head<4>().normalize();
    q.segment(mb.jointPosInParam(mb.jointIndexByName("j3")), 4).normalize();
    rbd::vectorToParam(q, mbc.q);
    forwardKinematics(mb, mbc);

    Eigen::Vector3d vec;
    vec.setRandom();

    // vectorBody
    Eigen::MatrixXd jac1MatDiff = jacobianVecBodyFromJac(mb, mbc, jac1, vec);
    Eigen::MatrixXd jac1Mat = jac1.vectorBodyJacobian(mb, mbc, vec);

    BOOST_CHECK_SMALL((jac1MatDiff.block(3, 0, 3, jac1.dof()) - jac1Mat.block(3, 0, 3, jac1.dof())).norm(), TOL);

    Eigen::MatrixXd jac2MatDiff = jacobianVecBodyFromJac(mb, mbc, jac2, vec);
    Eigen::MatrixXd jac2Mat = jac2.vectorBodyJacobian(mb, mbc, vec);

    BOOST_CHECK_SMALL((jac2MatDiff.block(3, 0, 3, jac2.dof()) - jac2Mat.block(3, 0, 3, jac2.dof())).norm(), TOL);

    // vector
    Eigen::MatrixXd jac3MatDiff = jacobianVecFromJac(mb, mbc, jac1, vec);
    Eigen::MatrixXd jac3Mat = jac1.vectorJacobian(mb, mbc, vec);

    BOOST_CHECK_SMALL((jac3MatDiff.block(3, 0, 3, jac1.dof()) - jac3Mat.block(3, 0, 3, jac1.dof())).norm(), TOL);

    Eigen::MatrixXd jac4MatDiff = jacobianVecFromJac(mb, mbc, jac2, vec);
    Eigen::MatrixXd jac4Mat = jac2.vectorJacobian(mb, mbc, vec);

    BOOST_CHECK_SMALL((jac4MatDiff.block(3, 0, 3, jac2.dof()) - jac4Mat.block(3, 0, 3, jac2.dof())).norm(), TOL);
  }
}

template<typename MatFunc, typename VectorFunc>
Eigen::Vector6d testMatrixAgainstVector(const rbd::MultiBody & mb,
                                        const rbd::MultiBodyConfig & mbc,
                                        const rbd::Jacobian & jac,
                                        MatFunc && matFunc,
                                        const Eigen::VectorXd & alpha,
                                        VectorFunc && vectorFunc)
{
  const Eigen::MatrixXd & mat = std::forward<MatFunc>(matFunc)(mb, mbc);
  Eigen::MatrixXd matFull(6, mb.nrDof());
  jac.fullJacobian(mb, mat, matFull);
  Eigen::Vector6d res = matFull * alpha;
  sva::MotionVecd mv = std::forward<VectorFunc>(vectorFunc)(mb, mbc);

  return mv.vector() - res;
}

BOOST_AUTO_TEST_CASE(JacobianVectorVelAccComputeTest)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  using namespace std::placeholders;
  namespace cst = boost::math::constants;

  MultiBody mb;
  MultiBodyConfig mbc;
  MultiBodyGraph mbg;
  std::tie(mb, mbc, mbg) = makeXYZSarm(false);

  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);

  Jacobian jac1(mb, "b3");
  Jacobian jac2(mb, "b4");

  for(int i = 0; i < 50; ++i)
  {
    Eigen::VectorXd q(mb.nrParams()), alpha(mb.nrDof());
    q.setRandom();
    alpha.setRandom();
    // normalize free flyier and spherical joint
    q.head<4>().normalize();
    q.segment(mb.jointPosInParam(mb.jointIndexByName("j3")), 4).normalize();
    rbd::vectorToParam(q, mbc.q);
    rbd::vectorToParam(alpha, mbc.alpha);
    forwardKinematics(mb, mbc);
    forwardVelocity(mb, mbc);
    // calcul the normal acceleration since alphaD is zero
    forwardAcceleration(mb, mbc);

    Vector3d point(Vector3d::Random());
    for(Jacobian & jac : {std::ref(jac1), std::ref(jac2)})
    {
      jac.point(point);

      BOOST_CHECK_SMALL(
          testMatrixAgainstVector(mb, mbc, jac, std::bind(worldJacobian_t(&Jacobian::jacobian), std::ref(jac), _1, _2),
                                  alpha, std::bind(worldVelocity_t(&Jacobian::velocity), std::ref(jac), _1, _2))
              .norm(),
          TOL);

      BOOST_CHECK_SMALL(testMatrixAgainstVector(mb, mbc, jac, std::bind(&Jacobian::bodyJacobian, std::ref(jac), _1, _2),
                                                alpha, std::bind(&Jacobian::bodyVelocity, std::ref(jac), _1, _2))
                            .norm(),
                        TOL);

      typedef MotionVecd (Jacobian::*normalAccel_func1)(const MultiBody &, const MultiBodyConfig &) const;
      typedef MotionVecd (Jacobian::*normalAccel_func2)(const MultiBody &, const MultiBodyConfig &,
                                                        const std::vector<sva::MotionVecd> &) const;

      BOOST_CHECK_SMALL(testMatrixAgainstVector(mb, mbc, jac, std::bind(&Jacobian::jacobianDot, std::ref(jac), _1, _2),
                                                alpha,
                                                std::bind(static_cast<normalAccel_func1>(&Jacobian::normalAcceleration),
                                                          std::ref(jac), _1, _2))
                            .norm(),
                        TOL);

      BOOST_CHECK_SMALL(
          testMatrixAgainstVector(
              mb, mbc, jac, std::bind(&Jacobian::bodyJacobianDot, std::ref(jac), _1, _2), alpha,
              std::bind(static_cast<normalAccel_func1>(&Jacobian::bodyNormalAcceleration), std::ref(jac), _1, _2))
              .norm(),
          TOL);

      BOOST_CHECK_SMALL(testMatrixAgainstVector(mb, mbc, jac, std::bind(&Jacobian::jacobianDot, std::ref(jac), _1, _2),
                                                alpha,
                                                std::bind(static_cast<normalAccel_func2>(&Jacobian::normalAcceleration),
                                                          std::ref(jac), _1, _2, std::ref(mbc.bodyAccB)))
                            .norm(),
                        TOL);

      BOOST_CHECK_SMALL(
          testMatrixAgainstVector(mb, mbc, jac, std::bind(&Jacobian::bodyJacobianDot, std::ref(jac), _1, _2), alpha,
                                  std::bind(static_cast<normalAccel_func2>(&Jacobian::bodyNormalAcceleration),
                                            std::ref(jac), _1, _2, std::ref(mbc.bodyAccB)))
              .norm(),
          TOL);
    }
  }
}


///////////////////////////////////////////////////////////////////////////////
// statistics::survival::data::algorithm::make_mean_events.hpp               //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_SURVIVAL_DATA_ALGORITHM_VECTORIZE_EVENTS_HPP_ER_2009
#define BOOST_STATISTICS_SURVIVAL_DATA_ALGORITHM_VECTORIZE_EVENTS_HPP_ER_2009
#include <boost/iterator/iterator_traits.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/type_traits/is_same.hpp>

namespace boost{
namespace statistics{
namespace survival{
namespace data{

// Copies the failure and event time of each event in a sequence to an
// output iterator
template<typename ItE,typename ItO>
ItO vectorize_events(
    ItE b,
    ItE e,
    ItO o
);

    // Implementation //

template<typename ItE,typename ItO>
ItO vectorize_events(
    ItE b,
    ItE e,
    ItO o
){
    typedef typename iterator_value<ItE>::type event_;
    typedef typename event_::value_type val_;
    typedef typename event_::failure_type fail_;
    BOOST_MPL_ASSERT((
        is_same<val_,fail_>
    ));

    while(b!=e){
        *o = (b->failure()); ++o;
        *o = (b->time()); ++o;
        ++b;
    }
    return o;
}

}// data
}// survival
}// statistics
}// boost

#endif


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.

#ifndef poplibs_test_Norms_hpp
#define poplibs_test_Norms_hpp

#include <boost/multi_array.hpp>

namespace poplibs_test {
namespace norm {

// Norms are supported for activations of shape [B][C][FIELD]
// where B is the number of batches
//       C is the number of channels
//       FIELD is the field size
// Fields of dimension > 1 are supported by flattening the field dimension

// Type of normalisations
enum class NormType { BatchNorm, GroupNorm, InstanceNorm, LayerNorm };

// Compute estimates of mean and standard deviation for a batch of activations
// and return
// 1) mean
// 2) 1/sqrt(stdDev * stdDev + eps)
void normStatistics(const boost::multi_array_ref<double, 3> actsIn, double eps,
                    bool unbiasedVarEstimate, bool stableAlgo,
                    boost::multi_array_ref<double, 1> mean,
                    boost::multi_array_ref<double, 1> invStdDev,
                    NormType normType, bool groupNormStridedChannelGrouping);

// Normalise activations togiven whitening statistics and parameters
// gamma and beta and estimates mean and inverse standard deviation
void normalise(const boost::multi_array_ref<double, 3> actsIn,
               const boost::multi_array_ref<double, 1> gamma,
               const boost::multi_array_ref<double, 1> beta,
               const boost::multi_array_ref<double, 1> mean,
               const boost::multi_array_ref<double, 1> invStdDev,
               boost::multi_array_ref<double, 3> actsOut,
               boost::multi_array_ref<double, 3> actsWhitened,
               NormType normType, bool groupNormStridedChannelGrouping);

// Compute gradients for normalisation given whitened activations,
// input gradients, inverse of standard deviation and gamma
void normGradients(const boost::multi_array_ref<double, 3> actsWhitened,
                   const boost::multi_array_ref<double, 3> gradsIn,
                   const boost::multi_array_ref<double, 1> invStdDev,
                   const boost::multi_array_ref<double, 1> gamma,
                   boost::multi_array_ref<double, 3> gradsOut,
                   NormType normType, bool groupNormStridedChannelGrouping);

// Update parameters gamma and beta given whitened activations
void normParamUpdate(const boost::multi_array_ref<double, 3> actsWhitened,
                     const boost::multi_array_ref<double, 3> gradsIn,
                     double learningRate,
                     boost::multi_array_ref<double, 1> gamma,
                     boost::multi_array_ref<double, 1> beta, NormType normType);
} // End namespace norm.
} // End namespace poplibs_test

#endif // poplibs_test_Norms_hpp


#include <avatar_locomanipulation/helpers/NeuralNetModel.hpp>

# include <cstdlib>
# include <iostream>
# include <iomanip>
# include <cmath>

#include <avatar_locomanipulation/helpers/IOUtilities.hpp>
#include <Eigen/Dense>
#include <ros/ros.h>
#include <avatar_locomanipulation/BinaryClassifierQuery.h>

#include <avatar_locomanipulation/models/robot_model.hpp>

#include <avatar_locomanipulation/helpers/yaml_data_saver.hpp>
#include <avatar_locomanipulation/helpers/param_handler.hpp>

#include <chrono>
typedef std::chrono::high_resolution_clock Clock;

#define CONTACT_TRANSITION_DATA_LEFT_FOOT_STANCE 0
#define CONTACT_TRANSITION_DATA_RIGHT_FOOT_STANCE 1

#define CONTACT_TRANSITION_DATA_LEFT_HAND 0
#define CONTACT_TRANSITION_DATA_RIGHT_HAND 1
#define CONTACT_TRANSITION_DATA_BOTH_HANDS 2

Eigen::Vector3d quatToVec(const Eigen::Quaterniond & ori){
	Eigen::AngleAxisd tmp_ori(ori.normalized()); // gets the normalized version of ori and sets it to an angle axis representation
  Eigen::Vector3d ori_vec = tmp_ori.axis()*tmp_ori.angle();
  return ori_vec;
}

void normalizeInputCalculate(const Eigen::VectorXd & x_in, const Eigen::VectorXd & data_mean, const Eigen::VectorXd & data_std_dev, Eigen::VectorXd & x_normalized) {
  // std::cout << x_in.rows() << " " << data_mean.rows() << " " << data_std_dev.rows() << std::endl;
  x_normalized = (x_in - data_mean).cwiseQuotient(data_std_dev);
}

int main(int argc, char ** argv){
  // Eigen::initParallel();
  // int num_cores = 4;
  // omp_set_num_threads(num_cores);
  // Eigen::setNbThreads(num_cores);
  int num_threads = Eigen::nbThreads();
  std::cout<<"CPU Parallelization, num_threads: "<<num_threads<<std::endl;

  int input_size = 10;

  ParamHandler param_handler;
  std::string model_path = THIS_PACKAGE_PATH"nn_models/layer3_20000pts/cpp_model/layer3_model.yaml";

  std::cout << "Loading Model..." << std::endl;
  myYAML::Node model = myYAML::LoadFile(model_path);
  NeuralNetModel nn_transition(model, false);
  std::cout << "Loaded" << std::endl;

  double stance_origin = CONTACT_TRANSITION_DATA_LEFT_FOOT_STANCE;
  double manipulation_type = CONTACT_TRANSITION_DATA_RIGHT_HAND;

  Eigen::Vector3d swing_foot_start_pos(0.15411,-0.2074,0.0);
  Eigen::Quaterniond swing_foot_start_ori(0.998433,0,0,-0.0559473);

  Eigen::Vector3d pelvis_pos(-0.04466, -0.0581, 0.9522);
  Eigen::Quaterniond pelvis_ori(0.999608,0,0,-0.0279);

  Eigen::Vector3d landing_foot_pos(0.08006, -0.3344725,0);
  Eigen::Quaterniond landing_foot_ori(0.9746387,0,0,-0.22378);

  Eigen::Vector3d right_hand_start_pos(0.37345209, -0.67836299, 1.0333);
  Eigen::Quaterniond right_hand_start_ori(0.778100, 0.22986, -0.234809 ,0.535339);

  Eigen::Vector3d left_hand_start_pos(0,0,0);
  Eigen::Quaterniond left_hand_start_ori(1,0,0,0);

  Eigen::VectorXd rawDatum(32);
  Eigen::VectorXd datum(32);

	Eigen::MatrixXd data(input_size, 32);

  rawDatum << stance_origin, manipulation_type,
    swing_foot_start_pos, quatToVec(swing_foot_start_ori),
    pelvis_pos, quatToVec(pelvis_ori),
    landing_foot_pos, quatToVec(landing_foot_ori),
    right_hand_start_pos, quatToVec(right_hand_start_ori),
    left_hand_start_pos, quatToVec(left_hand_start_ori);

  //Normalization Params
  param_handler.load_yaml_file(THIS_PACKAGE_PATH"nn_models/layer3_20000pts/cpp_model/normalization_params.yaml");
  std::vector<double> vmean;
  param_handler.getVector("x_train_mean", vmean);
  std::vector<double> vstd_dev;
  param_handler.getVector("x_train_std", vstd_dev);

  Eigen::VectorXd mean(32);
  Eigen::VectorXd std_dev(32);
  for (int ii = 0; ii < 32; ii++){
    mean[ii] = vmean[ii];
    std_dev[ii] = vstd_dev[ii];
  }

  normalizeInputCalculate(rawDatum, mean, std_dev, datum);
  for (int i = 0; i < input_size; i++){
    data.row(i) = datum;

  }
	// std::cout << "datum: " << datum << std::endl;

  Eigen::MatrixXd pred(input_size,1);
  pred = nn_transition.GetOutput(data);
  std::cout << "Prediction: " << pred << std::endl;

  // Test Inference speed:
  auto t1 = Clock::now();
  auto t2 = Clock::now();
  double time_span;

  Eigen::MatrixXd x_vec(1,32);
  Eigen::MatrixXd y_vec(1,1);
  x_vec.row(0) = datum;

  while(true){
    t1 = Clock::now();
    y_vec = nn_transition.GetOutput(x_vec);
    t2 = Clock::now();
    time_span = std::chrono::duration_cast< std::chrono::duration<double> >(t2 - t1).count();

    std::cout << " 1 pred = " << y_vec << std::endl;
    std::cout << " inference time = " << time_span << " seconds" << std::endl;
    std::cout << " freq = " << (1.0/time_span) << " Hz" << std::endl;
    std::cout << " " << std::endl;

    t1 = Clock::now();
    pred = nn_transition.GetOutput(data);
    t2 = Clock::now();

    time_span = std::chrono::duration_cast< std::chrono::duration<double> >(t2 - t1).count();
    std::cout << "  " << input_size << " pred = " << pred.transpose() << std::endl;
    std::cout << " inference time = " << time_span << " seconds" << std::endl;
    std::cout << " freq = " << (1.0/time_span) << " Hz" << std::endl;
    std::cout << " " << std::endl;

  }

  return 0;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_FUNCTIONAL_MATH_HPP
#define BOOST_COMPUTE_FUNCTIONAL_MATH_HPP

#include <boost/compute/functional/detail/macros.hpp>

namespace boost {
namespace compute {

BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(acos, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(acosh, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(acospi, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(asin, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(asinh, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(asinpi, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(atan, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(atan2, T (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(atanh, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(atanpi, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(atan2pi, T (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(cbrt, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(ceil, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(copysign, T (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(cos, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(cosh, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(cospi, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(erf, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(erfc, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(exp, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(exp2, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(exp10, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(expm1, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(fabs, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(fdim, T (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(floor, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(fma, T (T, T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(fmax, T (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(fmin, T (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(fmod, T (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(hypot, T (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(ilogb, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(lgamma, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(log, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(log2, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(log10, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(log1p, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(logb, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(mad, T (T, T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(nextafter, T (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(pow, T (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(pown, T (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(powr, T (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(remainder, T (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(rint, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(rootn, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(round, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(rsqrt, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(sin, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(sinh, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(sinpi, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(sqrt, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(tan, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(tanh, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(tanpi, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(tgamma, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(trunc, T (T), class T)

} // end compute namespace
} // end boost namespace

#endif // BOOST_COMPUTE_FUNCTIONAL_MATH_HPP


#include <cassert>
#include <iostream>  // for debug only
#include <boost/foreach.hpp>
#include "decode/field.h"
#include "decode/decode_output.h"
#include "libpalc/palc_simplefield.h"
#include "libpalc/palc_simpleblock.h" //
#include "libpalc/common_def.h"
#include "libpalc/field_utils.h"

#if 0
int64_t PalcSimpleField::GetRepeat(int64_t lenLeft, int64_t jump) {  // NOLINT
  if (repeatLower_ < 0 || repeatUpper_ < 0) {
    std::cout << "ERROR - both lower and upper repeat = 0" << std::endl;
    return 0;
  } else if (repeatLower_ == 0 && repeatUpper_ == 0) {
    return 1;     // By default, repeat = 1;
  } else {
    if (repeatLower_ > repeatUpper_) {
      return 0;
    } else if (repeatLower_ == repeatUpper_) {
      if (repeatLower_ * jump <= lenLeft) {
        return repeatLower_;
      } else {
        return (lenLeft / jump);
      }
    } else {
      if (repeatUpper_ * jump <= lenLeft) {
        return repeatUpper_;
      } else {
        return (lenLeft / jump);
      }
    }
  }
}

// set initial value = 0?
void PalcSimpleField::SetRepeat(int lower, int upper) {
  repeatLower_ = lower;
  repeatUpper_ = upper;
  if (repeatLower_ == ~0)
    repeatLower_ = 0;
  if (repeatUpper_ == ~0)
    repeatUpper_ = 0;
}
#else
int64_t PalcSimpleField::GetRepeat(int64_t lenLeft, int64_t jump) {  // NOLINT
  if (repeatUpper_ == 0 || jump == 0)
    return 1;
  else
    return (repeatUpper_ >(lenLeft / jump))?(lenLeft / jump):repeatUpper_;
}

void PalcSimpleField::SetRepeat(int lower, int upper) {
  repeatLower_ = lower;
  repeatUpper_ = upper;
}
#endif

void PalcSimpleField::AddCountRange(int lower, int upper) {
  if ((lower < countLower_) || (countLower_ == ~0)) {
    countLower_ = lower;
  }
  if ((upper > countUpper_) || (countUpper_ == ~0)){
    countUpper_ = upper;
  }
}

int64_t PalcSimpleField::GetCount(int64_t lenLeft, int64_t jump, int64_t repeat) {    // NOLINT
  if (countLower_ == ~0)
    countLower_ = 0;
  if (countUpper_ == ~0)
    countUpper_ = 0;

  if (countLower_ < 0 || countUpper_ < 0) {
    return 0;
  } else if (countLower_ == 0 && countUpper_ == 0) {
    return 1;     // By default, repeat = 1;
  } else {
    if (countLower_ > countUpper_) {
      return 0;
    } else if (countLower_ == countUpper_) {
      if (countLower_ * jump * repeat <= lenLeft) {
        return countLower_;
      } else {
        return (lenLeft / (jump * repeat));
      }
    } else {
      if (countUpper_ * jump * repeat <= lenLeft) {
        return countUpper_;
      } else {
        return (lenLeft / (jump * repeat));
      }
    }
  }
}

void PalcSimpleField::AddSegment(int lower, int upper) {
  PalcSegRang range;
  range.lower = lower;
  range.upper = upper;
  // mLocator.mSegs.mSegRanges.push_back(range);
  segs_.mSegRanges.push_back(range);
  CalcSegTotalBits();  // done in loading protocol, no performance hit on decode
}

// call after all AddSegment done
void PalcSimpleField::CalcSegTotalBits() {
  // calculate total bits
  // mLocator.mSegs.mTotalBits = 0;
  segs_.mTotalBits = 0;
  BOOST_FOREACH(PalcSegRang segRange, segs_.mSegRanges) {
    int d = segRange.upper - segRange.lower;
    segs_.mTotalBits += (d > 0 ? d + 1 : 1 - d);
  }
}

bool PalcSimpleField::IsSkipable() {
  if (interest_id_ == INVALID_INTEREST_ID && (valueMod_ == 0)) {
    return true;
  }
  return false;
}
int32_t PalcSimpleField::Decode(IrepContext &context) {
  if (interest_id_ == INVALID_INTEREST_ID && !context.mIsFieldDump_ && (valueMod_ == 0)) {
    Skip(context);
    return DECODE_SUCCESS;
  }
#ifdef DECODE_PERF_PROFILING
  context.IncProcessedFieldCount();
#endif

  if (!context.CheckLength()) {
    return DECODE_NOT_ENOUGH;
  }

  uint8_t* pBitStream = const_cast<uint8_t*>(context.mpBitStream_);
  int64_t* curbit = &context.mCurBit_;
  // int64_t jump = jump_;
  bool is_signed = mSigned;
  Field &fld_cache = *context.GetDecodeOutput()->GetFieldBuf(GetInterestId());

  int64_t &lenLeft = static_cast<PalcSimpleBlock*>(context.GetCurBlock())->lengthLeft_;
  PalcSimpleBlock::EnumBlockSortType sort = static_cast<PalcSimpleBlock*>(context.GetCurBlock())->blkSortType_;

  // debug
#ifdef DECODE_LOG_DUMP
  char first4Bytes[256];
  const uint8_t * pCurStream = context.mpBitStream_ + (context.mCurBit_ >> 3);

  if ((context.mTotalBit_ - context.mCurBit_ ) >= 32 ) {
    sprintf(first4Bytes, "Field id=%d,name=%s,curbit=%ld,curbytes=%ld,leftlen(bytes)=%ld,"
        "next4bytes=0x%02x%02x%02x%02x", id_, lastKfeName_.c_str(), context.mCurBit_, (context.mCurBit_ >>3),
        (lenLeft>>3), pCurStream[0], pCurStream[1], pCurStream[2], pCurStream[3]);
  } else {
    sprintf(first4Bytes, "Field id=%d,name=%s,curbit=%ld,curbytes=%ld,leftlen(bytes)=%ld,"
          "next4bytes=0x%02x------", id_, lastKfeName_.c_str(), context.mCurBit_, (context.mCurBit_ >>3),
          (lenLeft>>3), pCurStream[0]);
  }
#endif

  if (lenLeft <= 0) {
#ifdef DECODE_LOG_DUMP
    PALC_TRACE("[LENGTH not ENOUGH] - Field id=" << this->id_ << ",name=" << this->lastKfeName_ << ", lenLeft:=" << lenLeft);
#endif
    return DECODE_NOT_ENOUGH;
  }

  // field length checking
  int64_t repeat = GetRepeat(lenLeft, jump_);
  if (repeat <= 0) {
    return DECODE_NOT_ENOUGH;
  }

  int64_t count = GetCount(lenLeft, jump_, repeat);
  if (count <= 0) {
    return DECODE_NOT_ENOUGH;
  }

  for (int64_t i = 0; i < count; i++) {
    if (sort == PalcSimpleBlock::ENUM_BLOCK_SORT_NULL || sort == PalcSimpleBlock::ENUM_BLOCK_SORT_UNKNOWN) {
      if (segs_.mMsf) {
        FieldUtils::GetFieldMsfByLittleEndian(pBitStream, curbit,
                                                  context.mTotalBit_,
                                                  segs_, jump_, repeat,
                                                  is_signed, false, fld_cache);
      } else {
        FieldUtils::GetFieldLsfByLittleEndian(pBitStream, curbit,
                                                  context.mTotalBit_,
                                                  segs_, jump_, repeat,
                                                  is_signed, false, fld_cache);
      }
    } else if (sort == PalcSimpleBlock::ENUM_BLOCK_SORT_REVERSED) {
      if (segs_.mMsf) {
        FieldUtils::GetFieldMsfByBigEndian(pBitStream, curbit,
                                               context.mTotalBit_, segs_,
                                               jump_, repeat, is_signed, true, fld_cache);
      } else {
        FieldUtils::GetFieldLsfByBigEndian(pBitStream, curbit,
                                               context.mTotalBit_, segs_,
                                               jump_, repeat, is_signed, true, fld_cache);
      }
    }

    context.currentField_ = fld_cache.getValue();

#ifdef DECODE_LOG_DUMP
    PALC_TRACE(first4Bytes << ",value =" << std::hex << context.currentField_);
#endif

    DecodeOutput* decode_output = context.GetDecodeOutput();
    if (decode_output && (INVALID_INTEREST_ID != interest_id_ || context.mIsFieldDump_)) {
      decode_output->SaveField(&fld_cache);
#ifdef DECODE_LOG_DUMP
      PALC_TRACE("[Add to DO]Field id=" << this->id_ << ",interested_id=" << this->GetInterestId() << ",name="
                     << lastKfeName_ << ",value=" << std::hex << fld_cache.getValue());
#endif
    }
  }

  lenLeft -= jump_ * repeat * count;
  if (filterFunc_) {
    bool ret = filterFunc_(context.currentField_);
    if (ret)
      return DECODE_SUCCESS;
    else
      return DECODE_NOT_MATCH;
  }
  return DECODE_SUCCESS;
}

int32_t PalcSimpleField::Skip(IrepContext &context) {
#ifdef DECODE_PERF_PROFILING
  context.IncSkipFieldCount();
#endif

  if (!context.CheckLength()) {
    return DECODE_NOT_ENOUGH;
  }

  // int64_t jump = mLocator.mJump;

  int64_t &lenLeft = static_cast<PalcSimpleBlock*>(context.GetCurBlock())->lengthLeft_;
  // PalcSimpleBlock::EnumBlockSortType sort =  static_cast<PalcSimpleBlock*>(context.GetCurBlock())->blkSortType_;

  if (lenLeft <= 0) {
    #ifdef DECODE_LOG_DUMP
    PALC_TRACE("[LENGTH not ENOUGH] Skip Field id=" << id_ << ",name=" << lastKfeName_ << ",lenLeft: " << lenLeft);
    #endif
    return DECODE_NOT_ENOUGH;
  }

#ifdef DECODE_LOG_DUMP
  char first4Bytes[256];
  const uint8_t * pCurStream = context.mpBitStream_ + (context.mCurBit_ >> 3);

  if (lenLeft >= 32) {
    sprintf(first4Bytes, "0x%02x%02x%02x%02x", pCurStream[0], pCurStream[1], pCurStream[2], pCurStream[3]);
  } else {
    sprintf(first4Bytes, "0x%02x------", pCurStream[0]);
  }
  PALC_TRACE("Skip:Field id=" << id_ << ",name=" << lastKfeName_ << ",Cur bit: " << context.mCurBit_
             << ",Cur bytes=" << (context.mCurBit_ >> 3) << ",first 4 bytes=" << first4Bytes << ",leftlen(bytes)=" << (lenLeft >> 3));

#endif

  int64_t repeat = GetRepeat(lenLeft, jump_);
  if (repeat <= 0) {
    return DECODE_NOT_ENOUGH;
  }

  int64_t count = GetCount(lenLeft, jump_, repeat);
  if (count <= 0) {
    return DECODE_NOT_ENOUGH;
  }

  // Skip this field, Jump Only.
  context.mCurBit_ += jump_ * repeat * count;
  // context.lengthLeft_ -= jump * repeat * count;
  lenLeft -= jump_ * repeat * count;
  return DECODE_SUCCESS;
}


/**
 * @file RooFitExt_KSText.cc
 * @author  [Yi-Mu "Enoch" Chen](https://github.com/yimuchen)
 * @brief   Implementing extended RooFit functions for KSTest computation
 */

#ifdef CMSSW_GIT_HASH
#include "UserUtils/MathUtils/interface/RooFitExt.hpp"
#else
#include "UserUtils/MathUtils/RooFitExt.hpp"
#endif

#include <RooAbsData.h>
#include <RooAbsPdf.h>
#include <RooDataSet.h>
#include <RooRealVar.h>
#include <TMath.h>

#include <boost/format.hpp>
#include <memory>

namespace usr {

/**
 * @brief Helper structure for expressing a 1D data set (RooDataSet) simply
 *        as a list of numbers with weights.
 *
 * Additional helper routines are used for caching anc calculating the numbers
 * required for performing the Kolmogorov-Smirnov test. One in particular is the
 * "EffectiveNum" method, as the number of data points is required for the
 * calculation of the KS test, the "number of data points" for weighted data
 * sets would follow the "sum squared over sum of squared" approach:
 *
 * \f[
 *  N_\mathrm{eff} = \frac{\left(\sum w_i\right)^2 }{\sum w_i^2}
 * \f]
 *
 * The data set is also simplified to a list of doubles (with weights), with
 * the list ordered to make it simpler to calculated the KS test.
 */
struct SimplifiedData
{
  typedef std::pair<double, double> Evt;
  double           sum;   /**< @brief Sum of weights */
  double           sumsq; /**< @brief Sum of squared weights */
  std::vector<Evt> dataset;

  inline static bool
  Compare( const Evt& x, const Evt& y )
  { return x.first < y.first; }

  double EffectiveNum() const;

  SimplifiedData(
    RooDataSet&      data,
    RooRealVar&      x,
    const RooCmdArg& cut1,
    const RooCmdArg& cut2
    );
};

/**
 * @brief Helper structure for expressing a CDF object, and helping with
 *        adjusting the normalization of the CDF function when the Kolmogorov--
 *        Smirnov test is asked to be performed on certain regions of the
 *        variable range.
 *
 * The construction of the CDF functions as a RooFit RooAbsRel object handles
 * the normalization of the CDF function across the maximum range of the
 * variable used to constructed the CDF function. In the case that a range
 * is requested when calculating the KS test would require the effective CDF is
 * 0/1 at the edges of the range, so effectively:
 * \f[
 *     \mathrm{CDF}_{\mathrm{eff}}(x)
 *      = \frac{\mathrm{CDF}(x) -\mathrm{CDF}(x_\mathrm{min})}
 *             {\mathrm{CDF}(x_\mathrm{max})-\mathrm{CDF}(x_\mathrm{max})}
 * \f]
 *
 * In the case that two ranges is requested (say to test the goodness-of-fit
 * for a side band fit result), the effective CDF is construted such that CDF
 * is continuous for all values of \f$x\f$, an returns values 0/1 at the far
 * edges of the ranges. So effectively, given the two ranges \f$[a,b]\f$ and
 * \f$[c,d]\f$:
 *
 * \f[
 *  \mathrm{CDF}_\mathrm{eff}(x < b)
 *      = \frac{\mathrm{CDF}(x) - \mathrm{CDF}(a)}
 *      {\mathrm{CDF}(b)-\mathrm{CDF}(a) + \mathrm{CDF}(d)-\mathrm{CDF}(c)};
 *
 *  \mathrm{CDF}_\mathrm{eff}(x > c)
 *      = \frac
 *      {\mathrm{CDF}(x)-\mathrm{CDF}(c) + \mathrm{CDF}(b)-\mathrm{CDF}(a)}
 *      {\mathrm{CDF}(b)-\mathrm{CDF}(a) + \mathrm{CDF}(d)-\mathrm{CDF}(c)}
 * \f]
 *
 *
 * While this scheme readily extends to arbitrarily many ranges, our
 * implementation, would only allow for two ranges.
 */
struct SimplifiedCDF
{
  std::unique_ptr<RooAbsReal>             rawcdf;
  RooRealVar&                             var;
  std::vector<std::pair<double, double> > rangelist;
  double                                  norm;

  double operator()( const double x );
  double rawval( const double x );

  inline double
  min( unsigned x ){ return rangelist.at( x ).first; }
  inline double
  max( unsigned x ){ return rangelist.at( x ).second; }

  SimplifiedCDF(
    RooAbsPdf&       pdf,
    RooRealVar&      x,
    const RooCmdArg& cut1,
    const RooCmdArg& cut2
    );
  void AddRange( const RooCmdArg& cmd );
};

bool CheckCutCmd( const RooCmdArg& cmd, RooRealVar& x );

/**
 * @brief Calculating the Kolmogorov--Smirov distance of a RooDataSet and a
 *        RooAbsPdf in term of a variable.
 *
 * The calculation routine supports:
 *  - weighted data sets. To see how the number of data is calculated for
 *    weighted data sets, see the helper class SimplifiedData.
 *  - Up to two cuts in the data set. Currently you can only cut by specifying
 *    a range already declared in the variable of comparison. To see how
 *    the cumulative PDF is handled, see the helper class SimplifiedCDF.
 */
double
KSDistance(
  RooDataSet&      dataset,
  RooAbsPdf&       pdf,
  RooRealVar&      var,
  const RooCmdArg& cut1,
  const RooCmdArg& cut2
  )
{
  SimplifiedData sim( dataset, var, cut1, cut2 );
  SimplifiedCDF cdf( pdf, var, cut1, cut2 );

  double empcdf  = 0;
  double maxdist = 0;

  for( const auto& evt : sim.dataset ){
    // emperical cdf is the sum of weights
    // (normalization handled latter for stability )
    const double val = evt.first;
    const double wgt = evt.second;
    empcdf += wgt;
    maxdist = std::max( maxdist, fabs( cdf( val ) - ( empcdf/sim.sum ) ) );
  }

  return sqrt( sim.EffectiveNum() ) * maxdist;
}

/**
 * @brief Returning the corresponding KS probability after calculating the KS
 *        distance of between a data set and PDF.
 *
 * The probability function is already implemented in the
 * TMath::KolmogorovProb() function, will not attempt to recreate.
 */
double
KSProb(
  RooDataSet&      dataset,
  RooAbsPdf&       pdf,
  RooRealVar&      var,
  const RooCmdArg& cut1,
  const RooCmdArg& cut2
  )
{
  const double dist = KSDistance( dataset, pdf, var, cut1, cut2 );
  return TMath::KolmogorovProb( dist );
}

/**
 * @brief Calculating the Kolmogorov-Smirnov distance between two data sets.
 *
 * The code for looping though the two data set is based on the ROOT
 * implementation found in the TMath::KolmogorovTest() functions.
 * Supporting up to two range cuts and weighted data sets. For the calculation
 * of the effective number of events in a weighted data set, see the helper
 * class SimplifiedData.
 */
double
KSDistance(
  RooDataSet&      set1,
  RooDataSet&      set2,
  RooRealVar&      var,
  const RooCmdArg& cut1,
  const RooCmdArg& cut2
  )
{
  SimplifiedData sim1( set1, var, cut1, cut2 );
  SimplifiedData sim2( set2, var, cut1, cut2 );

  double empcdf1 = 0;
  double empcdf2 = 0;
  double maxdist = 0;

  for( auto evt1 = sim1.dataset.begin(), evt2 = sim2.dataset.begin();
       evt1 != sim1.dataset.end() && evt2 != sim2.dataset.end(); ){
    if( evt1->first < evt2->first ){
      empcdf1 += evt1->second;
      evt1++;
    } else if( evt1->first > evt2->first ){
      empcdf2 += evt2->second;
      evt2++;
    } else {// special case for identical value
      double x = evt1->first;

      while( evt1->first == x && evt1 != sim1.dataset.end() ){
        empcdf1 += evt1->second;
        evt1++;
      }

      while( evt2->first == x && evt2 != sim2.dataset.end() ){
        empcdf2 += evt2->second;
        evt2++;
      }
    }
    maxdist = TMath::Max( maxdist,
      fabs( ( empcdf1/sim1.sum ) - ( empcdf2/sim2.sum ) ) );
  }

  const double num1 = std::max( sim1.EffectiveNum(), sim2.EffectiveNum() );
  const double num2 = std::min( sim1.EffectiveNum(), sim2.EffectiveNum() );
  return maxdist * sqrt( ( num1/( num1 + num2 ) )*num2 );

}

/**
 * @brief Returning the KS probability of two data sets
 * @details this takes the KS distance of the two datasets and pass it directly
 * to the TMath::KolmogorovProb() functions.
 */
double
KSProb(
  RooDataSet&      set1,
  RooDataSet&      set2,
  RooRealVar&      var,
  const RooCmdArg& cut1,
  const RooCmdArg& cut2 )
{
  const double dist = KSDistance( set1, set2, var, cut1, cut2 );
  return TMath::KolmogorovProb( dist );
}

/**
 * @brief An alternative, effective p-value of the KS test for two data set.
 * @details the alternative calculation is described in
  [this]( https://en.wikipedia.org/wiki/Kolmogorov%E2%80%93Smirnov_test#Two-sample_Kolmogorov%E2%80%93Smirnov_test) page.
  Hard limiting the calculation output to 1 for a more "sane" output value
 */
double
KSProbAlt(
  RooDataSet&      set1,
  RooDataSet&      set2,
  RooRealVar&      var,
  const RooCmdArg& cut1,
  const RooCmdArg& cut2 )
{
  const double dist = KSDistance( set1, set2, var, cut1, cut2 );

  // Alternative calculation of p-value as described in wiki page
  return std::min( exp( -2. * dist * dist ) * 2., 1. );
}

/*-----------------------------------------------------------------------------
 *  SimplifiedData function implementations
   --------------------------------------------------------------------------*/
SimplifiedData::SimplifiedData(
  RooDataSet&      data,
  RooRealVar&      x,
  const RooCmdArg& cut1,
  const RooCmdArg& cut2 ) :
  sum( 0 ),
  sumsq( 0 )
{
  RooDataSet* op_set  = 0;
  RooDataSet* tmp_set = 0;
  if( !CheckCutCmd( cut1, x ) && !CheckCutCmd( cut2, x ) ){
    op_set = (RooDataSet*)( data.reduce( RooFit::SelectVars(x) ));
  } else if( CheckCutCmd( cut1, x ) && !CheckCutCmd( cut2, x ) ){
    op_set = (RooDataSet*)( data.reduce( RooFit::SelectVars( x ), cut1 ) );
  } else if( CheckCutCmd( cut2, x ) && !CheckCutCmd( cut1, x ) ){
    op_set = (RooDataSet*)( data.reduce( RooFit::SelectVars( x ), cut2 ) );
  } else {
    op_set  = (RooDataSet*)( data.reduce( RooFit::SelectVars( x ), cut1 ) );
    tmp_set = (RooDataSet*)( data.reduce( RooFit::SelectVars( x ), cut2 ) );
    op_set->append( *tmp_set );
  }

  if( !op_set->isWeighted() ){
    sum = sumsq = op_set->numEntries();
  }

  for( int i = 0; i < op_set->numEntries(); ++i ){
    const double d = op_set->get( i )->getRealValue( x.GetName() );
    const double w = op_set->weight();
    if( !op_set->isWeighted() ){
      dataset.push_back( std::pair<double, double>( d, 1 ) );
    } else {
      dataset.push_back( std::pair<double, double>( d, w ) );
      sum   += w;
      sumsq += w*w;
    }
  }

  delete op_set;
  if( tmp_set ){ delete tmp_set; }

  std::sort( dataset.begin(), dataset.end(), Compare );
}

/*----------------------------------------------------------------------------*/

double
SimplifiedData::EffectiveNum() const
{
  if( sum == sumsq ){// unweighted data set
    return sum;
  } else {
    // Effective number of events for weighted data set found here:
    // https://stats.stackexchange.com/questions/193439/two-sample-kolmogorov-smirnov-test-with-weights?utm_medium=organic&utm_source=google_rich_qa&utm_campaign=google_rich_qa
    return sum * sum / sumsq;
  }
}

/*-----------------------------------------------------------------------------
 *  SimplifiedCDF implementations
   --------------------------------------------------------------------------*/
SimplifiedCDF::SimplifiedCDF(
  RooAbsPdf&       pdf,
  RooRealVar&      x,
  const RooCmdArg& cut1,
  const RooCmdArg& cut2 ) :
  rawcdf( pdf.createCdf( RooArgSet( x ) ) ),
  var( x )
{
  if( CheckCutCmd( cut1, var ) ){ AddRange( cut1 ); }
  if( CheckCutCmd( cut2, var ) ){ AddRange( cut2 ); }

  if( rangelist.size() == 2 ){
    // Making sure the lower edge is the first range entry.
    if( min( 0 ) > min( 1 ) ){
      auto temp = rangelist.at( 0 );
      rangelist.at( 0 ) = rangelist.at( 1 );
      rangelist.at( 1 ) = temp;
    }

    // reducing the second range if overlaps
    if( max( 0 ) >= min( 1 ) ){
      rangelist.at( 0 ).second = rangelist.at( 1 ).second;
      rangelist.erase( rangelist.begin() + 1 );
    }
  }

  // Saving normalization denominator
  if( rangelist.size() == 0 ){
    norm = rawval( var.getMax() )  - rawval( var.getMin() );// Should be 1
  } else if( rangelist.size() == 1 ){
    norm = rawval( max( 0 ) ) - rawval( min( 0 ) );
  } else {
    norm = ( rawval( max( 0 ) ) - rawval( min( 0 ) ) )
           + ( rawval( max( 1 ) ) - rawval( min( 1 ) ) );
  }
}

/**
 * @brief Returning the effective CDF value with the cut ranges taken into
 *        account.
 */
double
SimplifiedCDF::operator()( const double x )
{
  double ans = rawval( x );
  if( rangelist.size() == 0 ){
    return ans;
  } else if( rangelist.size() == 1 ){
    if( min( 0 ) < x && x < max( 0 ) ){
      return ( ans - rawval( min( 0 ) ) )/norm;
    } else {
      return ans;
    }
  } else {
    if( min( 0 ) < x && x < max( 0 ) ){
      return ( ans - rawval( min( 0 ) ) )/norm;
    } else if( min( 1 ) < x && x < max( 1 ) ){
      return ( ans - rawval( min( 1 ) ) + ( rawval( max( 0 ) )-rawval( min( 0 ) ) ) )/norm;
    } else {
      return ans;
    }
  }
}

/**
 * @brief returning the raw CDF functions value as constructed by RooFit.
 */
double
SimplifiedCDF::rawval( const double x )
{
  var = x;
  return rawcdf->getVal();
}

/*----------------------------------------------------------------------------*/

void
SimplifiedCDF::AddRange( const RooCmdArg& cmd )
{
  rangelist.push_back( std::pair<double, double>(
    var.getMin( cmd.getString( 0 ) ),
    var.getMax( cmd.getString( 0 ) ) )
    );
}

/*----------------------------------------------------------------------------*/

bool
CheckCutCmd( const RooCmdArg& cmd, RooRealVar& var )
{
  if( std::string( cmd.GetName() ) != "CutRange" ){
    return false;
  } else if( !var.hasRange( cmd.getString( 0 ) ) ){
    return false;
  }
  return true;
}

}/* usr */


//---------------------------------------------------------------------------//
// Copyright (c) 2020-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Nikita Kaskov <nbering@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND 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.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_ALGEBRA_FIELDS_ELEMENT_FP_HPP
#define CRYPTO3_ALGEBRA_FIELDS_ELEMENT_FP_HPP

#include <nil/crypto3/algebra/fields/detail/exponentiation.hpp>

#include <nil/crypto3/multiprecision/ressol.hpp>
#include <nil/crypto3/multiprecision/inverse.hpp>
#include <nil/crypto3/multiprecision/number.hpp>
#include <nil/crypto3/multiprecision/cpp_int.hpp>

#include <boost/type_traits/is_integral.hpp>

#include <type_traits>

namespace nil {
    namespace crypto3 {
        namespace algebra {
            namespace fields {
                namespace detail {

                    template<typename FieldParams>
                    class element_fp {
                        typedef FieldParams policy_type;

                    public:
                        typedef typename policy_type::field_type field_type;

                        typedef typename policy_type::modular_type modular_type;
                        typedef typename policy_type::integral_type integral_type;

                        constexpr static const integral_type modulus = policy_type::modulus;

                        using data_type = modular_type;

                        data_type data;

                        constexpr element_fp() : data(data_type(0, modulus)) {};

                        constexpr element_fp(const data_type &data) : data(data) {};

                        template<typename Number,
                                 typename std::enable_if<(multiprecision::is_number<Number>::value &&
                                                          !multiprecision::is_modular_number<Number>::value) ||
                                                             std::is_integral<Number>::value,
                                                         bool>::type = true>
                        constexpr element_fp(const Number &data) : data(data, modulus) {};

                        constexpr element_fp(const element_fp &B) {
                            data = B.data;
                        };

                        constexpr inline static element_fp zero() {
                            return element_fp(0);
                        }

                        constexpr inline static element_fp one() {
                            return element_fp(1);
                        }

                        constexpr bool is_zero() const {
                            return data == data_type(0, modulus);
                        }

                        constexpr bool is_one() const {
                            return data == data_type(1, modulus);
                        }

                        constexpr bool operator==(const element_fp &B) const {
                            return data == B.data;
                        }

                        constexpr bool operator!=(const element_fp &B) const {
                            return data != B.data;
                        }

                        constexpr element_fp &operator=(const element_fp &B) {
                            data = B.data;

                            return *this;
                        }

                        constexpr element_fp operator+(const element_fp &B) const {
                            return element_fp(data + B.data);
                        }

                        constexpr element_fp operator-(const element_fp &B) const {
                            return element_fp(data - B.data);
                        }

                        constexpr element_fp &operator-=(const element_fp &B) {
                            data -= B.data;

                            return *this;
                        }

                        constexpr element_fp &operator+=(const element_fp &B) {
                            data += B.data;

                            return *this;
                        }

                        constexpr element_fp &operator*=(const element_fp &B) {
                            data *= B.data;

                            return *this;
                        }

                        constexpr element_fp &operator/=(const element_fp &B) {
                            data *= B.inversed().data;

                            return *this;
                        }

                        constexpr element_fp operator-() const {
                            return element_fp(-data);
                        }

                        constexpr element_fp operator*(const element_fp &B) const {
                            return element_fp(data * B.data);
                        }

                        constexpr element_fp operator/(const element_fp &B) const {
                            //                        return element_fp(data / B.data);
                            return element_fp(data * B.inversed().data);
                        }

                        constexpr bool operator<(const element_fp &B) const {
                            return data < B.data;
                        }

                        constexpr bool operator>(const element_fp &B) const {
                            return data > B.data;
                        }

                        constexpr bool operator<=(const element_fp &B) const {
                            return data <= B.data;
                        }

                        constexpr bool operator>=(const element_fp &B) const {
                            return data >= B.data;
                        }

                        constexpr element_fp &operator++() {
                            data = data + data_type(1, modulus);
                            return *this;
                        }

                        constexpr element_fp operator++(int) {
                            element_fp temp(*this);
                            ++*this;
                            return temp;
                        }

                        constexpr element_fp &operator--() {
                            data = data - data_type(1, modulus);
                            return *this;
                        }

                        constexpr element_fp operator--(int) {
                            element_fp temp(*this);
                            --*this;
                            return temp;
                        }

                        constexpr element_fp doubled() const {
                            return element_fp(data + data);
                        }

                        // TODO: maybe incorrect result here
                        constexpr element_fp sqrt() const {
                            return element_fp(ressol(data));
                        }

                        constexpr element_fp inversed() const {
                            return element_fp(inverse_extended_euclidean_algorithm(data));
                        }

                        // TODO: complete method
                        constexpr element_fp _2z_add_3x() {
                        }

                        constexpr element_fp squared() const {
                            return element_fp(data * data);    // maybe can be done more effective
                        }

                        // TODO: maybe error here
                        constexpr bool is_square() const {
                            return (this->sqrt() != -1);    // maybe can be done more effective
                        }

                        template<typename PowerType,
                                 typename = typename std::enable_if<boost::is_integral<PowerType>::value>::type>
                        constexpr element_fp pow(const PowerType pwr) const {
                            return element_fp(multiprecision::powm(data, multiprecision::uint128_t(pwr)));
                        }

                        template<typename Backend, multiprecision::expression_template_option ExpressionTemplates>
                        constexpr element_fp
                            pow(const multiprecision::number<Backend, ExpressionTemplates> &pwr) const {
                            return element_fp(multiprecision::powm(data, pwr));
                        }
                    };

                    template<typename FieldParams>
                    constexpr typename element_fp<FieldParams>::integral_type const element_fp<FieldParams>::modulus;

                }    // namespace detail
            }        // namespace fields
        }            // namespace algebra
    }                // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_ALGEBRA_FIELDS_ELEMENT_FP_HPP