/* Copyright © 2017 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at https://opensource.org/licenses/BSD-3-Clause
 */
#ifndef TURI_CONSTRAINTS_H_
#define TURI_CONSTRAINTS_H_

#include <string>
#include <core/data/flexible_type/flexible_type.hpp>

// Eigen
#include <Eigen/Core>
#include <Eigen/SparseCore>

// Optimizaiton
#include <ml/optimization/utils.hpp>
#include <ml/optimization/optimization_interface.hpp>
#include <ml/optimization/constraint_interface.hpp>

// TODO: List of todo's for this file
//------------------------------------------------------------------------------
//

namespace turi {

namespace optimization {

/**
 * \ingroup group_optimization
 * \addtogroup optimization_constraints Optimization Constraints
 * \{
 */

/**
 * Interface for non-negative constriants.
 *   x >= 0
 */
class non_negative_orthant : public constraint_interface {

  protected:

    size_t variables;                       /**< # Variables in the problem */

  public:

  /**
   * Default constructor.
   */
  non_negative_orthant(const size_t& _variables){
    variables = _variables;
  }

  /**
   * Default desctuctor. Do nothing.
   */
  ~non_negative_orthant(){
  }

  /**
   * Project a dense point into the constraint space.
   * \param[in,out]  point   Point (Dense Vector)
   *
   * Given a convex set X, the projection operator is given by
   *     P(y) = \std::max(x, o)
   *
   */
  inline void project(DenseVector &point) const {
    DASSERT_EQ(variables, point.size());
    point = point.cwiseMax(0);
  }

  /**
   * Project a block of a dense point into the constraint space.
   *
   * \param[in,out]  point        A block project the point.
   * \param[in]      block_start  Start index of the block
   * \param[in]      block_size   Size of the block
   *
   * Given a convex set X, the projection operator is given by
   *     P(y) = \std::max(x, o)
   *
   */
  inline void project_block(DenseVector &point, const size_t block_start, const
      size_t block_size) const {
    DASSERT_LE(variables, block_start + block_size);
    DASSERT_EQ(block_size, point.size());
    point = point.cwiseMax(0);
  }

  /**
   * Boolean function to deterstd::mine if a dense point is present in a constraint
   * space.
   * \param[in]  point   Point which we are querying.
   *
   */
  inline bool is_satisfied(const DenseVector &point) const {
    DASSERT_EQ(variables, point.size());
    for(size_t i=0; i < size_t(point.size()); i++){
      if( point(i) <= -OPTIMIZATION_ZERO)
        return false;
    }
    return true;
  }

  /**
   * A measure of the first order optimality conditions.
   *
   * \param[in]  point    Point which we are querying.
   * \param[in]  gradient Gradient at that point for a given function
   *
   * Use the Cauchy point as a measure of optimality. See Pg 486 of
   * Nocedal and Wright (Edition 2)
   *
   */
  inline double first_order_optimality_conditions(const DenseVector &point,
                                        const DenseVector& gradient) const{
    DASSERT_TRUE(is_satisfied(point));
    DenseVector proj_gradient = gradient;
    for(size_t i=0; i < size_t(point.size()); i++){
      if(point(i) <= OPTIMIZATION_ZERO){
        proj_gradient(i) = std::min(0.0, gradient(i));
      } else {
        proj_gradient(i) = gradient(i);
      }
    }
    return compute_residual(proj_gradient);
  }
};

/**
 * Interface for box-constraints on variables.
 *   lb <= x <= ub
 */
class box_constraints: public constraint_interface {

  protected:

    DenseVector lb;                  /**< # Upper bound */
    DenseVector ub;                  /**< # Lower bound */
    size_t variables;                /**< # Variables in the problem */

  public:

  /**
   * Default constructor.
   * \param[in]  _variables Number of variables
   * \param[in]  _lb Lower bound
   * \param[in]  _ub Upper bound
   */
  box_constraints(const double& _lb, const double& _ub, const size_t&
      _variables){
    variables = _variables;
    lb.resize(variables);
    lb.setConstant(_lb);
    ub.resize(variables);
    ub.setConstant(_ub);
  }

  /**
   * Default constructor.
   * \param[in]  _variables Number of variables
   * \param[in]  _lb Lower bound
   * \param[in]  _ub Upper bound
   */
  box_constraints(const DenseVector& _lb, const
      DenseVector& _ub){
    variables = _lb.size();
    DASSERT_EQ(variables, _ub.size());
    lb = _lb;
    ub = _ub;
  }

  /**
   * Default desctuctor. Do nothing.
   */
  ~box_constraints(){
  }

  /**
   * Project a dense point into the constraint space.
   * \param[in,out]  point   Point (Dense Vector)
   *
   * Given a convex set X, the projection operator is given by
   *     P(y) = \std::max(x, o)
   *
   */
  inline void project(DenseVector &point) const {
    DASSERT_EQ(variables, point.size());
    for(size_t i=0; i < size_t(point.size()); i++){
      point(i) = std::min(std::max(point(i), lb(i)), ub(i));
    }
  }

  /**
   * Project a block of a dense point into the constraint space.
   *
   * \param[in,out]  point        A block project the point.
   * \param[in]      block_start  Start index of the block
   * \param[in]      block_size   Size of the block
   *
   * Given a convex set X, the projection operator is given by
   *     P(y) = \std::max(x, o)
   *
   */
  inline void project_block(DenseVector &point, const size_t block_start, const
      size_t block_size) const {
    DASSERT_GE(variables, block_start + block_size);
    DASSERT_EQ(block_size, point.size());
    for(size_t i=0; i < block_size; i++){
      point(i) = std::min(std::max(point(i),
                                  lb(block_start + i)), ub(block_start + i));
    }
  }

  /**
   * Boolean function to determine if a dense point is present in a constraint
   * space.
   * \param[in]  point   Point which we are querying.
   *
   */
  inline bool is_satisfied(const DenseVector &point) const {
    DASSERT_EQ(variables, point.size());
    for(size_t i=0; i < size_t(point.size()); i++){
      if( point(i) <= lb(i) - OPTIMIZATION_ZERO ||
          point(i) >= ub(i) + OPTIMIZATION_ZERO)
        return false;
    }
    return true;
  }

  /**
   * A measure of the first order optimality conditions.
   *
   * \param[in]  point    Point which we are querying.
   * \param[in]  gradient Gradient at that point for a given function
   *
   * Use the Cauchy point as a measure of optimality. See Pg 486 of
   * Nocedal and Wright (Edition 2)
   *
   */
  inline double first_order_optimality_conditions(const DenseVector &point,
                                        const DenseVector& gradient) const{
    DASSERT_TRUE(is_satisfied(point));
    DenseVector proj_gradient = gradient;
    for(size_t i=0; i < size_t(point.size()); i++){
      if(point(i) <= OPTIMIZATION_ZERO + lb(i)){
        proj_gradient(i) = std::min(0.0, gradient(i));
      } else if (point(i) >= ub(i) - OPTIMIZATION_ZERO){
        proj_gradient(i) = std::max(0.0, gradient(i));
      } else {
        proj_gradient(i) = gradient(i);
      }
    }
    return compute_residual(proj_gradient);
  }

};

} // optimization
} // turicreate

#endif


#ifndef __GIFS_UTIL_HPP
#define __GIFS_UTIL_HPP

#include <armadillo>

namespace util{
  arma::vec center_of_mass(const arma::mat R, const arma::vec m);
  arma::vec sum_cross(const arma::mat A, const arma::mat B);
  //  arma::vec net(arma::vec (*op)(const arma::vec &a, const arma::vec &b), const arma::mat A, const arma::mat B);

  double hypot(std::complex<double> a, std::complex<double> b);
  arma::uword sample_discrete(const arma::vec &p);
  arma::uvec range(arma::uword a, arma::uword b); //[a, b)
  arma::uvec range(arma::uword n); // [0, n)

  template <typename T>
  inline bool approx_equal(T a, T b, T tol=arma::datum::eps){
    return std::abs(a-b) < tol;
  };
}

#endif


// Copyright 2008-2010 Gordon Woodhull
// 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)

// mplgraph.cpp : Defines the entry point for the console application.
//

#include <boost/msm/mpl_graph/incidence_list_graph.hpp>
#include <boost/msm/mpl_graph/mpl_utils.hpp>

namespace mpl_graph = boost::msm::mpl_graph;
namespace mpl_utils = mpl_graph::mpl_utils;
namespace mpl = boost::mpl;
/*
    test graph:
    A -> B -> C -\--> D
           \     |--> E
            \    \--> F
             \-----/
*/

// vertices
struct A{}; struct B{}; struct C{}; struct D{}; struct E{}; struct F{};

// edges
struct A_B{}; struct B_C{}; struct C_D{}; struct C_E{}; struct C_F{}; struct B_F{};

typedef mpl::vector<mpl::vector<A_B,A,B>,
               mpl::vector<B_C,B,C>,
               mpl::vector<C_D,C,D>,
               mpl::vector<C_E,C,E>,
               mpl::vector<C_F,C,F>,
               mpl::vector<B_F,B,F> >
    some_incidence_list;
typedef mpl_graph::incidence_list_graph<some_incidence_list> some_graph;

BOOST_MPL_ASSERT(( boost::is_same<mpl_graph::source<B_C,some_graph>::type, B> ));
BOOST_MPL_ASSERT(( boost::is_same<mpl_graph::source<C_D,some_graph>::type, C> ));

BOOST_MPL_ASSERT(( boost::is_same<mpl_graph::target<C_D,some_graph>::type, D> ));
BOOST_MPL_ASSERT(( boost::is_same<mpl_graph::target<B_F,some_graph>::type, F> ));

BOOST_MPL_ASSERT(( mpl_utils::set_equal<mpl_graph::out_edges<C,some_graph>::type, mpl::vector<C_D,C_E,C_F> > ));
BOOST_MPL_ASSERT(( mpl_utils::set_equal<mpl_graph::out_edges<B,some_graph>::type, mpl::vector<B_F,B_C> > ));

BOOST_MPL_ASSERT_RELATION( (mpl_graph::out_degree<B,some_graph>::value), ==, 2 );
BOOST_MPL_ASSERT_RELATION( (mpl_graph::out_degree<C,some_graph>::value), ==, 3 );

BOOST_MPL_ASSERT(( mpl_utils::set_equal<mpl_graph::in_edges<C,some_graph>::type, mpl::vector<B_C> > ));
BOOST_MPL_ASSERT(( mpl_utils::set_equal<mpl_graph::in_edges<F,some_graph>::type, mpl::vector<C_F,B_F> > ));

BOOST_MPL_ASSERT_RELATION( (mpl_graph::in_degree<A,some_graph>::value), ==, 0 );
BOOST_MPL_ASSERT_RELATION( (mpl_graph::in_degree<F,some_graph>::value), ==, 2 );

BOOST_MPL_ASSERT_RELATION( (mpl_graph::degree<A,some_graph>::value), ==, 1 );
BOOST_MPL_ASSERT_RELATION( (mpl_graph::degree<C,some_graph>::value), ==, 4 );

