// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.

// 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)

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/assign.hpp>

#include <boost/geometry/strategies/spherical/side_by_cross_track.hpp>
//#include <boost/geometry/strategies/spherical/side_via_plane.hpp>
#include <boost/geometry/strategies/spherical/ssf.hpp>
#include <boost/geometry/strategies/cartesian/side_by_triangle.hpp>

#include <boost/geometry/core/cs.hpp>

#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/segment.hpp>

namespace boost { namespace geometry {

template <typename Vector, typename Point1, typename Point2>
static inline Vector create_vector(Point1 const& p1, Point2 const& p2)
{
    Vector v;
    convert(p1, v);
    subtract_point(v, p2);
    return v;
}

}}

inline char side_char(int side)
{
    return side == 1 ? 'L'
        : side == -1 ? 'R'
        : '-'
        ;
}

template <typename Point>
void test_side1(std::string const& case_id, Point const& p1, Point const& p2, Point const& p3,
                   int expected, int expected_cartesian)
{
    // std::cout << case_id << ": ";
    //int s = bg::strategy::side::side_via_plane<>::apply(p1, p2, p3);
    int side_ssf = bg::strategy::side::spherical_side_formula<>::apply(p1, p2, p3);
    //int side2 = bg::strategy::side::side_via_plane<>::apply(p1, p2, p3);
    int side_ct = bg::strategy::side::side_by_cross_track<>::apply(p1, p2, p3);

    typedef bg::strategy::side::services::default_strategy<bg::cartesian_tag>::type cartesian_strategy;
    int side_cart = cartesian_strategy::apply(p1, p2, p3);

    BOOST_CHECK_EQUAL(side_ssf, expected);
    BOOST_CHECK_EQUAL(side_ct, expected);
    BOOST_CHECK_EQUAL(side_cart, expected_cartesian);
    /*
    std::cout
        << "exp: " << side_char(expected)
        << " ssf: " << side_char(side1)
        << " pln: " << side_char(side2)
        << " ct: " << side_char(side3)
        //<< " def: " << side_char(side4)
        << " cart: " << side_char(side5)
        << std::endl;
    */
}

template <typename Point>
void test_side(std::string const& case_id, Point const& p1, Point const& p2, Point const& p3,
                   int expected, int expected_cartesian = -999)
{
    if (expected_cartesian == -999)
    {
        expected_cartesian = expected;
    }
    test_side1(case_id, p1, p2, p3, expected, expected_cartesian);
    test_side1(case_id, p2, p1, p3, -expected, -expected_cartesian);
}

template <typename Point>
void test_all()
{
    typedef std::pair<double, double> pair;

    Point amsterdam(5.9, 52.4);
    Point barcelona(2.0, 41.0);
    Point paris(2.0, 48.0);
    Point milan(7.0, 45.0);

    //goto wrong;

    test_side<Point>("bp-m", barcelona, paris, milan, -1);
    test_side<Point>("bm-p", barcelona, milan, paris, 1);
    test_side<Point>("mp-b", milan, paris, barcelona, 1);

    test_side<Point>("am-p", amsterdam, milan, paris, -1);
    test_side<Point>("pm-a", paris, milan, amsterdam, 1);

    // http://www.gcmap.com/mapui?P=30N+10E-50N+50E,39N+30E
    Point gcmap_p1(10.0, 30.0);
    Point gcmap_p2(50.0, 50.0);
    test_side<Point>("blog1", gcmap_p1, gcmap_p2, Point(30.0, 41.0), -1, 1);
    test_side<Point>("blog1", gcmap_p1, gcmap_p2, Point(30.0, 42.0), -1, 1);
    test_side<Point>("blog1", gcmap_p1, gcmap_p2, Point(30.0, 43.0), -1, 1);
    test_side<Point>("blog1", gcmap_p1, gcmap_p2, Point(30.0, 44.0), 1);

    // http://www.gcmap.com/mapui?P=50N+80E-60N+50W,65N+30E
    Point gcmap_np1(80.0, 50.0);
    Point gcmap_np2(-50.0, 60.0);
    // http://www.gcmap.com/mapui?P=50N+140E-60N+10E,65N+30E
    //Point gcmap_np1(140.0, 50.0);
    //Point gcmap_np2(10.0, 60.0);
    //test_side<Point>(gcmap_np1, gcmap_np2, gcmap_np, 1);
    test_side<Point>("40", gcmap_np1, gcmap_np2, Point(30.0, 60.0), 1, -1);
    test_side<Point>("45", gcmap_np1, gcmap_np2, Point(30.0, 65.0), 1, -1);
    test_side<Point>("70", gcmap_np1, gcmap_np2, Point(30.0, 70.0), 1, -1);
    test_side<Point>("75", gcmap_np1, gcmap_np2, Point(30.0, 75.0), -1);
}

int test_main(int, char* [])
{
    test_all<bg::model::point<int, 2, bg::cs::spherical<bg::degree> > >();
    test_all<bg::model::point<double, 2, bg::cs::spherical_equatorial<bg::degree> > >();

#if defined(HAVE_TTMATH)
    typedef ttmath::Big<1,4> tt;
    test_all<bg::model::point<tt, 2, bg::cs::spherical_equatorial<bg::degree> > >();
#endif

    return 0;
}


// test file for special functions.

//  (C) Copyright Hubert Holin 2003. Permission to copy, use, modify, sell and
//  distribute this software is granted provided this copyright notice appears
//  in all copies. This software is provided "as is" without express or implied
//  warranty, and with no claim as to its suitability for any purpose.

#include <iomanip>
#include <iostream>
#include <functional>

#include <boost/bind.hpp>
#include <boost/function.hpp>

#include <boost/test/unit_test_suite_ex.hpp>

#include "sinc_test.hpp"
#include "sinhc_test.hpp"
#include "atanh_test.hpp"
#include "asinh_test.hpp"
#include "acosh_test.hpp"

