/*
 * Copyright 2015-2017 Guillermo Frontera <guillermo.frontera@upm.es>
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include "algorithms_2d.h"

#include <boost/geometry.hpp>

#include <log/logger.h>
#include <exc/exception.h>

#include "polygon_traits.h"

using namespace ues::geom;

const std::string component_name = "2D Geometric Algorithms";

point<2> ues::geom::segment_intersection ( const point<2> & s1p1,
                                           const point<2> & s1p2,
                                           const point<2> & s2p1,
                                           const point<2> & s2p2,
                                           const ues::math::numeric_type & epsilon )
{
    point<2> result;
    if ( check_segment_intersection ( s1p1, s1p2, s2p1, s2p2, result, epsilon ) )
    {
        return result;
    }
    else
    {
        std::ostringstream error;
        error << "No intersection found between segments ( " << s1p1 << ", " << s1p2 << " ) and ( " << s2p1 << ", " << s2p2 << " )";
        throw ues::exc::exception ( error.str(), UES_CONTEXT );
    }
}

bool ues::geom::check_segment_intersection ( const point<2> & s1p1,
                                             const point<2> & s1p2,
                                             const point<2> & s2p1,
                                             const point<2> & s2p2,
                                             point<2> & result,
                                             const ues::math::numeric_type & epsilon ) noexcept
{
    ues::log::logger lg;

    const ues::math::numeric_type & x1 = s1p1.get_x(), & y1 = s1p1.get_y(),
    & x2 = s1p2.get_x(), & y2 = s1p2.get_y(),
    & x3 = s2p1.get_x(), & y3 = s2p1.get_y(),
    & x4 = s2p2.get_x(), & y4 = s2p2.get_y();

    ues::math::numeric_type denominator = ( x1 - x2 ) * ( y3 - y4 ) - ( y1 - y2 ) * ( x3 - x4 );

    if ( denominator != 0 )
    {
        ues::math::numeric_type denominator_inv = 1 / denominator;
        ues::math::numeric_type first_factor = ( x1 * y2 - y1 * x2 );
        ues::math::numeric_type second_factor = ( x3 * y4 - y3 * x4 );
        ues::math::numeric_type x, y;
        x = ( first_factor * ( x3 - x4 ) - ( x1 - x2 ) * second_factor ) * denominator_inv;
        y = ( first_factor * ( y3 - y4 ) - ( y1 - y2 ) * second_factor ) * denominator_inv;

        result = { x, y };

        if ( x + epsilon >= std::max ( std::min ( x1, x2 ), std::min ( x3, x4 ) ) &&
                x - epsilon <= std::min ( std::max ( x1, x2 ), std::max ( x3, x4 ) ) &&
                y + epsilon >= std::max ( std::min ( y1, y2 ), std::min ( y3, y4 ) ) &&
                y - epsilon <= std::min ( std::max ( y1, y2 ), std::max ( y3, y4 ) ) )
        {

            if ( lg.min_level() <= ues::log::TRACE_LVL )
            {
                ues::log::event e ( ues::log::TRACE_LVL, component_name, "Segment intersection found" );
                e.message() << "Intersection between segments ( " << s1p1 << ", " << s1p2 << " ) and ( " << s2p1 << ", " << s2p2 << " ) is " << result << ".\n";
                lg.record ( std::move ( e ) );
            }

            return true;
        }
    }

    if ( lg.min_level() <= ues::log::TRACE_LVL )
    {
        ues::log::event e ( ues::log::TRACE_LVL, component_name, "Segment intersection not found" );
        e.message() << "No intersection found between segments ( " << s1p1 << ", " << s1p2 << " ) and ( " << s2p1 << ", " << s2p2 << " ).\n";
        if ( denominator != 0 )
        {
            e.message() << "Lines containing segments intersect at " << result << ", which is out of the boundaries of the segments.\n";
        }
        else
        {
            e.message() << "Segments are parallel.\n";
        }
        lg.record ( std::move ( e ) );
    }

    return false;
}

ues::math::numeric_type ues::geom::point_to_segment_distance ( const point<2> & origin_point,
                                                    const point<2> & segment_point1,
                                                    const point<2> & segment_point2,
                                                    const ues::math::numeric_type & angle,
                                                    const ues::math::numeric_type & epsilon )
{
    point<2> s1 = segment_point1;
    point<2> s2 = segment_point2;

    ues::math::matrix transform = rotation_matrix_2d ( -angle ) *
                       translation_matrix_2d ( -origin_point.get_x(), -origin_point.get_y() );
    s1.transform ( transform );
    s2.transform ( transform );

    if ( ( s1.get_y() > epsilon && s2.get_y() > epsilon ) ||
            ( s1.get_y() < -epsilon && s2.get_y() < -epsilon ) )
    {
        std::ostringstream out;
        out << "The angle provided (" << ( angle > ues::math::pi ? angle - 2 * ues::math::pi : angle ) << " rad) from point " << origin_point;
        out << " does not intersect segment " << segment_point1 << "-" << segment_point2;

        throw ues::exc::exception ( out.str(), UES_CONTEXT );
    }

    if ( std::abs ( s1.get_y() ) < ues::math::epsilon && std::abs ( s2.get_y() ) < ues::math::epsilon )
    {
        // The segment is perpendicular.
        return std::min ( s1.get_x(), s2.get_x() );
    }

    ues::math::numeric_type w1, w2;
    if ( s1.get_y() > 0 && s2.get_y() > 0 )
    {
        if ( s1.get_y() > s2.get_y() )
        {
            w1 = 0;
            w2 = 1;
        }
        else
        {
            w1 = 1;
            w2 = 0;
        }
    }
    else if ( s1.get_y() < 0 && s2.get_y() < 0 )
    {
        if ( s1.get_y() > s2.get_y() )
        {
            w1 = 1;
            w2 = 0;
        }
        else
        {
            w1 = 0;
            w2 = 1;
        }
    }
    else
    {
        ues::math::numeric_type inv_segment_height = 1 / std::abs ( s1.get_y() - s2.get_y() );
        w1 = std::abs ( s2.get_y() ) * inv_segment_height;
        w2 = std::abs ( s1.get_y() ) * inv_segment_height;
    }

    ues::math::numeric_type intersection_distance = w1 * s1.get_x() + w2 * s2.get_x();

    // Return distance from point to segment.
    return intersection_distance;
}

bool ues::geom::check_polygon_intersection ( const polygon & poly1, const polygon & poly2 ) noexcept
{
    return boost::geometry::intersects ( poly1, poly2 );
}

ues::math::matrix ues::geom::translation_matrix_2d ( const ues::math::numeric_type & x, const ues::math::numeric_type & y ) noexcept
{
    // A 3x3 matrix for homogeneous 2D transformation
    ues::math::matrix::fixed<3, 3> transform = { 1, 0, 0, 0, 1, 0, x, y, 1 };
    return transform;
}

ues::math::matrix ues::geom::rotation_matrix_2d ( const ues::math::numeric_type & angle ) noexcept
{

    // Compute the transformation matrix
    ues::math::numeric_type sine = std::sin ( angle );
    ues::math::numeric_type cosine = std::cos ( angle );

    // A 3x3 matrix for homogeneous 2D transformation
    ues::math::matrix::fixed<3, 3> transform = { cosine, sine, 0, -sine, cosine, 0, 0, 0, 1 };
    return transform;
}


#ifndef EIGHTPOINTALGO_H
#define EIGHTPOINTALGO_H
#include <vector>
#include <string>
#include <math.h>
#include <opencv2/opencv.hpp>
//#include <Eigen/Dense>

using namespace std;
using namespace cv;
//using namespace Eigen;

class EightPointAlgorithm
{
    public:
        EightPointAlgorithm(bool d = false);
        int getID();

        Mat F_Matrix_Eight_Point(vector<Point2d>&, vector<Point2d>&, bool norm = false);
        Mat F_Matrix_Normalized_Eight_Point(vector<Point2d>&, vector<Point2d>&);
        void Plot_Epipolar_lines(vector<Point2d>&, vector<Point2d>&, Mat&, Mat&);
    private:
        int id;
        bool debug;

        Mat Get_Homogen(vector<Point2d>&);
        Mat Get_Y(Mat&,Mat&);
        Mat Get_NormMat2d(Mat&);
        Mat Get_DrawLines(vector<Point2d>&,Mat&,vector<cv::Vec3d>&, Mat&);
};
#endif


#include "testing/testing.hpp"

#include "base/math.hpp"

#include <limits>

#include <boost/math/special_functions/next.hpp>

namespace
{
// Returns the next representable floating point value without using conversion to integer.
template <typename Float>
Float NextFloat(Float const x, int dir = 1)
{
  return boost::math::float_advance(x, dir);
}

template <typename Float>
void TestMaxULPs()
{
  for (unsigned int logMaxULPs = 0; logMaxULPs <= 8; ++logMaxULPs)
  {
    unsigned int const maxULPs = (1 << logMaxULPs) - 1;
    for (int base = -1; base <= 1; ++base)
    {
      for (int dir = -1; dir <= 1; dir += 2)
      {
        Float const x = base;
        Float y = x;
        for (unsigned int i = 0; i <= maxULPs; ++i)
        {
          TEST(base::AlmostEqualULPs(x, y, maxULPs), (x, y, maxULPs, x - y, dir));
          Float const nextY = NextFloat(y, dir);
          TEST_NOT_EQUAL(y, nextY, (i, base, dir));
          y = nextY;
        }
        TEST(!base::AlmostEqualULPs(x, y, maxULPs), (x, y, maxULPs, x - y));
      }
    }
  }
}
}  // namespace

UNIT_TEST(PowUInt)
{
  TEST_EQUAL(base::PowUint(3, 10), 59049, ());
}

UNIT_TEST(AlmostEqualULPs_double)
{
  TEST_ALMOST_EQUAL_ULPS(3.0, 3.0, ());
  TEST_ALMOST_EQUAL_ULPS(+0.0, -0.0, ());

  double const eps = std::numeric_limits<double>::epsilon();
  double const dmax = std::numeric_limits<double>::max();

  TEST_ALMOST_EQUAL_ULPS(1.0 + eps, 1.0, ());
  TEST_ALMOST_EQUAL_ULPS(1.0 - eps, 1.0, ());
  TEST_ALMOST_EQUAL_ULPS(1.0 - eps, 1.0 + eps, ());

  TEST_ALMOST_EQUAL_ULPS(dmax, dmax, ());
  TEST_ALMOST_EQUAL_ULPS(-dmax, -dmax, ());
  TEST_ALMOST_EQUAL_ULPS(dmax/2.0, dmax/2.0, ());
  TEST_ALMOST_EQUAL_ULPS(1.0/dmax, 1.0/dmax, ());
  TEST_ALMOST_EQUAL_ULPS(-1.0/dmax, -1.0/dmax, ());

  TEST(!base::AlmostEqualULPs(1.0, -1.0), ());
  TEST(!base::AlmostEqualULPs(2.0, -2.0), ());
  TEST(!base::AlmostEqualULPs(dmax, -dmax), ());
  TEST(!base::AlmostEqualULPs(0.0, eps), ());
}

UNIT_TEST(AlmostEqualULPs_float)
{
  TEST_ALMOST_EQUAL_ULPS(3.0f, 3.0f, ());
  TEST_ALMOST_EQUAL_ULPS(+0.0f, -0.0f, ());

  float const eps = std::numeric_limits<float>::epsilon();
  float const dmax = std::numeric_limits<float>::max();

  TEST_ALMOST_EQUAL_ULPS(1.0f + eps, 1.0f, ());
  TEST_ALMOST_EQUAL_ULPS(1.0f - eps, 1.0f, ());
  TEST_ALMOST_EQUAL_ULPS(1.0f - eps, 1.0f + eps, ());

  TEST_ALMOST_EQUAL_ULPS(dmax, dmax, ());
  TEST_ALMOST_EQUAL_ULPS(-dmax, -dmax, ());
  TEST_ALMOST_EQUAL_ULPS(dmax/2.0f, dmax/2.0f, ());
  TEST_ALMOST_EQUAL_ULPS(1.0f/dmax, 1.0f/dmax, ());
  TEST_ALMOST_EQUAL_ULPS(-1.0f/dmax, -1.0f/dmax, ());

  TEST(!base::AlmostEqualULPs(1.0f, -1.0f), ());
  TEST(!base::AlmostEqualULPs(2.0f, -2.0f), ());
  TEST(!base::AlmostEqualULPs(dmax, -dmax), ());
  TEST(!base::AlmostEqualULPs(0.0f, eps), ());
}

UNIT_TEST(AlmostEqual_Smoke)
{
  double const small = 1e-18;
  double const eps = 1e-10;

  TEST(base::AlmostEqualAbs(0.0, 0.0 + small, eps), ());
  TEST(!base::AlmostEqualRel(0.0, 0.0 + small, eps), ());
  TEST(!base::AlmostEqualULPs(0.0, 0.0 + small), ());

  TEST(base::AlmostEqualAbs(1.0, 1.0 + small, eps), ());
  TEST(base::AlmostEqualRel(1.0, 1.0 + small, eps), ());
  TEST(base::AlmostEqualULPs(1.0, 1.0 + small), ());

  TEST(base::AlmostEqualRel(123456789.0, 123456780.0, 1e-7), ());
}

UNIT_TEST(AlmostEqualULPs_MaxULPs_double)
{
  TestMaxULPs<double>();
}

UNIT_TEST(AlmostEqualULPs_MaxULPs_float)
{
  TestMaxULPs<float>();
}

UNIT_TEST(TEST_FLOAT_DOUBLE_EQUAL_macros)
{
  float const fx = 3;
  float const fy = NextFloat(NextFloat(NextFloat(fx)));
  TEST_ALMOST_EQUAL_ULPS(fx, fy, ());
  TEST_NOT_ALMOST_EQUAL_ULPS(fx, 2.0f, ());

  double const dx = 3;
  double const dy = NextFloat(NextFloat(NextFloat(dx)));
  TEST_ALMOST_EQUAL_ULPS(dx, dy, ());
  TEST_NOT_ALMOST_EQUAL_ULPS(dx, 2.0, ());
}

UNIT_TEST(GCD)
{
  TEST_EQUAL(base::GCD(6, 3), 3, ());
  TEST_EQUAL(base::GCD(14, 7), 7, ());
  TEST_EQUAL(base::GCD(100, 100), 100, ());
  TEST_EQUAL(base::GCD(7, 3), 1, ());
  TEST_EQUAL(base::GCD(8, 3), 1, ());
  TEST_EQUAL(base::GCD(9, 3), 3, ());
}

UNIT_TEST(LCM)
{
  TEST_EQUAL(base::LCM(6, 3), 6, ());
  TEST_EQUAL(base::LCM(14, 7), 14, ());
  TEST_EQUAL(base::LCM(100, 100), 100, ());
  TEST_EQUAL(base::LCM(7, 3), 21, ());
  TEST_EQUAL(base::LCM(8, 3), 24, ());
  TEST_EQUAL(base::LCM(9, 3), 9, ());
}

UNIT_TEST(Sign)
{
  TEST_EQUAL(1, base::Sign(1), ());
  TEST_EQUAL(1, base::Sign(10.4), ());

  TEST_EQUAL(0, base::Sign(0), ());
  TEST_EQUAL(0, base::Sign(0.0), ());

  TEST_EQUAL(-1, base::Sign(-11), ());
  TEST_EQUAL(-1, base::Sign(-10.4), ());
}


#include "../common.hpp"
#include "../program_options.hpp"
#include <crab/analysis/dataflow/assertion_crawler.hpp>

#include <boost/range/iterator_range.hpp>

using namespace std;
using namespace crab::cfg;
using namespace crab::cg;
using namespace crab::cfg_impl;
using namespace crab::domain_impl;

z_cfg_t *prog(variable_factory_t &vfac) {
  /*
     i := 0;
     x := 1;
     y := 0;
     z := 3;
     w := 3;
     while (i < 100) {
       x  := x + y;
       y  := y + 1;
       nd := *;
       z  := z xor nd;
       w  := w xor nd;
       i  := i + 1;
     }
     assert(i >= 100);
   */
  // Definining program variables
  z_var i(vfac["i"], crab::INT_TYPE, 32);
  z_var x(vfac["x"], crab::INT_TYPE, 32);
  z_var y(vfac["y"], crab::INT_TYPE, 32);
  z_var z(vfac["z"], crab::INT_TYPE, 32);
  z_var w(vfac["w"], crab::INT_TYPE, 32);
  z_var nd1(vfac["nd1"], crab::INT_TYPE, 32);
  z_var nd2(vfac["nd2"], crab::INT_TYPE, 32);
  // entry and exit block
  function_decl<z_number, varname_t> decl("main", {}, {});
  z_cfg_t *cfg = new z_cfg_t("entry", "ret", decl);
  // adding blocks
  z_basic_block_t &entry = cfg->insert("entry");
  z_basic_block_t &bb1 = cfg->insert("bb1");
  z_basic_block_t &bb1_t = cfg->insert("bb1_t");
  z_basic_block_t &bb1_f = cfg->insert("bb1_f");
  z_basic_block_t &bb2 = cfg->insert("bb2");
  z_basic_block_t &exit = cfg->insert("exit");
  z_basic_block_t &ret = cfg->insert("ret");
  // adding control flow
  entry >> bb1;
  bb1 >> bb1_t;
  bb1 >> bb1_f;
  bb1_t >> bb2;
  bb2 >> bb1;
  bb1_f >> exit;
  exit >> ret;
  // adding statements
  entry.assign(i, 0);
  entry.assign(x, 1);
  entry.assign(y, 0);
  entry.assign(z, 3);
  entry.assign(w, 3);
  bb1_t.assume(i <= 99);
  bb1_f.assume(i >= 100);
  bb2.callsite("bar",{x,y}, {x,y,z,w});
  bb2.add(i, i, 1);
  exit.assume(x <= y);
  exit.assertion(w >= 0);
  exit.assertion(x >= y);
  exit.assertion(i >= 0);
  return cfg;
}

