//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 boost.simd.arithmetic toolbox - fma/simd Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of boost.simd.arithmetic components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created by jt the 01/12/2010
///
#include <nt2/toolbox/arithmetic/include/functions/fma.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/toolbox/constant/common.hpp>

NT2_TEST_CASE_TPL ( fma_real__3_0,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using nt2::fma;
  using nt2::tag::fma_;
  typedef std::complex<T> cT;

  // return type conformity test
  NT2_TEST_TYPE_IS(  typename boost::dispatch::meta::call<fma_(cT,cT,cT)>::type, cT );

  // specific values tests
  NT2_TEST_EQUAL(fma(cT(nt2::Inf<T>()), cT(nt2::Inf<T>()), cT(nt2::Inf<T>())), cT(nt2::Inf<T>()));
  NT2_TEST_EQUAL(fma(cT(nt2::Minf<T>()), cT(nt2::Minf<T>()), cT(nt2::Minf<T>())), cT(nt2::Nan<T>()));
  NT2_TEST_EQUAL(fma(cT(nt2::Mone<T>()), cT(nt2::Mone<T>()), cT(nt2::Mone<T>())), cT(nt2::Zero<T>()));
  NT2_TEST_EQUAL(fma(cT(nt2::Nan<T>()), cT(nt2::Nan<T>()), cT(nt2::Nan<T>())), cT(nt2::Nan<T>()));
  NT2_TEST_EQUAL(fma(cT(nt2::One<T>()), cT(nt2::One<T>()), cT(nt2::One<T>())), cT(nt2::Two<T>()));
  NT2_TEST_EQUAL(fma(cT(nt2::Zero<T>()), cT(nt2::Zero<T>()), cT(nt2::Zero<T>())), cT(nt2::Zero<T>()));
} // end of test for floating_


/*******************************************************************************
 * Copyright 2013-2014 Sebastian Niemann <niemann@sra.uni-hannover.de>.
 *
 * Licensed under the MIT License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://opensource.org/licenses/MIT
 *
 * Developers:
 *   Sebastian Niemann - Lead developer
 *   Daniel Kiechle - Unit testing
 ******************************************************************************/
#include <Expected.hpp>
using armadilloJava::Expected;

#include <iostream>
using std::cout;
using std::endl;

#include <utility>
using std::pair;

#include <armadillo>
using arma::Mat;

#include <InputClass.hpp>
using armadilloJava::InputClass;

#include <Input.hpp>
using armadilloJava::Input;

namespace armadilloJava {
  class ExpectedInPlaceGenMatRowIndColIndGenDouble : public Expected {
    public:
      ExpectedInPlaceGenMatRowIndColIndGenDouble() {
        cout << "Compute ExpectedInPlaceGenMatRowIndColIndGenDouble(): " << endl;

          vector<vector<pair<string, void*>>> inputs = Input::getTestParameters({
            InputClass::GenMat,
            InputClass::RowInd,
            InputClass::ColInd,
            InputClass::GenDouble
          });

          for (vector<pair<string, void*>> input : inputs) {
            _fileSuffix = "";

            int n = 0;
            for (pair<string, void*> value : input) {
              switch (n) {
                case 0:
                  _fileSuffix += value.first;
                  _genMat = *static_cast<Mat<double>*>(value.second);
                  break;
                case 1:
                  _fileSuffix += "," + value.first;
                  _rowInd = *static_cast<int*>(value.second);
                  break;
                case 2:
                  _fileSuffix += "," + value.first;
                  _colInd = *static_cast<int*>(value.second);
                  break;
                case 3:
                  _fileSuffix += "," + value.first;
                  _genDouble = *static_cast<double*>(value.second);
                  break;
              }
              ++n;
            }

            cout << "Using input: " << _fileSuffix << endl;

            _copyOfGenMat = _genMat;
            _copyOfRowInd = _rowInd;
            _copyOfColInd = _colInd;
            _copyOfGenDouble = _genDouble;

            expectedMatAtEqual();

            _genMat = _copyOfGenMat;
            _rowInd = _copyOfRowInd;
            _colInd = _copyOfColInd;
            _genDouble = _copyOfGenDouble;
            expectedMatAtPlus();

            _genMat = _copyOfGenMat;
            _rowInd = _copyOfRowInd;
            _colInd = _copyOfColInd;
            _genDouble = _copyOfGenDouble;
            expectedMatAtMinus();

            _genMat = _copyOfGenMat;
            _rowInd = _copyOfRowInd;
            _colInd = _copyOfColInd;
            _genDouble = _copyOfGenDouble;
            expectedMatAtTimes();

            _genMat = _copyOfGenMat;
            _rowInd = _copyOfRowInd;
            _colInd = _copyOfColInd;
            _genDouble = _copyOfGenDouble;
            expectedMatAtDivide();
          }

          cout << "done." << endl;
        }

    protected:
      Mat<double> _genMat;
      Mat<double> _copyOfGenMat;

      int _rowInd;
      int _copyOfRowInd;

      int _colInd;
      int _copyOfColInd;

      double _genDouble;
      double _copyOfGenDouble;

      void expectedMatAtEqual() {
        if(!_genMat.in_range(_rowInd, _colInd)) {
          return;
        }

        _genMat.at(_rowInd, _colInd) = _genDouble;

        cout << "- Compute expectedMatAtEqual() ... ";
        save<double>("Mat.atEqual", _genMat);
        cout << "done." << endl;
      }

      void expectedMatAtPlus() {
        if(!_genMat.in_range(_rowInd, _colInd)) {
          return;
        }

        _genMat.at(_rowInd, _colInd) += _genDouble;

        cout << "- Compute expectedMatAtPlus() ... ";
        save<double>("Mat.atPlus", _genMat);
        cout << "done." << endl;
      }

      void expectedMatAtMinus() {
        if(!_genMat.in_range(_rowInd, _colInd)) {
          return;
        }

        _genMat.at(_rowInd, _colInd) -= _genDouble;

        cout << "- Compute expectedMatAtMinus() ... ";
        save<double>("Mat.atMinus", _genMat);
        cout << "done." << endl;
      }

      void expectedMatAtTimes() {
        if(!_genMat.in_range(_rowInd, _colInd)) {
          return;
        }

        _genMat.at(_rowInd, _colInd) *= _genDouble;

        cout << "- Compute expectedMatAtTimes() ... ";
        save<double>("Mat.atTimes", _genMat);
        cout << "done." << endl;
      }