boost::unit_test_framework::test_suite *    init_unit_test_suite(int, char *[])
{
    //::boost::unit_test_framework::unit_test_log::instance().
    //    set_log_threshold_level_by_name("messages");

    boost::unit_test_framework::test_suite *    test =
        BOOST_TEST_SUITE("special_functions_test");

#define    BOOST_SPECIAL_FUNCTIONS_COMMON_GENERATOR(fct,type)    \
    test->add(BOOST_TEST_CASE(::boost::bind(static_cast          \
        < void (*) (const char *) >(&fct##_test<type>), #type)));

#define    BOOST_SPECIAL_FUNCTIONS_COMMON_TEST(type)        \
    BOOST_SPECIAL_FUNCTIONS_COMMON_GENERATOR(atanh,type)    \
    BOOST_SPECIAL_FUNCTIONS_COMMON_GENERATOR(asinh,type)    \
    BOOST_SPECIAL_FUNCTIONS_COMMON_GENERATOR(acosh,type)    \
    BOOST_SPECIAL_FUNCTIONS_COMMON_GENERATOR(sinc_pi,type)  \
    BOOST_SPECIAL_FUNCTIONS_COMMON_GENERATOR(sinhc_pi,type)

#define    BOOST_SPECIAL_FUNCTIONS_TEMPLATE_TEMPLATE_TEST(type)      \
    BOOST_SPECIAL_FUNCTIONS_COMMON_GENERATOR(sinc_pi_complex,type)   \
    BOOST_SPECIAL_FUNCTIONS_COMMON_GENERATOR(sinhc_pi_complex,type)

#ifdef    BOOST_NO_TEMPLATE_TEMPLATES

#define    BOOST_SPECIAL_FUNCTIONS_TEST(type)                                 \
    BOOST_SPECIAL_FUNCTIONS_COMMON_TEST(type)                                 \
    BOOST_MESSAGE("Warning: no template templates; curtailed functionality.");

#else    /* BOOST_NO_TEMPLATE_TEMPLATES */

#define    BOOST_SPECIAL_FUNCTIONS_TEST(type)            \
    BOOST_SPECIAL_FUNCTIONS_COMMON_TEST(type)            \
    BOOST_SPECIAL_FUNCTIONS_TEMPLATE_TEMPLATE_TEST(type)

#endif    /* BOOST_NO_TEMPLATE_TEMPLATES */

    BOOST_SPECIAL_FUNCTIONS_TEST(float)
    BOOST_SPECIAL_FUNCTIONS_TEST(double)
    BOOST_SPECIAL_FUNCTIONS_TEST(long double)

#undef    BOOST_SPECIAL_FUNCTIONS_TEST

#undef    BOOST_SPECIAL_FUNCTIONS_TEMPLATE_TEMPLATE_TEST

#undef    BOOST_SPECIAL_FUNCTIONS_COMMON_TEST

#undef    BOOST_SPECIAL_FUNCTIONS_COMMON_GENERATOR

#ifdef    BOOST_SPECIAL_FUNCTIONS_TEST_VERBOSE

    using    ::std::numeric_limits;

    BOOST_MESSAGE("epsilon");

    BOOST_MESSAGE( ::std::setw(15) << numeric_limits<float>::epsilon()
                << ::std::setw(15) << numeric_limits<double>::epsilon()
                << ::std::setw(15) << numeric_limits<long double>::epsilon());

    BOOST_MESSAGE(" ");

    atanh_manual_check();
    asinh_manual_check();
    acosh_manual_check();
    sinc_pi_manual_check();
    sinhc_pi_manual_check();

#endif    /* BOOST_SPECIAL_FUNCTIONS_TEST_VERBOSE */

    return(test);
}


/*
 * Taken from http://www.andrew.cmu.edu/user/vanhoeve/mdd/ with the notice:
 * "The software can be freely used but comes with no warranty"
 * -----------------------------------------------------------
 * Data structure to store positive sets of integers.

 * Should be used in the cases where one needs to do
 * fast intersections and unions, and does not iterate
 * on all elements too many times.
 * -----------------------------------------------------------
 */

#ifndef INTSET_HPP_
#define INTSET_HPP_

#define NOT_COMPUTED -1     /**< indicates if the size was not computed */

#include <boost/dynamic_bitset.hpp>
#include <cassert>
#include <iostream>
#include <fstream>
#include "util.hpp"

/**
 * Integer Set structure
 */
struct IntSet {

    /** Constructor */
    IntSet(int _min, int _max, bool _filled);

    /** Empty constructor */
    IntSet();

    /** Check if set contains element */
    bool contains(int elem);

    /** Add an element to the set */
    void add(int elem);

    /** Add all possible elements to the set */
    void add_all_elements();

    /** Remove element, if it is contained */
    void remove(int elem);

    /** Get number of elements in the set */
    int get_size();

    /** Get the first element of the set */
    int get_first();

    /** Get next element higher than the one passed as parameter */
    int get_next(int elem);

    /** Get end of the set (beyond last element) */
    const int get_end();

    /** Clear set */
    void clear();

    /** Resize */
    void resize(int _min, int _max, bool _filled);

    /** Take the union with another intset */
    void union_with(IntSet& intset);

    /** Take the intersection with another intset */
    void intersect_with(IntSet& intset);

    /** Checks if one intersects with the other intset */
    void does_intersect(IntSet& intset);

    /** Assignment operator */
    IntSet& operator=(const IntSet& rhs);

    /** Returns if set is a subset of another */
    bool is_subset(const IntSet& other);

    /** Returns if one set equals another */
    bool equals_to(const IntSet& other);

    // parameters

    boost::dynamic_bitset<>     set;            /**< bitvector representing the set */
    const int                   end;            /**< position beyond end of the set */
    int                         size;           /**< number of elements in the set */
    int                         min;            /**< minimum possible element of the set */
    int                         max;            /**< maximum possible element of the set */
    //int                         shift;          /**< shift of element to be added in the set */
};

/**
 * Lexicographic comparator function for IntSet class.
 */
struct IntSetLexLessThan {
    bool operator()(const IntSet* setA, const IntSet* setB) const {
        return setA->set < setB->set;
    }
};

/**
 * -----------------------------------------------
 * Inline implementations
 * -----------------------------------------------
 */

/**
 * Constructor
 */
inline IntSet::IntSet(int _min, int _max, bool _filled) : end((int)set.npos) {
    resize(_min, _max, _filled);
    size = NOT_COMPUTED;
}

/**
 * Empty constructor
 */
inline IntSet::IntSet() : end((int)set.npos) {
}

/**
 * Add an element to the set
 */
inline bool IntSet::contains(int elem) {
    assert( elem >= min && elem <= max );
    return( set.test(elem) );
}

/**
 * Add an element to the set
 */
inline void IntSet::add(int elem) {
    assert( elem >= min && elem <= max );
    set.set(elem, true);
    size = NOT_COMPUTED;
}

/** Remove element, if it is contained */
inline void IntSet::remove(int elem) {
    assert( elem >= min && elem <= max );
    set.set(elem, false);
    size = NOT_COMPUTED;
}

/**
 * Get the first element of the set
 */
inline int IntSet::get_first() {
    return (set.find_first());
}

/**
 * Get next element higher than the one passed as parameter
 */
inline int IntSet::get_next(int elem) {
    assert( elem >= min && elem <= max );
    return (set.find_next(elem));
}

/**
 * Get end of the set (beyond last element)
 */
inline const int IntSet::get_end() {
    return end;
}

/**
 * Clear set
 */
inline void IntSet::clear() {
    set.reset();
    size = 0;
}

/**
 * Assignment operator
 */
inline IntSet& IntSet::operator=(const IntSet& rhs) {
    assert(rhs.max == max && rhs.min == min);
    if (this != &rhs) {
        set = rhs.set;
        size = NOT_COMPUTED;
    }
    return *this;
}

/**
 * Resize
 */
inline void IntSet::resize(int _min, int _max, bool _filled) {

	if( _min != 0 ) {
		exit(1);
	}

    min = _min;
    max = _max;

    set.resize(max - min + 1);

    if( _filled )
        set.set();
    else
        set.reset();

    size = NOT_COMPUTED;
}

/**
 * Take the union with another intset
 */
inline void IntSet::union_with(IntSet& intset) {
    set |= intset.set;
    size = NOT_COMPUTED;
}

/**
 * Take the intersection with another intset
 */
inline void IntSet::intersect_with(IntSet& intset) {
    set &= intset.set;
    size = NOT_COMPUTED;
}

/** Get number of elements in the set */
inline int IntSet::get_size() {
    if( size == NOT_COMPUTED ) {
        size = set.count();
    }
    return size;
}

/**
 * Add all possible elements to the set
 */
inline void IntSet::add_all_elements() {
    set.set();
    size = set.size();
}

/**
 * Stream output function
 */
inline std::ostream& operator<<(std::ostream &os, IntSet &intset) {
    os << "[ ";
    int val = intset.get_first();
    while( val != intset.get_end() ) {
        os << val << " ";
        val = intset.get_next(val);
    }
    os << "]";
    return os;
}

/**
 * Returns if set is a subset of another
 */
inline bool IntSet::is_subset(const IntSet& other) {
    return set.is_subset_of(other.set);
}

/**
 * Returns if one is equal to the other
 */
inline bool IntSet::equals_to(const IntSet& other) {
	return (set == other.set);
}

#endif /* INTSET_HPP_ */


#include <NTL/mat_ZZ.h>

NTL_START_IMPL

void add(mat_ZZ& X, const mat_ZZ& A, const mat_ZZ& B)
{
   long n = A.NumRows();
   long m = A.NumCols();

   if (B.NumRows() != n || B.NumCols() != m)
      LogicError("matrix add: dimension mismatch");

   X.SetDims(n, m);

   long i, j;
   for (i = 1; i <= n; i++)
      for (j = 1; j <= m; j++)
         add(X(i,j), A(i,j), B(i,j));
}

void sub(mat_ZZ& X, const mat_ZZ& A, const mat_ZZ& B)
{
   long n = A.NumRows();
   long m = A.NumCols();

   if (B.NumRows() != n || B.NumCols() != m)
      LogicError("matrix sub: dimension mismatch");

   X.SetDims(n, m);

   long i, j;
   for (i = 1; i <= n; i++)
      for (j = 1; j <= m; j++)
         sub(X(i,j), A(i,j), B(i,j));
}

void mul_aux(mat_ZZ& X, const mat_ZZ& A, const mat_ZZ& B)
{
   long n = A.NumRows();
   long l = A.NumCols();
   long m = B.NumCols();

   if (l != B.NumRows())
      LogicError("matrix mul: dimension mismatch");

   X.SetDims(n, m);

   long i, j, k;
   ZZ acc, tmp;

   for (i = 1; i <= n; i++) {
      for (j = 1; j <= m; j++) {
         clear(acc);
         for(k = 1; k <= l; k++) {
            mul(tmp, A(i,k), B(k,j));
            add(acc, acc, tmp);
         }
         X(i,j) = acc;
      }
   }
}

void mul(mat_ZZ& X, const mat_ZZ& A, const mat_ZZ& B)
{
   if (&X == &A || &X == &B) {
      mat_ZZ tmp;
      mul_aux(tmp, A, B);
      X = tmp;
   }
   else
      mul_aux(X, A, B);
}

static
void mul_aux(vec_ZZ& x, const mat_ZZ& A, const vec_ZZ& b)
{
   long n = A.NumRows();
   long l = A.NumCols();

   if (l != b.length())
      LogicError("matrix mul: dimension mismatch");

   x.SetLength(n);

   long i, k;
   ZZ acc, tmp;

   for (i = 1; i <= n; i++) {
      clear(acc);
      for (k = 1; k <= l; k++) {
         mul(tmp, A(i,k), b(k));
         add(acc, acc, tmp);
      }
      x(i) = acc;
   }
}

void mul(vec_ZZ& x, const mat_ZZ& A, const vec_ZZ& b)
{
   if (&b == &x || A.alias(x)) {
      vec_ZZ tmp;
      mul_aux(tmp, A, b);
      x = tmp;
   }
   else
      mul_aux(x, A, b);
}

static
void mul_aux(vec_ZZ& x, const vec_ZZ& a, const mat_ZZ& B)
{
   long n = B.NumRows();
   long l = B.NumCols();

   if (n != a.length())
      LogicError("matrix mul: dimension mismatch");

   x.SetLength(l);

   long i, k;
   ZZ acc, tmp;

   for (i = 1; i <= l; i++) {
      clear(acc);
      for (k = 1; k <= n; k++) {
         mul(tmp, a(k), B(k,i));
         add(acc, acc, tmp);
      }
      x(i) = acc;
   }
}

void mul(vec_ZZ& x, const vec_ZZ& a, const mat_ZZ& B)
{
   if (&a == &x) {
      vec_ZZ tmp;
      mul_aux(tmp, a, B);
      x = tmp;
   }
   else
      mul_aux(x, a, B);
}

void ident(mat_ZZ& X, long n)
{
   X.SetDims(n, n);
   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i == j)
            set(X(i, j));
         else
            clear(X(i, j));
}

static
long DetBound(const mat_ZZ& a)
{
   long n = a.NumRows();
   long i;
   ZZ res, t1;

   set(res);

   for (i = 0; i < n; i++) {
      InnerProduct(t1, a[i], a[i]);
      if (t1 > 1) {
         SqrRoot(t1, t1);
         add(t1, t1, 1);
      }
      mul(res, res, t1);
   }

   return NumBits(res);
}

void determinant(ZZ& rres, const mat_ZZ& a, long deterministic)
{
   long n = a.NumRows();
   if (a.NumCols() != n)
      LogicError("determinant: nonsquare matrix");

   if (n == 0) {
      set(rres);
      return;
   }

   zz_pBak zbak;
   zbak.save();

   ZZ_pBak Zbak;
   Zbak.save();

   long instable = 1;

   long gp_cnt = 0;

   long bound = 2+DetBound(a);

   ZZ res, prod;

   clear(res);
   set(prod);

   long i;
   for (i = 0; ; i++) {
      if (NumBits(prod) > bound)
         break;

      if (!deterministic &&
          !instable && bound > 1000 && NumBits(prod) < 0.25*bound) {
         ZZ P;

         long plen = 90 + NumBits(max(bound, NumBits(res)));
         GenPrime(P, plen, 90 + 2*NumBits(gp_cnt++));

         ZZ_p::init(P);

         mat_ZZ_p A;
         conv(A, a);

         ZZ_p t;
         determinant(t, A);

         if (CRT(res, prod, rep(t), P))
            instable = 1;
         else
            break;
      }

      zz_p::FFTInit(i);
      long p = zz_p::modulus();

      mat_zz_p A;
      conv(A, a);

      zz_p t;
      determinant(t, A);

      instable = CRT(res, prod, rep(t), p);
   }

   rres = res;

   zbak.restore();
   Zbak.restore();
}

void conv(mat_zz_p& x, const mat_ZZ& a)
{
   long n = a.NumRows();
   long m = a.NumCols();
   long i;

   x.SetDims(n, m);
   for (i = 0; i < n; i++)
      conv(x[i], a[i]);
}

void conv(mat_ZZ_p& x, const mat_ZZ& a)
{
   long n = a.NumRows();
   long m = a.NumCols();
   long i;

   x.SetDims(n, m);
   for (i = 0; i < n; i++)
      conv(x[i], a[i]);
}

long IsIdent(const mat_ZZ& A, long n)
{
   if (A.NumRows() != n || A.NumCols() != n)
      return 0;

   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i != j) {
            if (!IsZero(A(i, j))) return 0;
         }
         else {
            if (!IsOne(A(i, j))) return 0;
         }

   return 1;
}

void transpose(mat_ZZ& X, const mat_ZZ& A)
{
   long n = A.NumRows();
   long m = A.NumCols();

   long i, j;

   if (&X == & A) {
      if (n == m)
         for (i = 1; i <= n; i++)
            for (j = i+1; j <= n; j++)
               swap(X(i, j), X(j, i));
      else {
         mat_ZZ tmp;
         tmp.SetDims(m, n);
         for (i = 1; i <= n; i++)
            for (j = 1; j <= m; j++)
               tmp(j, i) = A(i, j);
         X.kill();
         X = tmp;
      }
   }
   else {
      X.SetDims(m, n);
      for (i = 1; i <= n; i++)
         for (j = 1; j <= m; j++)
            X(j, i) = A(i, j);
   }
}

long CRT(mat_ZZ& gg, ZZ& a, const mat_zz_p& G)
{
   long n = gg.NumRows();
   long m = gg.NumCols();

   if (G.NumRows() != n || G.NumCols() != m)
      LogicError("CRT: dimension mismatch");

   long p = zz_p::modulus();

   ZZ new_a;
   mul(new_a, a, p);

   long a_inv;
   a_inv = rem(a, p);
   a_inv = InvMod(a_inv, p);

   long p1;
   p1 = p >> 1;

   ZZ a1;
   RightShift(a1, a, 1);

   long p_odd = (p & 1);

   long modified = 0;

   long h;

   ZZ g;
   long i, j;

   for (i = 0; i < n; i++) {
      for (j = 0; j < m; j++) {
         if (!CRTInRange(gg[i][j], a)) {
            modified = 1;
            rem(g, gg[i][j], a);
            if (g > a1) sub(g, g, a);
         }
         else
            g = gg[i][j];

         h = rem(g, p);
         h = SubMod(rep(G[i][j]), h, p);
         h = MulMod(h, a_inv, p);
         if (h > p1)
            h = h - p;

         if (h != 0) {
            modified = 1;

            if (!p_odd && g > 0 && (h == p1))
               MulSubFrom(g, a, h);
            else
               MulAddTo(g, a, h);

         }

         gg[i][j] = g;
      }
   }

   a = new_a;

   return modified;

}

void mul(mat_ZZ& X, const mat_ZZ& A, const ZZ& b_in)
{
   ZZ b = b_in;
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 0; i < n; i++)
      for (j = 0; j < m; j++)
         mul(X[i][j], A[i][j], b);
}

void mul(mat_ZZ& X, const mat_ZZ& A, long b)
{
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 0; i < n; i++)
      for (j = 0; j < m; j++)
         mul(X[i][j], A[i][j], b);
}