z_cfg_t *foo_cfg(variable_factory_t &vfac) {
  // Definining program variables
  z_var i1(vfac["i1"], crab::INT_TYPE, 32);
  z_var i2(vfac["i2"], crab::INT_TYPE, 32);
  z_var i3(vfac["i3"], crab::INT_TYPE, 32);
  z_var i4(vfac["i4"], crab::INT_TYPE, 32);
  z_var o1(vfac["o1"], crab::INT_TYPE, 32);
  z_var o2(vfac["o2"], crab::INT_TYPE, 32);

  z_var tmp1(vfac["tmp1"], crab::INT_TYPE, 32);
  z_var tmp2(vfac["tmp2"], crab::INT_TYPE, 32);

  // entry and exit block
   z_cfg_t *cfg = new z_cfg_t("entry", "exit",
			      function_decl<z_number, varname_t>("foo", {i1,i2,i3,i4}, {o1,o2}));
  // adding blocks
  z_basic_block_t &entry = cfg->insert("entry");
  z_basic_block_t &exit = cfg->insert("exit");

  entry >> exit;
  entry.add(tmp1, i1,i2);
  entry.add(tmp2, i3,i4);

  exit.assign(o1, tmp1);
  exit.assign(o2, tmp2);

  return cfg;
}

z_cfg_t *bar_cfg(variable_factory_t &vfac) {
#if 0
  // Definining program variables
  z_var a1(vfac["a1"], crab::INT_TYPE, 32);
  z_var a2(vfac["a2"], crab::INT_TYPE, 32);
  z_var a3(vfac["a3"], crab::INT_TYPE, 32);
  z_var a4(vfac["a4"], crab::INT_TYPE, 32);
  z_var b1(vfac["b1"], crab::INT_TYPE, 32);
  z_var b2(vfac["b2"], crab::INT_TYPE, 32);

  // entry and exit block
   z_cfg_t *cfg = new z_cfg_t("entry", "exit",
			      function_decl<z_number, varname_t>("bar", {a1,a2,a3,a4}, {b1,b2}));
  // adding blocks
  z_basic_block_t &entry = cfg->insert("entry");
  z_basic_block_t &exit = cfg->insert("exit");
  entry >> exit;
  entry.callsite("foo", {b1,b2}, {a1,a2,a3,a4});
  exit.assertion(b1 >= 0);
  exit.assertion(b2 >= 0);
#else
  // Definining program variables
  z_var i1(vfac["i1"], crab::INT_TYPE, 32);
  z_var i2(vfac["i2"], crab::INT_TYPE, 32);
  z_var i3(vfac["i3"], crab::INT_TYPE, 32);
  z_var i4(vfac["i4"], crab::INT_TYPE, 32);
  z_var o1(vfac["o1"], crab::INT_TYPE, 32);
  z_var o2(vfac["o2"], crab::INT_TYPE, 32);

  // entry and exit block
   z_cfg_t *cfg = new z_cfg_t("entry", "exit",
  			      function_decl<z_number, varname_t>("bar", {i1,i2,i3,i4}, {o1,o2}));
  // adding blocks
  z_basic_block_t &entry = cfg->insert("entry");
  z_basic_block_t &exit = cfg->insert("exit");
  entry >> exit;
  entry.callsite("foo", {o1,o2}, {i1,i2,i3,i4});
  exit.assertion(o1 >= 0);
  exit.assertion(o2 >= 0);
#endif

  return cfg;
}

int main(int argc, char **argv) {
  bool stats_enabled = false;
  if (!crab_tests::parse_user_options(argc, argv, stats_enabled)) {
    return 0;
  }
  using callgraph_t = call_graph<z_cfg_ref_t>;
  variable_factory_t vfac;

  z_cfg_t *p1 = prog(vfac);
  crab::outs() << *p1 << "\n";

  z_cfg_t *p2 = foo_cfg(vfac);
  crab::outs() << *p2 << "\n";

  z_cfg_t *p3 = bar_cfg(vfac);
  crab::outs() << *p3 << "\n";

  vector<z_cfg_ref_t> cfgs({*p1, *p2, *p3});
  callgraph_t cg(cfgs);

  using crawler_t = crab::analyzer::inter_assertion_crawler<callgraph_t>;
  crawler_t crawler(cg);
  crawler.run();

  auto print_results = [&crawler](z_cfg_t &cfg) {
    // Print results in DFS to enforce a fixed order
    std::set<crab::cfg_impl::basic_block_label_t> visited;
    std::vector<crab::cfg_impl::basic_block_label_t> worklist;
    worklist.push_back(cfg.entry());
    visited.insert(cfg.entry());
    while (!worklist.empty()) {
      auto cur_label = worklist.back();
      worklist.pop_back();
      auto results = crawler.get_results(cfg, cur_label);
      crab::outs() << crab::basic_block_traits<crab::cfg_impl::z_basic_block_t>::to_string(cur_label)
		   << "=" << results << "\n";
      auto const &cur_node = cfg.get_node(cur_label);
      for (auto const& kid_label :
         boost::make_iterator_range(cur_node.next_blocks())) {
	if (visited.insert(kid_label).second) {
	  worklist.push_back(kid_label);
	}
      }
    }};

  crab::outs() << "Assertion Crawler Analysis for main\n";
  print_results(*p1);
  crab::outs() << "Assertion Crawler Analysis for bar\n";
  print_results(*p3);
  crab::outs() << "Assertion Crawler Analysis for foo\n";
  print_results(*p2);

  //crawler.write(crab::outs());

  delete p1;
  delete p2;
  delete p3;
  return 0;
}


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

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

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

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

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

/*
*
*   Tutorial: QR factorization of matrices from ViennaCL or Boost.uBLAS (qr.cpp and qr.cu are identical, the latter being required for compilation using CUDA nvcc)
*
*/

// activate ublas support in ViennaCL
#define VIENNACL_WITH_UBLAS

//
// include necessary system headers
//
#include <iostream>

//
// ViennaCL includes: We only need the qr-header
//
#include "viennacl/linalg/qr.hpp"

//
// Boost includes
//
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

//
// Testing
//
#include "viennacl/range.hpp"
#include "viennacl/linalg/prod.hpp"
#include "viennacl/matrix.hpp"
#include "viennacl/matrix_proxy.hpp"

//
// A helper function checking the result
//
template <typename MatrixType>
double check(MatrixType const & qr, MatrixType const & ref)
{
  bool do_break = false;
  double max_error = 0;
  for (std::size_t i=0; i<ref.size1(); ++i)
  {
    for (std::size_t j=0; j<ref.size2(); ++j)
    {
      if (qr(i,j) != 0.0 && ref(i,j) != 0.0)
      {
        double rel_err = fabs(qr(i,j) - ref(i,j)) / fabs(ref(i,j) );

        if (rel_err > max_error)
          max_error = rel_err;
      }

      if (qr(i,j) != qr(i,j))
      {
        std::cout << "!!!" << std::endl;
        std::cout << "!!! NaN detected at i=" << i << " and j=" << j << std::endl;
        std::cout << "!!!" << std::endl;
        do_break = true;
        break;
      }
    }
    if (do_break)
      break;
  }
  return max_error;
}