      void expectedMatAtDivide() {
        if(!_genMat.in_range(_rowInd, _colInd)) {
          return;
        }

        _genMat.at(_rowInd, _colInd) /= _genDouble;

        cout << "- Compute expectedMatAtDivide() ... ";
        save<double>("Mat.atDivide", _genMat);
        cout << "done." << endl;
      }
  };
}


/*
Copyright 2009-2021 Nicolas Colombe

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

#pragma once

#include <algorithm>
#include <boost/polygon/detail/polygon_sort_adaptor.hpp> //gtlsort not found on linux otherwise
#include <boost/polygon/polygon.hpp>
#include <math/math.hpp>

namespace boost
{
  namespace polygon
  {

    template <typename Real>
    struct geometry_concept < eXl::Vector2<Real> > { typedef point_concept type; };

    //Then we specialize the gtl point traits for our point type
    template <typename Real>
    struct point_traits < eXl::Vector2<Real> > {
      typedef Real coordinate_type;

      static inline coordinate_type get(const eXl::Vector2<Real>& point,
        orientation_2d orient) {
        if (orient == HORIZONTAL)
          return point.X();
        return point.Y();
      }
    };

    template <typename Real>
    struct point_mutable_traits < eXl::Vector2<Real> > {
      typedef int coordinate_type;

      static inline void set(eXl::Vector2<Real>& point, orientation_2d orient, int value) {
        if (orient == HORIZONTAL)
          point.X() = value;
        else
          point.Y() = value;
      }
      static inline eXl::Vector2<Real> construct(int x_value, int y_value) {
        eXl::Vector2<Real> retval;
        retval.X() = x_value;
        retval.Y() = y_value;
        return retval;
      }
    };

    template <typename Real>
    struct geometry_concept < std::pair<Real, Real> > { typedef interval_concept type; };

    template <typename Real>
    struct interval_traits < std::pair<Real, Real> > {
      typedef Real coordinate_type;

      static inline coordinate_type get(const std::pair<Real, Real>& interval, direction_1d dir) {
        if (dir == LOW)
          return interval.first;
        else
          return interval.second;
      }
    };

    template <typename Real>
    struct interval_mutable_traits < std::pair<Real, Real> > {
      typedef int coordinate_type;

      static inline void set(std::pair<Real, Real>& interval, direction_1d dir,
        typename interval_traits<std::pair<Real, Real> >::coordinate_type value) {
        if (dir == LOW)
          interval.first = value;
        else
          interval.second = value;
      }
      static inline std::pair<Real, Real> construct(typename interval_traits<std::pair<Real, Real> >::coordinate_type low_value,
        typename interval_traits<std::pair<Real, Real> >::coordinate_type high_value) {
        return std::make_pair(low_value, high_value);
      }
    };

    template <typename Real>
    struct geometry_concept < eXl::AABB2D<Real> > { typedef rectangle_concept type; };

    template <typename Real>
    struct rectangle_traits < eXl::AABB2D<Real>, gtl_no > {};

    template <typename Real, typename enable>
    struct rectangle_traits < eXl::AABB2D<Real>, enable > {
      typedef int coordinate_type;
      typedef std::pair<Real, Real> interval_type;
      static inline interval_type get(const eXl::AABB2D<Real>& rectangle, orientation_2d orient) {
        if (orient == HORIZONTAL)
          return std::make_pair(rectangle.m_Data[0].X(), rectangle.m_Data[1].X());
        else
          return std::make_pair(rectangle.m_Data[0].Y(), rectangle.m_Data[1].Y());
      }
    };

    template <typename Real>
    struct rectangle_mutable_traits < eXl::AABB2D<Real> > {
      template <typename T2>
      static inline void set(eXl::AABB2D<Real>& rectangle, orientation_2d orient, const T2& interval) {
        if (orient == HORIZONTAL)
        {
          rectangle.m_Data[0].X() = low(interval);
          rectangle.m_Data[1].X() = high(interval);
        }
        else
        {
          rectangle.m_Data[0].Y() = low(interval);
          rectangle.m_Data[1].Y() = high(interval);
        }
      }
      template <typename T2, typename T3>
      static inline eXl::AABB2D<Real> construct(const T2& interval_horizontal, const T3& interval_vertical) {
        eXl::AABB2D<Real> ret;
        ret.m_Data[0].X() = low(interval_horizontal);
        ret.m_Data[1].X() = high(interval_horizontal);
        ret.m_Data[0].Y() = low(interval_vertical);
        ret.m_Data[1].Y() = high(interval_vertical);
        return ret;
      }
    };

  }
}

#include <boost/polygon/segment_utils.hpp>
#include <math/segment.hpp>

namespace boost
{
  namespace polygon
  {
    template <typename Real>
    struct geometry_concept < eXl::Segment<Real> > { typedef segment_concept type; };

    template <typename Real>
    struct segment_traits<eXl::Segment<Real>>
    {
      typedef Real coordinate_type;
      typedef eXl::Vector2<Real> point_type;

      static inline point_type get(const eXl::Segment<Real>& segment, direction_1d dir) {
        return (dir == LOW ? segment.m_Ext1 : segment.m_Ext2);
      }
    };

    template <typename Real>
    struct segment_mutable_traits<eXl::Segment<Real>>
    {
      typedef typename segment_traits<eXl::Segment<Real>>::coordinate_type coordinate_type;
      typedef typename segment_traits<eXl::Segment<Real>>::point_type point_type;

      template <typename Point>
      static inline void set(eXl::Segment<Real>& segment, direction_1d dir, const Point& point)
      {
        (dir == LOW ? segment.m_Ext1 : segment.m_Ext2) = eXl::Vector2<Real>(point_traits<Point>::get(point, HORIZONTAL), point_traits<Point>::get(point, VERTICAL));
      }

      template <typename Point>
      static inline eXl::Segment<Real> construct(const Point& low, const Point& high) {
        eXl::Segment<Real> res;
        res.m_Ext1 = eXl::Vector2<Real>(point_traits<Point>::get(low, HORIZONTAL), point_traits<Point>::get(low, VERTICAL));
        res.m_Ext2 = eXl::Vector2<Real>(point_traits<Point>::get(high, HORIZONTAL), point_traits<Point>::get(high, VERTICAL));
        return res;
      }
    };

    template <typename Segment, typename SegmentIterator, typename Allocator>
    typename enable_if<
      typename gtl_and<
      typename gtl_if<
      typename is_segment_concept<
      typename geometry_concept<
      typename std::iterator_traits<SegmentIterator>::value_type
      >::type
      >::type
      >::type,
      typename gtl_if<
      typename is_segment_concept<
      typename geometry_concept<Segment>::type
      >::type
      >::type
      >::type,
      void
    >::type
      intersect_segments(
        std::vector<std::pair<std::size_t, Segment>, Allocator>& result,
        SegmentIterator first, SegmentIterator last) {
      typedef typename segment_traits<Segment>::coordinate_type Unit;
      typedef typename scanline_base<Unit>::Point Point;
      typedef typename scanline_base<Unit>::half_edge half_edge;
      typedef int segment_id;
      std::vector<std::pair<half_edge, segment_id> > half_edges;
      std::vector<std::pair<half_edge, segment_id> > half_edges_out;
      segment_id id_in = 0;
      half_edges.reserve(std::distance(first, last));
      for (; first != last; ++first) {
        Point l, h;
        assign(l, low(*first));
        assign(h, high(*first));
        half_edges.push_back(std::make_pair(half_edge(l, h), id_in++));
      }
      half_edges_out.reserve(half_edges.size());
      // Apparently no need to pre-sort data when calling validate_scan.
      if (half_edges.size() != 0) {
        line_intersection<Unit>::validate_scan(
          half_edges_out, half_edges.begin(), half_edges.end());
      }

      result.reserve(result.size() + half_edges_out.size());
      for (std::size_t i = 0; i < half_edges_out.size(); ++i) {
        std::size_t id = (std::size_t)(half_edges_out[i].second);
        Point l = half_edges_out[i].first.first;
        Point h = half_edges_out[i].first.second;
        result.push_back(std::make_pair(id, construct<Segment>(l, h)));
      }
    }
  }
}


/**
* @license Apache-2.0
*
* Copyright (c) 2018 The Stdlib Authors.
*
* 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.
*/