static
void ExactDiv(vec_ZZ& x, const ZZ& d)
{
   long n = x.length();
   long i;

   for (i = 0; i < n; i++)
      if (!divide(x[i], x[i], d))
         ArithmeticError("inexact division");
}

static
void ExactDiv(mat_ZZ& x, const ZZ& d)
{
   long n = x.NumRows();
   long m = x.NumCols();

   long i, j;

   for (i = 0; i < n; i++)
      for (j = 0; j < m; j++)
         if (!divide(x[i][j], x[i][j], d))
            ArithmeticError("inexact division");
}

void diag(mat_ZZ& X, long n, const ZZ& d_in)
{
   ZZ d = d_in;
   X.SetDims(n, n);
   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i == j)
            X(i, j) = d;
         else
            clear(X(i, j));
}

long IsDiag(const mat_ZZ& A, long n, const ZZ& d)
{
   if (A.NumRows() != n || A.NumCols() != n)
      return 0;

   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i != j) {
            if (!IsZero(A(i, j))) return 0;
         }
         else {
            if (A(i, j) != d) return 0;
         }

   return 1;
}

void solve(ZZ& d_out, vec_ZZ& x_out,
           const mat_ZZ& A, const vec_ZZ& b,
           long deterministic)
{
   long n = A.NumRows();

   if (A.NumCols() != n)
      LogicError("solve: nonsquare matrix");

   if (b.length() != n)
      LogicError("solve: dimension mismatch");

   if (n == 0) {
      set(d_out);
      x_out.SetLength(0);
      return;
   }

   zz_pBak zbak;
   zbak.save();

   ZZ_pBak Zbak;
   Zbak.save();

   vec_ZZ x(INIT_SIZE, n);
   ZZ d, d1;

   ZZ d_prod, x_prod;
   set(d_prod);
   set(x_prod);

   long d_instable = 1;
   long x_instable = 1;

   long check = 0;

   long gp_cnt = 0;

   vec_ZZ y, b1;

   long i;
   long bound = 2+DetBound(A);

   for (i = 0; ; i++) {
      if ((check || IsZero(d)) && !d_instable) {
         if (NumBits(d_prod) > bound) {
            break;
         }
         else if (!deterministic &&
                  bound > 1000 && NumBits(d_prod) < 0.25*bound) {

            ZZ P;

            long plen = 90 + NumBits(max(bound, NumBits(d)));
            GenPrime(P, plen, 90 + 2*NumBits(gp_cnt++));

            ZZ_p::init(P);

            mat_ZZ_p AA;
            conv(AA, A);

            ZZ_p dd;
            determinant(dd, AA);

            if (CRT(d, d_prod, rep(dd), P))
               d_instable = 1;
            else
               break;
         }
      }

      zz_p::FFTInit(i);
      long p = zz_p::modulus();

      mat_zz_p AA;
      conv(AA, A);

      if (!check) {
         vec_zz_p bb, xx;
         conv(bb, b);

         zz_p dd;

         solve(dd, xx, AA, bb);

         d_instable = CRT(d, d_prod, rep(dd), p);
         if (!IsZero(dd)) {
            mul(xx, xx, dd);
            x_instable = CRT(x, x_prod, xx);
         }
         else
            x_instable = 1;

         if (!d_instable && !x_instable) {
            mul(y, x, A);
            mul(b1, b, d);
            if (y == b1) {
               d1 = d;
               check = 1;
            }
         }
      }
      else {
         zz_p dd;
         determinant(dd, AA);
         d_instable = CRT(d, d_prod, rep(dd), p);
      }
   }

   if (check && d1 != d) {
      mul(x, x, d);
      ExactDiv(x, d1);
   }

   d_out = d;
   if (check) x_out = x;

   zbak.restore();
   Zbak.restore();
}

void inv(ZZ& d_out, mat_ZZ& x_out, const mat_ZZ& A, long deterministic)
{
   long n = A.NumRows();

   if (A.NumCols() != n)
      LogicError("solve: nonsquare matrix");

   if (n == 0) {
      set(d_out);
      x_out.SetDims(0, 0);
      return;
   }

   zz_pBak zbak;
   zbak.save();

   ZZ_pBak Zbak;
   Zbak.save();

   mat_ZZ x(INIT_SIZE, n, n);
   ZZ d, d1;

   ZZ d_prod, x_prod;
   set(d_prod);
   set(x_prod);

   long d_instable = 1;
   long x_instable = 1;

   long gp_cnt = 0;

   long check = 0;

   mat_ZZ y;

   long i;
   long bound = 2+DetBound(A);

   for (i = 0; ; i++) {
      if ((check || IsZero(d)) && !d_instable) {
         if (NumBits(d_prod) > bound) {
            break;
         }
         else if (!deterministic &&
                  bound > 1000 && NumBits(d_prod) < 0.25*bound) {

            ZZ P;

            long plen = 90 + NumBits(max(bound, NumBits(d)));
            GenPrime(P, plen, 90 + 2*NumBits(gp_cnt++));

            ZZ_p::init(P);

            mat_ZZ_p AA;
            conv(AA, A);

            ZZ_p dd;
            determinant(dd, AA);

            if (CRT(d, d_prod, rep(dd), P))
               d_instable = 1;
            else
               break;
         }
      }

      zz_p::FFTInit(i);
      long p = zz_p::modulus();

      mat_zz_p AA;
      conv(AA, A);

      if (!check) {
         mat_zz_p xx;

         zz_p dd;

         inv(dd, xx, AA);

         d_instable = CRT(d, d_prod, rep(dd), p);
         if (!IsZero(dd)) {
            mul(xx, xx, dd);
            x_instable = CRT(x, x_prod, xx);
         }
         else
            x_instable = 1;

         if (!d_instable && !x_instable) {
            mul(y, x, A);
            if (IsDiag(y, n, d)) {
               d1 = d;
               check = 1;
            }
         }
      }
      else {
         zz_p dd;
         determinant(dd, AA);
         d_instable = CRT(d, d_prod, rep(dd), p);
      }
   }

   if (check && d1 != d) {
      mul(x, x, d);
      ExactDiv(x, d1);
   }

   d_out = d;
   if (check) x_out = x;

   zbak.restore();
   Zbak.restore();
}

void negate(mat_ZZ& X, const mat_ZZ& A)
{
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 1; i <= n; i++)
      for (j = 1; j <= m; j++)
         negate(X(i,j), A(i,j));
}

long IsZero(const mat_ZZ& a)
{
   long n = a.NumRows();
   long i;

   for (i = 0; i < n; i++)
      if (!IsZero(a[i]))
         return 0;

   return 1;
}

void clear(mat_ZZ& x)
{
   long n = x.NumRows();
   long i;
   for (i = 0; i < n; i++)
      clear(x[i]);
}

mat_ZZ operator+(const mat_ZZ& a, const mat_ZZ& b)
{
   mat_ZZ res;
   add(res, a, b);
   NTL_OPT_RETURN(mat_ZZ, res);
}

mat_ZZ operator*(const mat_ZZ& a, const mat_ZZ& b)
{
   mat_ZZ res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(mat_ZZ, res);
}

mat_ZZ operator-(const mat_ZZ& a, const mat_ZZ& b)
{
   mat_ZZ res;
   sub(res, a, b);
   NTL_OPT_RETURN(mat_ZZ, res);
}

mat_ZZ operator-(const mat_ZZ& a)
{
   mat_ZZ res;
   negate(res, a);
   NTL_OPT_RETURN(mat_ZZ, res);
}

vec_ZZ operator*(const mat_ZZ& a, const vec_ZZ& b)
{
   vec_ZZ res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(vec_ZZ, res);
}

vec_ZZ operator*(const vec_ZZ& a, const mat_ZZ& b)
{
   vec_ZZ res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(vec_ZZ, res);
}

void inv(mat_ZZ& X, const mat_ZZ& A)
{
   ZZ d;
   inv(d, X, A);
   if (d == -1)
      negate(X, X);
   else if (d != 1)
      ArithmeticError("inv: non-invertible matrix");
}

void power(mat_ZZ& X, const mat_ZZ& A, const ZZ& e)
{
   if (A.NumRows() != A.NumCols()) LogicError("power: non-square matrix");

   if (e == 0) {
      ident(X, A.NumRows());
      return;
   }

   mat_ZZ T1, T2;
   long i, k;

   k = NumBits(e);
   T1 = A;

   for (i = k-2; i >= 0; i--) {
      sqr(T2, T1);
      if (bit(e, i))
         mul(T1, T2, A);
      else
         T1 = T2;
   }

   if (e < 0)
      inv(X, T1);
   else
      X = T1;
}

/***********************************************************

   routines for solving a linear system via Hensel lifting

************************************************************/

static
long MaxBits(const mat_ZZ& A)
{
   long m = 0;
   long i, j;
   for (i = 0; i < A.NumRows(); i++)
      for (j = 0; j < A.NumCols(); j++)
         m = max(m, NumBits(A[i][j]));

   return m;
}

// Computes an upper bound on the numerators and denominators
// to the solution x*A = b using Hadamard's bound and Cramer's rule.
// If A contains a zero row, then sets both bounds to zero.

static
void hadamard(ZZ& num_bound, ZZ& den_bound,
              const mat_ZZ& A, const vec_ZZ& b)
{
   long n = A.NumRows();

   if (n == 0) LogicError("internal error: hadamard with n = 0");

   ZZ b_len, min_A_len, prod, t1;

   InnerProduct(min_A_len, A[0], A[0]);

   prod = min_A_len;

   long i;
   for (i = 1; i < n; i++) {
      InnerProduct(t1, A[i], A[i]);
      if (t1 < min_A_len)
         min_A_len = t1;
      mul(prod, prod, t1);
   }

   if (min_A_len == 0) {
      num_bound = 0;
      den_bound = 0;
      return;
   }

   InnerProduct(b_len, b, b);

   div(t1, prod, min_A_len);
   mul(t1, t1, b_len);

   SqrRoot(num_bound, t1);
   SqrRoot(den_bound, prod);
}

static
void MixedMul(vec_ZZ& x, const vec_zz_p& a, const mat_ZZ& B)
{
   long n = B.NumRows();
   long l = B.NumCols();

   if (n != a.length())
      LogicError("matrix mul: dimension mismatch");

   x.SetLength(l);

   long i, k;
   ZZ acc, tmp;

   for (i = 1; i <= l; i++) {
      clear(acc);
      for (k = 1; k <= n; k++) {
         mul(tmp, B(k, i), rep(a(k)));
         add(acc, acc, tmp);
      }
      x(i) = acc;
    }
}