int main (int, const char **)
{
  typedef double               ScalarType;     //feel free to change this to 'double' if supported by your hardware
  typedef boost::numeric::ublas::matrix<ScalarType>        MatrixType;
  typedef boost::numeric::ublas::vector<ScalarType>        VectorType;
  typedef viennacl::matrix<ScalarType, viennacl::column_major>        VCLMatrixType;
  typedef viennacl::vector<ScalarType>        VCLVectorType;

  std::size_t rows = 113;   //number of rows in the matrix
  std::size_t cols = 54;   //number of columns

  //
  // Create matrices with some data
  //
  MatrixType ublas_A(rows, cols);
  MatrixType Q(rows, rows);
  MatrixType R(rows, cols);

  // Some random data with a bit of extra weight on the diagonal
  for (std::size_t i=0; i<rows; ++i)
  {
    for (std::size_t j=0; j<cols; ++j)
    {
      ublas_A(i,j) = -1.0 + (i+1)*(j+1)
                     + ( (rand() % 1000) - 500.0) / 1000.0;

      if (i == j)
        ublas_A(i,j) += 10.0;

      R(i,j) = 0.0;
    }

    for (std::size_t j=0; j<rows; ++j)
      Q(i,j) = 0.0;
  }

  // keep initial input matrix for comparison
  MatrixType ublas_A_backup(ublas_A);

  //
  // Setup the matrix in ViennaCL:
  //
  VCLVectorType dummy(10);
  VCLMatrixType vcl_A(ublas_A.size1(), ublas_A.size2());

  viennacl::copy(ublas_A, vcl_A);

  //
  // Compute QR factorization of A. A is overwritten with Householder vectors. Coefficients are returned and a block size of 3 is used.
  // Note that at the moment the number of columns of A must be divisible by the block size
  //

  std::cout << "--- Boost.uBLAS ---" << std::endl;
  std::vector<ScalarType> ublas_betas = viennacl::linalg::inplace_qr(ublas_A);  //computes the QR factorization

  //
  // A check for the correct result:
  //
  viennacl::linalg::recoverQ(ublas_A, ublas_betas, Q, R);
  MatrixType ublas_QR = prod(Q, R);
  double ublas_error = check(ublas_QR, ublas_A_backup);
  std::cout << "Max rel error (ublas): " << ublas_error << std::endl;

  //
  // QR factorization in ViennaCL using Boost.uBLAS for the panel factorization
  //
  std::cout << "--- Hybrid (default) ---" << std::endl;
  viennacl::copy(ublas_A_backup, vcl_A);
  std::vector<ScalarType> hybrid_betas = viennacl::linalg::inplace_qr(vcl_A);

  //
  // A check for the correct result:
  //
  viennacl::copy(vcl_A, ublas_A);
  Q.clear(); R.clear();
  viennacl::linalg::recoverQ(ublas_A, hybrid_betas, Q, R);
  double hybrid_error = check(ublas_QR, ublas_A_backup);
  std::cout << "Max rel error (hybrid): " << hybrid_error << std::endl;

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

  return EXIT_SUCCESS;
}


#include <boost/test/unit_test.hpp>

#include "Werk/Math/SimpleLinearRegression.hpp"

BOOST_AUTO_TEST_SUITE(SimpleLinearRegressionTest)

BOOST_AUTO_TEST_CASE(TestEmpty)
{
	Werk::SimpleLinearRegression r;
    BOOST_REQUIRE_EQUAL(r.count(), 0);
}

BOOST_AUTO_TEST_CASE(TestBasic)
{
    Werk::SimpleLinearRegression r;

    r.sample(1.0, 4.0);
    r.sample(3.0, 8.0);
    BOOST_REQUIRE_EQUAL(r.count(), 2);
    BOOST_REQUIRE_EQUAL(r.beta(), 2.0);
    BOOST_REQUIRE_EQUAL(r.correlation(), 1.0);

    r.sample(5.0, 12.0);
    BOOST_REQUIRE_EQUAL(r.count(), 3);
    BOOST_REQUIRE_EQUAL(r.correlation(), 1.0);
    BOOST_REQUIRE_CLOSE(r.beta(), 2.0, 0.000000001);
    BOOST_REQUIRE_CLOSE(r.alpha(), 2.0, 0.000000001);

    r.reset();
    BOOST_REQUIRE_EQUAL(r.count(), 0);
}

BOOST_AUTO_TEST_CASE(TestZero)
{
    Werk::SimpleLinearRegression r;

    r.sample(-1.0, 3.0);
    r.sample(0.0, 0.0);
    r.sample(1.0, 3.0);
    BOOST_REQUIRE_EQUAL(r.count(), 3);
    BOOST_REQUIRE_EQUAL(r.correlation(), 0.0);
    BOOST_REQUIRE_EQUAL(r.beta(), 0.0);
    BOOST_REQUIRE_CLOSE(r.alpha(), 2.0, 0.000000001);
}

BOOST_AUTO_TEST_SUITE_END()


#include <random>
#include <Eigen/Dense>
#include "HODLR_Tree.hpp"
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <squaredeMat.hpp> //squared exponential kernel
#include <squaredeP1Mat.hpp>

using std::normal_distribution;
namespace py = pybind11;

Eigen::MatrixXd predict(Eigen::MatrixXd X, Eigen::MatrixXd Y, Eigen::MatrixXd Xtest, Eigen::VectorXd sig_samps, Eigen::VectorXd rho_samps, Eigen::VectorXd tau_samps, double multiplier, int M, double tol, int nsamps) {
  /*

     Sample a draw of the GP function f*|f, sig, rho, tau, x*.
     Assume a squared exponential Gaussian Process based on
     observed function f at new test points x*.
     Temporarily assumes fit method was called with regression = true.

  */

    // Create the standard normal generator
    normal_distribution<double> norm(0, 1);
    std::mt19937 rng;
    auto r_std_normal = bind(norm, rng);

    int Ntest =  Xtest.rows();
    int N =  X.rows();
    int D =  X.cols();

    // HODLR details
    int n_levels = log(N / M) / log(2);
    bool is_sym = true;
    bool is_pd  = true;

    double tau;
    double rho;
    double sig;

    double sigsq;
    double tmpSSR;

    // Allocate space for output
    Eigen::MatrixXd fstarsamp(nsamps, Ntest);
    Eigen::VectorXd KobsNew(N);

    for (int s = 0; s < nsamps; s++) {

      sig = sig_samps(s);
      tau = tau_samps(s);
      rho = rho_samps(s);

      sigsq = pow(sig, 2.0);

      // HODLR approximation
      SQRExponentialP1_Kernel* L  = new SQRExponentialP1_Kernel(X, N, sig, rho, tau);
      HODLR_Tree* T = new HODLR_Tree(n_levels, tol, L); // With noise (i.e. Sigma + I/tau)
      T->assembleTree(is_sym, is_pd);
      T->factorize();

      for (int i = 0; i < Ntest; i++) {
        Eigen::RowVectorXd Xtest_i = Xtest.row(i);

        // Get covariance between X and Xtest
        for (int j = 0; j < N; j++) {
          Eigen::RowVectorXd tmp = X.row(j) - Xtest_i;
          tmpSSR = 0.0;
          for (int d = 0; d < D; d++) {
            tmpSSR = tmpSSR + pow(tmp(d), 2.0);
          }
          KobsNew(j) = sigsq * exp(- tmpSSR * rho);
        }

        // Get variance at Xtest
        double kNewNew = sigsq + 1e-8;

        // Get posterior mean and variance of f* at point xtest(i)
        double sdstar = pow(kNewNew - (multiplier * KobsNew.transpose() * T->solve(tau * KobsNew))(0, 0), 0.5);
        double mustar = (multiplier * KobsNew.transpose() * T->solve(tau * Y))(0, 0);

        auto normal_samp = r_std_normal();

        fstarsamp(s, i) =  sdstar * normal_samp + mustar;
      }
    }
    return fstarsamp;
}

void predict_module(py::module &m) {
    m.def("predict_f", &predict, "predicted samples of f at new X");
}


#include <iostream>
#include <Eigen/Dense>

#include <test_lib.h>

using std::cout;
using std::endl;
using Eigen::MatrixXd;

int main() {
  test_lib::test_lib_func();
  MatrixXd X(2, 2);
  X << 1, 2, 3, 4;
  cout << X << endl;
  return 0;
}


#ifndef CLASSICAL_MDS_HPP
#define CLASSICAL_MDS_HPP

#include <vector>
#include <utility>
#include <cassert>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>

namespace mathtoolbox
{
    // This function computes low-dimensional embedding by using classical multi-dimensional scaling (MDS)
    // - Input:  A distance (dissimilarity) matrix and a target dimension for embedding
    // - Output: A coordinate matrix whose i-th column corresponds to the embedded coordinates of the i-th entry
    extern inline Eigen::MatrixXd ComputeClassicalMds(const Eigen::MatrixXd& D, unsigned dim);

    /////////////////////////////////////////////////////////////////////////////////////////////////////////////

    // This function extract the N-largest eigen values and eigen vectors
    inline void ExtractNLargestEigens(unsigned n, Eigen::VectorXd& S, Eigen::MatrixXd& V)
    {
        // Note: m is the original dimension
        const unsigned m = S.rows();

        // Copy the original matrix
        const Eigen::MatrixXd original_V = V;

        // Sort by eigenvalue
        constexpr double epsilon = 1e-16;
        std::vector<std::pair<double, unsigned>> index_value_pairs(m);
        for (unsigned i = 0; i < m; ++ i) index_value_pairs[i] = std::make_pair(std::max(S(i), epsilon), i);
        std::partial_sort(index_value_pairs.begin(), index_value_pairs.begin() + n, index_value_pairs.end(), std::greater<std::pair<double, unsigned>>());

        // Resize matrices
        S.resize(n);
        V.resize(m, n);

        // Set values
        for (unsigned i = 0; i < n; ++ i)
        {
            S(i)     = index_value_pairs[i].first;
            V.col(i) = original_V.col(index_value_pairs[i].second);
        }
    }

    inline Eigen::MatrixXd ComputeClassicalMds(const Eigen::MatrixXd& D, unsigned dim)
    {
        assert(D.rows() == D.cols());
        assert(D.rows() >= dim);
        const unsigned n = D.rows();
        const Eigen::MatrixXd H = Eigen::MatrixXd::Identity(n, n) - (1.0 / static_cast<double>(n)) * Eigen::VectorXd::Ones(n) * Eigen::VectorXd::Ones(n).transpose();
        const Eigen::MatrixXd K = - 0.5 * H * D.cwiseAbs2() * H;
        const Eigen::EigenSolver<Eigen::MatrixXd> solver(K);
        Eigen::VectorXd S = solver.eigenvalues().real();
        Eigen::MatrixXd V = solver.eigenvectors().real();
        ExtractNLargestEigens(dim, S, V);
        const Eigen::MatrixXd X = Eigen::DiagonalMatrix<double, Eigen::Dynamic>(S.cwiseSqrt()) * V.transpose();
        return X;
    }
}

#endif // CLASSICAL_MDS_HPP


#ifndef TBODEINT_HPP_
#define TBODEINT_HPP_

/* Boost libs/numeric/odeint/examples/solar_system.cpp
 Copyright 2010-2012 Karsten Ahnert
 Copyright 2011 Mario Mulansky
 Solar system example for Hamiltonian stepper
 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)
 */

#include <iostream>
#include <array.hpp>

#include <boost/numeric/odeint.hpp>

//#include "point_type.hpp"
#include "body.hpp"
#include "threebodies.hpp"
#include <glm/vec3.hpp>
class Body;

class TB_odeint {
 public:
  static void updateSystem(ThreeBodies system, float deltaTime);
 private:
  friend OpenGLWindow;

  //Define universal gravitation constant
  double G = 6.67408e-11; //N-m2/kg2

  const size_t n = 3;

  typedef glm::vec3 point_type;
  typedef boost::array< point_type , n > container_type;
  typedef boost::array< double , n > mass_type;
    //]