/**
* Generate Boost test fixtures.
*
* ## Notes
*
* -   Run this script from the directory in which fixtures should be written.
*
*/
#include <stdlib.h>
#include <stdio.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/math/special_functions/factorials.hpp>

using boost::random::uniform_real_distribution;
using boost::random::uniform_int_distribution;
using boost::random::mt19937;

// Define a new pseudorandom number generator:
mt19937 rng;

/**
* Generates a linearly spaced numeric array of doubles.
*
* ## Notes
*
* -   Assumes that the output array has at least 2 elements.
* -   Output array is guaranteed to include both the start and end values.
*
*
* @param out    output array
* @param len    array length
* @param start  first value
* @param end    last value
*/
void linspace_f64( double *out, const unsigned int len, const double start, const double end ) {
	unsigned int i;
	double incr;

	incr = (end-start) / (len-1);
	for ( i = 0; i < len-1; i++ ) {
		out[ i ] = start + (incr*i);
	}
	out[ i ] = end;
}

/**
* Generates a linearly spaced numeric array of integers.
*
* ## Notes
*
* -   Assumes that the output array has at least 2 elements.
* -   Output array is guaranteed to include both the start and end values.
*
*
* @param out    output array
* @param len    array length
* @param start  first value
* @param end    last value
*/
void linspace_i32( int *out, const unsigned int len, const int start, const int end ) {
	unsigned int i;
	int incr;

	incr = (end-start) / (len-1);
	for ( i = 0; i < len-1; i++ ) {
		out[ i ] = start + (incr*i);
	}
	out[ i ] = end;
}

/**
* Generates an array of pseudorandom doubles drawn from a uniform distribution.
*
* @param out  output array
* @param len  array length
* @param a    lower bound (inclusive)
* @param b    upper bound (exclusive)
*/
void rand_array_f64( double *out, const unsigned int len, const double a, const double b ) {
	unsigned int i;

	// Define a uniform distribution for generating pseudorandom numbers:
	uniform_real_distribution<> randu( a, b );

	for ( i = 0; i < len; i++ ) {
		out[ i ] = randu( rng );
	}
}

/**
* Generates an array of pseudorandom integers drawn from a uniform distribution.
*
* @param out  output array
* @param len  array length
* @param a    lower bound (inclusive)
* @param b    upper bound (exclusive)
*/
void rand_array_i32( int *out, const unsigned int len, const int a, const int b ) {
	unsigned int i;

	// Define a uniform distribution for generating pseudorandom numbers:
	uniform_int_distribution<> randi( a, b );

	for ( i = 0; i < len; i++ ) {
		out[ i ] = randi( rng );
	}
}

/**
* Writes an array of doubles to a file as a series of comma-separated values.
*
* @param f    file to write to
* @param x    array of doubles
* @param len  array length
*/
void write_array_f64( FILE *f, const double *x, const unsigned int len ) {
	unsigned int i;
	for ( i = 0; i < len; i++ ) {
		fprintf( f, "%.17g", x[ i ] );
		if ( i < len-1 ) {
			fprintf( f, "," );
		}
	}
}

/**
* Writes an array of integers to a file as a series of comma-separated values.
*
* @param f    file to write to
* @param x    array of integers
* @param len  array length
*/
void write_array_i32( FILE *f, const int *x, const unsigned int len ) {
	unsigned int i;

	for ( i = 0; i < len; i++ ) {
		fprintf( f, "%d", x[ i ] );
		if ( i < len-1 ) {
			fprintf( f, "," );
		}
	}
}

/**
* Writes a named array of doubles to a file as JSON.
*
* @param f     file to write to
* @param name  array name
* @param x     data
* @param len   array length
*/
void write_named_array_f64( FILE *f, const char *name, const double *x, const unsigned int len ) {
	fprintf( f, "\"%s\":[", name );
	write_array_f64( f, x, len );
	fprintf( f, "]" );
}

/**
* Writes a named array of integers to a file as JSON.
*
* @param f     file to write to
* @param name  array name
* @param x     data
* @param len   array length
*/
void write_named_array_i32( FILE *f, const char *name, const int *x, const unsigned int len ) {
	fprintf( f, "\"%s\":[", name );
	write_array_i32( f, x, len );
	fprintf( f, "]" );
}

/**
* Writes data to a file as JSON.
*
* ## Notes
*
* -   This function SHOULD be tailored to the input data (e.g., input types, output types, number of arguments, etc) and may vary from use case to use case.
*
*
* @param f    file to write to
* @param x    first parameter
* @param n    second parameter
* @param y    results
* @param len  array length
*/
void write_data_as_json( FILE *f, const double *x, const int *n,const double *y, const unsigned int len ) {
	fprintf( f, "{" );
	write_named_array_f64( f, "x", x, len );
	fprintf( f, "," );
	write_named_array_i32( f, "n", n, len );
	fprintf( f, "," );
	write_named_array_f64( f, "expected", y, len );
	fprintf( f, "}" );
}