static
void SubDiv(vec_ZZ& e, const vec_ZZ& t, long p)
{
   long n = e.length();
   if (t.length() != n) LogicError("SubDiv: dimension mismatch");

   ZZ s;
   long i;

   for (i = 0; i < n; i++) {
      sub(s, e[i], t[i]);
      div(e[i], s, p);
   }
}

static
void MulAdd(vec_ZZ& x, const ZZ& prod, const vec_zz_p& h)
{
   long n = x.length();
   if (h.length() != n) LogicError("MulAdd: dimension mismatch");

   ZZ t;
   long i;

   for (i = 0; i < n; i++) {
      mul(t, prod, rep(h[i]));
      add(x[i], x[i], t);
   }
}

static
void double_MixedMul1(vec_ZZ& x, double *a, double **B, long n)
{
   long i, k;
   double acc;

   for (i = 0; i < n; i++) {
      double *bp = B[i];
      acc = 0;
      for (k = 0; k < n; k++) {
         acc += bp[k] * a[k];
      }
      conv(x[i], acc);
    }
}

static
void double_MixedMul2(vec_ZZ& x, double *a, double **B, long n, long limit)
{
   long i, k;
   double acc;
   ZZ acc1, t;
   long j;

   for (i = 0; i < n; i++) {
      double *bp = B[i];

      clear(acc1);
      acc = 0;
      j = 0;

      for (k = 0; k < n; k++) {
         acc += bp[k] * a[k];
         j++;
         if (j == limit) {
            conv(t, acc);
            add(acc1, acc1, t);
            acc = 0;
            j = 0;
         }
      }

      if (j > 0) {
         conv(t, acc);
         add(acc1, acc1, t);
      }

      x[i] = acc1;
    }
}

static
void long_MixedMul1(vec_ZZ& x, long *a, long **B, long n)
{
   long i, k;
   long acc;

   for (i = 0; i < n; i++) {
      long *bp = B[i];
      acc = 0;
      for (k = 0; k < n; k++) {
         acc += bp[k] * a[k];
      }
      conv(x[i], acc);
    }
}

static
void long_MixedMul2(vec_ZZ& x, long *a, long **B, long n, long limit)
{
   long i, k;
   long acc;
   ZZ acc1, t;
   long j;

   for (i = 0; i < n; i++) {
      long *bp = B[i];

      clear(acc1);
      acc = 0;
      j = 0;

      for (k = 0; k < n; k++) {
         acc += bp[k] * a[k];
         j++;
         if (j == limit) {
            conv(t, acc);
            add(acc1, acc1, t);
            acc = 0;
            j = 0;
         }
      }

      if (j > 0) {
         conv(t, acc);
         add(acc1, acc1, t);
      }

      x[i] = acc1;
    }
}

void solve1(ZZ& d_out, vec_ZZ& x_out, const mat_ZZ& A, const vec_ZZ& b)
{
   long n = A.NumRows();

   if (A.NumCols() != n)
      LogicError("solve1: nonsquare matrix");

   if (b.length() != n)
      LogicError("solve1: dimension mismatch");

   if (n == 0) {
      set(d_out);
      x_out.SetLength(0);
      return;
   }

   ZZ num_bound, den_bound;

   hadamard(num_bound, den_bound, A, b);

   if (den_bound == 0) {
      clear(d_out);
      return;
   }

   zz_pBak zbak;
   zbak.save();

   long i;
   long j;

   ZZ prod;
   prod = 1;

   mat_zz_p B;

   for (i = 0; ; i++) {
      zz_p::FFTInit(i);

      mat_zz_p AA, BB;
      zz_p dd;

      conv(AA, A);
      inv(dd, BB, AA);

      if (dd != 0) {
         transpose(B, BB);
         break;
      }

      mul(prod, prod, zz_p::modulus());

      if (prod > den_bound) {
         d_out = 0;
         return;
      }
   }

   long max_A_len = MaxBits(A);

   long use_double_mul1 = 0;
   long use_double_mul2 = 0;
   long double_limit = 0;

   if (max_A_len + NTL_SP_NBITS + NumBits(n) <= NTL_DOUBLE_PRECISION-1)
      use_double_mul1 = 1;

   if (!use_double_mul1 && max_A_len+NTL_SP_NBITS+2 <= NTL_DOUBLE_PRECISION-1) {
      use_double_mul2 = 1;
      double_limit = (1L << (NTL_DOUBLE_PRECISION-1-max_A_len-NTL_SP_NBITS));
   }

   long use_long_mul1 = 0;
   long use_long_mul2 = 0;
   long long_limit = 0;

   if (max_A_len + NTL_SP_NBITS + NumBits(n) <= NTL_BITS_PER_LONG-1)
      use_long_mul1 = 1;

   if (!use_long_mul1 && max_A_len+NTL_SP_NBITS+2 <= NTL_BITS_PER_LONG-1) {
      use_long_mul2 = 1;
      long_limit = (1L << (NTL_BITS_PER_LONG-1-max_A_len-NTL_SP_NBITS));
   }

   if (use_double_mul1 && use_long_mul1)
      use_long_mul1 = 0;
   else if (use_double_mul1 && use_long_mul2)
      use_long_mul2 = 0;
   else if (use_double_mul2 && use_long_mul1)
      use_double_mul2 = 0;
   else if (use_double_mul2 && use_long_mul2) {
      if (long_limit > double_limit)
         use_double_mul2 = 0;
      else
         use_long_mul2 = 0;
   }

   double **double_A=0;
   double *double_h=0;

   Unique2DArray<double> double_A_store;
   UniqueArray<double> double_h_store;

   if (use_double_mul1 || use_double_mul2) {
      double_h_store.SetLength(n);
      double_h = double_h_store.get();

      double_A_store.SetDims(n, n);
      double_A = double_A_store.get();

      for (i = 0; i < n; i++)
         for (j = 0; j < n; j++)
            double_A[j][i] = to_double(A[i][j]);
   }

   long **long_A=0;
   long *long_h=0;

   Unique2DArray<long> long_A_store;
   UniqueArray<long> long_h_store;

   if (use_long_mul1 || use_long_mul2) {
      long_h_store.SetLength(n);
      long_h = long_h_store.get();

      long_A_store.SetDims(n, n);
      long_A = long_A_store.get();

      for (i = 0; i < n; i++)
         for (j = 0; j < n; j++)
            long_A[j][i] = to_long(A[i][j]);
   }

   vec_ZZ x;
   x.SetLength(n);

   vec_zz_p h;
   h.SetLength(n);

   vec_ZZ e;
   e = b;

   vec_zz_p ee;

   vec_ZZ t;
   t.SetLength(n);

   prod = 1;

   ZZ bound1;
   mul(bound1, num_bound, den_bound);
   mul(bound1, bound1, 2);

   while (prod <= bound1) {
      conv(ee, e);

      mul(h, B, ee);

      if (use_double_mul1) {
         for (i = 0; i < n; i++)
            double_h[i] = to_double(rep(h[i]));

         double_MixedMul1(t, double_h, double_A, n);
      }
      else if (use_double_mul2) {
         for (i = 0; i < n; i++)
            double_h[i] = to_double(rep(h[i]));

         double_MixedMul2(t, double_h, double_A, n, double_limit);
      }
      else if (use_long_mul1) {
         for (i = 0; i < n; i++)
            long_h[i] = to_long(rep(h[i]));

         long_MixedMul1(t, long_h, long_A, n);
      }
      else if (use_long_mul2) {
         for (i = 0; i < n; i++)
            long_h[i] = to_long(rep(h[i]));

         long_MixedMul2(t, long_h, long_A, n, long_limit);
      }
      else
         MixedMul(t, h, A); // t = h*A

      SubDiv(e, t, zz_p::modulus()); // e = (e-t)/p
      MulAdd(x, prod, h);  // x = x + prod*h

      mul(prod, prod, zz_p::modulus());
   }

   vec_ZZ num, denom;
   ZZ d, d_mod_prod, tmp1;

   num.SetLength(n);
   denom.SetLength(n);

   d = 1;
   d_mod_prod = 1;

   for (i = 0; i < n; i++) {
      rem(x[i], x[i], prod);
      MulMod(x[i], x[i], d_mod_prod, prod);

      if (!ReconstructRational(num[i], denom[i], x[i], prod,
           num_bound, den_bound))
          LogicError("solve1 internal error: rat recon failed!");

      mul(d, d, denom[i]);

      if (i != n-1) {
         if (denom[i] != 1) {
            div(den_bound, den_bound, denom[i]);
            mul(bound1, num_bound, den_bound);
            mul(bound1, bound1, 2);

            div(tmp1, prod, zz_p::modulus());
            while (tmp1 > bound1) {
               prod = tmp1;
               div(tmp1, prod, zz_p::modulus());
            }

            rem(tmp1, denom[i], prod);
            rem(d_mod_prod, d_mod_prod, prod);
            MulMod(d_mod_prod, d_mod_prod, tmp1, prod);
         }
      }
   }

   tmp1 = 1;
   for (i = n-1; i >= 0; i--) {
      mul(num[i], num[i], tmp1);
      mul(tmp1, tmp1, denom[i]);
   }

   x_out.SetLength(n);

   for (i = 0; i < n; i++) {
      x_out[i] = num[i];
   }

   d_out = d;
}

NTL_END_IMPL


// 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.
// #define CRS_CVEC_MULT_NO_ACCEL

// #define CRS_CVEC_MULT_NO_ACCEL // for benchmarking
// #define MTL_LAZY_LOOP_WO_UNROLL // for benchmarking

#include <iostream>
#include <typeinfo>
#include <boost/timer.hpp>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

int main()
{
    mtl::vampir_trace<9999> tracer;
  // For a more realistic example set size to 1000 or larger
  const int size = 1000, N = size * size;
  using namespace mtl;

  typedef unsigned size_type;
  // typedef std::size_t size_type;
  std::cout << "sizeof in size_type is " << sizeof(size_type) << '\n';
  typedef mat::parameters<row_major, mtl::index::c_index, non_fixed::dimensions, false, size_type> para;
  typedef compressed2D<double, para>  matrix_type;
  matrix_type          A(N, N);
  laplacian_setup(A, size, size);

  itl::pc::ilu_0<matrix_type, float>     P(A);
  itl::pc::identity<matrix_type>  P2(A);

  mtl::dense_vector<double> x(N, 1.0), b(N);

  b = A * x;

  for (int l= 2; l <= 10; l++) {
      x= 0;
      itl::cyclic_iteration<double> iter(b, 100, 1.e-6, 0.0, 100);
      boost::timer t;
      bicgstab_ell(A, x, b, P, P2, iter, l);
      std::cout << "BiCGStab(" << l << ") took " << t.elapsed() << "s.\n";
  }

  return 0;
}


#include <mex.h>
#include <Eigen/Eigen>
#include "localize.h"

// Takes an mxArray and returns its contents as an eigen matrix by reference
void input_matrix(const mxArray* p, Eigen::MatrixXd& mat) {
    int m = mxGetM(p);
    int n = mxGetN(p);

    mat.resize(m,n);

    double *data = mxGetPr(p);

    for(int j = 0; j < n; j++) {
        for(int i = 0; i < m; i++) {
            mat(i,j) = data[m*j + i];
        }
    }
}