BOOST_MPL_ASSERT(( mpl_utils::set_equal<mpl_graph::adjacent_vertices<A,some_graph>::type, mpl::vector<B> > ));
BOOST_MPL_ASSERT(( mpl_utils::set_equal<mpl_graph::adjacent_vertices<C,some_graph>::type, mpl::vector<D,E,F> > ));

BOOST_MPL_ASSERT(( mpl_utils::set_equal<mpl_graph::vertices<some_graph>::type, mpl::vector<A,B,C,D,E,F> > ));

BOOST_MPL_ASSERT_RELATION( mpl_graph::num_vertices<some_graph>::value, ==, 6 );

BOOST_MPL_ASSERT_RELATION( mpl_graph::num_edges<some_graph>::value, ==, 6 );

int main(int argc, char* argv[])
{
    return 0;
}


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

#include <OpenTissue/core/math/math_random.h>
#include <OpenTissue/core/math/big/big_types.h>
#include <OpenTissue/core/math/big/big_conjugate_gradient.h>

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

template<typename solver_type, typename matrix_type,typename vector_type>
void test(matrix_type const & A, vector_type  & x, vector_type const & b, vector_type const & y)
{
  typedef typename matrix_type::value_type real_type;
  typedef typename matrix_type::size_type  size_type;

  real_type const tol = boost::numeric_cast<real_type>(1.0);

  solver_type S;

  real_type epsilon = boost::numeric_cast<real_type>(10e-10);
  size_t max_iterations = 100;
  size_t iterations;

  S(A,x,b, max_iterations, epsilon, iterations);

  // Test for convergence
  BOOST_CHECK( iterations < max_iterations );

  for(size_type i = 0; i < x.size();++i)
    BOOST_CHECK_CLOSE( real_type( x(i) ), real_type( y(i) ), tol );
}

BOOST_AUTO_TEST_SUITE(opentissue_math_big_conjugate_gradient);

BOOST_AUTO_TEST_CASE(logic_and_valid_arguments_testing)
{
  typedef ublas::compressed_matrix<double> matrix_type;
  typedef ublas::vector<double>            vector_type;
  typedef vector_type::size_type           size_type;

  typedef OpenTissue::math::big::ConjugateGradientFunctor solver_type;

  {
    matrix_type A;
    vector_type x,b;
    solver_type S;
    BOOST_CHECK_THROW( S(A,x,b), std::invalid_argument );
  }
  {
    matrix_type A;
    vector_type x,b;
    A.resize(4,4,false);
    solver_type S;
    BOOST_CHECK_THROW( S(A,x,b), std::invalid_argument );
  }
  {
    matrix_type A;
    vector_type x,b;
    A.resize(4,4,false);
    b.resize(4,false);
    solver_type S;
    BOOST_CHECK_THROW( S(A,x,b), std::invalid_argument );
  }
  {
    matrix_type A;
    vector_type x,b;
    A.resize(4,4,false);
    x.resize(4,false);
    solver_type S;
    BOOST_CHECK_THROW( S(A,x,b), std::invalid_argument );
  }
  {
    matrix_type A;
    vector_type x,b;
    A.resize(4,4,false);
    x.resize(4,false);
    b.resize(4,false);
    A(0,0) = A(1,1) = A(2,2) = A(3,3) = 1.0;
    solver_type S;
    BOOST_CHECK_NO_THROW( S(A,x,b) );
  }
  {
    matrix_type A;
    vector_type x,b;
    x.resize(4,false);
    b.resize(4,false);
    solver_type S;
    BOOST_CHECK_THROW( S(A,x,b), std::invalid_argument );
  }
  {
    matrix_type A;
    vector_type x,b;
    A.resize(4,4,false);
    x.resize(3,false);
    b.resize(4,false);
    solver_type S;
    BOOST_CHECK_THROW( S(A,x,b), std::invalid_argument );
  }
  {
    matrix_type A;
    vector_type x,b;
    A.resize(4,4,false);
    x.resize(4,false);
    b.resize(3,false);
    solver_type S;
    BOOST_CHECK_THROW( S(A,x,b), std::invalid_argument );
  }
  {
    matrix_type A;
    vector_type x,b;
    A.resize(4,4,false);
    x.resize(3,false);
    b.resize(3,false);
    solver_type S;
    BOOST_CHECK_THROW( S(A,x,b), std::invalid_argument );
  }
  {
    matrix_type A;
    vector_type x,b;
    A.resize(4,5,false);
    x.resize(5,false);
    b.resize(4,false);
    solver_type S;
    BOOST_CHECK_THROW( S(A,x,b), std::invalid_argument );
  }
}

BOOST_AUTO_TEST_CASE(random_test_case)
{

  typedef ublas::compressed_matrix<double> matrix_type;
  typedef ublas::vector<double>            vector_type;
  typedef vector_type::size_type           size_type;

  typedef OpenTissue::math::big::ConjugateGradientFunctor solver_type;

  for(size_type tst=0;tst<1000;++tst)
  {
    size_type N = 10;

    matrix_type A;
    A.resize(N,N,false);

    vector_type x;
    x.resize(N,false);

    vector_type b;
    b.resize(N,false);

    vector_type y;
    y.resize(N,false);

    matrix_type R;
    R.resize(N,N,false);

    OpenTissue::math::Random<double> value(0.0,1.0);
    for(size_t i=0;i<R.size1();++i)
    {
      b(i) = value();
      x(i) = value();
      y(i) = value();
      for(size_t j=0;j<R.size2();++j)
        R(i,j) = value();
    }
    ublas::noalias(A) = ublas::sparse_prod<matrix_type>( ublas::trans(R), R );
    // forcing A to become PD matrix (it should be non-singular at all times)
    for(size_t i=0;i<R.size1();++i)
      A(i,i) += 0.5;

    ublas::noalias(b) = ublas::prod(A,x);
    y.assign(x);

    x.clear();
    test<solver_type>(A,x,b,y);
  }

}

BOOST_AUTO_TEST_SUITE_END();


//  (C) Copyright Eric Niebler, Olivier Gygi 2006.
//  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)

// Test case for p_square_cumul_dist.hpp

#include <cmath>
#include <boost/random.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/accumulators/numeric/functional/vector.hpp>
#include <boost/accumulators/numeric/functional/complex.hpp>
#include <boost/accumulators/numeric/functional/valarray.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/p_square_cumul_dist.hpp>
#include <sstream>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>

using namespace boost;
using namespace unit_test;
using namespace boost::accumulators;

///////////////////////////////////////////////////////////////////////////////
// erf() not known by VC++ compiler!
// my_erf() computes error function by numerically integrating with trapezoidal rule
//
double my_erf(double const& x, int const& n = 1000)
{
    double sum = 0.;
    double delta = x/n;
    for (int i = 1; i < n; ++i)
        sum += std::exp(-i*i*delta*delta) * delta;
    sum += 0.5 * delta * (1. + std::exp(-x*x));
    return sum * 2. / std::sqrt(3.141592653);
}

typedef accumulator_set<double, stats<tag::p_square_cumulative_distribution> > accumulator_t;
typedef iterator_range<std::vector<std::pair<double, double> >::iterator > histogram_type;

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    // tolerance in %
    double epsilon = 3;

    accumulator_t acc(p_square_cumulative_distribution_num_cells = 100);

    // two random number generators
    boost::lagged_fibonacci607 rng;
    boost::normal_distribution<> mean_sigma(0,1);
    boost::variate_generator<boost::lagged_fibonacci607&, boost::normal_distribution<> > normal(rng, mean_sigma);

    for (std::size_t i=0; i<1000000; ++i)
    {
        acc(normal());
    }

    histogram_type histogram = p_square_cumulative_distribution(acc);

    for (std::size_t i = 0; i < histogram.size(); ++i)
    {
        // problem with small results: epsilon is relative (in percent), not absolute!
        if ( histogram[i].second > 0.001 )
            BOOST_CHECK_CLOSE( 0.5 * (1.0 + my_erf( histogram[i].first / std::sqrt(2.0) )), histogram[i].second, epsilon );
    }
}

///////////////////////////////////////////////////////////////////////////////
// test_persistency
//
void test_persistency()
{
    // "persistent" storage
    std::stringstream ss;
    // tolerance in %
    double epsilon = 3;
    {
        accumulator_t acc(p_square_cumulative_distribution_num_cells = 100);

        // two random number generators
        boost::lagged_fibonacci607 rng;
        boost::normal_distribution<> mean_sigma(0,1);
        boost::variate_generator<boost::lagged_fibonacci607&, boost::normal_distribution<> > normal(rng, mean_sigma);

        for (std::size_t i=0; i<1000000; ++i)
        {
            acc(normal());
        }

        histogram_type histogram = p_square_cumulative_distribution(acc);

        BOOST_CHECK_CLOSE(0.5 * (1.0 + my_erf(histogram[25].first / std::sqrt(2.0))), histogram[25].second, epsilon);
        BOOST_CHECK_CLOSE(0.5 * (1.0 + my_erf(histogram[50].first / std::sqrt(2.0))), histogram[50].second, epsilon);
        BOOST_CHECK_CLOSE(0.5 * (1.0 + my_erf(histogram[75].first / std::sqrt(2.0))), histogram[75].second, epsilon);
        boost::archive::text_oarchive oa(ss);
        acc.serialize(oa, 0);
    }
    accumulator_t acc(p_square_cumulative_distribution_num_cells = 100);
    boost::archive::text_iarchive ia(ss);
    acc.serialize(ia, 0);
    histogram_type histogram = p_square_cumulative_distribution(acc);
    BOOST_CHECK_CLOSE(0.5 * (1.0 + my_erf(histogram[25].first / std::sqrt(2.0))), histogram[25].second, epsilon);
    BOOST_CHECK_CLOSE(0.5 * (1.0 + my_erf(histogram[50].first / std::sqrt(2.0))), histogram[50].second, epsilon);
    BOOST_CHECK_CLOSE(0.5 * (1.0 + my_erf(histogram[75].first / std::sqrt(2.0))), histogram[75].second, epsilon);
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("p_square_cumulative_distribution test");

    test->add(BOOST_TEST_CASE(&test_stat));
    test->add(BOOST_TEST_CASE(&test_persistency));

    return test;
}


/*
 * GraphBLAS Template Library (GBTL), Version 3.0
 *
 * Copyright 2020 Carnegie Mellon University, Battelle Memorial Institute, and
 * Authors.
 *
 * THIS MATERIAL WAS PREPARED AS AN ACCOUNT OF WORK SPONSORED BY AN AGENCY OF
 * THE UNITED STATES GOVERNMENT.  NEITHER THE UNITED STATES GOVERNMENT NOR THE
 * UNITED STATES DEPARTMENT OF ENERGY, NOR THE UNITED STATES DEPARTMENT OF
 * DEFENSE, NOR CARNEGIE MELLON UNIVERSITY, NOR BATTELLE, NOR ANY OF THEIR
 * EMPLOYEES, NOR ANY JURISDICTION OR ORGANIZATION THAT HAS COOPERATED IN THE
 * DEVELOPMENT OF THESE MATERIALS, MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR
 * ASSUMES ANY LEGAL LIABILITY OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS,
 * OR USEFULNESS OR ANY INFORMATION, APPARATUS, PRODUCT, SOFTWARE, OR PROCESS
 * DISCLOSED, OR REPRESENTS THAT ITS USE WOULD NOT INFRINGE PRIVATELY OWNED
 * RIGHTS.
 *
 * Released under a BSD-style license, please see LICENSE file or contact
 * permission@sei.cmu.edu for full terms.
 *
 * [DISTRIBUTION STATEMENT A] This material has been approved for public release
 * and unlimited distribution.  Please see Copyright notice for non-US
 * Government use and distribution.
 *
 * This Software includes and/or makes use of the following Third-Party Software
 * subject to its own license:
 *
 * 1. Boost Unit Test Framework
 * (https://www.boost.org/doc/libs/1_45_0/libs/test/doc/html/utf.html)
 * Copyright 2001 Boost software license, Gennadiy Rozental.
 *
 * DM20-0442
 */