/**
* Generates test fixtures.
*
* @param x     domain
* @param len   number of values in the domain
* @param name  output filename
*/
void generate( double *x, int *n, const unsigned int len, const char *name ) {
	unsigned int i;
	double *y;
	FILE *f;

	// Allocate an output array:
	y = (double*) malloc( len * sizeof(double) );
	if ( y == NULL ) {
		printf( "Error allocating memory.\n" );
		exit( 1 );
	}

	// Generate fixture data:
	for ( i = 0; i < len; i++ ) {
		y[ i ] = boost::math::falling_factorial( x[ i ], n[ i ] );
	}
	// Open a new file:
	f = fopen( name, "w" );
	if ( f == NULL ) {
		printf( "Error opening file.\n" );
		exit( 1 );
	}
	// Write data as JSON:
	write_data_as_json( f, x, n, y, len );

	// Close the file:
	fclose( f );

	// Free allocated memory:
	free( y );
}

/**
* Main execution sequence.
*/
int main( void ) {
	unsigned int len;
	double *x;
	int *n;

	// Define the array length:
	len = 1000;

	// Allocate arrays:
	x = (double*) malloc( len * sizeof(double) );
	if ( x == NULL ) {
		printf( "Error allocating memory.\n" );
		exit( 1 );
	}
	n = (int*) malloc( len * sizeof(int) );
	if ( n == NULL ) {
		printf( "Error allocating memory.\n" );
		exit( 1 );
	}

	// Generate fixture data:
	rand_array_i32( n, len, 1, 100 );

	linspace_f64( x, len, 0.0, 0.5 );
	generate( x, n, len, "small.json" );

	linspace_f64( x, len, 1.0, 40.0 );
	generate( x, n, len, "large.json" );

	linspace_f64( x, len, -0.1, -40.0 );
	generate( x, n, len, "negative.json" );

	// Free allocated memory:
	free( x );
	free( n );

	return 0;
}


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

// Copyright (c) 2007-2013 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2013 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2013 Mateusz Loskot, London, UK.
// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_POINT_IS_EQUAL_OR_SPIKE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_POINT_IS_EQUAL_OR_SPIKE_HPP

#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/algorithms/detail/convert_point_to_point.hpp>
#include <boost/geometry/algorithms/detail/recalculate.hpp>
#include <boost/geometry/policies/robustness/robust_point_type.hpp>
#include <boost/geometry/strategies/side.hpp>
#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

// Checks if a point ("last_point") causes a spike w.r.t.
// the specified two other points (segment_a, segment_b)
//
//  x-------x------x
//  a       lp     b
//
// Above, lp generates a spike w.r.t. segment(a,b)
// So specify last point first, then (a,b) (this is unordered, so unintuitive)
template <typename Point1, typename Point2, typename Point3>
static inline bool point_is_spike_or_equal(Point1 const& last_point,
            Point2 const& segment_a,
            Point3 const& segment_b)
{
    typedef typename strategy::side::services::default_strategy
    <
        typename cs_tag<Point1>::type
    >::type side_strategy;

    typedef Point1 vector_type;

    int const side = side_strategy::apply(last_point, segment_a, segment_b);
    if (side == 0)
    {
        // Last point is collinear w.r.t previous segment.
        // Check if it is equal
        vector_type diff1;
        conversion::convert_point_to_point(last_point, diff1);
        geometry::subtract_point(diff1, segment_b);
        int const sgn_x1 = math::sign(geometry::get<0>(diff1));
        int const sgn_y1 = math::sign(geometry::get<1>(diff1));
        if (sgn_x1 == 0 && sgn_y1 == 0)
        {
            return true;
        }

        // Check if it moves forward
        vector_type diff2;
        conversion::convert_point_to_point(segment_b, diff2);
        geometry::subtract_point(diff2, segment_a);
        int const sgn_x2 = math::sign(geometry::get<0>(diff2));
        int const sgn_y2 = math::sign(geometry::get<1>(diff2));

        return sgn_x1 != sgn_x2 || sgn_y1 != sgn_y2;
    }
    return false;
}

template
<
    typename Point1,
    typename Point2,
    typename Point3,
    typename RobustPolicy
>
static inline bool point_is_spike_or_equal(Point1 const& last_point,
            Point2 const& segment_a,
            Point3 const& segment_b,
            RobustPolicy const& robust_policy)
{
    if (point_is_spike_or_equal(last_point, segment_a, segment_b))
    {
        return true;
    }

    if (! RobustPolicy::enabled)
    {
        return false;
    }

    // Try using specified robust policy
    typedef typename geometry::robust_point_type
    <
        Point1,
        RobustPolicy
    >::type robust_point_type;

    robust_point_type last_point_rob, segment_a_rob, segment_b_rob;
    geometry::recalculate(last_point_rob, last_point, robust_policy);
    geometry::recalculate(segment_a_rob, segment_a, robust_policy);
    geometry::recalculate(segment_b_rob, segment_b, robust_policy);

    return point_is_spike_or_equal
        (
            last_point_rob,
            segment_a_rob,
            segment_b_rob
        );
}

} // namespace detail
#endif

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_POINT_IS_EQUAL_OR_SPIKE_HPP


/**
    @author Jenna Reher (jreher@caltech.edu)
*/

#ifndef CASSIE_PD_CONTROLLER_HPP
#define CASSIE_PD_CONTROLLER_HPP

#include <Eigen/Dense>
#include <eigen_utilities/assert_size.hpp>
#include <iostream>

class PD_Controller {
public:
    Eigen::VectorXd Kp;
    Eigen::VectorXd Kd;
    Eigen::VectorXi zero_indices; // TODO
    Eigen::VectorXd u;

    PD_Controller() {
        this->isInitialized = false;
    }

    PD_Controller(Eigen::VectorXd &Kp, Eigen::VectorXd &Kd) {
        this->reconfigure(Kp, Kd);
    }

    void reconfigure(Eigen::VectorXd &Kp, Eigen::VectorXd &Kd) {
        eigen_utilities::assert_size(Kp, Kd.size());
        this->Kp.resize(Kp.size());
        this->Kd.resize(Kd.size());
        this->Kp << Kp;
        this->Kd << Kd;
        this->u.setZero(Kp.size());
        this->isInitialized = true;
    }