// Should be called as:
// [P, inliers] = CameraPoseRANSAC_Mex(X,x,tol,nIters), where X is a 3xN matrix containing the 3D points, and
// x is a 3xN matrix containing the image points as unit vectors and tol is the largest allowed
// angular deviation (in radians) for a correspondence to be considered an inlier, and nIters is the max number of iterations.
// The output P is the calculated camera matrix as well as an integer designating the number of inliers.
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    // Validate the input
    if(nrhs != 4)
        mexErrMsgTxt("Invalid input! The function takes three inputs: \n\t3d structure X as a 3xN matrix, \n\ta 3xN matrix representing image points as unit vectors, \n\tthe maximum allowed angle deviation for a correspondence to be considered an inlier,\n\tnumber of RANSAC iterations.");
    if(nlhs != 2)
        mexErrMsgTxt("Invalid output! The function returns two outputs: P representing the triangulated camera matrix and an integer designating the number of inliers for P. ");

    Eigen::MatrixXd x;
    Eigen::MatrixXd X;
    double tol = mxGetScalar(prhs[2]);
    int nIter = int(mxGetScalar(prhs[3]));

    input_matrix(prhs[0],X);
    input_matrix(prhs[1],x);

    if(x.rows() != 3 || X.rows() != 3 || x.cols() != X.cols())
        mexErrMsgTxt("Input matrices must both have three rows and the same number of columns!");

    // Inputs and outputs are ok! Calculate camera pose!
    int numInliers = 0;
    Eigen::Matrix<double,3,4> P = Eigen::MatrixXd::Zero(3,4);
    bool success = cameraPoseRANSAC(x,X,nIter,tol,P,numInliers);
    if (!success)
    {
        numInliers = 0;
    }

    // Create output
    plhs[0] = mxCreateDoubleMatrix(P.rows(),4,mxREAL);
    double* outputMatrix = mxGetPr(plhs[0]);
    int index = 0;
    for (int j = 0; j < P.cols(); j++) {
        for (int i = 0; i < P.rows(); i++) {
            outputMatrix[index] = P(i,j);
            index++;
        }
    }

    plhs[1] = mxCreateDoubleMatrix(1,1,mxREAL);
    double* output = mxGetPr(plhs[1]);
    output[0] = static_cast<double>(numInliers);
}


// Copyright 2018 Hans Dembinski
//
// 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_HISTOGRAM_ACCUMULATORS_WEIGHTED_MEAN_HPP
#define BOOST_HISTOGRAM_ACCUMULATORS_WEIGHTED_MEAN_HPP

#include <boost/histogram/fwd.hpp>
#include <type_traits>

namespace boost {
namespace histogram {
namespace accumulators {

/**
  Calculates mean and variance of weighted sample.

  Uses West's incremental algorithm to improve numerical stability
  of mean and variance computation.
*/
template <typename RealType>
class weighted_mean {
public:
  weighted_mean() = default;
  weighted_mean(const RealType& wsum, const RealType& wsum2, const RealType& mean,
                const RealType& variance)
      : sum_of_weights_(wsum)
      , sum_of_weights_squared_(wsum2)
      , weighted_mean_(mean)
      , sum_of_weighted_deltas_squared_(
            variance * (sum_of_weights_ - sum_of_weights_squared_ / sum_of_weights_)) {}

  void operator()(const RealType& x) { operator()(1, x); }

  void operator()(const RealType& w, const RealType& x) {
    sum_of_weights_ += w;
    sum_of_weights_squared_ += w * w;
    const auto delta = x - weighted_mean_;
    weighted_mean_ += w * delta / sum_of_weights_;
    sum_of_weighted_deltas_squared_ += w * delta * (x - weighted_mean_);
  }

  template <typename T>
  weighted_mean& operator+=(const weighted_mean<T>& rhs) {
    const auto tmp = weighted_mean_ * sum_of_weights_ +
                     static_cast<RealType>(rhs.weighted_mean_ * rhs.sum_of_weights_);
    sum_of_weights_ += static_cast<RealType>(rhs.sum_of_weights_);
    sum_of_weights_squared_ += static_cast<RealType>(rhs.sum_of_weights_squared_);
    weighted_mean_ = tmp / sum_of_weights_;
    sum_of_weighted_deltas_squared_ +=
        static_cast<RealType>(rhs.sum_of_weighted_deltas_squared_);
    return *this;
  }

  weighted_mean& operator*=(const RealType& s) {
    weighted_mean_ *= s;
    sum_of_weighted_deltas_squared_ *= s * s;
    return *this;
  }

  template <typename T>
  bool operator==(const weighted_mean<T>& rhs) const noexcept {
    return sum_of_weights_ == rhs.sum_of_weights_ &&
           sum_of_weights_squared_ == rhs.sum_of_weights_squared_ &&
           weighted_mean_ == rhs.weighted_mean_ &&
           sum_of_weighted_deltas_squared_ == rhs.sum_of_weighted_deltas_squared_;
  }

  template <typename T>
  bool operator!=(const T& rhs) const noexcept {
    return !operator==(rhs);
  }

  const RealType& sum_of_weights() const noexcept { return sum_of_weights_; }
  const RealType& value() const noexcept { return weighted_mean_; }
  RealType variance() const {
    return sum_of_weighted_deltas_squared_ /
           (sum_of_weights_ - sum_of_weights_squared_ / sum_of_weights_);
  }

  template <class Archive>
  void serialize(Archive&, unsigned /* version */);

private:
  RealType sum_of_weights_ = RealType(), sum_of_weights_squared_ = RealType(),
           weighted_mean_ = RealType(), sum_of_weighted_deltas_squared_ = RealType();
};

} // namespace accumulators
} // namespace histogram
} // namespace boost

#ifndef BOOST_HISTOGRAM_DOXYGEN_INVOKED
namespace std {
template <class T, class U>
/// Specialization for boost::histogram::accumulators::weighted_mean.
struct common_type<boost::histogram::accumulators::weighted_mean<T>,
                   boost::histogram::accumulators::weighted_mean<U>> {
  using type = boost::histogram::accumulators::weighted_mean<common_type_t<T, U>>;
};
} // namespace std
#endif

#endif


#define BOOST_TEST_MODULE Gpufit

#include "Gpufit/gpufit.h"

#include <boost/test/included/unit_test.hpp>

#include <array>

BOOST_AUTO_TEST_CASE( Fletcher_Powell_Helix )
{
    /*
        Performs a single fit using the FLETCHER_POWELL_HELIX model.
        - zero data is passed in.
        - Checks final chi_square to be near by zero.
        - Checks fitted parameters equalling the true parameters.
    */

    std::size_t const n_fits{ 1 } ;
    std::size_t const n_points{ 3 } ;
    std::size_t const n_parameters{ 3 } ;

    std::array< REAL, n_parameters > const true_parameters{ { 1., 0., 0. } };

    std::array< REAL, n_points > data{ { 0., 0., 0. } } ;

    std::array< REAL, n_parameters > initial_parameters{ { -1., 0., 0. } } ;

    REAL tolerance{ 1e-8f } ;

    int max_n_iterations{ 50 } ;

    std::array< int, n_parameters > parameters_to_fit{ { 1, 1, 1 } } ;

    std::array< REAL, n_parameters > output_parameters ;
    int output_state ;
    REAL output_chi_square ;
    int output_n_iterations ;

    // test initial_parameters * 1.
    int status = gpufit
        (
            n_fits,
            n_points,
            data.data(),
            0,
            FLETCHER_POWELL_HELIX,
            initial_parameters.data(),
            tolerance,
            max_n_iterations,
            parameters_to_fit.data(),
            LSE,
            0,
            0,
            output_parameters.data(),
            & output_state,
            & output_chi_square,
            & output_n_iterations
        ) ;

    BOOST_CHECK( status == 0 ) ;
    BOOST_CHECK( output_state == 0 );
    BOOST_CHECK( output_n_iterations <= 9 );
    BOOST_CHECK( output_chi_square < 1e-26f );

    BOOST_CHECK(std::abs(output_parameters[0] - true_parameters[0]) < 1e-13);
    BOOST_CHECK(std::abs(output_parameters[1] - true_parameters[1]) < 1e-13);
    BOOST_CHECK(std::abs(output_parameters[2] - true_parameters[2]) < 1e-13);

    // test initial_parameters * 10.
    for (int i = 0; i < n_parameters; i++)
        initial_parameters[i] *= 10.;

    status = gpufit
    (
        n_fits,
        n_points,
        data.data(),
        0,
        FLETCHER_POWELL_HELIX,
        initial_parameters.data(),
        tolerance,
        max_n_iterations,
        parameters_to_fit.data(),
        LSE,
        0,
        0,
        output_parameters.data(),
        &output_state,
        &output_chi_square,
        &output_n_iterations
    );

    BOOST_CHECK(status == 0);
    BOOST_CHECK(output_state == 0);
    BOOST_CHECK(output_n_iterations <= 21);
    BOOST_CHECK(output_chi_square < 1e-26);

    BOOST_CHECK(std::abs(output_parameters[0] - true_parameters[0]) < 1e-13);
    BOOST_CHECK(std::abs(output_parameters[1] - true_parameters[1]) < 1e-13);
    BOOST_CHECK(std::abs(output_parameters[2] - true_parameters[2]) < 1e-13);

    // test initial_parameters * 100.
    for (int i = 0; i < n_parameters; i++)
        initial_parameters[i] *= 10.;

    status = gpufit
    (
        n_fits,
        n_points,
        data.data(),
        0,
        FLETCHER_POWELL_HELIX,
        initial_parameters.data(),
        tolerance,
        max_n_iterations,
        parameters_to_fit.data(),
        LSE,
        0,
        0,
        output_parameters.data(),
        &output_state,
        &output_chi_square,
        &output_n_iterations
    );

    BOOST_CHECK(status == 0);
    BOOST_CHECK(output_state == 0);
    BOOST_CHECK(output_n_iterations <= 29);
    BOOST_CHECK(output_chi_square < 1e-20);

    BOOST_CHECK(std::abs(output_parameters[0] - true_parameters[0]) < 1e-10);
    BOOST_CHECK(std::abs(output_parameters[1] - true_parameters[1]) < 1e-10);
    BOOST_CHECK(std::abs(output_parameters[2] - true_parameters[2]) < 1e-10);
}


/*
 * Copyright 2019 © Centre Interdisciplinaire de développement en Cartographie des Océans (CIDCO), Tous droits réservés
 */

/*
 * File:   SideScanGeoreferencingTest.hpp
 * Author: Jordan McManus <jordan.mcmanus@cidco.ca>
 *
 * Created on March 11, 2020, 1:18 PM
 */

#ifndef SIDESCANGEOREFERENCINGTEST_HPP
#define SIDESCANGEOREFERENCINGTEST_HPP

#include "catch.hpp"
#include <Eigen/Dense>
#include "../src/Position.hpp"
#include "../src/sidescan/SideScanGeoreferencing.hpp"
#include "../src/math/CoordinateTransform.hpp"
#include "../src/utils/Constants.hpp"