#include <iostream>

#include <algorithms/metrics.hpp>
#include <graphblas/graphblas.hpp>

using namespace grb;
using namespace algorithms;

#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE metrics_test_suite

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

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

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

static std::vector<double> gr={0,1,1,2,2,2,2,3,3,3,3,4,4,4,5,6,6,6,8,8};
static std::vector<double> gc={3,3,6,4,5,6,8,0,1,4,6,2,3,8,2,1,2,3,2,4};
static std::vector<double> gv(gr.size(), 1);

//static Matrix<double, DirectedMatrixTag> G_tn_answer(
//    {{2, 2, 3, 1, 2, 4, 2, -, 3},
//     {2, 2, 2, 1, 2, 3, 1, -, 3},
//     {3, 2, 2, 2, 1, 1, 1, -, 1},
//     {1, 1, 2, 2, 1, 3, 1, -, 2},
//     {2, 2, 1, 1, 2, 2, 2, -, 1},
//     {4, 3, 1, 3, 2, 2, 2, -, 2},
//     {2, 1, 1, 1, 2, 2, 2, -, 2},
//     {-, -, -, -, -, -, -, -, -},
//     {3, 3, 1, 2, 1, 2, 2, -, 2}},
//    INF);

static std::vector<double> tr={0,0,1,1,2,2,2,2,3,3,4,4};
static std::vector<double> tc={1,2,0,2,0,1,3,4,2,4,2,3};
static std::vector<double> tv(tr.size(), 1);

//static Matrix<double, DirectedMatrixTag> test5x5(
//    {{-, 1, 1, -, -},
//     {1, -, 1, -, -},
//     {1, 1, -, 1, 1},
//     {-, -, 1, -, 1},
//     {-, -, 1, 1, -}},
//    INF);