    Eigen::VectorXd compute(Eigen::VectorXd &q_desired, Eigen::VectorXd &dq_desired, Eigen::VectorXd &q_actual, Eigen::VectorXd &dq_actual) {
        // Cannot run if not initialized
        assert(this->isInitialized);

        // Size check everything!
        eigen_utilities::assert_size(q_desired, this->Kd.size());
        eigen_utilities::assert_size(dq_desired, this->Kd.size());
        eigen_utilities::assert_size(q_actual, this->Kd.size());
        eigen_utilities::assert_size(dq_actual, this->Kd.size());

        // Compute and return the vector
        this->u << - (this->Kp.cwiseProduct(q_actual - q_desired) + this->Kd.cwiseProduct(dq_actual - dq_desired));
        return this->u;
    }
private:
    bool isInitialized;
};

#endif // CASSIE_PD_CONTROLLER_HPP


// (c) RIKEN 2015. All rights reserved.
// Author: Keitaro Yamashita
//
// This software is released under the new BSD License; see LICENSE.

#include <boost/python.hpp>
#include <boost/python/args.hpp>
#include <scitbx/array_family/boost_python/flex_fwd.h>
#include <scitbx/array_family/flex_types.h>
#include <cctbx/miller.h>
#include <cctbx/miller/match.h>
#include <cctbx/miller/index_span.h>
#include <algorithm>
#include <functional>
#include <map>
#include <vector>

namespace bp = boost::python;
namespace af = scitbx::af;

typedef cctbx::miller::index <> miller_index_type;
typedef af::const_ref <miller_index_type> flex_miller_index;

struct miller_packed_less_than
{
  miller_packed_less_than(const flex_miller_index &indices)
    : span(indices) {
  }
  bool operator()(const miller_index_type &lhs, const miller_index_type &rhs) {
    return span.pack(lhs) < span.pack(rhs);
  }
private:
  cctbx::miller::index_span span;
};

/* Very specific function for add_saturation_info_to_mtz.py
 * "data" are assumed to be ZD or ZOBS (spot centroid on frame number unit)
 * This tries to find the same reflection (indices must be the same, frame number must be similar).
 * PRIOR CONDITION: rhs must be sorted!!
 */
af::flex_bool
make_selection_for_xds_unmerged(const flex_miller_index &lhs_indices,
                                const af::const_ref<double> &lhs_data,
                                const flex_miller_index &rhs_indices,
                                const af::const_ref<double> &rhs_data,
                                double threshold)
{
  CCTBX_ASSERT(lhs_indices.size() == lhs_data.size());
  CCTBX_ASSERT(rhs_indices.size() == rhs_data.size());

  af::flex_bool ret(rhs_data.size());
  for (int i = 0; i < ret.size(); ++i)
    ret[i] = false;

  for (int i = 0; i < lhs_indices.size(); ++i) {
    //    std::cout << i << "/" << lhs_indices.size() << "\n";

    const miller_index_type *p = std::lower_bound(rhs_indices.begin(), rhs_indices.end(), lhs_indices[i],
                                                  miller_packed_less_than(rhs_indices));

    //    std::cout << (*p)[0] << " " << (*p)[1] << " " << (*p)[2]
    //              << " ?= " << lhs_indices[i][0] << " " << lhs_indices[i][1] << " " << lhs_indices[i][2] << "\n";
    int found = 0;
    for (; p != rhs_indices.end(); ++p) {
      if (*p != lhs_indices[i])
        break;

      const int j = p - rhs_indices.begin();

      if (std::abs(rhs_data[j] - lhs_data[i]) < threshold) {
        ret[j] = true;
        ++found;
      }
      // else
      //   std::cout << "Diff= " << std::abs(rhs_data[j] - lhs_data[i]) << " "
      //             << "(" << (*p)[0] << " " << (*p)[1] << " " << (*p)[2] << ") "
      //             << rhs_data[j] << ":" << lhs_data[i] << " j=" << j
      //             << "\n";
    }
    if (found != 1)
      std::cout << found << " Found\n";

    /* // This loop is slow!
    for (int j = 0; j < rhs_indices.size(); ++j) {
      if (lhs_indices[i] != rhs_indices[j])
        continue;
      if (std::abs(rhs_data[i] - lhs_data[j]) < threshold)
        ret[j] = true;
    }
    */
  }
  return ret;
}

struct comp {
  comp(af::shared<cctbx::miller::index<> > const& ind): ind(ind) {}
  bool operator()(size_t i, size_t j) { return _comp(ind[i],ind[j]); }
  cctbx::miller::fast_less_than<> _comp;
  af::shared<cctbx::miller::index<> > const& ind;
};

af::shared<size_t>
sort_permutation_fast_less(af::shared<cctbx::miller::index<> > const& indices)
{
  af::shared<size_t> ret(indices.size());
  for (int i = 0; i < indices.size(); ++i) ret[i] = i;

  std::sort(ret.begin(), ret.end(), comp(indices));
  return ret;
}

boost::python::tuple
my_common_indices(af::shared<cctbx::miller::index<> > const& indices_0,
		  af::shared<cctbx::miller::index<> > const& indices_1)
{
  cctbx::miller::fast_less_than<> lt;

  af::shared<size_t> ret0, ret1;
  ret0.reserve(indices_0.size());
  ret1.reserve(indices_1.size());

  for (int i = 0, j = 0; i < indices_0.size() && j < indices_1.size(); ++j) {
    while (i < indices_0.size() && lt(indices_0[i], indices_1[j])) ++i;
    if (i < indices_0.size() && indices_0[i] == indices_1[j]) {
      ret0.push_back(i);
      ret1.push_back(j);
    }
  }
  return boost::python::make_tuple(ret0, ret1);
}

BOOST_PYTHON_MODULE(yamtbx_utils_ext)
{

  typedef bp::return_value_policy<bp::return_by_value> rbv;

  bp::def("make_selection_for_xds_unmerged", make_selection_for_xds_unmerged);
  bp::def("sort_permutation_fast_less", sort_permutation_fast_less, rbv());
  bp::def("my_common_indices", my_common_indices, rbv());
}


#pragma once

#include "object.hpp"
#include <metaSMT/frontend/QF_BV.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/function.hpp>
#include <queue>