  //[ coordinate_function
  //const double gravitational_constant = 2.95912208286e-4;
  const double gravitational_constant = 1.0;

  struct tb_system_coor{
    const mass_type &m_masses;

    tb_system_coor( const mass_type &masses ) : m_masses( masses ) { }

    void operator()( const container_type &p , container_type &dqdt ) const
    {
        for( size_t i=0 ; i<n ; ++i )
            dqdt[i] = p[i] / m_masses[i];
    }
  };
    //]

    //[ momentum_function
  struct tb_system_momentum{
    const mass_type &m_masses;

    tb_system_momentum( const mass_type &masses ) : m_masses( masses ) { }

    void operator()( const container_type &q , container_type &dpdt ) const
    {
        const size_t n = q.size();
        for( size_t i=0 ; i<n ; ++i )
        {
            dpdt[i] = 0.0;
            for( size_t j=0 ; j<i ; ++j )
            {
                point_type diff = q[j] - q[i];
                double d = abs( diff );
                diff *= ( gravitational_constant * m_masses[i] * m_masses[j] / d / d / d );
                dpdt[i] += diff;
                dpdt[j] -= diff;
            }
        }
    }
  };
    //]

  //[ streaming_observer
  struct streaming_observer{
    std::ostream& m_out;

    streaming_observer( std::ostream &out ) : m_out( out ) { }

    template< class State >
    void operator()( const State &x , double t ) const
    {
        container_type &q = x.first;
        m_out << t;
        for( size_t i=0 ; i<q.size() ; ++i ) m_out << "\t" << q[i];
        m_out << "\n";
    }
  };
//]

  point_type center_of_mass( const container_type &x , const mass_type &m );
  double energy( const container_type &q , const container_type &p , const mass_type &masses );
};

#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <utility>
#include <cmath>
// #include <boost/test/minimal.hpp>

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>
#include <boost/numeric/itl/smoother/gauss_seidel.hpp>

using namespace std;

int main(int, char**)
{
    using namespace mtl;

    const int s= 10;
    typedef mtl::dense_vector<double> Vector;
    typedef mtl::compressed2D<double> Matrix;
    Vector       x(s*s, 8), b(s*s);
    Matrix   A;
    laplacian_setup(A, s, s);
    if (s < 10)
      std::cout<< "x= " << x << "\n";

    b= A*x;
    x= 0;

    itl::gauss_seidel<Matrix> gs(A);
    for (int i =0 ; i< 30; i++)
        gs(x, b);

    if (s < 10) {
      std::cout<< "x=" << x << "\n";
      Vector tmp(b-A*x);
      assert(two_norm(tmp) < 1.0e-4);
    }

    return 0;
}


#include <Eigen/Dense>
#include <iostream>

int main(){
	Eigen::MatrixXd X(2,2);
	X << 2,1,-1, 3;
	std::cout << X << std::endl;
	Eigen::VectorXd y;
	y=Eigen::MatrixXd::Map(X.data(),4,1);
	std::cout << y << std::endl;
	return 0;
}


#include "problemes.h"
#include "chiffres.h"
#include "utilitaires.h"

#include <boost/rational.hpp>

typedef unsigned long long nombre;
typedef std::vector<nombre> vecteur;
typedef boost::rational<nombre> fraction;

namespace {
    void test(const std::vector<fraction> &n, std::set<nombre> &p) {
        if (n.size() == 1) {
            if (n.front().numerator() % n.front().denominator() == 0)
                p.insert(n.front().numerator() / n.front().denominator());
            return;
        }
        for (nombre i = 0; i < n.size(); ++i)
            for (nombre j = i + 1; j < n.size(); ++j) {
                fraction a = n[i];
                fraction b = n[j];
                auto m = n;
                m.erase(std::next(m.begin(), j));
                m[i] = a + b;
                test(m, p);
                m[i] = a * b;
                test(m, p);
                if (a >= b) {
                    m[i] = a - b;
                    test(m, p);
                }
                if (a <= b) {
                    m[i] = b - a;
                    test(m, p);
                }
                if (a != nullptr) {
                    m[i] = b / a;
                    test(m, p);
                }
                if (b != nullptr) {
                    m[i] = a / b;
                    test(m, p);
                }
            }
    }

}

ENREGISTRER_PROBLEME(93, "Arithmetic expressions") {
    // By using each of the digits from the set, {1, 2, 3, 4}, exactly once, and making use of the four arithmetic
    // operations (+, −, *, /) and brackets/parentheses, it is possible to form different positive integer targets.
    //
    // For example,
    //
    //      8 = (4 * (1 + 3)) / 2
    //      14 = 4 * (3 + 1 / 2)
    //      19 = 4 * (2 + 3) − 1
    //      36 = 3 * 4 * (2 + 1)
    //
    // Note that concatenations of the digits, like 12 + 34, are not allowed.
    //
    // Using the set, {1, 2, 3, 4}, it is possible to obtain thirty-one different target numbers of which 36 is the
    // maximum, and each of the numbers 1 to 28 can be obtained before encountering the first non-expressible number.
    //
    // Find the set of four distinct digits, a < b &lt c < d, for which the longest set of consecutive positive integers,
    // 1 to n, can be obtained, giving your answer as a string: abcd.
    vecteur iteration;
    for (nombre n = 1; n < 10000; ++n) iteration.push_back(n);
    vecteur resultat;
    nombre maximum = 0;
    for (nombre a = 0; a < 10; ++a)
        for (nombre b = a + 1; b < 10; ++b)
            for (nombre c = b + 1; c < 10; ++c)
                for (nombre d = c + 1; d < 10; ++d) {
                    std::vector<fraction> v = {a, b, c, d};
                    std::set<nombre> e;
                    test(v, e);

                    vecteur difference;
                    std::set_difference(iteration.begin(), iteration.end(), e.begin(), e.end(),
                                        std::inserter(difference, difference.begin()));
                    if (difference.front() > maximum) {
                        maximum = difference.front();
                        resultat = {a, b, c, d};
                    }
                }

    return std::to_string(chiffres::conversion_nombre<nombre>(resultat.begin(), resultat.end()));
}


#include <boost/container/static_vector.hpp>
#include <gtest/gtest.h>
#include <glm/glm.hpp>

#include <optional>

#include "../src/tridexel.h"
#include "../src/IO.h"

namespace {
	struct Sphere {
		glm::vec3 center;
		float radius;
	};

	auto solveQuadraticEquation(double a, double b, double c) -> boost::container::static_vector<double, 2> {
		const double discriminat = std::pow(b, 2) - 4 * a * c;
		if (discriminat < 0)
			return {};

		if (discriminat == 0)
			return { -b / 2 * a };

		const auto x1 = (-b - std::sqrt(discriminat)) / 2 * a;
		const auto x2 = (-b + std::sqrt(discriminat)) / 2 * a;
		return { x1, x2 };
	}

	void extractSphere(int resolution) {
		const auto sphere = Sphere{glm::vec3{0, 0, 0}, 5};
		const auto box = BoundingBox{sphere.center - sphere.radius, sphere.center + sphere.radius};
		const auto triangles = tridexel(box, resolution, [&](Ray ray, HitCallback hc) {
			// from https://www.scratchapixel.com/lessons/3d-basic-rendering/minimal-ray-tracer-rendering-simple-shapes/ray-sphere-intersection
			// solve quadratic equation
			const auto L = ray.origin - sphere.center;
			const auto a = 1.0;
			const auto b = 2 * glm::dot(ray.direction, L);
			const auto c = glm::dot(L, L) - std::pow(sphere.radius, 2);
			const auto solutions = solveQuadraticEquation(a, b, c);
			for (const auto& t : solutions) {
				const auto point = ray.origin + (float)t * ray.direction;
				const auto normal = glm::normalize(point - sphere.center);
				hc(glm::dot(ray.origin, ray.direction) + (float)t, normal);
			}
		});
		saveTriangles("sphere" + std::to_string(resolution) + ".stl", triangles);
	}
}

TEST(sphere, extract5) {
	extractSphere(5);
}

TEST(sphere, extract10) {
	extractSphere(10);
}

TEST(sphere, extract100) {
	extractSphere(100);
}

TEST(sphere, extract200) {
	extractSphere(200);
}


// Testing PPR
#include "pauli_product.cpp"
#include "decode.cpp"
#include <armadillo>
#include <chrono>
#include <iostream>
#include <stdlib.h>

using namespace std;
using namespace arma;

// Checking if the optimized overlap function is working properly.
int main()
{
  int n = 256;
  int n_itt = 100;
  unsigned int x, z;

  cx_vec psi, psi_cpy;
  psi.randn(n);
  psi = normalise(psi, 2);

  double a1, a2, a3, theta;

  for (int x=0;x<n;x++)
    {
      for (int z=0;z<n;z++)
	{
	  a1 = coeff_a1(0, psi);
	  a2 = coeff_a2(0, x, psi);
	  a3 = coeff_a3(0, x, z, psi);
	  theta = optimized_theta(0, x, z, psi);
	  apply_ppr(x, z, theta, psi);
	  double my_overlap = optimal_overlap(0, x, z, psi);
	  double actual_overlap = coeff_a1(0, psi);
	  apply_ppr(x, z, -theta, psi);

	  if (abs(actual_overlap-my_overlap)>0.00001)
	    {
	      cout << psi << endl;
	      cout << "x, z=" << x << ", " << z << endl;
	      cout << "a1=" << a1 << endl;
	      cout << "a2=" << a2 << endl;
	      cout << "a3=" << a3 << endl;
	      cout << "theta_opt=" << theta << endl;
	      cout << "optimal_overlap=" << my_overlap << endl;
	      cout << "actual overlap after the update: " << actual_overlap << endl;
	    }
	}
    }

}


#include <bits/stdc++.h>
#include <armadillo>
#include "logistic_regression.hpp"

using namespace std;
//constructor to initialize number of dependent variables
LogisticRegression::LogisticRegression(int num){
    number_of_variables = num;
    weights = arma::mat(number_of_variables + 1, 1);
    weights.randu();
}

arma::mat LogisticRegression::predict(arma::mat X_predict){
    arma::mat x = X_predict;
    x.insert_cols(0, arma::ones<arma::mat>(X_predict.n_rows, 1));
    arma::mat regpred = x * weights;

    for(int i=0;i<X_predict.n_rows;i++)
    {
        regpred(i,0)=(1/(1+exp(-regpred(i,0))));
    }
    return regpred;
}

arma::mat LogisticRegression::getMatrix() {
	return X_mat;
}

void LogisticRegression::train(arma::mat X_train, arma::mat y_train, float alpha, int epochs){
        arma::mat x = X_train;
        x.insert_cols(0, arma::ones<arma::mat>(X_train.n_rows, 1));
        for(int i=1;i<=epochs;i++)
        {
             //updating weights using gradient descent method
            arma::mat delta=(x.t() * (predict(X_train)-y_train));
            weights -= ((alpha * delta)/X_train.n_rows);
            cout<<"Epoch completed = "<<i<<weights<< " " << endl;
        }
}


/*
(c) 2019 M. Werner - Part of the GIS++ tutorial
- https://www.martinwerner.de/teaching/spatial-cpp
- https://github.com/mwernerds/spatial-cpp

Program: Points
Compile: g++ -I $(BOOST_DIR) -Wall -std=c++11  -o 01_sphere 01_sphere.cpp
*/

#include<iostream>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

using namespace boost::geometry;

typedef boost::geometry::model::point
    <
        double, 2, boost::geometry::cs::spherical_equatorial<boost::geometry::degree>
    > spherical_point;

typedef boost::geometry::model::point
    <
        double, 3, boost::geometry::cs::cartesian
    > cartesian_point;

typedef boost::geometry::model::point
    <
        double, 2, boost::geometry::cs::geographic<boost::geometry::degree>
    > geographic_point;

spherical_point amsterdam(4.90, 52.37);
spherical_point paris(2.35, 48.86);

double const earth_radius = 6371; // now km

std::ostream &operator<< (std::ostream &os, const cartesian_point &p)
{
    os << "(" << get<0>(p) << ";" << get<1>(p) << ";" << get<2>(p)<< ")";
    return os;
}
std::ostream &operator<< (std::ostream &os, const geographic_point &p)
{
    os << "(" << get<0>(p) << ";" << get<1>(p) << ")";
    return os;
}