//****************************************************************************
BOOST_AUTO_TEST_CASE(metrics_test_in_degree)
{
    Matrix<double, DirectedMatrixTag> G_tn(9,9);
    G_tn.build(gr.begin(), gc.begin(), gv.begin(), gv.size());
    IndexType result = vertex_in_degree(G_tn, 1);
    BOOST_CHECK_EQUAL(result, 2);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(metrics_test_out_degree)
{
    Matrix<double, DirectedMatrixTag> G_tn(9,9);
    G_tn.build(gr.begin(), gc.begin(), gv.begin(), gv.size());
    IndexType result = vertex_out_degree(G_tn, 2);
    BOOST_CHECK_EQUAL(result, 4);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(metrics_test_vertex_degree)
{
    Matrix<double, DirectedMatrixTag> G_tn(9,9);
    G_tn.build(gr.begin(), gc.begin(), gv.begin(), gv.size());
    IndexType result = vertex_degree(G_tn, 0);
    BOOST_CHECK_EQUAL(result, 2);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(metrics_test_graph_distance)
{
    Matrix<double, DirectedMatrixTag> test5x5(5,5);
    test5x5.build(tr.begin(), tc.begin(), tv.begin(), tv.size());
    Vector<double> result(5);

    graph_distance(test5x5, 2, result);

    //Matrix<double, DirectedMatrixTag> answer(
    //     {1, 1, 0, 1, 1},
    //    INF);
    std::vector<double> ac={0,1,2,3,4};
    std::vector<double> av={1, 1, 0, 1, 1};
    Vector<double> answer(5);
    answer.build(ac.begin(), av.begin(), av.size());

    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(metrics_test_graph_distance_matrix)
{
    Matrix<double, DirectedMatrixTag> test5x5(5,5);
    test5x5.build(tr.begin(), tc.begin(), tv.begin(), tv.size());
    Matrix<double, DirectedMatrixTag> result(5,5);

    graph_distance_matrix(test5x5, result);

    //Matrix<double, DirectedMatrixTag> answer(
    //    {{0, 1, 1, 2, 2},
    //     {1, 0, 1, 2, 2},
    //     {1, 1, 0, 1, 1},
    //     {2, 2, 1, 0, 1},
    //     {2, 2, 1, 1, 0}},
    //    INF);

    std::vector<double> ar={0,0,0,0,0,1,1,1,1,1,2,2,2,2,2,3,3,3,3,3,4,4,4,4,4};
    std::vector<double> ac={0,1,2,3,4,0,1,2,3,4,0,1,2,3,4,0,1,2,3,4,0,1,2,3,4};
    std::vector<double> av={0, 1, 1, 2, 2,
                            1, 0, 1, 2, 2,
                            1, 1, 0, 1, 1,
                            2, 2, 1, 0, 1,
                            2, 2, 1, 1, 0};
    Matrix<double, DirectedMatrixTag> answer(5,5);
    answer.build(ar.begin(), ac.begin(), av.begin(), av.size());

    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(metrics_test_graph_eccentricity)
{
    Matrix<double, DirectedMatrixTag> test5x5(5,5);
    test5x5.build(tr.begin(), tc.begin(), tv.begin(), tv.size());
    double result = vertex_eccentricity(test5x5, 2);
    BOOST_CHECK_EQUAL(result, 1);
    result = vertex_eccentricity(test5x5, 1);
    BOOST_CHECK_EQUAL(result, 2);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(metrics_test_graph_radius)
{
    Matrix<double, DirectedMatrixTag> test5x5(5,5);
    test5x5.build(tr.begin(), tc.begin(), tv.begin(), tv.size());
    double result = graph_radius(test5x5);
    BOOST_CHECK_EQUAL(result, 1);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(metrics_test_graph_diameter)
{
    Matrix<double, DirectedMatrixTag> test5x5(5,5);
    test5x5.build(tr.begin(), tc.begin(), tv.begin(), tv.size());
    double result = graph_diameter(test5x5);
    BOOST_CHECK_EQUAL(result, 2);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(metrics_test_closeness_centrality)
{
    Matrix<double, DirectedMatrixTag> test5x5(5,5);
    test5x5.build(tr.begin(), tc.begin(), tv.begin(), tv.size());
    double result = closeness_centrality(test5x5, 2);
    BOOST_CHECK_EQUAL(result, 4);
}

BOOST_AUTO_TEST_SUITE_END()


//          Copyright Carl Philipp Reh 2009 - 2016.
// 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 <fcppt/container/grid/make_spiral_range.hpp>
#include <fcppt/container/grid/pos.hpp>
#include <fcppt/math/diff.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <fcppt/config/external_end.hpp>

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	container_grid_spiral_range
)
{
FCPPT_PP_POP_WARNING

	typedef
	int
	int_type;

	typedef
	fcppt::container::grid::pos<
		int_type,
		2
	>
	pos;

	typedef
	std::vector<
		pos
	>
	pos_vector;

	pos_vector positions;

	positions.reserve(
		100
	);

	auto const manhattan_distance(
		[](
			pos const _p1,
			pos const _p2
		)
		{
			return
				fcppt::math::diff(
					_p1.x(),
					_p2.y()
				)
				+
				fcppt::math::diff(
					_p1.y(),
					_p2.y()
				);
		}
	);

	pos const start(
		5,
		5
	);

	for(
		int_type distance = 0;
		distance < 4;
		++distance
	)
	{
		pos_vector result;

		for(
			auto const cur
			:
			fcppt::container::grid::make_spiral_range(
				pos(
					5,
					5
				),
				2
			)
		)
			result.push_back(
				cur
			);

		BOOST_REQUIRE(
			!result.empty()
		);

		pos_vector::const_iterator it(
			result.begin()
		);

		BOOST_CHECK_EQUAL(
			manhattan_distance(
				*it,
				start
			),
			0
		);

		++it;

		for(
			int_type cur = 1;
			cur < distance;
			++cur
		)
		{
			int_type const count(
				cur * 4
			);

			for(
				int_type i = 0;
				i < count;
				++i
			)
				BOOST_CHECK_EQUAL(
					manhattan_distance(
						*it++,
						start
					),
					cur
				);
		}
	}
}


#include <boost/math/distributions/binomial.hpp>


#pragma once
#include <cmath>
#include <tuple>
#include <cstddef>
#include <utility>
#include <algorithm>
#include <functional>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

#include "utils/types.hpp"
#include "utils/utils.hpp"
#include "utils/assert.hpp"
#include "utils/comparators.hpp"

namespace ample {

    namespace _impl {

        template<typename T>
        using vector_t = Eigen::Matrix<T, Eigen::Dynamic, 1>;

        template<typename T>
        using matrix_t = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;

        template<typename T>
        struct solver_type {

            using type = Eigen::EigenSolver<matrix_t<T>>;

        };

        template<typename T>
        struct solver_type<std::complex<T>> {

            using type = Eigen::ComplexEigenSolver<matrix_t<std::complex<T>>>;

        };

        template<typename T>
        using solver_type_t = typename solver_type<T>::type;

        template<typename T>
        struct pade_coefficients {

            auto operator()(const types::vector1d_t<T>& c, const size_t& n, const size_t& m) {
                vector_t<T> b(m);
                matrix_t<T> A(m, m);

                for (size_t i = 0, ci = n + 1; i < m; ++i, ++ci) {
                    b(i) = -c[ci];

                    for (size_t j = 0; j < std::min(ci, m); ++j)
                        A(i, j) = c[ci - j - 1];
                }

                const vector_t<T> x = A.colPivHouseholderQr().solve(b);

                types::vector1d_t<T> ac(n + 1), bc(m + 1, T(1));
                for (size_t i = 0; i < n + 1; ++i) {
                    ac[i] = c[i];
                    for (size_t j = 0; j < std::min(i, m); ++j)
                        ac[i] += c[i - j - 1] * x(j);
                }

                for (size_t i = 0; i < m; ++i)
                    bc[i + 1] = x(i);

                return std::make_tuple(ac, bc);
            }

        };

        template<typename T>
        struct theta_pade_coefficients {

            T theta{};

            auto operator()(const types::vector1d_t<T>& c, const size_t& n, const size_t& m) {
                utils::dynamic_assert(n == m, "coefficients: n(", n, ") must be equal to m(", m, ") for theta-weighted coefficients");

                pade_coefficients<T> coefficients{};
                const auto [a0, b0] = coefficients(c, n - 1, n);
                      auto [a1, b1] = coefficients(c, n, n);

                const auto theta1 = T(1) - theta;
                for (size_t i = 0; i < a0.size(); ++i)
                    a1[i] = a1[i] * theta + a0[i] * theta1;

                a1.back() *= theta;

                for (size_t i = 0; i < b0.size(); ++i)
                    b1[i] = b1[i] * theta + b0[i] * theta1;

                return std::make_tuple(a1, b1);
            }

        };

        template<typename T>
        struct exp_taylor {

            static constexpr auto on = T(1);
            static constexpr auto tw = T(2);
            static constexpr auto fo = T(4);
            static constexpr auto si = T(6);

            auto operator()(const T& value, const size_t& n) {
                const auto v2 = std::pow(value, 2);

                types::vector1d_t<T> c(n);
                c[0] = on;
                c[1] = value /tw;

                for (size_t i = 2, m = 0; i < n; ++i, ++m) {
                    const auto mm = T(m);
                    c[i] = (v2 * c[m] - (tw + si * mm + fo * mm * mm) * c[m + 1]) / (fo * (on + mm) * (tw + mm));
                }

                return c;
            }

        };

        template<typename T>
        struct root_taylor {

            auto operator()(const T& value, const size_t& n) {
                types::vector1d_t<T> c(n, T(0));
                std::for_each(c.begin() + 1, c.end(),
                    [&value, p=T(1), i=size_t(0), f=T(1)](auto& x) mutable {
                        ++i;
                        x = value * (p *= (T(1.5) - T(i))) / (f *= i);
                    }
                );

                return c;
            }

        };

        template<typename T>
        struct no_transform {

            auto operator()(types::vector1d_t<T>& p, types::vector1d_t<T>& q) {}

        };

        template<typename T>
        struct wampe_transform {

            auto operator()(types::vector1d_t<T>& p, types::vector1d_t<T>& q) {
                for (size_t i = 0; i < p.size(); ++i) {
                    const auto q2 = T(2) * q[i];
                    std::tie(p[i], q[i]) = std::make_tuple(-p[i] - q2, p[i] - q2);
                }

                for (size_t i = p.size(); i < q.size(); ++i) {
                    const auto q2 = T(-2) * q[i];
                    p.push_back(q2);
                    q[i] = q2;
                }
            }

        };

        template<typename T>
        auto get_roots(const types::vector1d_t<T>& c) {
            const auto n = c.size() - 1;
            matrix_t<T> A = matrix_t<T>::Zero(n, n);

            for (size_t i = 0; i < n - 1; ++i)
                A(i + 1, i) = T(1);

            for (size_t i = 0; i < n; ++i)
                A(0, i) = -c[n - i - 1] / c.back();

            solver_type_t<T> solver;
            solver.compute(A);
            const vector_t<T> values = solver.eigenvalues();

            auto result = utils::make_vector_i(n, [&values](const auto& i) { return values(i); });
            std::sort(result.begin(), result.end(), utils::less<T>{});
            return result;
        }

    }// namespace ample::_impl

    template<typename T>
    class coefficients {

    public:

        using taylor_func_t      = std::function<types::vector1d_t<T>(const T&, const size_t&)>;
        using transform_func_t   = std::function<void (types::vector1d_t<T>&, types::vector1d_t<T>&)>;
        using coeficients_func_t = std::function<std::tuple<types::vector1d_t<T>, types::vector1d_t<T>>(const types::vector1d_t<T>&, const size_t&, const size_t&)>;

        coefficients(const size_t& n,
                     taylor_func_t taylor,
                     transform_func_t transform,
                     coeficients_func_t coefficients) : ample::coefficients<T>(n, n, std::move(taylor), std::move(transform), std::move(coefficients)) {}

        coefficients(const size_t& n, const size_t& m,
                     taylor_func_t taylor,
                     transform_func_t transform,
                     coeficients_func_t coefficients) : _n(n), _m(m), _taylor(std::move(taylor)), _transform(std::move(transform)), _coefficients(std::move(coefficients)) {
            utils::dynamic_assert(n > 0,  "coefficients: n(", n, ") must be positive");
            utils::dynamic_assert(n <= m, "coefficients: n(", n, ") must be less or equal to m(", m, ")");
        }

        auto get(const T& value) const {
            const auto tc = _taylor(value, _n + _m + 1);
            auto [np, dp] = _coefficients(tc, _n, _m);

            _transform(np, dp);

            const auto nr = _impl::get_roots<T>(np);
            const auto dr = _impl::get_roots<T>(dp);

            types::vector1d_t<size_t> ix(_m);
            std::iota(ix.begin(), ix.end(), size_t(0));

            auto a = types::vector1d_t<T>(_m);
            for (size_t i = 0; i < _m; ++i) {
                const auto p = std::accumulate(nr.begin(), nr.end(), np.back(), [&x=dr[i]](const auto& a, const auto& x0) { return a * (x - x0); });
                const auto q = std::accumulate(ix.begin(), ix.end(), dp.back(),
                                               [&x=dr[i], &i, &dr](const auto& a, const auto& j) {
                                                   return i == j ? a : a * (x - dr[j]);
                                               }
                );

                a[i] = -p / q / dr[i];
            }

            auto a0 = np.front() / dp.front() - std::accumulate(a.begin(), a.end(), T(0));
            return std::make_tuple(a0, std::move(a), utils::make_vector(dr, [](const auto& x) { return -T(1) / x; }));
        }

        [[nodiscard]] auto nc() const {
            return _m;
        }

    private:

        size_t _n{}, _m{};
        taylor_func_t _taylor;
        transform_func_t _transform;
        coeficients_func_t _coefficients;

    };

    template<typename T>
    auto ssp_coefficients(const size_t& n, const size_t& m) {
        return coefficients<T>(n, m, _impl::exp_taylor<T>{}, _impl::no_transform<T>{}, _impl::pade_coefficients<T>{});
    }

    template<typename T>
    auto ssp_coefficients(const size_t& n) {
        return ssp_coefficients<T>(n, n);
    }

    template<typename T>
    auto theta_ssp_coefficients(const size_t& n, const T& theta) {
        return coefficients<T>(n, n, _impl::exp_taylor<T>{}, _impl::no_transform<T>{}, _impl::theta_pade_coefficients<T>{ theta });
    }

    template<typename T>
    auto wampe_coefficients(const size_t& n, const size_t& m) {
        return coefficients<T>(n, m, _impl::root_taylor<T>{}, _impl::wampe_transform<T>{}, _impl::pade_coefficients<T>{});
    }

    template<typename T>
    auto wampe_coefficients(const size_t& n) {
        return wampe_coefficients<T>(n, n);
    }

    template<typename T>
    auto theta_wampe_coefficients(const size_t& n, const T& theta) {
        return coefficients<T>(n, n, _impl::root_taylor<T>{}, _impl::wampe_transform<T>{}, _impl::theta_pade_coefficients<T>{ theta });
    }

}// namespace ample


#pragma once
#include<complex>
#include<vector>
#include"mkl_lapacke.h"
#include <Eigen/Sparse>
#include <Eigen/Dense>

namespace Many_Body{
  struct TriDiagMat{
    TriDiagMat(const Eigen::VectorXd& diagel, const Eigen::VectorXd& offDiag): diagel(diagel), offDiag(offDiag){}
    Eigen::VectorXd diagel;
    Eigen::VectorXd offDiag;
    auto dData(){return diagel.data();}
      auto offData(){return offDiag.data();}
  };

  void diag(    Many_Body::TriDiagMat& tri, Eigen::MatrixXd& z, Eigen::VectorXd& ev)
  {
    MKL_INT N= ev.size();
        MKL_INT info= LAPACKE_dstedc(LAPACK_COL_MAJOR, 'I', N, tri.dData(), tri.offData(), z.data(), N);
    if(info!=0)
      {
  	std::cout << " diagonalization failed" << '\n';
      }
    ev=tri.diagel;

   }
  template<typename Vector, typename Matrix>
  struct threeLVec
  {
    threeLVec(size_t dim, const Matrix& A): A(A), beta(1), alpha(0) {
      Vector vecOne=Eigen::VectorXcd::Zero(dim);
    Vector vecTwo=Eigen::VectorXcd::Zero(dim);
    Vector vecThree=Eigen::VectorXcd::Zero(dim);
    }
    const Matrix& A;
    double beta{1};
    double alpha{};
    Vector vecOne;
    Vector vecTwo;
    Vector vecThree;

void iterate()
    {
      	vecThree=A*vecTwo;
	//	std::complex<double> c=qk.adjoint()*qMiddle;
	std::complex<double> c=(vecTwo).adjoint()*vecThree;

      	alpha=real(c);

	//	Eigen::VectorXcd  rk=qMiddle - alpha*qk -beta*qkmin;

	Eigen::VectorXcd  rk=vecThree - alpha*vecTwo -beta*vecOne;

     	beta=rk.norm();
	vecOne=vecTwo;

	//qk=rk/rk.norm();
	vecTwo=rk/rk.norm();
    }
    void lastIterate()
    {
      	vecThree=A*vecTwo;

	std::complex<double> c=(vecTwo).adjoint()*vecThree;

      	alpha=real(c);

    }

  };
  template<typename Matrix>
  TriDiagMat Lanczos3Vec(Matrix& A, Eigen::VectorXcd& state, const size_t iterations)
  {
    using namespace Eigen::internal;
    using namespace Eigen;
    	assert(std::abs(state.norm() -1.) < Many_Body::err);

	threeLVec< Eigen::VectorXcd, Matrix>  threeLanczVec(A.rows(), A);
	threeLanczVec.vecTwo=state;
        long double beta=1;

    double alpha(0);
    Eigen::VectorXd bandTdiag(iterations);
        Eigen::VectorXd bandTOff(iterations-1);
    bandTdiag.setZero();
        bandTOff.setZero();

    Eigen::VectorXcd qk=state;
    Eigen::VectorXcd qkmin(A.rows());
    qkmin.setZero();
    threeLanczVec.vecOne=qkmin;
    size_t dim=0;

     for (size_t k = 1; k < iterations; ++k)
       {
	 threeLanczVec.iterate();

      	alpha=threeLanczVec.alpha;
     	beta=threeLanczVec.beta;

	bandTdiag(k-1)=alpha;
     	bandTOff(k-1)=beta;

	  	 if( std::abs(beta)<0.00001)
     	   {

    		  bandTdiag.resize( k);
    		  bandTOff.resize( k-1);

     	    break;
     	   }
    	 dim=k;
    	 }

     {

       threeLanczVec.lastIterate();
       bandTdiag(dim)=threeLanczVec.alpha;

    }

    TriDiagMat T(bandTdiag, bandTOff);
    return T;
  }
    template<typename Matrix>
  TriDiagMat Lanczos(Matrix& A, Eigen::VectorXcd& state, const size_t iterations, Eigen::MatrixXcd& Q)
  {
    using namespace Eigen::internal;
    using namespace Eigen;
    	assert(std::abs(state.norm() -1.) < Many_Body::err);

	Q.setZero();
	Q.col(0)=state;
        long double beta=1;
    double alpha(0);
     Eigen::VectorXd bandTdiag(iterations);
         Eigen::VectorXd bandTOff(iterations-1);
	   bandTdiag.setZero();
        bandTOff.setZero();

    Eigen::VectorXcd qk=state;
    Eigen::VectorXcd qkmin(A.rows());
    qkmin.setZero();
    size_t dim=0;
 for (size_t k = 1; k < iterations; ++k)
        {
	  Eigen::VectorXcd qMiddle=A*qk;

	    	std::complex<double> c=qk.adjoint()*qMiddle;
	     	alpha=real(c);

      	Eigen::VectorXcd  rk=qMiddle - alpha*qk -beta*qkmin;
     	beta=rk.norm();

	bandTdiag(k-1)=alpha;
     	bandTOff(k-1)=beta;

      	qkmin=qk;
	qk=rk/rk.norm();
	     Q.col(k)=qk;

	  	 if( std::abs(beta)<0.0001)
     	   {
	     Eigen::MatrixXcd W=Q;
	           Q.resize(A.rows(), k);
		 for (size_t i = 0; i < k; ++i)
		   {
		     Q.col(i)=W.col(i);

		   }
    		  bandTdiag.resize( k);
    		  bandTOff.resize( k-1);
		  break;
     	   }
	dim=k;
             }
     {
       Eigen::VectorXcd qMiddle=A*qk;
       	std::complex<double> c=qk.adjoint()*qMiddle;
     	alpha=real(c);

       		        bandTdiag(dim)=(alpha);

     }
     //             assert(std::abs( (Q.adjoint()*Q).sum() -(dim+1)) < Many_Body::err);
     std::cout<<"Qdag Q sum "<< (Q.adjoint()*Q).sum()<<std::endl;

     TriDiagMat T(bandTdiag, bandTOff);
    return T;
  }

  template<typename Vector, typename Vector2, typename Matrix>
  auto lanczTrafo(const  Vector & state, const Vector2 & iniState, size_t iteration, const Matrix& A)
    ->Eigen::VectorXcd
  {
    Many_Body::threeLVec< Eigen::VectorXcd, Matrix>  threeLanczVec(A.rows(), A);
    Eigen::VectorXcd newVec=Eigen::VectorXcd::Zero(A.rows());
    threeLanczVec.vecTwo=iniState;
    Eigen::VectorXcd qkmin(A.rows());
    qkmin.setZero();
    threeLanczVec.vecOne=qkmin;
    for(size_t i=0; i<iteration; i++)
      {
	newVec+=threeLanczVec.vecTwo*state(i);
	threeLanczVec.iterate();

      }

    return newVec;
  }

};


#include "friendly_graph.h"

#include "../../Constants.h"
#include "../../navtypes.h"
#include "../../Util.h"

#include <iostream>
#include <vector>

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

using namespace navtypes;
using util::toPose;
using util::toTransform;
using util::toTransformRotateFirst;

namespace filters::pose_graph {

constexpr int LM_SIZE = 2;
constexpr int POSE_SIZE = 3;

FriendlyGraph::FriendlyGraph(int num_landmarks, int max_num_poses, float camera_std,
							 float gps_xy_std, float wheel_noise_rate)
	: _num_landmarks(num_landmarks), _max_pose_id(0), _min_pose_id(0),
	  _max_num_poses(max_num_poses), _current_guess(LM_SIZE * num_landmarks),
	  _landmark_factor_ids(num_landmarks), _odom_cov_inv(), _sensor_cov_inv(), _gps_cov_inv(),
	  _graph() {
	covariance<3> odom_cov = covariance<3>::Zero();
	// TODO what are the right numbers here? Should y be correlated with theta?
	odom_cov << wheel_noise_rate, 0, 0, 0, wheel_noise_rate, wheel_noise_rate / 2.0, 0,
		wheel_noise_rate / 2.0, wheel_noise_rate;
	_odom_cov_inv = odom_cov.inverse();
	covariance<2> sensor_cov = covariance<2>::Zero();
	sensor_cov << camera_std * camera_std, 0, 0, camera_std * camera_std;
	_sensor_cov_inv = sensor_cov.inverse();
	// GPS has no heading measurements
	// which we represent using a large covariance
	covariance<3> gps_cov = covariance<3>::Zero();
	gps_cov << gps_xy_std * gps_xy_std, 0, 0, 0, gps_xy_std * gps_xy_std, 0, 0, 0, 50.0 * 50.0;
	_gps_cov_inv = gps_cov.inverse();
	for (int id = 0; id < num_landmarks; id++) {
		_landmark_factor_ids[id] = -1;
	}
}

int FriendlyGraph::numPoses() {
	return _max_pose_id - _min_pose_id;
}

int FriendlyGraph::nonincrementingPoseIdx(int pose_id) {
	return _num_landmarks * LM_SIZE + (pose_id - _min_pose_id) * POSE_SIZE;
}

void FriendlyGraph::incrementNumPoses() {
	_max_pose_id++;
	_current_guess.conservativeResize(nonincrementingPoseIdx(_max_pose_id));
}

int FriendlyGraph::poseIdx(int pose_id) {
	if (pose_id == _max_pose_id) {
		incrementNumPoses();
	} else if (pose_id > _max_pose_id) {
		printf("Error: skipped a pose id (given %d, current %d)\n", pose_id, _max_pose_id);
		throw 1;
	}
	return nonincrementingPoseIdx(pose_id);
}

int FriendlyGraph::landmarkIdx(int lm_id) {
	assert(lm_id < _num_landmarks);
	return lm_id * LM_SIZE;
}

void FriendlyGraph::trimToMaxNumPoses() {
	if (numPoses() > _max_num_poses) {
		if (numPoses() != _max_num_poses + 1) {
			printf("Error: skipped a trim\n");
			throw 2;
		}
		hessian sol_cov = _graph.covariance();
		int base_idx = poseIdx(_min_pose_id + 1);
		measurement<3> first_pose = getPoseEstimate(_min_pose_id + 1);
		covariance<3> first_pose_cov = sol_cov.block(base_idx, base_idx, POSE_SIZE, POSE_SIZE);
		values old_guess = _current_guess;
		// printf("Trimming. Current uncertainty on base pose: %f %f %f\n",
		//     sqrt(first_pose_cov(0,0)), sqrt(first_pose_cov(1,1)),
		//     sqrt(first_pose_cov(2,2)));
		_graph.shiftIndices(POSE_SIZE, poseIdx(_min_pose_id));
		_min_pose_id += 1;
		_current_guess.conservativeResize(nonincrementingPoseIdx(_max_pose_id));
		_current_guess.block(poseIdx(_min_pose_id), 0, _max_num_poses * POSE_SIZE, 1) =
			old_guess.block(poseIdx(_min_pose_id + 1), 0, _max_num_poses * POSE_SIZE, 1);
		addPosePrior(_min_pose_id, toTransform(first_pose), first_pose_cov);
	}
}

pose_t FriendlyGraph::getPoseEstimate(int pose_id) {
	assert("bad pose id" && pose_id >= _min_pose_id && pose_id < _max_pose_id);
	pose_t p = _current_guess.block(poseIdx(pose_id), 0, POSE_SIZE, 1);
	return p;
}

void FriendlyGraph::addGPSMeasurement(int pose_id, const transform_t& gps_tf) {
	pose_t gps = toPose(gps_tf, 0); // heading doesn't matter
	_graph.add(new OdomFactor2D(poseIdx(pose_id), -1, _gps_cov_inv, gps));
}

void FriendlyGraph::addOdomMeasurement(int pose2_id, int pose1_id, const transform_t& pose2_tf,
									   const transform_t& pose1_tf) {
	transform_t rel_tf = pose2_tf * pose1_tf.inverse();
	pose_t diff = toPose(rel_tf, 0.0);
	float lin_dist = diff(0);
	// Turning introduces more noise than going in a straight line
	float ang_dist = Constants::WHEEL_BASE * diff(2) * 4;
	float noise_distance_sq = lin_dist * lin_dist + ang_dist * ang_dist;
	_graph.add(new OdomFactor2D(poseIdx(pose2_id), poseIdx(pose1_id),
								_odom_cov_inv / noise_distance_sq, diff));
	pose_t pose1_est = getPoseEstimate(pose1_id);
	transform_t new_pose_tf = rel_tf * toTransform(pose1_est);
	pose_t pose2_est = toPose(new_pose_tf, pose1_est(2));
	_current_guess.block(poseIdx(pose2_id), 0, POSE_SIZE, 1) = pose2_est;
}

void FriendlyGraph::addLandmarkMeasurement(int pose_id, int lm_id, const point_t& bearing,
										   bool overwrite) {
	measurement<2> lm = measurement<2>{bearing(0), bearing(1)};
	if (overwrite && _landmark_factor_ids[lm_id] != -1) {
		_graph.deleteFactor(_landmark_factor_ids[lm_id]);
	}
	int id = _graph.add(
		new LandmarkFactor2D(landmarkIdx(lm_id), poseIdx(pose_id), _sensor_cov_inv, lm));
	_landmark_factor_ids[lm_id] = id;
}

void FriendlyGraph::addLandmarkPrior(int lm_id, point_t location, double xy_std) {
	covariance<2> prior_cov = covariance<2>::Zero();
	prior_cov << xy_std * xy_std, 0, 0, xy_std * xy_std;
	covariance<2> prior_cov_inv = prior_cov.inverse();
	measurement<2> lm = measurement<2>{location(0), location(1)};
	_graph.add(new LandmarkFactor2D(landmarkIdx(lm_id), -1, prior_cov_inv, lm));
	_current_guess.block(landmarkIdx(lm_id), 0, LM_SIZE, 1) = lm;
}

void FriendlyGraph::addPosePrior(int pose_id, const transform_t& pose_tf, covariance<3>& cov) {
	covariance<3> prior_cov_inv = cov.inverse();
	pose_t pose = toPose(pose_tf, 0);
	_graph.add(new OdomFactor2D(poseIdx(pose_id), -1, prior_cov_inv, pose));
	_current_guess.block(poseIdx(pose_id), 0, POSE_SIZE, 1) = pose;
}

// Guarantee: after solve(), _graph.solution() == _current_guess
void FriendlyGraph::solve() {
	trimToMaxNumPoses();
	_graph.solve(_current_guess, 0.99);
	_current_guess = _graph.solution();
}

points_t FriendlyGraph::getLandmarkLocations() {
	points_t lms({});
	for (int i = 0; i < _num_landmarks * LM_SIZE; i += LM_SIZE) {
		point_t lm({0, 0, 1});
		lm.topRows(LM_SIZE) = _current_guess.block(i, 0, LM_SIZE, 1);
		lms.push_back(lm);
	}
	return lms;
}

trajectory_t FriendlyGraph::getSmoothedTrajectory() {
	const values& x = _current_guess;
	trajectory_t tfs({});
	for (int i = poseIdx(_min_pose_id); i < nonincrementingPoseIdx(_max_pose_id);
		 i += POSE_SIZE) {
		if (POSE_SIZE == 3) { // 2D
			tfs.push_back(toTransformRotateFirst(0, 0, x(i + 2)) *
						  toTransformRotateFirst(x(i), x(i + 1), 0));
		} else { // 1D
			tfs.push_back(toTransformRotateFirst(x(i), 0, 0));
		}
	}
	return tfs;
}

int FriendlyGraph::getMaxNumPoses() const {
	return _max_num_poses;
}

} // namespace filters::pose_graph


#include "decode.hpp"
#include "pauli_product.hpp"
#include <armadillo>
#include <iostream>

using namespace std;
using namespace arma;

double coeff_a1(unsigned int q_idx, cx_dvec &psi)
{
  int d = psi.size();
  double mysum = 0.0;
  for (int y=0; y<d; y++)
    {
      if (!(y & (1<<q_idx)))
	{
	   mysum += std::norm(psi(y));
	}
    }
  return mysum;
}

double coeff_a2(unsigned int q_idx, unsigned int x, cx_dvec &psi)
{
  int d = psi.size();
  double mysum = 0.0;
  for (int y=0; y<d ; y++)
    {
      int temp = 1<< q_idx;
      int temp2 = y^x;
      if (!((y^x) & (1<<q_idx)))
	{
	  mysum += std::norm(psi(y));
	}
      else
	{
	}
    }
  return mysum;

}

double coeff_a3(unsigned int q_idx, unsigned int x, unsigned int z, cx_dvec &psi)
{
  int d = psi.size();
  double mysum = 0.0;
  for (int y=0; y<d; y++)
    {
      unsigned int xz = __builtin_popcount(x&z);
      unsigned int yz = __builtin_popcount(y&z);

      cx_double phase;
      switch((xz + 2*yz)%4)
	{
	case 0:
	  phase = cx_double(1.0, 0.0);
	  break;
	case 1:
	  phase = cx_double(0.0, 1.0);
	  break;
	case 2:
	  phase = cx_double(-1.0, 0.0);
	  break;
	case 3:
	  phase = cx_double(0.0, -1.0);
	  break;
	}
      if (!((y^x) & (1<<q_idx)))
	{
	  //	  cout << y << endl;
	  //	  cout << (y^x) << endl;
	  mysum += (-2) * std::imag(std::conj(psi(y^x)) * psi(y) * phase);
	}
    }
  return mysum;
}

// Given a Pauli string, find the rotation angle with respect to the corresponding PPR that gives the optimal overlap with the |0>_{q_idx} state.
double optimized_theta(unsigned int q_idx, unsigned int x, unsigned int z, cx_dvec &psi)
{
  double a1 = coeff_a1(q_idx, psi);
  double a2 = coeff_a2(q_idx, x, psi);
  double a3 = coeff_a3(q_idx, x, z, psi);

  double theta1, theta2;
  if (a1==a2)
    theta1= 3.14159265358979/2.0;
  else
    theta1 = atan(a3/(a1 - a2))/2.0;

  theta2 = theta1 + 3.14159265358979/2.0;

  double f1 = ((a1-a2) * cos(2*theta1) + a3 * sin(2*theta1))/2.0;
  double f2 = ((a1-a2) * cos(2*theta2) + a3 * sin(2*theta2))/2.0;

  f1 += (a1+a2)/2.0;
  f2 += (a1+a2)/2.0;

  if (f1>f2)
    return theta1;
  else
    return theta2;
}

// Given a Pauli string, find the optimal overlap with the |0>_{q_idx} using
// a PPR.
double optimal_overlap(unsigned int q_idx, unsigned int x, unsigned int z, cx_dvec &psi)
{
  double a1 = coeff_a1(q_idx, psi);
  double a2 = coeff_a2(q_idx, x, psi);
  double a3 = coeff_a3(q_idx, x, z, psi);

  double theta1, theta2;
  if (a1==a2)
    theta1= 3.14159265358979/2.0;
  else
    theta1 = atan(a3/(a1 - a2))/2.0;

  theta2 = theta1 + 3.14159265358979/2.0;
  double f1 = ((a1-a2) * cos(2*theta1) + a3 * sin(2*theta1))/2.0;
  double f2 = ((a1-a2) * cos(2*theta2) + a3 * sin(2*theta2))/2.0;

  f1 += (a1+a2)/2.0;
  f2 += (a1+a2)/2.0;

  if (f1>f2)
    return f1;
  else
    return f2;
}

// Find the optimal Pauli string that maximizes the overlap with |0>_q
// and apply the corresponding PPR to psi. Here the PPR is assumed to be restricted
// to a set of first 'bits' qubits
void optimal_update(unsigned int q_idx, unsigned int setbits, cx_dvec &psi)
{
  int i_best, j_best;
  double f = 0.0;
  double theta = 0.0;
  for (int i=0; i<(1<<setbits); i++)
    {
      for (int j=0; j<(1<<setbits); j++)
	{
	  /*	  cout << "State=" << endl;
	  cout << psi << endl;
	  cout << "i, j=" << i << ", " << j << endl;
	  double a1 = coeff_a1(0, psi);
	  double a2 = coeff_a2(0, i, psi);
	  double a3 = coeff_a3(0, i, j, psi);
	  double f_opt = optimal_overlap(0, i, j, psi);
	  cout << "a1=" << a1 << endl;
	  cout << "a2=" << a2 << endl;
	  cout << "a3=" << a3 << endl;
	  cout << "f_opt=" << f_opt << endl;
	  if ((a1==a2) && (abs(a3)>0.0001))
	  exit (EXIT_FAILURE);*/

	  // Apply optimal update only if the PPR acts nontrivially on q_idx.
	  if ((i & (1<<q_idx)) || (j & (1<<q_idx)))
	    {
	      if (optimal_overlap(q_idx, i, j, psi) > f)
		{
		  f = optimal_overlap(q_idx, i, j, psi);
		  //	      cout << "New record: " << f << endl;
		  i_best = i;
		  j_best = j;
		}
	    }

	}
    }
  theta = optimized_theta(0, i_best, j_best, psi);
  apply_ppr(i_best, j_best, theta, psi);
}

// Skip the given choice of i and j with probability 1-ratio.
void randomized_update(unsigned int q_idx, unsigned int setbits, double ratio, cx_dvec &psi)
{
  int i_best, j_best;
  double f = 0.0;
  double theta = 0.0;
  for (int i=0; i<(1<<setbits); i++)
    {
      for (int j=0; j<(1<<setbits); j++)
	{
	  if ((i & (1<<q_idx)) || (j & (1<<q_idx)))
	    {
	      if ((double)rand() / (double)RAND_MAX < ratio)
		{
		  if (optimal_overlap(q_idx, i, j, psi) > f)
		    {
		      f = optimal_overlap(q_idx, i, j, psi);
		      //	      cout << "New record: " << f << endl;
		      i_best = i;
		      j_best = j;
		    }
		}
	    }

	}
    }
  theta = optimized_theta(0, i_best, j_best, psi);
  apply_ppr(i_best, j_best, theta, psi);
}

// Decode and print the result.
void decode(unsigned q_idx, cx_dvec &psi, unsigned int setbits, double eps)
{
  double f = 0.0;
  int itt = 0;
  while (f<1-eps)
    {
      itt++;
      optimal_update(q_idx, setbits, psi);
      f = coeff_a1(0, psi);
      cout << itt << ":" << f << endl;
    }
}

void randomized_decode(unsigned q_idx, cx_dvec &psi, unsigned int setbits, double ratio, double eps)
{
  double f = 0.0;
  int itt = 0;
  while (f<1-eps)
    {
      itt++;
      randomized_update(q_idx, setbits, ratio, psi);
      f = coeff_a1(0, psi);
      cout << itt << ":" << f << endl;
    }
}

double decode_cost(unsigned q_idx, cx_dvec &psi, unsigned int setbits, double eps)
{
  double f = 0.0;
  double cost = 0.0;
  int itt = 0;
  while (f<1-eps)
    {
      itt++;
      optimal_update(q_idx, setbits, psi);
      f = coeff_a1(0, psi);
      //      cout << itt << ":" << f << endl;
      cost += 1.0;
    }
  return cost;
}

double randomized_decode_cost(unsigned q_idx, cx_dvec &psi, unsigned int setbits, double ratio, double eps)
{
  double f = 0.0;
  double cost = 0.0;
  int itt = 0;
  while (f<1-eps)
    {
      itt++;
      randomized_update(q_idx, setbits, ratio, psi);
      f = coeff_a1(0, psi);
      //      cout << itt << ":" << f << endl;
      cost += 1.0;
    }
  return cost;
}


/*
*  @file 		ex8.cpp
*  @details  	This file is the solution to exercise 8.
*  @author    	Alexander Rettkowski
*  @date      	12.07.2017
*/
#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_object.hpp>
#include <boost/fusion/include/adapt_struct.hpp>
#include <boost/fusion/include/io.hpp>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths_no_color_map.hpp>
#include <boost/property_map/property_map.hpp>

#include <boost/timer/timer.hpp>
#include <boost/chrono.hpp>

#include <iostream>
#include <string>
#include <complex>
#include <fstream>
#include <queue>

using namespace boost;

namespace exercise8
{
	namespace qi = boost::spirit::qi;
	namespace ascii = boost::spirit::ascii;
	struct edge
	{
		int startNode;
		int endNode;
		int length;
	};
}

BOOST_FUSION_ADAPT_STRUCT(
	exercise8::edge,
	(int, startNode)
	(int, endNode)
	(int, length)
)

namespace exercise8
{
	template <typename Iterator>
	struct line_parser : qi::grammar<Iterator, edge()>
	{
		line_parser() : line_parser::base_type(start)
		{
			using qi::int_;
			start %= int_ >> ' ' >> int_ >> ' ' >> int_;
		}

		qi::rule<Iterator, edge()> start;
	};
}

/**
* The function that calculates the shortest paths using Dijkstra's method.
* @param numberOfNodes Number of nodes in the graph.
* @param graph The graph represented as a vector auf edge-vectors.
* @param startNode The id of the node where the search starts.
*/
std::vector<int> dijkstra(int numberOfNodes, std::vector< std::vector< std::pair<int, int> > > graph, int startNode, std::vector<int>& predecessors)
{
	predecessors.resize(numberOfNodes, -1);
	std::vector<int> distanceTo(numberOfNodes, INT_MAX);
	std::priority_queue< std::pair<int, int>, std::vector< std::pair<int, int> >, std::greater< std::pair<int, int> > > queue;
	queue.push(std::pair<int, int>(startNode, 0));
	distanceTo[startNode] = 0;
	//predecessors[startNode] = -1;

	int currentNode, compareNode, compareNodeDistance, currentNodeDistance;

	while (!queue.empty()) {
		currentNode = queue.top().first;
		currentNodeDistance = queue.top().second;
		queue.pop();

		if (distanceTo[currentNode] < currentNodeDistance) continue;

		for (int i = 0; i < graph[currentNode].size(); i++) {
			compareNode = graph[currentNode][i].first;
			compareNodeDistance = graph[currentNode][i].second;
			if (distanceTo[compareNode] > distanceTo[currentNode] + compareNodeDistance) {
				distanceTo[compareNode] = distanceTo[currentNode] + compareNodeDistance;
				queue.push(std::pair<int, int>(compareNode, distanceTo[compareNode]));
				predecessors[compareNode] = currentNode;
			}
		}
	}
	return distanceTo;
}

/**
* This method checks (in a naive way), if a given number is prime or not.
* @param number The number to check.
* @returns True, if number is prime; false otherwise.
*/
bool isPrime(int number) {
	if (number == 2)
		return true;
	if (number % 2 == 0)
		return false;
	for (int i = 3; (i*i) <= number; i += 2) {
		if (number % i == 0) return false;
	}
	return true;
}

/**
* The main function that reads in a file and processes it.
* @param argc Number of command line arguments.
* @param *argv a pointer to the array of command line arguments.
*/
int main(int argc, char *argv[])
{
	timer::cpu_timer boostTimer;

	using boost::spirit::ascii::space;
	typedef std::string::const_iterator iterator_type;
	typedef exercise8::line_parser<iterator_type> line_parser;
	line_parser parser;
	std::string currentLine;
	std::ifstream file(argv[1]);

	// get number of nodes
	char delimiter = ' ';
	getline(file, currentLine, delimiter);
	const int numberOfNodes = stoi(currentLine);
	getline(file, currentLine);
	int constSub = 1;

	std::vector<std::vector<std::pair<int, int>>> edges(numberOfNodes);
	while (getline(file, currentLine))
	{
		exercise8::edge parsedLine;
		std::string::const_iterator currentPosition = currentLine.begin();
		std::string::const_iterator lineEnd = currentLine.end();
		bool parsingSucceeded = phrase_parse(currentPosition, lineEnd, parser, space, parsedLine);

		if (parsingSucceeded && currentPosition == lineEnd)
		{
			std::pair<int, int> *tempEdge = new std::pair<int, int>();
			tempEdge->first = parsedLine.endNode - constSub;
			tempEdge->second = parsedLine.length;
			edges[parsedLine.startNode - constSub].push_back(*tempEdge);

			std::pair<int, int> *reverseEdge = new std::pair<int, int>();
			reverseEdge->first = parsedLine.startNode - constSub;
			reverseEdge->second = parsedLine.length;
			edges[parsedLine.endNode - constSub].push_back(*reverseEdge);
		}
	}

	file.close();

	std::vector<int> connectedVertices, unconnectedTerminals;
	for (int i = 0; i < numberOfNodes; i++)
	{
		if (isPrime(i))
			unconnectedTerminals.push_back(i);
	}

	connectedVertices.push_back(unconnectedTerminals.back());
	unconnectedTerminals.pop_back();

	std::map<int, std::vector<int>> distancesFromTerminals;
	std::map<int, std::vector<int>> predecessorsFromTerminals;
	for (int i : unconnectedTerminals)
	{
		std::vector<int> predecessors;
		std::vector<int> distancesFromI = dijkstra(numberOfNodes, edges, i, predecessors);
		distancesFromTerminals.insert(std::pair<int, std::vector<int>>(i, distancesFromI));
		predecessorsFromTerminals.insert(std::pair<int, std::vector<int>>(i, predecessors));
	}

	int steinerWeight = 0;

	while (unconnectedTerminals.size() > 0)
	{
		int connectingPointId = -1, connectingPointDistance = INT32_MAX;
		std::vector<int> path;

		int pathFrom = -1, pathTo = -1, mindist = INT32_MAX;
		for (int i = 0; i < unconnectedTerminals.size(); i++)
		{
			int vertexId = unconnectedTerminals[i];
			for (int j : connectedVertices)
			{
				if (distancesFromTerminals[vertexId][j] < mindist)
				{
					mindist = distancesFromTerminals[vertexId][j];
					pathTo = j;
					pathFrom = vertexId;
				}
			}
		}

		path.push_back(pathFrom);
		do
		{
			path.push_back(pathTo);
			pathTo = predecessorsFromTerminals[pathFrom][pathTo];
		} while (pathTo != pathFrom);

		// get path from
		for (int i : path)
		{
			// if the vertex wasn't in the tree before, add
			if (std::find(connectedVertices.begin(), connectedVertices.end(), i) == connectedVertices.end())
			{
				connectedVertices.push_back(i);
			}
			// if it was an unconnected terminal, delete from the list
			auto pos = std::find(unconnectedTerminals.begin(), unconnectedTerminals.end(), i);
			if (pos != unconnectedTerminals.end())
			{
				unconnectedTerminals.erase(pos);
			}
		}

		steinerWeight += mindist;

	}

	std::cout << steinerWeight;

	return 0;
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      110702    K. Kumar          File created.
 *      110726    K. Kumar          Changed filename and class name.
 *      110802    K. Kumar          Added standard deviation and chi-squared
 *                                  test; added note; renamed filename.
 *      110905    S. Billemont      Reorganized includes.
 *                                  Moved (con/de)structors and getter/setters to header.
 *      120509    K. Kumar          Boostified unit test.
 *      120516    A. Ronse          Updated namespaces and corrected reference. Added unit tests
 *                                  for horizontal and vertical cases. Adjusted precision.
 *
 *    References
 *      Burden, R.L., Faires, J.D. Numerical Analysis, 7th Edition, Books/Cole, 2001.
 *
 *    Notes
 *
 */

#define BOOST_TEST_MAIN

#include <cmath>
#include <limits>
#include <map>

#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include "Tudat/Mathematics/Statistics/simpleLinearRegression.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_simple_linear_regression )

//! Test if simple linear regression method computes fit correctly.
BOOST_AUTO_TEST_CASE( testSimpleLinearRegressionBF )
{
    // Test 1: Test implementation of simple linear regression method against benchmark data from
    //         pg. 487, example 1 of (Burden and Faires, 2001). Standard deviations were benchmarked
    //         using MATLAB's lscov() function.

    // Benchmark data.
    std::map< double, double > benchmarkInputData;
    benchmarkInputData[ 1.0 ] = 1.3;
    benchmarkInputData[ 2.0 ] = 3.5;
    benchmarkInputData[ 3.0 ] = 4.2;
    benchmarkInputData[ 4.0 ] = 5.0;
    benchmarkInputData[ 5.0 ] = 7.0;
    benchmarkInputData[ 6.0 ] = 8.8;
    benchmarkInputData[ 7.0 ] = 10.1;
    benchmarkInputData[ 8.0 ] = 12.5;
    benchmarkInputData[ 9.0 ] = 13.0;
    benchmarkInputData[ 10.0 ] = 15.6;

    // Expected coefficients of linear fit.
    const double expectedCoefficientOfConstantTerm = -0.359999999999999999;
    const double expectedCoefficientOfLinearTerm = 1.5381818181818181818;

    // Expected standard deviations of fit coefficients.
    const double expectedStandardDeviationOfCoefficientOfConstantTerm = 0.369832066721825;
    const double expectedStandardDeviationOfCoefficientOfLinearTerm = 0.059603834439483;

    // Expected chi-squared value.
    const double expectedChiSquared = 2.344727272727272;

    // Declare simple linear regression object and set input data.
    statistics::SimpleLinearRegression simpleLinearRegression( benchmarkInputData );

    // Compute linear fit.
    simpleLinearRegression.computeFit( );

    // Check that computed coefficient of constant term matches expected value.
    BOOST_CHECK_CLOSE_FRACTION( expectedCoefficientOfConstantTerm,
                                simpleLinearRegression.getCoefficientOfConstantTerm( ),
                                1.0e-14 );

    // Check that computed coefficient of linear term matches expected value.
    BOOST_CHECK_CLOSE_FRACTION( expectedCoefficientOfLinearTerm,
                                simpleLinearRegression.getCoefficientOfLinearTerm( ),
                                1.0e-15 );

    // Compute linear fit errors.
    simpleLinearRegression.computeFitErrors( );

    // Check that computed standard deviation of coefficient of constant term matches expected
    // value.
    BOOST_CHECK_CLOSE_FRACTION( expectedStandardDeviationOfCoefficientOfConstantTerm,
                                simpleLinearRegression
                                .getStandardDeviationOfCoefficientOfConstantTerm( ),
                                1.0e-13 );

    // Check that computed standard deviation of coefficient of linear term matches expected
    // value.
    BOOST_CHECK_CLOSE_FRACTION( expectedStandardDeviationOfCoefficientOfLinearTerm,
                                simpleLinearRegression
                                .getStandardDeviationOfCoefficientOfLinearTerm( ),
                                1.0e-13 );

    // Check that computed chi-squared fit matches expected value.
    BOOST_CHECK_CLOSE_FRACTION( expectedChiSquared,
                                simpleLinearRegression.getChiSquared( ),
                                1.0e-15 );
}

BOOST_AUTO_TEST_CASE( testSimpleLinearRegressionHorizontal )
{
    // Test 2: Test implementation of simple linear regression method in case of sample points
    // coinciding with the x-axis.

    std::map< double, double > benchmarkInputData;
    benchmarkInputData[ 1.0 ] = 0.0;
    benchmarkInputData[ 2.0 ] = 0.0;
    benchmarkInputData[ 3.0 ] = 0.0;
    benchmarkInputData[ 4.0 ] = 0.0;
    benchmarkInputData[ 5.0 ] = 0.0;
    benchmarkInputData[ 6.0 ] = 0.0;
    benchmarkInputData[ 7.0 ] = 0.0;
    benchmarkInputData[ 8.0 ] = 0.0;
    benchmarkInputData[ 9.0 ] = 0.0;
    benchmarkInputData[ 10.0 ] = 0.0;

    // Declare simple linear regression object and set input data.
    statistics::SimpleLinearRegression simpleLinearRegression( benchmarkInputData );

    // Compute linear fit.
    simpleLinearRegression.computeFit( );

    // Check that computed coefficient of constant term is zero.
    BOOST_CHECK_SMALL( simpleLinearRegression.getCoefficientOfConstantTerm( ),
                       std::numeric_limits< double >::min( ) );

    // Check that computed coefficient of linear term matches is zero.
    BOOST_CHECK_SMALL( simpleLinearRegression.getCoefficientOfLinearTerm( ),
                       std::numeric_limits< double >::min( ) );

    // Compute linear fit errors.
    simpleLinearRegression.computeFitErrors( );

    // Check that computed standard deviation of coefficient of constant term is zero.
    BOOST_CHECK_SMALL( simpleLinearRegression.getStandardDeviationOfCoefficientOfConstantTerm( ),
                       std::numeric_limits< double >::min( ) );

    // Check that computed standard deviation of coefficient of linear term is zero.
    BOOST_CHECK_SMALL( simpleLinearRegression.getStandardDeviationOfCoefficientOfLinearTerm( ),
                       std::numeric_limits< double >::min( ) );

    // Check that computed chi-squared fit is zero.
    BOOST_CHECK_SMALL( simpleLinearRegression.getChiSquared( ),
                       std::numeric_limits< double >::min( ) );
}

BOOST_AUTO_TEST_CASE( testSimpleLinearRegressionVertical )
{
    // Test 3: Test implementation of simple linear regression method in case of sample points
    // coinciding with the y-axis.

    std::map< double, double > benchmarkInputData;
    benchmarkInputData[ 0.0 ] = 1.3;
    benchmarkInputData[ 0.0 ] = 3.5;
    benchmarkInputData[ 0.0 ] = 4.2;
    benchmarkInputData[ 0.0 ] = 5.0;
    benchmarkInputData[ 0.0 ] = 7.0;
    benchmarkInputData[ 0.0 ] = 8.8;
    benchmarkInputData[ 0.0 ] = 10.1;
    benchmarkInputData[ 0.0 ] = 12.5;
    benchmarkInputData[ 0.0 ] = 13.0;
    benchmarkInputData[ 0.0 ] = 15.6;

    // Declare simple linear regression object and set input data.
    statistics::SimpleLinearRegression simpleLinearRegression( benchmarkInputData );

    // Compute linear fit.
    simpleLinearRegression.computeFit( );

    // Check that computed coefficient of constant term matches expected value.
    BOOST_CHECK( boost::math::isnan( simpleLinearRegression.getCoefficientOfConstantTerm( ) ) );

    // Check that computed coefficient of linear term matches expected value.
    BOOST_CHECK( boost::math::isnan( simpleLinearRegression.getCoefficientOfLinearTerm( ) ) );

    // Compute linear fit errors.
    simpleLinearRegression.computeFitErrors( );

    // Check that computed standard deviation of coefficient of constant term matches expected
    // value.
    BOOST_CHECK( boost::math::isnan( simpleLinearRegression
                             .getStandardDeviationOfCoefficientOfConstantTerm( ) ) );

    // Check that computed standard deviation of coefficient of linear term matches expected
    // value.
    BOOST_CHECK( boost::math::isnan(simpleLinearRegression
                            .getStandardDeviationOfCoefficientOfLinearTerm( ) ) );

    // Check that computed chi-squared fit matches expected value.
    BOOST_CHECK( boost::math::isnan(simpleLinearRegression.getChiSquared( ) ) );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


// Filename: size_type_example2.cpp (part of MTL4)

#include <iostream>
#include <boost/cstdint.hpp>
#include <boost/numeric/mtl/mtl.hpp>

int main()
{
    using namespace mtl;

    typedef mat::parameters<row_major, mtl::index::c_index, non_fixed::dimensions, false, boost::uint_least32_t> para;
    compressed2D<double, para>   A;
    laplacian_setup(A, 2, 3);

    std::cout << "A is\n" << A << "\nsize of index is " << sizeof(A.ref_minor()[0]) << '\n';
    return 0;
}


/*
 * Copyright 2013-2015 Raphael Bost
 *
 * This file is part of ciphermed.

 *  ciphermed is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  ciphermed is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with ciphermed.  If not, see <http://www.gnu.org/licenses/>. 2
 *
 */

#include <math/num_th_alg.hh>
#include <math/math_util.hh>
#include <math/util_gmp_rand.h>
#include <math/prime_seq.hh>
#include <math/mpz_class.hh>
#include <NTL/ZZ.h>
#include <cassert>

#include <iostream>

/* The algorithms here are from Victor Shoup's Book
 * A Computational Introduction to Number Theory and Algebra
 * or Shoup's NTL library (for Sophie Germain primes)
 */

std::vector<mpz_class> gen_rand_non_increasing_seq(const mpz_class &m, gmp_randstate_t state)
{
    std::vector<mpz_class> seq;

    mpz_class n = m;
    mpz_class new_n;
    do {
        // pick a new n between 1...n
        mpz_urandomm(new_n.get_mpz_t(),state,n.get_mpz_t());
        n = new_n + 1;
        seq.push_back(n);
    } while (n != 1);

    return seq;
}

std::vector<mpz_class> extract_prime_seq(const std::vector<mpz_class> &seq, int reps)
{
    std::vector<mpz_class> primes;
    for (size_t i = 0; i < seq.size(); i++) {
        if (mpz_class_probab_prime_p(seq[i],reps) != 0) {
            primes.push_back(seq[i]);
        }
    }

    return primes;
}

// generates a random integer with its factorization (returns the factorization)
std::vector<mpz_class> gen_rand_number_factorization(const mpz_class &m, mpz_class *result, gmp_randstate_t state, int reps)
{
    for (; ; ) {
        std::vector<mpz_class> seq = gen_rand_non_increasing_seq(m,state);
        std::vector<mpz_class> primes = extract_prime_seq(seq, reps);

        mpz_class y = 1;
        for (size_t i = 0; i < primes.size(); i++) {
            y *= primes[i];
            if (y > m) {
                break;
            }
        }
        if (y > m) {
            continue;
        }

        mpz_class x;
        mpz_urandomm(x.get_mpz_t(),state,m.get_mpz_t());
        x += 1;

        if (x <= y) {
            if (result) {
                *result = y;
            }
            return primes;
        }
    }
}

// generates a random prime p with the factorization of p-1 (returns the factorization)
std::vector<mpz_class> gen_rand_prime_with_factorization(const mpz_class &m, mpz_class *p, gmp_randstate_t state, int reps)
{
    for (; ; ) {
        mpz_class n;
        std::vector<mpz_class> factorization = gen_rand_number_factorization(m,&n,state,reps);

        if (mpz_class_probab_prime_p(n+1, reps) != 0) {
            if (p) {
                *p = n+1;
            }
            return factorization;
        }
    }
}

mpz_class simple_safe_prime_gen(size_t n_bits, gmp_randstate_t state, int reps)
{
    for (size_t count = 1; ; count ++) {
        mpz_class n;
        mpz_urandom_len(n.get_mpz_t(),state,n_bits);

        if (mpz_class_probab_prime_p(n,reps) !=0) {
            if (mpz_class_probab_prime_p(2*n+1,reps) != 0 ) {
                std::cout << count << " iterations needed to generate safe prime" << std::endl;
                return n;
            }
        }
    }
}

static long bit_count(long a)
{
    unsigned long aa;
    if (a < 0)
    aa = - ((unsigned long) a);
    else
    aa = a;

    long k = 0;
    while (aa) {
        k++;
        aa = aa >> 1;
    }

    return k;
}

/* The following code is just NTL's code for generating Germain primes using GMP */

// prime_bound computes a reasonable bound for trial
// division in the Miller-Rabin test.
// It is computed a bit on the "low" side, since being a bit
// low doesn't hurt much, but being too high can hurt a lot.

static
long prime_bound(long bn)
{
    long wn = (bn+NBITS_MAX-1)/NBITS_MAX;

    long fn;

    if (wn <= 36)
    fn = wn/4 + 1;
    else
    fn = long(1.67*sqrt(double(wn)));

    long prime_bnd;

    if (bit_count(bn) + bit_count(fn) > NBITS_MAX)
        prime_bnd = (1L << NBITS_MAX);
    else
        prime_bnd = bn*fn;

    return prime_bnd;
}

static
long ErrBoundTest(long kk, long tt, long nn)

{
    const double fudge = (1.0 + 1024.0/NTL_FDOUBLE_PRECISION);
    const double log2_3 = log2(3.0);
    const double log2_7 = log2(7.0);
    const double log2_20 = log2(20.0);

    double k = kk;
    double t = tt;
    double n = nn;

    if (k < 3 || t < 1) return 0;
    if (n < 1) return 1;

    // the following test is largely academic
    assert(9*t < NTL_FDOUBLE_PRECISION);

    double log2_k = log2(k);

    if ((n + log2_k)*fudge <= 2*t)
    return 1;

    if ((2*log2_k + 4.0 + n)*fudge <= 2*sqrt(k))
    return 2;

    if ((t == 2 && k >= 88) || (3 <= t && 9*t <= k && k >= 21)) {
        if ((1.5*log2_k + t + 4.0 + n)*fudge <= 0.5*log2(t) + 2*(sqrt(t*k)))
        return 3;
    }

    if (k <= 9*t && 4*t <= k && k >= 21) {
        if ( ((log2_3 + log2_7 + log2_k + n)*fudge <= log2_20 + 5*t)  &&
            ((log2_3 + (15.0/4.0)*log2_k + n)*fudge <= log2_7 + k/2 + 2*t) &&
            ((2*log2_3 + 2 + log2_k + n)*fudge <= k/4 + 3*t) )
        return 4;
    }

    if (4*t >= k && k >= 21) {
        if (((15.0/4.0)*log2_k + n)*fudge <= log2_7 + k/2 + 2*t)
        return 5;
    }

    return 0;
}

static long make_odd(mpz_class &n)
{
    long k = 0;

    while (mpz_even_p(n.get_mpz_t())) {
        n >>= 1;
        k++;
    }

    return k;
}

static long is_Miller_witness(const mpz_class& n, const mpz_class& x)
{
    mpz_class m(0), y(0), z(0);
    long j, k;

    if (x == 0) return 0;

    m = n-1;
    k = make_odd(m);

    z = mpz_class_powm(x,m,n);

    if (z == 1) return 0;

    j = 0;
    do {
        y = z;
        z = (y*y) %n;
        j++;
    } while (j != k && z != 1);

    if (z != 1) return 1;
    y = y + 1;
    if (y != n) return 1;
    return 0;
}

void gen_germain_prime(mpz_class& n, long k, gmp_randstate_t state, long err)
{
    assert(k > 1);
    assert(k <= (1L << 20));

    if (err < 1) err = 1;
    if (err > 512) err = 512;

    if (k == 2) {
        if (gmp_urandomm_ui(state,2))
        n = 3;
        else
        n = 2;

        return;
    }

    long prime_bnd = prime_bound(k);

    if (bit_count(prime_bnd) >= k/2)
    prime_bnd = (1L << (k/2-1));

    mpz_class two;
    two = 2;

    mpz_class n1;

    PrimeSeq s;

    mpz_class iter;
    iter = 0;

    for (;;) {
        iter++;

        mpz_urandom_len(n.get_mpz_t(),state,k);

        if (mpz_even_p(n.get_mpz_t())) {
            n = n+1;
        }

        s.reset(3);
        long p;

        long sieve_passed = 1;

        p = s.next();
        while (p && p < prime_bnd) {
            mpz_class r;
            mpz_tdiv_r_ui(r.get_mpz_t(),n.get_mpz_t(),p);

            if (r == 0) {
                sieve_passed = 0;
                break;
            }

            // test if 2*r + 1 = 0 (mod p)
            if (r == p-r-1) {
                sieve_passed = 0;
                break;
            }

            p = s.next();
        }

        if (!sieve_passed) continue;

        if (is_Miller_witness(n, two)) continue;

        n1 = 2*n+1;

        if (is_Miller_witness(n1, two)) continue;

        // now do t M-R iterations...just to make sure

        // First compute the appropriate number of M-R iterations, t
        // The following computes t such that
        //       p(k,t)*8/k <= 2^{-err}/(5*iter^{1.25})
        // which suffices to get an overall error probability of 2^{-err}.
        // Note that this method has the advantage of not requiring
        // any assumptions on the density of Germain primes.
        long iter_n_bits = mpz_sizeinbase(iter.get_mpz_t(),2);
        long err1 = std::max(1L, err + 7 + (5*iter_n_bits + 3)/4 - bit_count(k));
        long t;
        t = 1;
        while (!ErrBoundTest(k, t, err1))
        t++;

        if(mpz_probab_prime_p(n.get_mpz_t(),t))
            break;
    }
}

// Constructs a generator for the cyclic group \Z^*_p where p is a Sophie Germain prime
mpz_class get_generator_for_cyclic_group(const mpz_class &p, gmp_randstate_t state)
{
    mpz_class q = (p >> 1);
    mpz_class g;

    // find a generator for ZZ*_p
    // Shoup's algorithm

    mpz_class alpha, beta;
    do {
        mpz_urandomm(alpha.get_mpz_t(),state,p.get_mpz_t());

        beta = mpz_class_powm(alpha,q,p);
    } while (beta == 1);

    g = beta;

    do {
        mpz_urandomm(alpha.get_mpz_t(),state,p.get_mpz_t());
        beta = (alpha*alpha) %p;

    } while (beta == 1);

    g = (g*beta) %p;

    return g;
}