namespace metaSMT {
  namespace cardinality {
    namespace adder {
    namespace cardtags= logic::cardinality::tag;
      template <typename Context, typename Boolean>
      typename Context::result_type
      cardinality_any( Context &ctx, std::vector<Boolean> const &ps ) {
        typedef typename Context::result_type result_type;

	/*
        boost::function< result_type ( result_type const &, result_type const & ) >
          lazy_HA_sum = metaSMT::lazy( ctx, logic::Xor(arg1, arg2) );

        boost::function< result_type ( result_type const &, result_type const & ) >
          lazy_HA_carry = metaSMT::lazy( ctx, logic::And(arg1, arg2) );*/
	/*
        boost::function< result_type ( result_type const &, result_type const &, result_type const & ) >
          lazy_FA_sum = metaSMT::lazy( ctx, logic::Xor(logic::Xor(arg1, arg2), arg3) );*/

        // boost::function< result_type ( result_type const &, result_type const &, result_type const & ) >
        //   lazy_FA_carry = metaSMT::lazy( ctx, Xor(And(arg1, arg2), And(Xor(arg1, arg2), arg3)) );

        // boost::function< result_type ( result_type const &, result_type const &, result_type const & ) >
        //   lazy_FA_carry = metaSMT::lazy( ctx, Or(And(arg1, arg2), And(Xor(arg1, arg2), arg3)) );

        // boost::function< result_type ( result_type const &, result_type const &, result_type const & ) >
        //   lazy_FA_carry = metaSMT::lazy( ctx, Ite(arg1, Ite(arg2, bit1, arg3), Ite(arg2, arg3, bit0)) );
	/*
        boost::function< result_type ( result_type const &, result_type const &, result_type const & ) >
          lazy_FA_carry = metaSMT::lazy( ctx, logic::Ite(arg1, logic::Or(arg2, arg3), logic::And(arg2, arg3)) );*/

        std::queue< result_type > line0, line1;
        std::queue< result_type > *wq0 = &line0;
        std::queue< result_type > *wq1 = &line1;
        std::vector< result_type > r;

        // initialize wq0
        for ( unsigned u = 0; u < ps.size(); ++u ) {
          wq0->push( evaluate(ctx, ps[u]) );
      }

        while ( !wq0->empty() ) {
          while ( wq0->size() >= 3 ) {
            result_type x = wq0->front(); wq0->pop();
            result_type y = wq0->front(); wq0->pop();
            result_type z = wq0->front(); wq0->pop();
            wq0->push( evaluate(ctx, logic::Xor(logic::Xor(x, y), z) ));

            wq1->push( evaluate(ctx, logic::Ite(x, logic::Or(y, z), logic::And(y, z)) ) );
          }

          if ( wq0->size() == 2 ) {
            result_type x = wq0->front(); wq0->pop();
            result_type y = wq0->front(); wq0->pop();
            wq0->push( evaluate(ctx, logic::Xor(x,y)) );
            wq1->push( evaluate(ctx, logic::And(x,y)) );
          }

          r.push_back( wq0->front() );
          wq0->pop();

          swap(wq0, wq1);
        }

        logic::QF_BV::bitvector n = logic::QF_BV::new_bitvector( r.size() );
        for ( unsigned u = 0; u < r.size(); ++u ) {
          assertion( ctx, logic::equal(logic::QF_BV::extract(u, u, n), logic::Ite(r[u], logic::QF_BV::bit1, logic::QF_BV::bit0)) );
        }

        return evaluate( ctx, n );

        // convert result to bitvector
        //result_type n = evaluate(ctx, logic::Ite(r[0], logic::QF_BV::bit1, logic::QF_BV::bit0));
        //for ( unsigned u = 1; u < r.size(); ++u ) {
        //  n = evaluate(ctx, concat(logic::Ite(r[u], logic::QF_BV::bit1, logic::QF_BV::bit0), n));
        //}
        //return n;
      }

      template < typename Context, typename Boolean, typename Tag >
      typename Context::result_type
      cardinality(Context & , cardinality::Cardinality<Tag, Boolean> const & ) {
        /** error: unknown tag **/
        BOOST_MPL_ASSERT_NOT( (boost::mpl::contains<cardtags::Cardinality_Tags, Tag>) );
      }

      template <typename Context, typename Boolean>
      typename Context::result_type
      cardinality(Context &ctx, cardinality::Cardinality<cardtags::eq_tag, Boolean> const &c) {
        std::vector<Boolean> const &ps = c.ps;
        unsigned const cardinality = c.cardinality;

        assert(ps.size() > 0 && "Equal cardinality constraint requires at least one input variable");
        unsigned const w = log2( ps.size() ) + 1;
        return evaluate(ctx, logic::equal(cardinality_any(ctx, ps), logic::QF_BV::bvuint(cardinality, w)));
      }

      template <typename Context, typename Boolean>
      typename Context::result_type
      cardinality(Context &ctx, cardinality::Cardinality<cardtags::le_tag, Boolean> const &c) {
        std::vector<Boolean> const &ps = c.ps;
        unsigned const cardinality = c.cardinality;

        assert(ps.size() > 0 && "Lower equal cardinality constraint requires at least one input variable");
        unsigned const w = log2( ps.size() ) + 1;
        return evaluate(ctx, bvule(cardinality_any(ctx, ps), logic::QF_BV::bvuint(cardinality, w)));
      }

      template <typename Context, typename Boolean>
      typename Context::result_type
      cardinality(Context &ctx, cardinality::Cardinality<cardtags::lt_tag, Boolean> const &c) {
        std::vector<Boolean> const &ps = c.ps;
        unsigned const cardinality = c.cardinality;

        assert(ps.size() > 0 && "Lower than cardinality constraint requires at least one input variable");
        unsigned const w = log2( ps.size() ) + 1;
        return evaluate(ctx, bvult(cardinality_any(ctx, ps), logic::QF_BV::bvuint(cardinality, w)));
      }

      template <typename Context, typename Boolean>
      typename Context::result_type
      cardinality(Context &ctx, cardinality::Cardinality<cardtags::gt_tag, Boolean> const &c) {
        std::vector<Boolean> const &ps = c.ps;
        unsigned const cardinality = c.cardinality;

        assert(ps.size() > 0 && "Greater than cardinality constraint requires at least one input variable");
        unsigned const w = log2( ps.size() ) + 1;
        return evaluate(ctx, bvugt(cardinality_any(ctx, ps), logic::QF_BV::bvuint(cardinality, w)));
      }

      template <typename Context, typename Boolean>
      typename Context::result_type
      cardinality(Context &ctx, cardinality::Cardinality<cardtags::ge_tag, Boolean> const &c) {
        std::vector<Boolean> const &ps = c.ps;
        unsigned const cardinality = c.cardinality;

        assert(ps.size() > 0 && "Greater equal cardinality constraint requires at least one input variable");
        unsigned const w = log2( ps.size() ) + 1;
        return evaluate(ctx, bvuge(cardinality_any(ctx, ps), logic::QF_BV::bvuint(cardinality, w)));
      }
    } // adder
  } // cardinality
} // metaSMT