int main(int argc, char **argv)
{
    std::cout << "Distance in miles: " << distance(amsterdam, paris) * earth_radius << std::endl;
    cartesian_point paris3d, amsterdam3d;
    transform(paris, paris3d);
    transform(amsterdam, amsterdam3d);

    std::cout << "Paris 3D (on unit sphere)" << paris3d << std::endl;
    std::cout << "Amsterdam 3D (on unit sphere)" << amsterdam3d << std::endl;
    std::cout << "Distance (drill hole): " << distance(paris3d,amsterdam3d)*earth_radius << " miles" << std::endl;

    // Let us test some intuitive things like north to south pole

    // transformer: a functional trick
    auto tf = [](const spherical_point &p)->cartesian_point { cartesian_point ret; transform(p,ret); return ret;};

    std::cout << "North Pole: " << tf(spherical_point(0,90)) << std::endl;
    std::cout << "South Pole: " << tf(spherical_point(0,-90)) << std::endl;
    std::cout << "Distance: " << distance(tf(spherical_point(0,90)), tf(spherical_point(0,-90)))*earth_radius << std::endl;

    // an "external" cast without recalculating (which is not supported!)
    auto cast = [](const spherical_point &p) {return geographic_point(get<0>(p),get<1>(p));};
    std::cout << "Amsterdam (geo)" << cast(amsterdam) << std::endl;
    std::cout << "Paris (geo)" << cast(paris) << std::endl;
    // Geodesic distance
    std::cout << distance(cast(amsterdam),cast(paris)) << std::endl;

    return 0;
}


#ifndef DISTRIBUTION_HPP
#define DISTRIBUTION_HPP

#include <math.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <iostream>
#include <memory>
#include <random>
#include <vector>

namespace ds {
enum DIRECTION { INCREASE, DECREASE };
class DrProcess {
 public:
  int totallClass;
  Eigen::VectorXd params;
  int sum;
  double alpha;  // para for  new class;

  DrProcess(double alpha) : totallClass(0), alpha(alpha), sum(0) {}

  double calProb(int classNum) {
    if (classNum > totallClass)
      return 0.0;
    else if (classNum == totallClass)
      return alpha / (sum + alpha);
    else
      return params(classNum) / (sum + alpha);
  }
  bool update(int classNum, DIRECTION drc) {
    if (classNum >= totallClass) {
      return false;
    }
    if (drc == INCREASE) {
      params(classNum)++;
      sum++;
      return true;
    } else {
      params(classNum)--;
      sum--;
      if (params(classNum) < 0)
        params(classNum) = 0;
      if (sum < 0)
        sum = 0;
      return true;
    }
  }
  bool newClass() {
    totallClass++;
    params.resize(totallClass);
    params(totallClass - 1) = 1.0;
    sum++;
  }
};
class DirDS {
 private:
  int totallClass;  // num of categories;
  Eigen::VectorXd params;
  Eigen::VectorXd samples;
  std::gamma_distribution<double> gamma;
  std::default_random_engine generator;

 public:
  explicit DirDS(int totallClass)
      : totallClass(totallClass),
        params(Eigen::VectorXd::Zero(totallClass)),
        samples(Eigen::VectorXd::Zero(totallClass)),
        gamma(1.0, 1.0),
        generator() {}

  bool update(int classNum) {
    if (classNum >= totallClass) {
      std::cerr << "more than totallClass\n";
      return false;
    } else {
      ++params(classNum);
      return true;
    }
  }

  Eigen::VectorXd sampling() {
    samples.resize(totallClass);
    for (int i = 0; i < totallClass; i++) {
      gamma.param(std::gamma_distribution<double>::param_type(params(i), 1.0));
      samples(i) = gamma(generator);
    }
    double sum = samples.sum();
    samples = samples / sum;
    return samples;
  }
};

class CatDS {
 private:
  int totallClass;
  Eigen::VectorXd params;
  DirDS dir;

 public:
  CatDS(int totallClass)
      : totallClass(totallClass),
        params(Eigen::VectorXd::Zero(totallClass)),
        dir(totallClass) {}

  bool update(int classNum) {
    dir.update(classNum);
    params = dir.sampling();
    return true;
  }
  double calProb(int classNum) {
    if (classNum >= totallClass)
      return 0.0;
    else {
      return params(classNum);
    }
  }
  void maxPro(int& classNum, double& pro) {
    Eigen::VectorXd::Index maxClass;
    pro = params.maxCoeff(&maxClass);
    classNum = int(maxClass);
    return;
  }
};
};  // namespace ds

#endif  // DISTRIBUTION_HPP


#pragma once

#include <array>
#include <cstdint>
#include <cmath>
#include <crest/geometry/vertex.hpp>
#include <crest/quadrature/rules.hpp>

#include <Eigen/Dense>

namespace crest
{
    template <typename Scalar>
    class ReferenceTriangleTransform
    {
    public:
        ReferenceTriangleTransform(Vertex<Scalar> v0, Vertex<Scalar> v1, Vertex<Scalar> v2);

        Vertex<Scalar> transform_from_reference(const Scalar & x, const Scalar & y) const;
        Scalar absolute_determinant() const { return _absdet; }

        Eigen::Matrix<Scalar, 2, 2> jacobian() const;

    private:
        Vertex<Scalar> _v0;
        Vertex<Scalar> _v1;
        Vertex<Scalar> _v2;
        Scalar _absdet;
    };

    template <typename S>
    ReferenceTriangleTransform<S>::ReferenceTriangleTransform(Vertex<S> v0,
                                                              Vertex<S> v1,
                                                              Vertex<S> v2)
            : _v0(v0), _v1(v1), _v2(v2)
    {
        _absdet = S(1.0 / 4.0) * std::abs(_v1.x * _v2.y - _v1.y * _v2.x);
    }

    /**
     *The reference triangle the quadrature rules are defined on is given by the vertices
     *(-1, 1), (-1, -1), (1, -1),
     *so for a triangle with corners A, B, C, we have that each coordinate x in R^2 is given by the relation
     *
     * x = 1/2 (A + B) + 1/2 * (A - C) z_1 + 1/2 * (B - C) z_2
     *
     * for a reference coordinate z = (z_1, z_2) in R^2. This implies the following mapping:
     *  A <-> ( 1, -1)
     *  B <-> (-1,  1)
     *  C <-> (-1, -1)
     */
    template <typename S>
    Vertex<S> ReferenceTriangleTransform<S>::transform_from_reference(const S & x, const S & y) const
    {

        return S(0.5) * (_v0 + x * _v1 + y * _v2);
    }

    template <typename Scalar>
    Eigen::Matrix<Scalar, 2, 2> ReferenceTriangleTransform<Scalar>::jacobian() const
    {
        Eigen::Matrix<Scalar, 2, 2> J;
        J(0, 0) = Scalar(0.5) * _v1.x;
        J(0, 1) = Scalar(0.5) * _v2.x;
        J(1, 0) = Scalar(0.5) * _v1.y;
        J(1, 1) = Scalar(0.5) * _v2.y;
        return J;
    };

    /**
     *
     * Returns an instance of a ReferenceTriangleTransform<Scalar>, which transforms points from
     * the reference triangle used by triquad to points in the triangle defined by the vertices (a, b, c).
     *
     * @param a
     * @param b
     * @param c
     * @return An instance of ReferenceTriangleTransform<Scalar>.
     */
    template <typename Scalar>
    ReferenceTriangleTransform<Scalar> triquad_transform(const Vertex<Scalar> & a,
                                                         const Vertex<Scalar> & b,
                                                         const Vertex<Scalar> & c)
    {
        const auto v0 = a + b;
        const auto v1 = a - c;
        const auto v2 = b - c;

        return ReferenceTriangleTransform<Scalar>(v0, v1, v2);
    };

    /**
     * Computes the integral of the function f(x, y) -> R on the reference triangled defined by the vertices
     * (-1, 1), (-1, -1), (1, -1).
     * @param f
     * @return
     */
    template <unsigned int Strength, typename Scalar, typename Function2D>
    constexpr inline Scalar triquad_ref(const Function2D & f)
    {
        using quadrature = ::crest::quadrature_rules::quadrature<Scalar, Strength>;
        constexpr auto quad = quadrature();

        Scalar result = static_cast<Scalar>(0.0);
        for (unsigned int i = 0; i < quadrature::num_points; ++i)
        {
            result += quad.w[i] * f(quad.x[i], quad.y[i]);
        }

        return result;
    };

    /**
     * Computes the integral of the function f over the triangle determined by the vertices a, b and c,
     * using Gauss quadrature rules of the given Strength.
     *
     * The Strength of the quadrature rule determines the highest degree polynomial that the quadrature rule
     * is able to integrate *exactly*. For example, a Strength of 4 indicates that triquad computes the exact
     * integral of any polynomial function of *total degree* 4.
     * @param f A callable function f(x, y) that operates on real numbers.
     * @param a
     * @param b
     * @param c
     * @return
     */
    template <unsigned int Strength, typename Scalar, typename Function2D>
    constexpr inline Scalar triquad(
            const Function2D & f,
            const Vertex<Scalar> & a,
            const Vertex<Scalar> & b,
            const Vertex<Scalar> & c)
    {
        const auto transform = triquad_transform(a, b, c);
        const auto f_transformed = [&f, &transform] (auto x, auto y)
        {
            const auto coords = transform.transform_from_reference(x, y);
            return f(coords.x, coords.y);
        };
        return transform.absolute_determinant() * triquad_ref<Strength, Scalar>(f_transformed);
    };
}


#include <iostream>
#include "problem_0001.h"

using namespace euler;

#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE Project_Euler_Problem_0001
#include <boost/test/included/unit_test.hpp>

BOOST_AUTO_TEST_CASE(sum_multiples_of_3_or_5_below_1000)
{
    const std::vector<int> multof   = {3,5};
    constexpr int below             = 1000;
    int res = sum_mults(multof, below);
    BOOST_TEST( 233168 == res );
}


////////////////////////////////////////////////////////////////////////////////////////////////////
//                               This file is part of CosmoScout VR                               //
//      and may be used under the terms of the MIT license. See the LICENSE file for details.     //
//                        Copyright: (c) 2019 German Aerospace Center (DLR)                       //
////////////////////////////////////////////////////////////////////////////////////////////////////

#ifndef CS_UTILS_CONVERSIONS_HPP
#define CS_UTILS_CONVERSIONS_HPP

#include "cs_utils_export.hpp"

#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

#include <glm/glm.hpp>
#include <glm/gtc/quaternion.hpp>