TEST_CASE("Georeferencing Side Scan No LeverArm No Layback test") {

    double earthRadius = 6.3781e6; // in meters
    double objectLatRadians = 0.0;
    double objectLonRadians = 0.0 + 10 / (earthRadius); // 10 meters east of Greenwich
    double objectHeight = 0.0;

    double shipLat = 0.0;
    double shipLon = 0.0;
    double shipHeight = 0.0;
    Position shipPositionAtEquatorGreenwichMeridian(0, shipLat, shipLon, shipHeight);

    Eigen::Vector3d shipPositionEcef;
    CoordinateTransform::getPositionECEF(shipPositionEcef, shipPositionAtEquatorGreenwichMeridian);

    Eigen::Vector3d sideDistanceEcef(0, 10, 0);

    Eigen::Vector3d antenna2TowPointLeverArmEcef(0, 0, 0);
    Eigen::Vector3d laybackEcef(0, 0, 0);

    Position objectPosition(0, 0.0, 0.0, 0.0);
    SideScanGeoreferencing::georeferenceSideScanEcef(
            shipPositionEcef,
            antenna2TowPointLeverArmEcef,
            laybackEcef,
            sideDistanceEcef,
            objectPosition);

    double latLonTreshold = 1e-9; // about 5.7e-8 degrees
    double heightTrashold = 1e-5; // 10 micrometers
    REQUIRE(std::abs(objectPosition.getLatitude() * D2R - objectLatRadians) < latLonTreshold);
    REQUIRE(std::abs(objectPosition.getLongitude() * D2R - objectLonRadians) < latLonTreshold);
    REQUIRE(std::abs(objectPosition.getEllipsoidalHeight() - objectHeight) < heightTrashold);
}

#endif /* SIDESCANGEOREFERENCINGTEST_HPP */


// Copyright (c) 2013-2016. This code was produced by the
// Australian Centre for Field Robotics, The University of Sydney under
// the Future Flight Planning project, University Reference 13996, contract
// NSW-CPS-2011-015264, Work Orders 5, 7 and 8. The intellectual property
// ownership is as set out in these contracts, as registered with
// Commercial Development and Industry Partnerships.

#include <iostream>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <comma/application/command_line_options.h>
#include <comma/csv/stream.h>
#include <comma/visiting/traits.h>
#include "../../../math/range_bearing_elevation.h"
#include "../coordinates.h"
#include "../sample.h"
#include "../traits.h"

static const std::string app_name = "sphere-calc";

static void usage( bool verbose )
{
    std::cerr << std::endl;
    std::cerr << "generates a sample ever the whole sphere" << std::endl;
    std::cerr << std::endl;
    std::cerr << "usage: sphere-calc <options> > sample.csv" << std::endl;
    std::cerr << std::endl;
    std::cerr << "options" << std::endl;
    std::cerr << "    -h|--help                       Show this help (-v|--verbose to show csv options)" << std::endl;
    std::cerr << std::endl;
    std::cerr << "sample type options" << std::endl;
    std::cerr << "    --random                        Random uniform sample" << std::endl;
    std::cerr << "    --regular                       Regular grid sample" << std::endl;
    std::cerr << "    --regular-uniform,--uniform     Regular, but pretty uniform" << std::endl;
    std::cerr << std::endl;
    std::cerr << "random seed options" << std::endl;
    std::cerr << "    -s|--seed=<random-seed>         seed for the random sample" << std::endl;
    std::cerr << "    -t|--seed-time                  use current time as random number seed" << std::endl;
    std::cerr << std::endl;
    std::cerr << "sample options" << std::endl;
    std::cerr << "    --begin,--from=<latitude,longitude>; default: -90, -180" << std::endl;
    std::cerr << "    --end,--to=<latitude,longitude>; end not included, default: 90.00000001, 180" << std::endl;
    std::cerr << "    --resolution,-r=<value>         Sample resolution in degrees" << std::endl;
    if( verbose ) { std::cerr << std::endl << "csv options" << std::endl << comma::csv::options::usage() << std::endl; }
    std::cerr << std::endl;
    exit( 0 );
}

template< typename T > T get_option( comma::command_line_options &options, const std::string option_name, const T &default_val )
{
    if( !options.exists( option_name ) ) { return default_val; }
    T result = default_val;
    result = comma::csv::ascii< T >().get( options.value< std::string >( option_name ) );
    return result;
}

using namespace snark;

int main( int ac, char** av )
{
    try
    {
        using snark::spherical::coordinates;

        comma::command_line_options options( ac, av, usage );
        if( options.exists( "--verbose,-v" ) ) { std::cerr << "sphere-calc: called as: " << options.string() << std::endl; }
        double resolution = options.value< double >( "--resolution,-r" ) * M_PI / 180;
        comma::csv::output_stream< coordinates > ostream( std::cout, comma::csv::options( options ) );
        options.assert_mutually_exclusive( "--random,--regular" );
        bool regular = options.exists( "--regular" );
        bool random = options.exists( "--random" );
        bool regular_uniform = options.exists( "--regular-uniform,--uniform" );
        if( !regular && !random && !regular_uniform ) { std::cerr << "sphere-calc: expected sample type (--random, --regular, or --uniform)" << std::endl; return 1; }
        coordinates begin = get_option< coordinates >( options, "--begin", coordinates( -M_PI / 2, -M_PI ) );
        static const double epsilon = 0.00000001;
        coordinates end = get_option< coordinates >( options, "--end", coordinates( M_PI / 2 + epsilon, M_PI - epsilon ) );
        if( begin.latitude < -M_PI / 2 ) { begin.latitude = -M_PI / 2; }
        if( begin.longitude < -M_PI ) { begin.longitude = -M_PI; }
        if( end.latitude > M_PI / 2 ) { end.latitude = M_PI / 2; }
        if( end.longitude > M_PI ) { end.longitude = M_PI; }
        if( regular )
        {
            for( coordinates c = begin; comma::math::less( c.latitude, end.latitude ); c.latitude += resolution )
            {
                for( c.longitude = begin.longitude; comma::math::less( c.longitude, end.longitude ); c.longitude += resolution )
                {
                    ostream.write( c );
                }
            }
        }
        else if( random || regular_uniform )
        {
            options.assert_mutually_exclusive( "--seed,-s,--seed-time,-t" );
            boost::optional< unsigned long > seed = options.optional< unsigned long >( "--seed,-s" );
            if( options.exists( "--seed-time,-t" ) ) { seed = static_cast< unsigned long >( std::time( 0 ) ); }
            boost::mt19937 generator;
            if( seed ) { generator.seed( *seed ); }
            boost::uniform_real< double > distribution( 0, 1 );
            boost::variate_generator< boost::mt19937&, boost::uniform_real< double > > r( generator, distribution );
            for( coordinates c = begin; comma::math::less( c.latitude, end.latitude ); c.latitude += resolution )
            {
                double radius = std::abs( std::cos( c.latitude ) );
                if( radius == 0 ) { continue; }
                double step = resolution / radius;
                double offset = random ? ( r() * 2 - 1 ) * step : 0.0;
                for( c.longitude = begin.longitude + offset; comma::math::less( c.longitude, end.longitude + offset ); c.longitude += step )
                {
                    ostream.write( regular_uniform ? c : pretty_uniform_sample( c, resolution / 2 ) );
                }
            }
        }
        return 0;
    }
    catch( std::exception& ex ) { std::cerr << "sphere-calc: " << ex.what() << std::endl; }
    catch( ... ) { std::cerr << "sphere-calc: unknown exception" << std::endl; }
    return 1;
}


// Copyright Abel Sinkovics (abel@sinkovics.hu)  2011.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <boost/metaparse/foldl1.hpp>
#include <boost/metaparse/build_parser.hpp>
#include <boost/metaparse/transform.hpp>
#include <boost/metaparse/one_of_c.hpp>
#include <boost/metaparse/entire_input.hpp>
#include <boost/metaparse/string.hpp>

#include <boost/metaparse/util/digit_to_int.hpp>

#include <boost/mpl/int.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/times.hpp>

#include <iostream>

using boost::metaparse::foldl1;
using boost::metaparse::build_parser;
using boost::metaparse::transform;
using boost::metaparse::one_of_c;
using boost::metaparse::entire_input;

using boost::metaparse::util::digit_to_int;

using boost::mpl::int_;
using boost::mpl::plus;
using boost::mpl::times;

/*
 * The grammar
 *
 * expression ::= ('1' | '0')*
 */

struct next_element
{
  template <class Acc, class B>
  struct apply : plus<times<Acc, int_<2> >, B> {};
};

typedef
  foldl1<transform<one_of_c<'0', '1'>, digit_to_int<> >, int_<0>, next_element>
  S;

typedef build_parser<entire_input<S> > binary_parser;

template <class S>
struct binary : binary_parser::apply<S>::type {};

#ifdef _STR
#  error _STR already defined
#endif
#define _STR BOOST_METAPARSE_STRING

#if BOOST_METAPARSE_STD < 2011

int main()
{
  using std::cout;
  using std::endl;
  using boost::metaparse::string;

  cout
    << binary<string<'1','0','0'> >::value << endl
    << binary<string<'1','0','1','1'> >::value << endl
    << binary<string<'1'> >::value << endl;
}
#else
int main()
{
  using std::cout;
  using std::endl;

  cout
    << binary<_STR("100")>::value << endl
    << binary<_STR("1011")>::value << endl
    << binary<_STR("1")>::value << endl;
}
#endif


#ifndef INTEGRATION_HPP
#define INTEGRATION_HPP

#include <Eigen/Dense>
using namespace Eigen;

class PhysicalSystem {
    // parent class for physical systems that support implicit integration
public:
    // return number of *positional* degrees of freedom (not velocity)
    virtual int getDOFs() = 0;
    // write position and velocity into vectors
    virtual void getState(VectorXd &x, VectorXd &v) = 0;
    // read position and velocity from vectors
    virtual void setState(const VectorXd &x, const VectorXd &v) = 0;
    // write mass matrix
    virtual void getInertia(MatrixXd &M) = 0;
    // write forces
    virtual void getForces(VectorXd &f) = 0;
    // write Jacobians
    virtual void getJacobians(MatrixXd &Jx, MatrixXd &Jv) = 0;
};

// perform a forward Euler step of length dt
void forwardEulerStep(PhysicalSystem *system, double dt);

// perform a backward Euler step of length dt
void backwardEulerStep(PhysicalSystem *system, double dt);

#endif