/**
 * @date Sat Mar 19 19:49:51 2011 +0100
 * @author Laurent El Shafey <Laurent.El-Shafey@idiap.ch>
 *
 * Copyright (C) Idiap Research Institute, Martigny, Switzerland
 */

#include <stdexcept>
#include <boost/shared_array.hpp>

#include <bob.math/linsolve.h>

#include <bob.math/linear.h>

#include <bob.core/assert.h>
#include <bob.core/check.h>
#include <bob.core/array_copy.h>

// Declaration of the external LAPACK function (Linear system solvers)
extern "C" void dgesv_( const int *N, const int *NRHS, double *A,
  const int *lda, int *ipiv, double *B, const int *ldb, int *info);
extern "C" void dposv_( const char* uplo, const int *N, const int *NRHS,
  double *A, const int *lda, double *B, const int *ldb, int *info);

void bob::math::linsolve(const blitz::Array<double,2>& A,
  const blitz::Array<double,1>& b, blitz::Array<double,1>& x)
{
  // Check x and b
  bob::core::array::assertZeroBase(x);
  bob::core::array::assertZeroBase(b);
  bob::core::array::assertSameDimensionLength(x.extent(0), b.extent(0));

  // Check A
  bob::core::array::assertZeroBase(A);
  bob::core::array::assertSameDimensionLength(A.extent(0), A.extent(1));
  bob::core::array::assertSameDimensionLength(A.extent(1), b.extent(0));

  bob::math::linsolve_(A, b, x);
}

void bob::math::linsolve_(const blitz::Array<double,2>& A,
  const blitz::Array<double,1>& b, blitz::Array<double,1>& x)
{
  // Defines dimensionality variables
  const int N = A.extent(0);

  // Prepares to call LAPACK function
  // Initialises LAPACK arrays
  boost::shared_array<int> ipiv(new int[N]);
  // Transpose (C: row major order, Fortran: column major)
  // Ugly fix for old blitz version support
  blitz::Array<double,2> A_blitz_lapack(
    bob::core::array::ccopy(const_cast<blitz::Array<double,2>&>(A).transpose(1,0)));
  double* A_lapack = A_blitz_lapack.data();
  // Tries to use X directly
  bool x_direct_use = bob::core::array::isCZeroBaseContiguous(x);
  blitz::Array<double,1> x_blitz_lapack;
  if (x_direct_use)
  {
    x_blitz_lapack.reference(x);
    x_blitz_lapack = b;
  }
  else
    x_blitz_lapack.reference(bob::core::array::ccopy(b));
  double *x_lapack = x_blitz_lapack.data();
  // Remaining variables
  int info = 0;
  const int lda = N;
  const int ldb = N;
  const int NRHS = 1;

  // Calls the LAPACK function (dgesv(
  dgesv_( &N, &NRHS, A_lapack, &lda, ipiv.get(), x_lapack, &ldb, &info );

  // Check info variable
  if (info != 0)
    throw std::runtime_error("The LAPACK dgesv function returned a non-zero value.");

  // Copy result back to x if required
  if (!x_direct_use)
    x = x_blitz_lapack;
}

void bob::math::linsolve(const blitz::Array<double,2>& A,
  const blitz::Array<double,2>& B, blitz::Array<double,2>& X)
{
  // Checks dimensionality and zero base
  bob::core::array::assertZeroBase(A);
  bob::core::array::assertZeroBase(X);
  bob::core::array::assertZeroBase(B);
  bob::core::array::assertSameDimensionLength(A.extent(0), A.extent(1));
  bob::core::array::assertSameDimensionLength(A.extent(1), X.extent(0));
  bob::core::array::assertSameDimensionLength(A.extent(0), B.extent(0));
  bob::core::array::assertSameDimensionLength(X.extent(1), B.extent(1));

  bob::math::linsolve_(A, B, X);
}

void bob::math::linsolve_(const blitz::Array<double,2>& A,
  const blitz::Array<double,2>& B, blitz::Array<double,2>& X)
{

  // Defines dimensionality variables
  const int N = A.extent(0);
  const int P = X.extent(1);

  // Prepares to call LAPACK function (dgesv)
  // Initialises LAPACK arrays
  boost::shared_array<int> ipiv(new int[N]);
  // Transpose (C: row major order, Fortran: column major)
  // Ugly fix for old blitz version support
  blitz::Array<double,2> A_blitz_lapack(
    bob::core::array::ccopy(const_cast<blitz::Array<double,2>&>(A).transpose(1,0)));
  double* A_lapack = A_blitz_lapack.data();
  // Tries to use X directly
  blitz::Array<double,2> Xt = X.transpose(1,0);
  bool X_direct_use = bob::core::array::isCZeroBaseContiguous(Xt);
  blitz::Array<double,2> X_blitz_lapack;
  if (X_direct_use)
  {
    X_blitz_lapack.reference(Xt);
    // Ugly fix for old blitz version support
    X_blitz_lapack = const_cast<blitz::Array<double,2>&>(B).transpose(1,0);
  }
  else
    X_blitz_lapack.reference(
      bob::core::array::ccopy(const_cast<blitz::Array<double,2>&>(B).transpose(1,0)));
  double *X_lapack = X_blitz_lapack.data();
  // Remaining variables
  int info = 0;
  const int lda = N;
  const int ldb = N;
  const int NRHS = P;

  // Calls the LAPACK function (dgesv)
  dgesv_( &N, &NRHS, A_lapack, &lda, ipiv.get(), X_lapack, &ldb, &info );

  // Checks info variable
  if (info != 0)
    throw std::runtime_error("The LAPACK dgesdd function returned a non-zero value.");

  // Copy result back to X if required
  if (!X_direct_use )
    X = X_blitz_lapack.transpose(1,0);
}

void bob::math::linsolveSympos(const blitz::Array<double,2>& A,
  const blitz::Array<double,1>& b, blitz::Array<double,1>& x)
{
  // Check x and b
  bob::core::array::assertZeroBase(x);
  bob::core::array::assertZeroBase(b);
  bob::core::array::assertSameDimensionLength(x.extent(0), b.extent(0));

  // Check A
  bob::core::array::assertZeroBase(A);
  bob::core::array::assertSameDimensionLength(A.extent(0), A.extent(1));
  bob::core::array::assertSameDimensionLength(A.extent(1), b.extent(0));

  bob::math::linsolveSympos_(A, b, x);
}