/// This namespace contains utility functions for converting numbers between different units of
/// measuring. Most of the coordinate system conversion methods are based on the code from the
/// excellent book "3D Engine Design for Virtual Globes" by Patrick Cozzi and Kevin Ring
/// (http://virtualglobebook.com/).
namespace cs::utils::convert {

template <typename T>
T lightyearsToMeters(T lightyears) {
  return lightyears * 9460730472580800.0;
}

template <typename T>
T metersToLightyears(T meters) {
  return meters / 9460730472580800.0;
}

/// Converts AU to meters. One AU is equivalent to the average distance between the Earth and the
///// Sun.
template <typename T>
T astronomicalUnitsToMeters(T astronomicalUnits) {
  return astronomicalUnits * 149597870700.0;
}

/// Converts meters to AU. One AU is equivalent to the average distance between the Earth and the
/// Sun.
template <typename T>
T metersToAstronomicalUnits(T meters) {
  return meters / 149597870700.0;
}

template <typename T>
T toRadians(T degrees) {
  return static_cast<T>(degrees * glm::pi<double>() / 180.0);
}

template <typename T>
T toDegrees(T radians) {
  return static_cast<T>(radians * 180.0 / glm::pi<double>());
}

/// Projects an arbitrary cartesian point to the surface of an origin-centered ellipsoid with the
/// given radii. The projection happens towards the center of the ellipsoid.
CS_UTILS_EXPORT glm::dvec3 scaleToGeocentricSurface(
    glm::dvec3 const& cartesian, glm::dvec3 const& radii);

/// Projects an arbitrary cartesian point to the surface of an origin-centered ellipsoid with the
/// given radii. The projection happens along the surface normal of the ellipsoid. This is
/// potentially a rather expensive operation, as it involes an iterative search for the correct
/// surface normal.
CS_UTILS_EXPORT glm::dvec3 scaleToGeodeticSurface(
    glm::dvec3 const& cartesian, glm::dvec3 const& radii);

/// Computes longitude and geodetic latitude for a given surface point on an origin-centered
/// ellipsoid with the given radii.
CS_UTILS_EXPORT glm::dvec2 surfaceToLngLat(glm::dvec3 const& cartesian, glm::dvec3 const& radii);

/// Computes longitude and geodetic latitude for an arbitrary cartesian point relative to a
/// origin-centered ellipsoid with the given radii. This is potentially a rather expensive
/// operation, as it involes scaleToGeodeticSurface().
CS_UTILS_EXPORT glm::dvec2 cartesianToLngLat(glm::dvec3 const& cartesian, glm::dvec3 const& radii);

/// Same as above, but returns the distance to the surface of the ellipsoid as well.
CS_UTILS_EXPORT glm::dvec3 cartesianToLngLatHeight(
    glm::dvec3 const& cartesian, glm::dvec3 const& radii);

/// Transforms longitude and geodetic latitude and elevation height to cartesian (x,y,z)
/// coordinates for an origin-centered ellipsoid with the given radii. Height is an
/// offset along the geodetic normal of the ellipsoid at lngLat.
CS_UTILS_EXPORT glm::dvec3 toCartesian(
    glm::dvec2 const& lngLat, glm::dvec3 const& radii, double height = 0.0);

/// Returns the geodetic normal vector with unit length at geodetic coordinates (lng, lat) lngLat.
CS_UTILS_EXPORT glm::dvec3 lngLatToNormal(glm::dvec2 const& lngLat);

/// Returns the geodetic normal vector with unit length for a given point on the surface of an
/// origin-centered ellipsoid with the given radii.
CS_UTILS_EXPORT glm::dvec3 surfaceToNormal(glm::dvec3 const& cartesian, glm::dvec3 const& radii);

/// Returns the geodetic normal vector with unit length for an arbitrary cartesian point relative to
/// an origin-centered ellipsoid with the given radii. This is potentially a rather expensive
/// operation, as it involes scaleToGeodeticSurface().
CS_UTILS_EXPORT glm::dvec3 cartesianToNormal(glm::dvec3 const& cartesian, glm::dvec3 const& radii);

/// Time in CosmoScout VR is passed around in different formats.
/// * Strings usually store time in the ISO format YYYY-MM-DDTHH:MM:SS.fffZ. The 'Z' suffix is not
///   really required on the C++ side, as time strings are always considered to be in UTC. This
///   format is also directly convertible to JavaScript Dates, here however the 'Z' is required!
///   Else the Date object will be in your local time zone. So it's a good practice to always append
///   the 'Z'.
/// * boost::posix_time::ptime is used for conversions and is also always in UTC.
/// * SPICE time is stored in doubles representing Barycentric Dynamical Time (TDB, seconds since
///   2000-01-01 12:00:00). Note that this is not the same as UTC seconds since 2000-01-01 12:00:00
///   because TDB considers leap seconds. The conversion methods below take this into account.
namespace time {

/// Converts boost::posix_time::ptime to spice time, which is defined by the Barycentric Dynamical
/// Time. Be aware, that SPICE kernels with leap seconds have to be loaded for this method to work.
/// This means, SolarSystem::init() must have been called before.
CS_UTILS_EXPORT double toSpice(boost::posix_time::ptime const& tIn);

/// Converts a time string to spice time, which is defined by the Barycentric Dynamical Time. The
/// string can be in the format YYYY-MM-DD HH:MM:SS.fff, YYYY-MM-DDTHH:MM:SS.fff, or
/// YYYY-MM-DDTHH:MM:SS.fffZ and is always interpreted as UTC. Be aware, that SPICE kernels with
/// leap seconds have to be loaded for this method to work. This means, SolarSystem::init() must
/// have been called before.
CS_UTILS_EXPORT double toSpice(std::string const& tIn);

/// Converts a time string to boost::posix_time time. The string can be in the format
/// YYYY-MM-DD HH:MM:SS.fff, YYYY-MM-DDTHH:MM:SS.fff, or YYYY-MM-DDTHH:MM:SS.fffZ and is always
/// interpreted as UTC.
CS_UTILS_EXPORT boost::posix_time::ptime toPosix(std::string const& tIn);

/// Converts a Barycentric Dynamical Time to boost::posix_time::ptime. Be aware, that SPICE kernels
/// with leap seconds have to be loaded for this method to work. This means, SolarSystem::init()
/// must have been called before.
CS_UTILS_EXPORT boost::posix_time::ptime toPosix(double tIn);

/// Converts a Barycentric Dynamical Time time to a time string in the format
/// YYYY-MM-DDTHH:MM:SS.fffZ. Be aware, that SPICE kernels with leap seconds have to be loaded for
/// this method to work. This means, SolarSystem::init() must have been called before.
CS_UTILS_EXPORT std::string toString(double tIn);

/// Converts a boost::posix_time::ptime time to a time string in the format
/// YYYY-MM-DDTHH:MM:SS.fffZ.
CS_UTILS_EXPORT std::string toString(boost::posix_time::ptime const& tIn);

} // namespace time

} // namespace cs::utils::convert

#endif // CS_UTILS_CONVERSIONS_HPP


#include <Eigen/Dense>
#include <ancse/cfl_condition.hpp>
#include <gtest/gtest.h>

//// ANCSE_CUT_START_TEMPLATE
// This will check the CFL condition for Burgers equation.
void check_cfl_condition(const CFLCondition &cfl_condition,
                         const Grid &grid,
                         double cfl_number) {
    int n_ghost = grid.n_ghost;
    int n_cells = grid.n_cells;

    Eigen::VectorXd u(n_cells);
    for (int i = 0; i < n_cells; ++i) {
        u[i] = -i * i;
    }
    double max_abs_u = (n_cells - n_ghost - 1) * (n_cells - n_ghost - 1);

    for (int i = 0; i < n_ghost; ++i) {
        u[i] = 2.0 * max_abs_u;
        u[n_cells - n_ghost + i] = 2.0 * max_abs_u;
    }

    double dt_cfl_approx = cfl_condition(u);
    double dt_cfl_exact = cfl_number * grid.dx / max_abs_u;

    ASSERT_DOUBLE_EQ(dt_cfl_approx, dt_cfl_exact);
}

TEST(CFLCondition, Example) {
    auto n_ghost = 2;
    auto n_cells = 10 + 2 * n_ghost;
    auto grid = Grid({0.9, 1.0}, n_cells, n_ghost);
    auto model = Model();
    double cfl_number = 0.5;

    auto cfl_condition = StandardCFLCondition(grid, model, cfl_number);
    check_cfl_condition(cfl_condition, grid, cfl_number);
}
//// ANCSE_END_TEMPLATE


#include <PCP/Common/Option.h>
#include <PCP/Common/Log.h>
#include <PCP/Common/Progress.h>
#include <PCP/Common/String.h>

#include <PCP/Geometry/Geometry.h>
#include <PCP/Geometry/PLY.h>

#include <PCP/SpacePartitioning/KdTree.h>

#include <Eigen/Eigenvalues>

using namespace pcp;

int main(int argc, char *argv[])
{
    Option opt(argc, argv);
    const String in_input  = opt.get_string("input",  "i").set_required();
    const String in_output = opt.get_string("output", "o").set_default("output");
    const Scalar in_scale  = opt.get_float( "scale"     ).set_default(0.01);
    const int    in_iter   = opt.get_int(   "iter"      ).set_default(30);
    const int    in_every  = opt.get_int(   "every"     ).set_default(30);

    bool ok = opt.ok();
    if(!ok) return 1;
    info() << opt;

    Geometry g;
    ok = PLY::load(in_input, g);
    if(!ok) return 1;
    PCP_ASSERT(g.has_normals());
    Geometry g2 = g;

    const auto aabb = g.aabb();
    const auto aabb_diag = aabb.diagonal().norm();
    const auto radius = in_scale * aabb_diag;
    info() << "radius = " << radius;

    auto prog = Progress(in_iter);
    const int digits = std::to_string(in_iter).size();

    for(int iter=1; iter<=in_iter; ++iter)
    {
        g.build_kdtree();

        #pragma omp parallel for
        for(int i=0; i<g.size(); ++i)
        {
            Matrix3 C    = Matrix3::Zero();
            Vector3 m    = Vector3::Zero();
            Scalar sum_w = 0;

            for(int j : g.kdtree().range_neighbors(g[i], radius))
            {
                const Vector3 p = g[j] - g[i];

                Scalar w = p.norm() / radius;
                w = (w*w - 1);
                w = w*w;

                C += w * p * p.transpose();
                m += w * p;
                sum_w += w;
            }
            m /= sum_w;
            C = C/sum_w - m * m.transpose();

            Eigen::SelfAdjointEigenSolver<Matrix3> solver(C);
            Vector3 N = solver.eigenvectors().col(0); // vector of least eigenvalue

            // re-orient if necessary (used only for rendering)
            if(N.dot(g.normal(i)) < 0) N *= -1;

            const Vector3 P = g[i] + m;

            // projection on plane {P,N}
            g2.point(i) = g[i] - N.dot(g[i] - P) * N;
            g2.normal(i) = N;
        }
        std::swap(g, g2);

        // save
        if(iter % in_every == 0 || iter == in_iter)
        {
            PLY::save(in_output + "_" + str::to_string(iter,digits) + ".ply", g, false);
        }

        ++prog;
    }

    return 0;
}


// Exercise generating random data

#include "GHFilter.h"

#include <iostream>
#include <vector>
#include <random>
#include <cmath>
#include <Eigen/Dense>
#include <sciplot/sciplot.hpp>

using namespace sciplot;

std::vector<float> randomised_data(std::vector<float>& data, float noise_factor){
    std::random_device rd;
    std::mt19937 gen(rd());

    std::normal_distribution<float> dis(2, 4);
    std::vector<float> randomised;
    for(auto val: data){
        randomised.push_back(val + dis(gen));
    }

    return randomised;
}

std::vector<float> data_gen(float start, float step, int count){
    std::vector<float> data;
    float accel = 9;
    for(int i = 0; i < count; i++){
        data.push_back(start + accel * pow(i, 2));
    }
    return data;
}
int main(){
    // get data
    std::vector<float> data_ = data_gen(10, 0, 20);
    std::vector<float> data = randomised_data(data_, 2);

    GHFilter filter(data, 150 ,0.5, 0.4, 0.04, 1);

    std::vector<float> filtered = filter.get_sensored();

    Vec x = linspace(0.0, data.size(), data.size());

    Plot plt;
    plt.palette("set2");
    plt.legend()
        .atOutsideBottom()
        .displayHorizontal()
        .displayExpandWidthBy(2);
    plt.drawPoints(x, data).label("Measurements");
    plt.drawCurve(x, filtered).label("Filtered Data");
    plt.save("./plots/test_2_accel.pdf");
    plt.save("./plots/test_2_accel.png");
    plt.show();

}


#include <frovedis.hpp>
#include <frovedis/ml/clustering/spectral_embedding.hpp>
#include <boost/program_options.hpp>

using namespace boost;
using namespace frovedis;

int main(int argc, char** argv) {
  use_frovedis use(argc,argv);

  using namespace boost::program_options;

  options_description opt("option");
  opt.add_options()
      ("help,h", "produce help message")
      ("input,i" , value<std::string>(), "input rowmajor matrix data.")
      ("n-components" , value<int>(), "number of eigenvectors to use for the spectral embedding.[default = 2]")
      ("gamma,g", value<double>(), "kernel coefficient for rbf, poly, sigmoid, laplacian and chi2 kernels.[default = 1.0]")
      ("norm-laplacian", value<bool>(), "[default: true]")
      ("precomputed", value<bool>(), "[default: false]")
      ("drop-first", value<bool>(), "[default: false]")
      ("mode", value<int>(), "[default: 1]")
      ("verbose", "set loglevel to DEBUG")
      ("verbose2", "set loglevel to TRACE");

  variables_map argmap;
  store(command_line_parser(argc,argv).options(opt).allow_unregistered().
        run(), argmap);
  notify(argmap);

  int n_comp = 2;
  double gamma = 1.0;
  bool norm_laplacian = true;
  bool precomputed = false;
  bool drop_first = false;
  int mode = 1;
  std::string data_p;

  if(argmap.count("help")){
    std::cerr << opt << std::endl;
    exit(1);
  }
  if(argmap.count("n-components")){
    n_comp = argmap["n-components"].as<int>();
  }
  if(argmap.count("gamma")){
    gamma = argmap["gamma"].as<double>();
  }
  if(argmap.count("norm-laplacian")){
    norm_laplacian = argmap["norm-laplacian"].as<bool>();
  }
  if(argmap.count("precomputed")){
    precomputed = argmap["precomputed"].as<bool>();
  }
  if(argmap.count("drop-first")){
    drop_first = argmap["drop-first"].as<bool>();
  }
  if(argmap.count("mode")){
    mode = argmap["mode"].as<int>();
  }
  if(argmap.count("input")){
    data_p = argmap["input"].as<std::string>();
  }else {
    std::cerr << "input is not specified" << std::endl;
    std::cerr << opt << std::endl;
    exit(1);
  }

  auto mat = make_rowmajor_matrix_load<double>(data_p); //./test_data

  // hyper-parameters
  time_spent embed(INFO);
  embed.lap_start();
  auto model = spectral_embedding<double>(std::move(mat), n_comp, norm_laplacian,
                                          precomputed, drop_first, gamma, mode);
  embed.lap_stop();
  embed.show_lap("embedding time: ");

  std::cout << "\n embedding matrix: \n";
  model.embed_matrix.debug_print();
  return 0;
}