/*
Copyright (c) 2015 Jeff Epler

This software is provided 'as-is', without any express or implied
warranty. In no event will the authors be held liable for any damages
arising from the use of this software.

Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it
freely, subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not
   claim that you wrote the original software. If you use this software
   in a product, an acknowledgement in the product documentation would be
   appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be
   misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

#ifndef DASHING_H
#define DASHING_H

#include <cmath>
#include <cassert>
#include <vector>
#include <string>
#include <fstream>
#include <stdexcept>
#include <boost/algorithm/string/replace.hpp>
#include <boost/algorithm/string/trim.hpp>

namespace dashing
{

struct PSMatrix {
    double a, b, c, d, e, f;

    PSMatrix inverse() const;
    double determinant() const { return a * d - b * c; }
};

PSMatrix Translation(double x, double y);
PSMatrix Rotation(double theta);
PSMatrix XSkew(double xk);
PSMatrix YScale(double ys);

struct Point { double x, y; };
inline Point operator*(const Point &p, const PSMatrix &m) {
    return Point{ p.x*m.a + p.y*m.c + m.e,
                  p.x*m.b + p.y*m.d + m.f };
}
inline Point operator*(const Point &p, double d)
{
    return Point{ p.x * d, p.y * d };
}
inline Point operator*(double d, const Point &p)
{
    return Point{ p.x * d, p.y * d };
}
inline Point operator+(const Point &p, const Point &q)
{
    return Point{ p.x + q.x, p.y * q.y };
}

PSMatrix operator*(const PSMatrix &m1, const PSMatrix m2);

struct Dash {
    PSMatrix tr, tf;
    std::vector<double> dash, sum;

    Dash(double th, double x0, double y0, double dx, double dy,
            const std::vector<double>::const_iterator dbegin,
            const std::vector<double>::const_iterator dend);

    static Dash FromString(const std::string &line, double scale);
};

struct Segment { Point p, q; bool swapped; };
struct Intersection { double u; bool positive; };
inline bool operator<(const Intersection &a, const Intersection &b)
{
    return a.u < b.u;
}

// "sort" a segment so that its first component has the lower y-value
inline void ysort(Segment &s) {
    if(s.p.y < s.q.y) return;
    s.swapped = ! s.swapped;
    std::swap(s.p, s.q);
}

inline double intceil(double x) { return int(ceil(x)); }
inline double intfloor(double x) { return int(floor(x)); }

inline double pythonmod(double a, double b) {
    auto r = a - floor(a / b) * b;
    if(r == b) return 0;
    return r;
}

inline size_t utoidx(const Dash &d, double u, double &o) {
    u = pythonmod(u, d.sum.back());
    for(size_t i = 1; i != d.sum.size(); i++) {
        if(u < d.sum[i]) { o = u - d.sum[i-1]; return i-1; }
    }
    abort(); // should be unreachable
}

template<class Cb>
void uvdraw(const Dash &pattern, double v, double u1, double u2, Cb cb) {
    if(pattern.dash.empty()) { cb(v, u1, u2); return;  }
    double o;
    auto i = utoidx(pattern, u1, o);
    const auto &pi = pattern.dash[i];
    if(pi >= 0) { cb(v, u1, std::min(u2, u1+pi-o)); u1 += pi-o; }
    else { u1 -= pi+o; }
    i = i + 1;
    if(i == pattern.dash.size()) i = 0;
    for(auto u = u1; u < u2;) {
        const auto &pi = pattern.dash[i];
        if(pi >= 0) { cb(v, u, std::min(u2, u+pi)); u += pi; }
        else { u -= pi; }
        i = i + 1;
        if(i == pattern.dash.size()) i = 0;
    }
}

template<class Cb, class Wr>
void uvspans(const Dash &pattern, std::vector<Segment> && segments, Cb cb, std::vector<Intersection> &uu, Wr wr) {
    if(segments.empty()) return; // no segments

    for(auto &s : segments) ysort(s);
    std::sort(segments.begin(), segments.end(),
        [](const Segment &a, const Segment &b) {
            return a.p.y < b.p.y; // sort in increasing p.y
        });

    // we want to maintain the heap condition in such a way that we can always
    // quickly pop items that our span has moved past.
    // C++ heaps are max-heaps, so we need a decreasing sort.
    auto heapcmp = [](const Segment &a, const Segment &b) {
            return b.q.y < a.q.y; // sort in decreasing q.y;
        };

    auto segments_begin = segments.begin();
    auto heap_begin = segments.begin(), heap_end = segments.begin();

    auto vstart = intfloor(segments.front().p.y);
    auto vend = intceil(std::max_element(segments.begin(), segments.end(),
                [](const Segment &a, const Segment &b) {
                        return a.q.y < b.q.y; // sort in increasing q.y;
                })->q.y);

    // sweep-line algorithm to intersects spans with segments
    // "active" holds segments that may intersect with this span;
    // when v moves below an active segment, drop it from the active heap.
    // when v moves into a remaining segment, move it from segments to active.
    for(auto v = vstart; v != vend; v++) {
        uu.clear();

        while(heap_begin != heap_end && heap_begin->q.y < v)
        {
            std::pop_heap(heap_begin, heap_end, heapcmp);
            heap_end --;
        }
        while(segments_begin != segments.end() && segments_begin->p.y < v) {
            const auto &s = *segments_begin;
            if(s.q.y >= v) {
                *heap_end++ = s;
                std::push_heap(heap_begin, heap_end, heapcmp);
            }
            segments_begin ++;
        }

        for(const auto &s : boost::make_iterator_range(heap_begin, heap_end)) {
            auto du = s.q.x - s.p.x;
            auto dv = s.q.y - s.p.y;
            assert(dv);
            if(dv) uu.push_back(
                    Intersection{s.p.x + du * (v - s.p.y) / dv,s.swapped});
        }
        std::sort(uu.begin(), uu.end());
        int winding = 0;
        double old_u = -std::numeric_limits<double>::infinity();
        for(const auto &isect : uu) {
            if(wr(winding)) uvdraw(pattern, v, old_u, isect.u, cb);
            winding += 2*isect.positive - 1;
            old_u = isect.u;
        }
    }
}

struct HatchPattern {
    std::vector<Dash> d;
    static HatchPattern FromFile(std::istream &fi, double scale) {
        HatchPattern result;

        std::string line;
        while(getline(fi, line)) {
            auto i = line.find(";");
            if(i != line.npos) line.erase(i, line.npos);
            boost::algorithm::trim(line);
            if(line.empty()) continue;
            if(line[0] == '*') continue;
            result.d.push_back(Dash::FromString(line, scale));
        }
        return result;
    }

    static HatchPattern FromFile(const char *filename, double scale) {
        std::ifstream fi(filename);
        return FromFile(fi, scale);
    }
};

template<class It, class Cb, class Wr>
void xyhatch(const Dash &pattern, It start, It end, Cb cb, std::vector<Segment> &uvsegments, std::vector<Intersection> &uu, Wr wr) {
    uvsegments.clear();
    bool swapped = pattern.tf.determinant() < 0;
    std::transform(start, end, std::back_inserter(uvsegments),
        [&](const Segment &s)
        { return Segment{s.p * pattern.tf, s.q * pattern.tf, swapped != s.swapped };
    });
    uvspans(pattern, std::move(uvsegments), [&](double v, double u1, double u2) {
        Point p{u1, v}, q{u2, v};
        Segment xy{ p * pattern.tr, q * pattern.tr, false };
        cb(xy);
    }, uu, wr);
}

template<class It, class Cb, class Wr>
void xyhatch(const HatchPattern &pattern, It start, It end, Cb cb, Wr wr) {
    std::vector<Segment> uvsegments;
    uvsegments.reserve(end-start);
    std::vector<Intersection> uu;
    uu.reserve(8);
    for(const auto &i : pattern.d) xyhatch(i, start, end, cb, uvsegments, uu, wr);
}

template<class C, class Cb, class Wr>
void xyhatch(const HatchPattern &pattern, const C &c, Cb cb, Wr wr) {
    xyhatch(pattern, c.begin(), c.end(), cb, wr);
}

}
#endif


#include <dai/alldai.h>
#include <iostream>
#include <fstream>
#include <string>
#include "dai/emrun.h"
#include <dai/util.h>
#include <string>
#include <sys/stat.h>
#include <time.h>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/generator_iterator.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>
#include <boost/random/normal_distribution.hpp>
#include <iomanip>      // std::setprecision
#include <sstream>
#include <string>
#include <cstdlib>
#include <boost/lexical_cast.hpp>
#include <boost/tokenizer.hpp>
#include <algorithm>
#include<math.h>
using namespace std;
using namespace dai;
using namespace boost;
void seed()
{
	/*Declare variable to hold seconds on clock.*/
    //	time_t seconds;
	/*Get value from system clock and place in seconds variable.*/
    //	time(&seconds);
	/*Convert seconds to a unsigned integer.*/
    struct timeval time;
    gettimeofday(&time,NULL);

    // microsecond has 1 000 000
    srand((time.tv_sec * 1000) + (time.tv_usec));
    //  rnd_seed((time.tv_sec * 1000) + (time.tv_usec / 10) );
}
double unifRand()
{
    return rand()/double(RAND_MAX);
}

// Define a random number generator and initialize it with a reproducible
// seed.
mt19937 generator(42);

// Define a uniform random number distribution which produces "double"
// values between 0 and 1 (0 inclusive, 1 exclusive).
boost::uniform_real<> uni_dist(0,1);
boost::variate_generator<mt19937&, boost::uniform_real<> > uni(generator, uni_dist);

boost::normal_distribution<> nd(0.0, 1.0);
boost::variate_generator<mt19937&, boost::normal_distribution<> > normal(generator, nd);
int main()
{
    generator.seed(static_cast<unsigned int>(std::time(0)));
    seed();

    for (int i=0; i<10; i++)
    {

    cout<<normal()<<" "<<uni()<<" "<<rnd_stdnormal()<<" "<<rnd_uniform()<<" "<<unifRand()<<endl;
    }
    return 0;
}
//
//int main()
//{
//
//std::vector<double> vec;
//seed();
//  for (int i=0; i<10; i++) {
//	vec.push_back(unifRand());
//	//cout<<vec[i]<<endl;
//	//++p[int(number)];
//  }
//
//const double total = std::accumulate(vec.begin(), vec.end(), 0.0);
//for (double& value: vec)
//	{
// 	value /= total;
//	cout<<value <<endl;
//	}
//
//  return 0;
//}


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

// Copyright Paul A. Bristow 2015 - 2016.
// Copyright Christopher Kormanyos 2015 - 2016.

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.
// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

// This file also includes Doxygen-style documentation about the function of the code.
// See http://www.doxygen.org for details.

//! \file

// Below are snippets of code that can be included into a Quickbook file.

#include <exception>
#include <iomanip>
#include <iostream>
#include <limits>
#include <typeinfo>

//[fixed_point_include_1
#include <boost/fixed_point/fixed_point.hpp>
//] [/fixed_point_include_1]

//[fixed_point_typedef_1
typedef boost::fixed_point::negatable<15, -16> fixed_point_type;
//] [/fixed_point_typedef_1]

int main()
{
  try
  {
    std::cout.precision(std::numeric_limits<fixed_point_type>::digits10 + 1);

//[show_numeric_limits_1

    std::cout << "Numeric_limits for type"                                                 << std::endl
              << typeid(fixed_point_type).name()                                           << std::endl
              << "digits10     = " <<  std::numeric_limits<fixed_point_type>::digits10     << std::endl
              << "max_digits10 = " <<  std::numeric_limits<fixed_point_type>::max_digits10 << std::endl
              << "radix        = " <<  std::numeric_limits<fixed_point_type>::radix        << std::endl
              << "epsilon      = " <<  std::numeric_limits<fixed_point_type>::epsilon()    << std::endl
              << "max          = " << (std::numeric_limits<fixed_point_type>::max)()       << std::endl
              << "min          = " << (std::numeric_limits<fixed_point_type>::min)()       << std::endl
              << "lowest       = " <<  std::numeric_limits<fixed_point_type>::lowest()     << std::endl
              ;

    if(std::numeric_limits<fixed_point_type>::has_infinity)
    {
      std::cout << "infinity = " << std::numeric_limits<fixed_point_type>::infinity() << std::endl;
    }
    else
    {
      std::cout << "Type does not have an infinity" << std::endl;
    }

    if (std::numeric_limits<fixed_point_type>::has_quiet_NaN)
    {
      std::cout << "NaN = " << std::numeric_limits<fixed_point_type>::quiet_NaN() << std::endl;
    }
    else
    {
      std::cout << "Type does not have a NaN" << std::endl;
    }
//] [/show_numeric_limits_1]

    std::cout.setf(std::ios::boolalpha | std::ios::showpoint); // Show any trailing zeros.
    std::cout << std::endl;

//[fixed_example_1

    fixed_point_type x = fixed_point_type(123) /   100;
    fixed_point_type y = fixed_point_type(456) / 10000;

    // Show all the significant digits for this particular floating-point type.
    std::cout.precision(std::numeric_limits<fixed_point_type>::digits10);

    std::cout << "x = fixed_point_type(123) /   100 = "
              << x // 1.22999573 is the nearest representation of decimal digit string 1.23.
              << std::endl;

    std::cout << "y = fixed_point_type(456) / 10000 = "
              << y // 0.0455932617 is the nearest representation of decimal digit string 0.0456
              << std::endl;

    x = -x / 2; // Do some trivial arithmetic.

    std::cout <<"x = -x / 2 = "
              << x // -0.614990234  is the nearest representation of decimal digit string -0.615
              << std::endl;

//] [/fixed_example_1]

//[fixed_example_functions

    int exponential;

    x = frexp(x, &exponential);

    std::cout << "x = frexp(x, &exponential) = "
              << x // 0.614990234
              << " exponential = "
              << exponential
              << std::endl; // 0

    exponential++; // double the value.

    std::cout << "double the value" << std::endl;

    x = ldexp(x, exponential);

    // Show the fraction and exponent parts after changing the exponent.
    std::cout << "x = frexp(x, &exponential) = "
              << frexp(x, &exponential) // -0.614990234
              << " exponential = "
              << exponential
              << std::endl; // 1

    // Other C numeric math functions (cmath) are available, of course, for example:
    std::cout << "ldexp(x, exponential); = "
              << x
              << std::endl; // -1.22998047

    std::cout << "abs  (x)       = "
              <<  abs(x)
              << std::endl; // 1.22998047

    std::cout << "fabs (x)       = " << fabs(x) << std::endl; // 1.22998047

    std::cout << "sqrt (fabs(x)) = "
              <<  sqrt(fabs(x))
              << std::endl; //  = 0.

    std::cout << "sqrt (y)       = "
              <<  sqrt(y)
              << std::endl; //  = 0.213516235

//] [/fixed_example_functions]
  }
  catch(const std::exception& ex)
  {
    std::cout << ex.what() << std::endl;
  }
}