void bob::math::linsolveSympos_(const blitz::Array<double,2>& A,
  const blitz::Array<double,1>& b, blitz::Array<double,1>& x)
{

  // Defines dimensionality variables
  const int N = A.extent(0);

  // Prepares to call LAPACK function
  // Initialises LAPACK arrays
  // Transpose (C: row major order, Fortran: column major)
  // Ugly fix for old blitz version support
  blitz::Array<double,2> A_blitz_lapack(
    bob::core::array::ccopy(const_cast<blitz::Array<double,2>&>(A).transpose(1,0)));
  double* A_lapack = A_blitz_lapack.data();
  // Tries to use X directly
  bool x_direct_use = bob::core::array::isCZeroBaseContiguous(x);
  blitz::Array<double,1> x_blitz_lapack;
  if (x_direct_use)
  {
    x_blitz_lapack.reference(x);
    x_blitz_lapack = b;
  }
  else
    x_blitz_lapack.reference(bob::core::array::ccopy(b));
  double *x_lapack = x_blitz_lapack.data();
  // Remaining variables
  int info = 0;
  const char uplo = 'U';
  const int lda = N;
  const int ldb = N;
  const int NRHS = 1;

  // Calls the LAPACK function (dposv)
  dposv_( &uplo, &N, &NRHS, A_lapack, &lda, x_lapack, &ldb, &info );

  // Check info variable
  if (info != 0)
    throw std::runtime_error("The LAPACK dposv function returned a \
      non-zero value. This might be caused by a non-symmetric definite \
      positive matrix.");

  // Copy result back to x if required
  if (!x_direct_use )
    x = x_blitz_lapack;
}

void bob::math::linsolveSympos(const blitz::Array<double,2>& A,
  const blitz::Array<double,2>& B, blitz::Array<double,2>& X)
{
  // Checks dimensionality and zero base
  bob::core::array::assertZeroBase(A);
  bob::core::array::assertZeroBase(X);
  bob::core::array::assertZeroBase(B);
  bob::core::array::assertSameDimensionLength(A.extent(0), A.extent(1));
  bob::core::array::assertSameDimensionLength(A.extent(1), X.extent(0));
  bob::core::array::assertSameDimensionLength(A.extent(0), B.extent(0));
  bob::core::array::assertSameDimensionLength(X.extent(1), B.extent(1));

  bob::math::linsolveSympos_(A, B, X);
}

void bob::math::linsolveSympos_(const blitz::Array<double,2>& A,
  const blitz::Array<double,2>& B, blitz::Array<double,2>& X)
{
  // Defines dimensionality variables
  const int N = A.extent(0);
  const int P = X.extent(1);

  // Prepares to call LAPACK function (dposv)
  // Initialises LAPACK arrays
  // Transpose (C: row major order, Fortran: column major)
  // Ugly fix for old blitz version support
  blitz::Array<double,2> A_blitz_lapack(
    bob::core::array::ccopy(const_cast<blitz::Array<double,2>&>(A).transpose(1,0)));
  double* A_lapack = A_blitz_lapack.data();
  // Tries to use X directly
  blitz::Array<double,2> Xt = X.transpose(1,0);
  bool X_direct_use = bob::core::array::isCZeroBaseContiguous(Xt);
  blitz::Array<double,2> X_blitz_lapack;
  if (X_direct_use)
  {
    X_blitz_lapack.reference(Xt);
    // Ugly fix for old blitz version support
    X_blitz_lapack = const_cast<blitz::Array<double,2>&>(B).transpose(1,0);
  }
  else
    X_blitz_lapack.reference(
      bob::core::array::ccopy(const_cast<blitz::Array<double,2>&>(B).transpose(1,0)));
  double *X_lapack = X_blitz_lapack.data();
  // Remaining variables
  int info = 0;
  const char uplo = 'U';
  const int lda = N;
  const int ldb = N;
  const int NRHS = P;

  // Calls the LAPACK function (dposv)
  dposv_( &uplo, &N, &NRHS, A_lapack, &lda, X_lapack, &ldb, &info );

  // Check info variable
  if (info != 0)
    throw std::runtime_error("The LAPACK dposv function returned a \
      non-zero value. This might be caused by a non-symmetric definite \
      positive matrix.");

  // Copy result back to X if required
  if (!X_direct_use)
    X = X_blitz_lapack.transpose(1,0);
}

void bob::math::linsolveCGSympos(const blitz::Array<double,2>& A,
  const blitz::Array<double,1>& b, blitz::Array<double,1>& x,
  const double acc, const int max_iter)
{
  // Dimensionality of the problem
  const int N = b.extent(0);

  // Check x and b
  bob::core::array::assertZeroBase(x);
  bob::core::array::assertZeroBase(b);
  bob::core::array::assertSameDimensionLength(x.extent(0), N);

  // Check A
  bob::core::array::assertZeroBase(A);
  bob::core::array::assertSameDimensionLength(A.extent(0), N);
  bob::core::array::assertSameDimensionLength(A.extent(1), N);

  bob::math::linsolveCGSympos_(A, b, x, acc, max_iter);
}

void bob::math::linsolveCGSympos_(const blitz::Array<double,2>& A,
  const blitz::Array<double,1>& b, blitz::Array<double,1>& x,
  const double acc, const int max_iter)
{
  // Dimensionality of the problem
  const int N = b.extent(0);

  blitz::Array<double,1> r(N), d(N), best_x(N), q(N), tmp(N);
  x = 0.;
  r = b;
  d = b;

  double delta = bob::math::dot(r,r);
  double delta0 = bob::math::dot(b,b);

  int n_iter = 0;
  best_x = x;
  double best_res = sqrt(delta / delta0);

  while (n_iter < max_iter && delta > acc*acc*delta0)
  {
    // q = A*d
    bob::math::prod_(A, d, q);

    // alpha = delta/(d'*q);
    double alpha = delta / bob::math::dot(d,q);
    x = x + alpha * d;

    if (n_iter+1 % 50 == 0)
    {
      bob::math::prod(A,x,tmp);
      r = b - tmp;
    }
    else
      r = r - alpha * q;

    double delta_old = delta;
    delta = bob::math::dot(r,r);
    double beta = delta / delta_old;
    d = r + beta * d;
    ++n_iter;

    if (sqrt(delta/delta0) < best_res)
    {
      best_x = x;
      best_res = sqrt(delta/delta0);
    }
  }

  x = best_x;

  // TODO return best_res and number of iterations?
  //double res = best_res;
}