#include "graph.h"

#include <iostream>
#include <vector>

#include <Eigen/Core>
#include <Eigen/LU>

namespace filters::pose_graph {

AbstractFactor::~AbstractFactor() {}

Graph::Graph()
	: _x0(values::Zero(1)), _sol(values::Zero(1)), _sol_cov(hessian::Zero(1, 1)), _factors({}),
	  _factor_counter(0) {}

Graph::~Graph() {
	for (auto f : _factors) {
		delete f;
	}
}

int Graph::add(AbstractFactor* f) {
	f->id = _factor_counter++;
	_factors.push_back(f);
	return f->id;
}

double Graph::eval(const values& x) {
	double sum = 0.0;
	for (auto f : _factors)
		sum += f->eval(x);
	return sum;
}

void Graph::solve(const values& x0, double alpha, int maxiters, double tol) {
	_x0 = x0;
	values x = x0;
	double error = 2 * tol;
	int i = 0;
	int N = x0.size();
	while (error > tol && i < maxiters) {
		values grad = values::Zero(N);
		hessian hess = hessian::Zero(N, N);
		for (auto f : _factors) {
			grad += f->gradient_at(x);
			hess += f->hessian_at(x);
		}
		for (int j = 0; j < N; j++) {
			// Presumably we have no factors affecting this variable
			if (hess(j, j) == 0)
				hess(j, j) = 0.001; // avoid singular matrix
		}
		x -= alpha * (hess.inverse() * grad);
		error = sqrt(grad.transpose() * grad);
		i += 1;
		if (i % 100 == 0)
			std::cout << "Graph::solve Iteration " << i << ": " << error << std::endl;
	}
	_sol = x;

	hessian hess = hessian::Zero(N, N);
	for (auto f : _factors)
		hess += f->hessian_at(_sol);
	for (int j = 0; j < N; j++) {
		if (hess(j, j) == 0)
			hess(j, j) = 0.001; // Again, avoid singular matrix
	}
	_sol_cov = hess.inverse();
}

values Graph::x0() {
	return _x0;
}

values Graph::solution() {
	return _sol;
}

hessian Graph::covariance() {
	return _sol_cov;
}

void Graph::deleteFactor(int id) {
	auto it = _factors.begin();
	while (it != _factors.end()) {
		if ((*it)->id == id) {
			delete *it;
			it = _factors.erase(it);
			return;
		} else {
			++it;
		}
	}
}

void Graph::shiftIndices(int poseSize, int firstPoseIdx) {
	auto it = _factors.begin();
	while (it != _factors.end()) {
		if (!(*it)->shiftIndices(poseSize, firstPoseIdx)) {
			delete *it;
			it = _factors.erase(it);
		} else {
			++it;
		}
	}
}

} // namespace filters::pose_graph


#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  MatrixXd X = MatrixXd::Random(5,5);
MatrixXd A = X + X.transpose();
cout << "Here is a random symmetric 5x5 matrix:" << endl << A << endl << endl;

VectorXd diag(5);
VectorXd subdiag(4);
internal::tridiagonalization_inplace(A, diag, subdiag, true);
cout << "The orthogonal matrix Q is:" << endl << A << endl;
cout << "The diagonal of the tridiagonal matrix T is:" << endl << diag << endl;
cout << "The subdiagonal of the tridiagonal matrix T is:" << endl << subdiag << endl;

  return 0;
}


/*
*Topic: Implementation of Sequential Quadratic Programming in C++
*Library: Eigen, a c++ matrix operation library.
*Author: Muqian, Chen
*Current Date: 31.01.2021
*Log: The function to calculate a quadratic optimal programming constrainted by inequation
*        conditions are totally implemnted. It did take a long time because I am not familiar
*        with lots of implicit mechamism of c++. But I feel so lucky that I really started to
*        program with it. I think that I am moving towards to the aim that I really implement
*        something based on ROS. The next task is to cover the case of equation constraints
*        and add a function which automatically calculates the initial working set. Over.
*/

#include <iostream>
#include <Eigen>
#include <typeinfo>
#include <iomanip>

using namespace std;
using namespace Eigen;
VectorXd eqpCalculation(MatrixXd A, VectorXd b, MatrixXd G,
						VectorXd d, VectorXd theta);
double calculationAlpha(MatrixXd& A_working, VectorXd& b_working,
						MatrixXd& A_deactive,VectorXd& b_deactive,
						VectorXd stepLength, VectorXd theta);

//这里若是加上&就会变成按引用传递了亲，已经用test检验过了。
VectorXd eqpCalculation(MatrixXd A, VectorXd b, MatrixXd G,
	                    VectorXd d, VectorXd theta) {
	VectorXd g, h;
	g = d + G * theta;
	h = A * theta - b;
	Index g_row = g.rows();
	Index h_row = h.rows();

	//catenate the A and G as KKT Matrix
	Index A_row = A.rows();
	Index A_col = A.cols();
	Index G_row = G.rows();

	//first catenate them in horizont direction
	MatrixXd K1(G_row, A_row + G_row);
	K1 << G, A.transpose();
	MatrixXd zeros = MatrixXd::Zero(A_row, A_row);
	MatrixXd K2(A_row, A_col + A_row);
	K2 << A, zeros;
	MatrixXd K(A_row + G_row, A_row + G_row);
	K << K1, K2;

	//then catenate [g;h]
	VectorXd g_h(g_row + h_row, 1);
	g_h << g, h;
	//for debug
	//cout << "[INFO]Current KKT Matrix is:" << endl << K << endl;
	//cout << "[INFO]Current g_h is:" << endl << g_h << endl;

	//calculate KKT-Equation
	VectorXd result(G_row + A_row, 1);
	result = K.inverse() * g_h;

	return result;
	};

//implementation of a function template for removing a specific row from MatrixXf or VectorXf
//After validation, this function could work at situation MatrixXf or VectorXf
template <typename T1, typename T2> void removeRow(T1& a, T2 b) {
	// a: Matrix or Vector which need to remove the row
	// b: the row number of the removed row.

	const int newRow = a.rows() - 1;
	const int newCol = a.cols();

	if (b < a.rows()) {
		a.block(b, 0, newRow - b, a.cols()) =
			a.block(b + 1, 0, newRow - b, a.cols());
	}
	else {
		cout << "[INFO] The row to remove is out of boundary of input !" << endl;
	};
	a.conservativeResize(newRow, newCol);
}

//Implementation of a function template for adding a trivial but dimension-matched row into
//MatrixXf or VectorXf
template <typename T1, typename T2> void addRow(T1& a, T2 b) {
	// a: the target Matrix or Vector
	// b: added row into target
	// column number should be unchanged.

	int newRow = a.rows() + 1;
	int newCol = b.cols();

	a.conservativeResize(newRow, newCol);
	a.block(newRow - 1, 0, 1, newCol) = b;
};

//Implementation of a fucntion template
void setInitial(MatrixXd A, MatrixXd& A_working,  MatrixXd& A_deactive, VectorXd b, VectorXd& b_working,
			    VectorXd& b_deactive, VectorXd  initialTheta, int f) {
	//f: amount of inequations
	VectorXd result(f ,1);
	result = A * initialTheta - b;
	ArrayXd resultArray;
	resultArray = result.array().abs();
	for (int i = 0; i < f; i++) {
		if ( result(i, 0) < 1e-7) {
			//add the matched constraints into working set
			addRow(A_working, A.row(i));
			addRow(b_working, b.row(i));
		}
		else {
			//add the unmatched constraints into deacitve set
			addRow(A_deactive, A.row(i));
			addRow(b_deactive, b.row(i));
		}
	}
}

//Pass by reference could be used here for updating the working set.
double calculationAlpha(MatrixXd& A_working, VectorXd& b_working,
					   MatrixXd& A_deactive, VectorXd& b_deactive, VectorXd stepLength, VectorXd theta, int i) {
	double alpha = -1;
	int deactRow = A_deactive.rows();
	int deactCol = A_deactive.cols();

	VectorXd resultAiP(deactRow, 1);
	resultAiP = A_deactive * stepLength;

	VectorXd resultAiTheta(deactRow, 1);
	resultAiTheta = b_deactive - A_deactive * theta;

	ArrayXd result(deactRow, 1);
	ArrayXd resultUp(deactRow, 1);
	ArrayXd resultDown(deactRow, 1);
	resultUp = resultAiTheta.array();
	resultDown = resultAiP.array();
	//for debug
	//cout << "[INFO]: current resultUp is:" << endl << resultUp << endl;
	//cout << "[INFO]: current resultDown is:" << endl << resultDown << endl;

	for (int i = 0; i < deactRow; i++) {
		if (resultDown(i,0)< 0) {
			result(i, 0) = resultUp(i, 0) / resultDown(i, 0);
		}
		else {
			result(i, 0) = 1000.0;
		}
	}
	//for debug
	//cout << "[INFO]: current result is:" << endl << result << endl;

	Index minRow, minCol;
	double min = 0;
	min = result.matrix().minCoeff(&minRow, &minCol);

	//=============================Attention！！！！======================================
	//这里有个坑，就是c++中除号/出来的结果会直接舍弃小数点后面的数，比如这里min应该是1.429
	//但是只输出1，但是在这个程序中我可以不用管这个问题，因为之后是要拿去跟1做比较的。
	//不对啊，我得管，因为之后alpha若是算出小于一则按照这个规则直接就是零了。。。
	//遭重了，得把所有矩阵都改成double类型。
	//全部都改成也不行，当你使用mat.array()的时候它会自动将类型换掉（或者有别的我不懂的内
	//隐操作），通过实验得出，必须要另外声明ArrayXd变量来储存mat.array()的结果，这样才能在
	//相除操作中得到完美的小数点后面的数。
	//-----31.01.2021
	//ArrayXd result(deactRow, 1);
	//ArrayXd resultUp(deactRow, 1);
	//ArrayXd resultDown(deactRow, 1);
	//resultUp = resultAiTheta.array();
	//resultDown = resultAiP.array();
	//result = resultUp / resultDown;
	//cout << "[INFO]Current result is :" << result << endl;
	//=============================Attention！！！！======================================

	if (min < 1) {
		alpha = min;

		//add the blocking constraint into working set
		addRow(A_working, A_deactive.row(minRow));
		addRow(b_working, b_deactive.row(minRow));
		cout << "[INFO] " << i << ". loop. A constraint is added to working set and deleted from deactive set." << endl;

		//delete the blocking constraint from deactive set
		removeRow(A_deactive, minRow);
		removeRow(b_deactive, minRow);

		return alpha;
	}
	else {
		alpha = 1;
		return alpha;
	}
}