/*
//[numeric_limits_output_1
Numeric_limits for type
class boost::fixed_point::negatable<15,-16,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined>
digits10     = 9
max_digits10 = 11
radix        = 2
epsilon      = 3.051757813e-005
max          = 32767.99998
min          = 1.525878906e-005
lowest       = -32768
Type does not have an infinity
Type does not have a NaN

x = fixed_point_type(123) /   100 = 1.22999573
y = fixed_point_type(456) / 10000 = 0.0455932617
x = -x / 2 = -0.614990234
//] [/numeric_limits_output_1]

//[fixed_example_functions_output
x = frexp(x, &exponential) = -0.614990234 exponential = 0
double the value
x = frexp(x, &exponential) = -0.614990234 exponential = 1
ldexp(x, exponential); = -1.22998047
abs  (x)       = 1.22998047
fabs (x)       = 1.22998047
sqrt (fabs(x)) = 1.10902405
sqrt (y)       = 0.213516235
//] [/fixed_example_functions_output]

*/


#include "consts.hpp"
#include "gamma.hpp"

#include <math.h>
#include <limits>
#include <boost/numeric/conversion/cast.hpp>

namespace cephes {

constexpr double ASYMP_FACTOR = 1e6;

double lbeta_asymp(double a, double b, int *sgn);
double lbeta_negint(int a, double b);
double beta_negint(int a, double b);

double beta(double a, double b)
{
    double y;
    int sign = 1;

    if (a <= 0.0) {
        if (a == std::floor(a)) {
            if (a == boost::numeric_cast<int>(a)) {
                return beta_negint(boost::numeric_cast<int>(a), b);
            }
            else {
                goto overflow;
            }
        }
    }

    if (b <= 0.0) {
        if (b == std::floor(b)) {
            if (b == boost::numeric_cast<int>(b)) {
                return beta_negint(boost::numeric_cast<int>(b), a);
            }
            else {
                goto overflow;
            }
        }
    }

    if (std::fabs(a) < std::fabs(b)) {
        y = a; a = b; b = y;
    }

    if (std::fabs(a) > ASYMP_FACTOR * std::fabs(b) && a > ASYMP_FACTOR) {
        /* Avoid loss of precision in lgam(a + b) - lgam(a) */
        y = lbeta_asymp(a, b, &sign);
        return sign * std::exp(y);
    }

    y = a + b;
    if (std::fabs(y) > MAXGAM || std::fabs(a) > MAXGAM || std::fabs(b) > MAXGAM) {
	int sgngam;
	y = lgam_sgn(y, &sgngam);
	sign *= sgngam;		/* keep track of the sign */
	y = lgam_sgn(b, &sgngam) - y;
	sign *= sgngam;
	y = lgam_sgn(a, &sgngam) + y;
	sign *= sgngam;
	if (y > MAXLOG) {
	    goto overflow;
	}
	return (sign * std::exp(y));
    }

    y = Gamma(y);
    a = Gamma(a);
    b = Gamma(b);
    if (y == 0.0)
        goto overflow;

    if (std::fabs(std::fabs(a) - std::fabs(y)) > std::fabs(std::fabs(b) - std::fabs(y))) {
        y = b / y;
        y *= a;
    }
    else {
        y = a / y;
        y *= b;
    }

    return (y);

overflow:
    return (sign * std::numeric_limits<double>::infinity());
}

/* Natural log of |beta|. */

double lbeta(double a, double b)
{
    double y;
    int sign;

    sign = 1;

    if (a <= 0.0) {
        if (a == std::floor(a)) {
            if (a == boost::numeric_cast<int>(a)) {
                return lbeta_negint(boost::numeric_cast<int>(a), b);
            }
            else {
                goto over;
            }
        }
    }

    if (b <= 0.0) {
        if (b == std::floor(b)) {
            if (b == boost::numeric_cast<int>(b)) {
                return lbeta_negint(boost::numeric_cast<int>(b), a);
            }
            else {
                goto over;
            }
        }
    }

    if (std::fabs(a) < std::fabs(b)) {
        y = a; a = b; b = y;
    }

    if (std::fabs(a) > ASYMP_FACTOR * std::fabs(b) && a > ASYMP_FACTOR) {
        /* Avoid loss of precision in lgam(a + b) - lgam(a) */
        y = lbeta_asymp(a, b, &sign);
        return y;
    }

    y = a + b;
    if (std::fabs(y) > MAXGAM || std::fabs(a) > MAXGAM || std::fabs(b) > MAXGAM) {
	int sgngam;
	y = lgam_sgn(y, &sgngam);
	sign *= sgngam;		/* keep track of the sign */
	y = lgam_sgn(b, &sgngam) - y;
	sign *= sgngam;
	y = lgam_sgn(a, &sgngam) + y;
	sign *= sgngam;
	return (y);
    }

    y = Gamma(y);
    a = Gamma(a);
    b = Gamma(b);
    if (y == 0.0) {
      over:
        return (sign * std::numeric_limits<double>::infinity());
    }

    if (std::fabs(std::fabs(a) - std::fabs(y)) > std::fabs(std::fabs(b) - std::fabs(y))) {
        y = b / y;
        y *= a;
    }
    else {
        y = a / y;
        y *= b;
    }

    if (y < 0) {
	y = -y;
    }

    return std::log(y);
}

/*
 * Asymptotic expansion for  ln(|B(a, b)|) for a > ASYMP_FACTOR*max(|b|, 1).
 */
double lbeta_asymp(double a, double b, int *sgn)
{
    double r = lgam_sgn(b, sgn);
    r -= b * std::log(a);

    r += b*(1-b)/(2*a);
    r += b*(1-b)*(1-2*b)/(12*a*a);
    r += - b*b*(1-b)*(1-b)/(12*a*a*a);

    return r;
}

/*
 * Special case for a negative integer argument
 */

double beta_negint(int a, double b)
{
    int sgn;
    if (b == boost::numeric_cast<int>(b) && 1 - a - b > 0) {
        sgn = (boost::numeric_cast<int>(b) % 2 == 0) ? 1 : -1;
        return sgn * beta(1 - a - b, b);
    }
    else {
        return std::numeric_limits<double>::infinity();
    }
}

double lbeta_negint(int a, double b)
{
    double r;
    if (b == boost::numeric_cast<int>(b) && 1 - a - b > 0) {
        r = lbeta(1 - a - b, b);
        return r;
    }
    else {
        return std::numeric_limits<double>::infinity();
    }
}

}


#ifndef OCV_ARMA_DIST_METRIC_HPP
#define OCV_ARMA_DIST_METRIC_HPP

#include "../arma/type_traits.hpp"

#include <armadillo>

/*!
 *  \addtogroup ocv
 *  @{
 */
namespace ocv{

/*!
 *  \addtogroup armd
 *  @{
 */
namespace armd{

namespace details{

template<typename T,
         typename Hist1,
         typename Hist2,
         typename Index>
struct type_constraint
{
    static_assert(std::is_floating_point<T>::value,
                  "T should be floating point");
    static_assert(std::is_same<typename Hist1::elem_type,
                  typename Hist2::elem_type>::value,
                  "elem type of Hist1 and Hist2 "
                  "should be the same one");
    static_assert(std::is_floating_point<typename Hist1::elem_type>::value,
                  "elem type of Hist1 and Hist2 should "
                  "be floating point");
    static_assert(std::is_integral<Index>::value,
                  "Index should be integral");
    static_assert(is_two_dim<Hist2>::value,
                  "Hist2 should be arma::Mat or "
                  "arma::SpMat");
};

template<typename T, typename U>
inline
typename std::enable_if<
arma::is_arma_sparse_type<U>::value ||
(arma::is_arma_type<U>::value &&
 !std::is_same<T, typename U::elem_type>::value),
arma::Col<T>>::type
to_colvec(U const &input)
{
    return arma::Col<T>(input);
}

template<typename T, typename U>
inline
typename std::enable_if<
arma::is_arma_type<U>::value &&
std::is_same<T, typename U::elem_type>::value,
U>::type const&
to_colvec(U const &input)
{
    return input;
}

}

/**
 * measure chi square distance
 * @tparam T return type of compare
 */
template<typename T = float>
struct cosine_similarity
{
    using result_type = T;

    template<typename Hist1,
             typename Hist2,
             typename Index>
    T compare(Hist1 const &query_hist,
              Hist2 const &datahist,
              Index const &index) const
    {
        using namespace details;
        details::type_constraint<T,Hist1,Hist2,Index>();

        return similarity_compute(to_colvec<T>(query_hist),
                                  to_colvec<T>(datahist.col(index)));
    }

private:
    template<typename U, typename V>
    T similarity_compute(U const &lhs,
                         V const &rhs) const
    {
        auto const denom =
                std::sqrt(arma::sum(lhs.col(0) % lhs.col(0))) *
                std::sqrt(arma::sum(rhs % rhs)) +
                T(1e-10);

        return arma::sum((lhs % rhs)) / (denom);
    }
};

/**
 * measure chi square distance
 * @tparam T return type of compare
 */
template<typename T = float>
struct chi_square{

    using result_type = T;

    template<typename Hist1,
             typename Hist2,
             typename Index>
    T compare(Hist1 const &query_hist,
              Hist2 const &datahist,
              Index const &index) const
    {
        using namespace details;
        type_constraint<T,Hist1,Hist2,Index>();

        return chi_square_compute(to_colvec<T>(query_hist),
                                  to_colvec<T>(datahist.col(index)));
    }

private:
    template<typename U, typename V>
    T chi_square_compute(U const &lhs,
                         V const &rhs) const
    {
        return arma::sum(arma::square(lhs - rhs) /
                         (lhs + rhs + T(1e-10)));
    }

};

} /*! @} End of Doxygen Groups*/

} /*! @} End of Doxygen Groups*/

#endif // DIST_METRIC_HPP