int main(int argc, char** argv)
{
	//define G and d of objective function
	MatrixXd G(2,2);
	G << 2.0, 0.0,
		 0.0, 2.0;
	VectorXd d(2,1);
	d << -2.0,
		 -5.0;

	//define initialized theta
	VectorXd theta(2, 1);
	theta(0) = 2.0;
	theta(1) = 0.0;
	int ParameterNum = theta.rows();

	//define equation constraints
	MatrixXd AEquation(0, ParameterNum);
	VectorXd bEquation(0, 1);

	//define inequation constraints
	MatrixXd AInequation(5, ParameterNum);
	AInequation << 1.0, -2.0,
				  -1.0, -2.0,
				  -1.0,  2.0,
				   1.0,  0.0,
				   0.0,  1.0;

	int AInitialRowNum = AInequation.rows();

	VectorXd bInequation(5, 1);
	bInequation << -2.0,
				   -6.0,
				   -2.0,
				    0.0,
				    0.0;

	//define working set
	MatrixXd A_working(0, ParameterNum);
	VectorXd b_working(0, 1);

	A_working = AEquation;
	b_working = bEquation;

	MatrixXd A_deactive(0, ParameterNum);
	VectorXd b_deactive(0, 1);

	setInitial(AInequation, A_working, A_deactive, bInequation,
			   b_working, b_deactive, theta, AInitialRowNum);

	//for debug
	//cout << "[INFO]Current A inequation constraints is:" << endl << A_working << endl;
	//cout << "[INFO]Current b inequation constraints is:" << endl << b_working << endl;
	//define the main variable during the calculating loop
	Index constraintsNumber = b_working.rows();
	Index thetaNumber = theta.rows();

	VectorXd result(thetaNumber+constraintsNumber, 1);
	VectorXd stepLength(thetaNumber, 1);
	VectorXd lambdaStar(constraintsNumber, 1);

	int i = 1;

	while (true) {

		result = eqpCalculation(A_working, b_working, G, d, theta);

		constraintsNumber = b_working.rows();
		stepLength << -result.head(thetaNumber);
		lambdaStar = result.tail(constraintsNumber);
		//for debug
		cout << "[INFO]: current stepLength is:" << endl << stepLength << endl;
		cout << "[INFO]: current lambdaStar is:" << endl << lambdaStar << endl;

		if ( ( stepLength.array().abs() < 1e-10 ).all() == 1 ) {
			if ((lambdaStar.array() > 0).all() == 1) {
				break;
			}
			else {
				VectorXf::Index minRow, minCol;
				float min_lambda = lambdaStar.minCoeff(&minRow, &minCol);

				//add the deleted constraints into deactive set
				addRow(A_deactive, A_working.row(minRow));
				addRow(b_deactive, b_working.row(minRow));

				//delete the constraints from working set
				removeRow(A_working, minRow);
				removeRow(b_working, minRow);
				cout << "[INFO] " << i << ". loop. A constraint is deleted from working set and added in deactive set." << endl;
				i++;
			}
		}
		else {
			float alpha = 0.0;
			alpha = calculationAlpha(A_working, b_working, A_deactive, b_deactive, stepLength, theta, i);
			cout << "[INFO]" << i << ". loop.Current alpha is :" << endl << alpha << endl;
			theta = theta + alpha * stepLength;
			cout << "[INFO]" << i << ". loop.Current Theta is :" <<endl << theta << endl;
			i++;
		}
	}

}


// -*- coding: utf-8 -*-
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <doctest/doctest.h>
#include <netoptim/min_cycle_ratio.hpp>
#include <netoptim/neg_cycle.hpp> // import negCycleFinder
#include <py2cpp/nx2bgl.hpp>
#include <utility> // for std::pair

using graph_t = boost::adjacency_list<boost::listS, boost::vecS,
    boost::directedS, boost::no_property,
    boost::property<boost::edge_weight_t, int,
        boost::property<boost::edge_index_t, int>>>;
using Vertex = boost::graph_traits<graph_t>::vertex_descriptor;
using Edge_it = boost::graph_traits<graph_t>::edge_iterator;

static xn::grAdaptor<graph_t> create_test_case1()
{
    using Edge = std::pair<int, int>;
    const auto num_nodes = 5;
    enum nodes
    {
        A,
        B,
        C,
        D,
        E
    };
    static Edge edge_array[] = {
        Edge {A, B}, Edge {B, C}, Edge {C, D}, Edge {D, E}, Edge {E, A}};
    int weights[] = {-5, 1, 1, 1, 1};
    int num_arcs = sizeof(edge_array) / sizeof(Edge);
    auto g =
        graph_t(edge_array, edge_array + num_arcs, weights, num_nodes);
    return xn::grAdaptor<graph_t> {std::move(g)};
}

static xn::grAdaptor<graph_t> create_test_case2()
{
    using Edge = std::pair<int, int>;
    const auto num_nodes = 5;
    enum nodes
    {
        A,
        B,
        C,
        D,
        E
    };
    static Edge edge_array[] = {
        Edge {A, B}, Edge {B, C}, Edge {C, D}, Edge {D, E}, Edge {E, A}};
    int weights[] = {2, 1, 1, 1, 1};
    int num_arcs = sizeof(edge_array) / sizeof(Edge);
    auto g =
        graph_t(edge_array, edge_array + num_arcs, weights, num_nodes);
    return xn::grAdaptor<graph_t> {std::move(g)};
}

static auto create_test_case_timing() -> xn::grAdaptor<graph_t>
{
    using Edge = std::pair<int, int>;
    constexpr auto num_nodes = 3;
    enum nodes
    {
        A,
        B,
        C
    };
    static Edge edge_array[] = {Edge {A, B}, Edge {B, A}, Edge {B, C},
        Edge {C, B}, Edge {B, C}, Edge {C, B}, Edge {C, A}, Edge {A, C}};
    int weights[] = {7, 0, 3, 1, 6, 4, 2, 5};
    constexpr int num_arcs = sizeof(edge_array) / sizeof(Edge);
    auto g =
        graph_t(edge_array, edge_array + num_arcs, weights, num_nodes);
    return xn::grAdaptor<graph_t> {std::move(g)};
}

static auto create_test_case_timing2() -> xn::grAdaptor<graph_t>
{
    using Edge = std::pair<int, int>;
    constexpr auto num_nodes = 3;
    enum nodes
    {
        A,
        B,
        C
    };
    static Edge edge_array[] = {Edge {A, B}, Edge {B, A}, Edge {B, C},
        Edge {C, B}, Edge {B, C}, Edge {C, B}, Edge {C, A}, Edge {A, C}};
    int weights[] = {3, -4, -1, -3, 2, 0, -2, 1};
    constexpr int num_arcs = sizeof(edge_array) / sizeof(Edge);
    auto g =
        graph_t(edge_array, edge_array + num_arcs, weights, num_nodes);
    return xn::grAdaptor<graph_t> {std::move(g)};
}

auto do_case(const xn::grAdaptor<graph_t>& G) -> bool
{
    using edge_t = decltype(*(std::begin(G.edges())));

    const auto get_weight = [&](const edge_t& e) -> int
    {
        const auto& weightmap = boost::get(boost::edge_weight, G);
        return weightmap[e];
    };

    auto dist = std::vector<int>(G.number_of_nodes(), 0);
    auto N = negCycleFinder<xn::grAdaptor<graph_t>> {G};
    const auto cycle = N.find_neg_cycle(dist, get_weight);
    return !cycle.empty();
}

auto do_case_float(const xn::grAdaptor<graph_t>& G) -> bool
{
    using edge_t = decltype(*(std::begin(G.edges())));

    const auto get_weight = [&](const edge_t& e) -> double
    {
        const auto& weightmap = boost::get(boost::edge_weight, G);
        return weightmap[e];
    };

    auto dist = std::vector<double>(G.number_of_nodes(), 0.0);
    auto N = negCycleFinder<xn::grAdaptor<graph_t>> {G};
    const auto cycle = N.find_neg_cycle(dist, get_weight);
    return !cycle.empty();
}

TEST_CASE("Test Negative Cycle (boost)")
{
    const auto G = create_test_case1();
    const auto hasNeg = do_case(G);
    CHECK(hasNeg);
}

TEST_CASE("Test No Negative Cycle (boost)")
{
    const auto G = create_test_case2();
    const auto hasNeg = do_case(G);
    CHECK(!hasNeg);
}

TEST_CASE("Test Timing Graph (boost)")
{
    const auto G = create_test_case_timing();
    const auto hasNeg = do_case(G);
    CHECK(!hasNeg);
}

TEST_CASE("Test Timing Graph 2 (boost)")
{
    const auto G = create_test_case_timing2();
    const auto hasNeg = do_case(G);
    CHECK(hasNeg);
}

TEST_CASE("Test Timing Graph float (boost)")
{
    const auto G = create_test_case_timing();
    const auto hasNeg = do_case_float(G);
    CHECK(!hasNeg);
}

TEST_CASE("Test Timing Graph 2 (boost)")
{
    const auto G = create_test_case_timing2();
    const auto hasNeg = do_case_float(G);
    CHECK(hasNeg);
}


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/module/core.hpp>
#include <eve/module/special.hpp>
#include <boost/math/special_functions/erf.hpp>

//==================================================================================================
// Types tests
//==================================================================================================
EVE_TEST_TYPES( "Check return types of erf_inv"
            , eve::test::simd::ieee_reals
            )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;

  TTS_EXPR_IS( eve::erf_inv(T())  , T);
  TTS_EXPR_IS( eve::erf_inv(v_t()), v_t);
};

//==================================================================================================
// erf_inv  tests
//==================================================================================================
EVE_TEST( "Check behavior of erf_inv on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(-1.0, 1.0))
        )
<typename T>(T const& a0 )
{
  using v_t = eve::element_type_t<T>;
  using eve::erf_inv;
  using eve::as;
  TTS_ULP_EQUAL( erf_inv(a0),  map([](auto e){return boost::math::erf_inv(e);}, a0), 2);
  auto derf_inv = [](auto e){return v_t(0.886226925452758013649)*std::exp(eve::sqr(erf_inv(e)));};
  TTS_ULP_EQUAL( eve::diff(erf_inv)(a0),  map(derf_inv, a0), 2);

  TTS_ULP_EQUAL(erf_inv(T(0.5)), T(boost::math::erf_inv(v_t(0.5))), 1. );

  if constexpr(eve::platform::supports_denormals)
  {
    TTS_ULP_EQUAL ( erf_inv(T(eve::smallestposval(as<T>())))
                  , T(boost::math::erf_inv(eve::smallestposval(as<v_t>())))
                  , 0.5
                  );
  }

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_IEEE_EQUAL(erf_inv(eve::nan(eve::as<T>()))  , eve::nan(eve::as<T>()) );
    TTS_IEEE_EQUAL(erf_inv(eve::inf(eve::as<T>()))  , eve::nan(eve::as<T>()) );
    TTS_IEEE_EQUAL(erf_inv(eve::minf(eve::as<T>())) , eve::nan(eve::as<T>()) );
  }

  TTS_ULP_EQUAL(erf_inv(T(35)), eve::nan(eve::as<T>()), 0.5);
  TTS_ULP_EQUAL(erf_inv(T(-35)), eve::nan(eve::as<T>()), 0.5);

  TTS_IEEE_EQUAL(erf_inv(T( 0 )),T(0)  );
  TTS_IEEE_EQUAL(erf_inv(T(-0.)), T(0) );
  TTS_ULP_EQUAL(erf_inv(T( 0.1 )), T( boost::math::erf_inv(0.1)), 0.5 );
  TTS_ULP_EQUAL(erf_inv(T( 0.2 )), T( boost::math::erf_inv(0.2)), 0.5 );
  TTS_ULP_EQUAL(erf_inv(T( 0.3 )), T( boost::math::erf_inv(0.3)), 0.5 );
  TTS_ULP_EQUAL(erf_inv(T( 0.5 )), T( boost::math::erf_inv(0.5)),  1 );
  TTS_ULP_EQUAL(erf_inv(T( 0.15)), T( boost::math::erf_inv(0.15)), 0.5 );
  TTS_ULP_EQUAL(erf_inv(T( 0.75)), T( boost::math::erf_inv(0.75)), 0.5 );
  TTS_ULP_EQUAL(erf_inv(T(- 0.1 )), T( boost::math::erf_inv(-0.1)), 0.5 );
  TTS_ULP_EQUAL(erf_inv(T( -0.2 )), T( boost::math::erf_inv(-0.2)), 0.5 );
  TTS_ULP_EQUAL(erf_inv(T( -0.3 )), T( boost::math::erf_inv(-0.3)), 0.5 );
  TTS_ULP_EQUAL(erf_inv(T( -0.5 )), T( boost::math::erf_inv(-0.5)),  1 );
  TTS_ULP_EQUAL(erf_inv(T( -0.15)), T( boost::math::erf_inv(-0.15)), 0.5 );
  TTS_ULP_EQUAL(erf_inv(T( -0.75)), T( boost::math::erf_inv(-0.75)), 0.5 );
};