//==============================================================================
//         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
//==============================================================================
#ifndef NT2_LINALG_FUNCTIONS_DETAILS_RREF_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_DETAILS_RREF_HPP_INCLUDED

#include <nt2/linalg/details/utility/f77_wrapper.hpp>
#include <nt2/include/functions/height.hpp>
#include <nt2/include/functions/width.hpp>
#include <nt2/include/functions/expand.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/include/functions/ismatrix.hpp>
#include <nt2/include/functions/max.hpp>
#include <nt2/include/functions/globalmax.hpp>
#include <nt2/include/functions/mnorminf.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <boost/dispatch/meta/mpl.hpp>
#include <nt2/core/container/table/table.hpp>
//  rref   reduced row echelon form.
//     r = rref(a) produces the reduced row echelon form of a.

//     [r,jb] = rref(a) also returns a vector, jb, so that:
//         r = length(jb) is this algorithm's idea of the rank of a,
//         x(jb) are the bound variables in a linear system, ax = b,
//         a(:,jb) is a basis for the range of a,
//         r(1:r,jb) is the r-by-r identity matrix.

//     [r,jb] = rref(a,tol) uses the given tolerance in the rank tests.

//     roundoff errors may cause this algorithm to compute a different
//     value for the rank than rank, orth and null.

namespace nt2 { namespace details
{
  template<class T> struct rref_result
  {
    typedef typename meta::strip<T>::type                   source_t;
    typedef typename source_t::value_type                     type_t;
    typedef typename source_t::index_type                    index_t;
    typedef typename meta::as_real<type_t>::type              base_t;
    typedef T                                                 data_t;
    typedef typename meta::as_integer<base_t, signed>::type  itype_t;
    typedef nt2::container::table<type_t,nt2::matlab_index_>              tab_t;
    typedef nt2::container::table<base_t,nt2::matlab_index_>             btab_t;
    typedef nt2::container::table<itype_t,nt2::matlab_index_>            itab_t;
    typedef nt2::container::table<nt2_la_int,nt2::matlab_index_>         ibuf_t;
    typedef nt2::container::table<type_t,index_t>                   result_type;
    typedef nt2::container::table<base_t,index_t>                  bresult_type;
    typedef nt2::container::table<itype_t,index_t>                 iresult_type;
    //must be dry I think

    template<class Input>
    rref_result ( Input& xpr, base_t tol)
      : tol_(tol)
      , a_(xpr)
      , n_( nt2::height(a_)  )
      , m_( nt2::width(a_)  )
      , jb_(of_size(1, n_))
    {
      BOOST_ASSERT_MSG(ismatrix(a_), "input to rref must be matrix");
      if (tol < Zero<base_t>()) tol = nt2::max(m_,n_)*nt2::Eps<base_t>()*nt2::mnorminf(a_);
      itype_t i = 1, j = 1;
      itype_t k = 0;
      base_t p;
      itype_t cnt = 1;
      while(i <= m_ && j <= n_)
      {
        //          tie(p, k) =  nt2::max(nt2::abs(a_(_(i, m_),j))); //TODO
        p = nt2::globalmax(nt2::abs(a_(_(i, m_),j)));
        for(int l = i; l <= m_; ++l) if (nt2::abs(a_(l, j)) == p) { k = l; break; }
        //k = k+i-1;
        if (p <= tol)
        {
          // the column is negligible, zero it out.
          a_(_(i, m_),j) = nt2::zeros(m_-i+1, 1, meta::as_<type_t>());
          ++j;
        }
        else
        {
          // remember column index
          //jb_ = cath(jb_, j); //TODO
          jb_(cnt) = j; ++cnt;
          // swap i-th and k-th rows.
          tab_t tmp = a_(i, _(j, n_));
          a_(i, _(j, n_)) = a_(k, _(j, n_));
          a_(k, _(j, n_)) = tmp;
          //              a_(cath(i, k),_(j, n)) = a_(cath(k, i),_(j, n));
          // divide the pivot row by the pivot element.
          type_t tmp1 =  a_(i, j);
          a_(i,_(j, n_)) = a_(i,_(j, n_))/tmp1;
          // subtract multiples of the pivot row from all the other rows.
          for (itype_t kk = 1; kk <= m_; ++kk)//[1:i-1 i+1:m]
          {
            if (kk!=i)
            {
              type_t tmp2 = a_(kk,j);
              a_(kk,_(j, n_)) = a_(kk,_(j, n_))- tmp2*a_(i,_(j, n_));
            }
          }
          ++i; ++j;
        }
      }
      jb_ =  nt2::expand(jb_, 1, --cnt);
    }

    rref_result& operator=(rref_result const& src)
    {
      tol_    = src.tol_;
      a_      = src.a_;
      n_      = src.n_;
      m_      = src.m_;
      jb_     = src.jb_;
      return *this;
    }

    rref_result(rref_result const& src)
      : tol_(src.tol_),
        a_(src.a_),
        n_(src.n_),
        m_(src.m_),
        jb_(src.jb_)
    {}

    //==========================================================================
    // Return raw values
    //==========================================================================
    data_t values() const { return a_; }
    data_t rref() const { return a_; }

    //==========================================================================
    // Return permutation
    //==========================================================================
    const itab_t& jb() const
    {
      //typedef typename boost::mpl::at_c<typename index_t::type,0>::type base;
      return jb_; //+ base::value + Mone<itype_t>();
    }

  private:
    btab_t                         tol_;
    data_t                           a_;
    nt2_la_int                       n_;
    nt2_la_int                       m_;
    itab_t                          jb_;
  };
} }

#endif

// /////////////////////////////////////////////////////////////////////////////
// End of rref.hpp
// /////////////////////////////////////////////////////////////////////////////


#include <boost/math/tools/convert_from_string.hpp>


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

#ifndef ITL_ITL_INCLUDE
#define ITL_ITL_INCLUDE

#include <boost/numeric/itl/iteration/basic_iteration.hpp>
#include <boost/numeric/itl/iteration/cyclic_iteration.hpp>
#include <boost/numeric/itl/iteration/noisy_iteration.hpp>

#include <boost/numeric/itl/krylov/cg.hpp>
#include <boost/numeric/itl/krylov/cgs.hpp>
#include <boost/numeric/itl/krylov/bicg.hpp>
#include <boost/numeric/itl/krylov/bicgstab.hpp>
#include <boost/numeric/itl/krylov/bicgstab_2.hpp>
#include <boost/numeric/itl/krylov/bicgstab_ell.hpp>
#include <boost/numeric/itl/krylov/fsm.hpp>
#include <boost/numeric/itl/krylov/idr_s.hpp>
#include <boost/numeric/itl/krylov/gmres.hpp>
#include <boost/numeric/itl/krylov/tfqmr.hpp>
#include <boost/numeric/itl/krylov/qmr.hpp>
#include <boost/numeric/itl/krylov/pc_solver.hpp>

#include <boost/numeric/itl/krylov/repeating_solver.hpp>

#include <boost/numeric/itl/minimization/quasi_newton.hpp>

#include <boost/numeric/itl/pc/identity.hpp>
#include <boost/numeric/itl/pc/is_identity.hpp>
#include <boost/numeric/itl/pc/diagonal.hpp>
#include <boost/numeric/itl/pc/ilu.hpp>
#include <boost/numeric/itl/pc/ilu_0.hpp>
#include <boost/numeric/itl/pc/ilut.hpp>
#include <boost/numeric/itl/pc/ic_0.hpp>

#include <boost/numeric/itl/pc/imf_preconditioner.hpp>
#include <boost/numeric/itl/pc/imf_algorithms.hpp>

#include <boost/numeric/itl/pc/sub_matrix_pc.hpp>
#include <boost/numeric/itl/pc/concat.hpp>

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

#include <boost/numeric/itl/stepper/armijo.hpp>
#include <boost/numeric/itl/stepper/wolf.hpp>

#include <boost/numeric/itl/updater/bfgs.hpp>
#include <boost/numeric/itl/updater/broyden.hpp>
#include <boost/numeric/itl/updater/dfp.hpp>
#include <boost/numeric/itl/updater/psb.hpp>
#include <boost/numeric/itl/updater/sr1.hpp>

#endif // ITL_ITL_INCLUDE


//////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::map_pdf::product_pdf::product_pdf.hpp              //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  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)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_MAP_PDF_PRODUCT_PDF_PRODUCT_PDF_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_MAP_PDF_PRODUCT_PDF_PRODUCT_PDF_HPP_ER_2009
#include <boost/statistics/detail/distribution_common/meta/inherit/policy.hpp>
#include <boost/statistics/detail/distribution_common/meta/inherit/value.hpp>
#include <boost/statistics/detail/distribution_toolkit/meta/is_pseudo_scalar_distribution.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace toolkit{

    // A pseudo distribution resulting from product of the pdf of two
    // distributions
    template<typename A,typename B>
    class product_pdf : public distribution::meta::inherit_policy<
        A,
        distribution::meta::inherit_value<A>
    >{

        public:

        product_pdf(){}
        explicit product_pdf(
            const A& a,
            const B& b
        ):a_(a),b_(b){}

        product_pdf(const product_pdf& that):a_(that.a_),b_(that.b_){}

        product_pdf&
        operator=(const product_pdf& that){
            if(&that!=this){
                a_ = that.a_;
                b_ = that.b_;
            }
            return *this;
        }

        const A& first()const{ return a_; }
        const B& second()const{ return b_; }

        protected:
        A a_;
        B b_;
    };

    template<typename A,typename B>
    product_pdf<A,B>
    make_product_pdf(const A& a,const B& b){
        return product_pdf<A,B>(a,b);
    }

}// distribution
}// toolkit

}// detail
}// statistics
}// boost

#endif


/* Copyright (C) 2012,2013 IBM Corp.
 * This program 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 2 of the License, or
 * (at your option) any later version.
 *
 * This program 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 this program; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */
// debugging.cpp - debugging utilities
#include <NTL/ZZ.h>
NTL_CLIENT
#include "FHE.h"
#include "EncryptedArray.h"

#define FLAG_PRINT_ZZX  1
#define FLAG_PRINT_POLY 2
#define FLAG_PRINT_VEC  4

FHESecKey* dbgKey=NULL;
EncryptedArray* dbgEa=NULL;
ZZX dbg_ptxt;
Vec<ZZ> ptxt_pwr;

void decryptAndPrint(ostream& s, const Ctxt& ctxt, const FHESecKey& sk,
		     const EncryptedArray& ea, long flags)
{
  const FHEcontext& context = ctxt.getContext();
  xdouble noiseEst = sqrt(ctxt.getNoiseVar());
  xdouble modulus = xexp(context.logOfProduct(ctxt.getPrimeSet()));
  vector<ZZX> ptxt;
  ZZX p, pp;
  sk.Decrypt(p, ctxt, pp);

  s << "plaintext space mod "<<ctxt.getPtxtSpace()
       << ", level="<<ctxt.findBaseLevel()
       << ", \n           |noise|=q*" << (coeffsL2Norm(pp)/modulus)
       << ", |noiseEst|=q*" << (noiseEst/modulus)
       <<endl;

  if (flags & FLAG_PRINT_ZZX) {
    s << "   before mod-p reduction=";
    printZZX(s,pp) <<endl;
  }
  if (flags & FLAG_PRINT_POLY) {
    s << "   after mod-p reduction=";
    printZZX(s,p) <<endl;
  }
  if (flags & FLAG_PRINT_VEC) {
    ea.decode(ptxt, p);
    if (ea.getAlMod().getTag() == PA_zz_p_tag
	&& ctxt.getPtxtSpace() != ea.getAlMod().getPPowR()) {
      long g = GCD(ctxt.getPtxtSpace(), ea.getAlMod().getPPowR());
      for (long i=0; i<ea.size(); i++)
	PolyRed(ptxt[i], g, true);
    }
    s << "   decoded to ";
    if (deg(p) < 40) // just pring the whole thing
      s << ptxt << endl;
    else if (ptxt.size()==1) // a single slot
      printZZX(s, ptxt[0]) <<endl;
    else { // print first and last slots
      printZZX(s, ptxt[0],20) << "--";
      printZZX(s, ptxt[ptxt.size()-1], 20) <<endl;
    }
  }
}


// Boost.Geometry

// Copyright (c) 2019-2021, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_TEST_CS_UNDEFINED_TEST_RELOPS_HPP
#define BOOST_GEOMETRY_TEST_CS_UNDEFINED_TEST_RELOPS_HPP

#include "common.hpp"

#include <boost/geometry/algorithms/covered_by.hpp>
#include <boost/geometry/algorithms/crosses.hpp>
#include <boost/geometry/algorithms/disjoint.hpp>
#include <boost/geometry/algorithms/equals.hpp>
#include <boost/geometry/algorithms/intersects.hpp>
#include <boost/geometry/algorithms/overlaps.hpp>
#include <boost/geometry/algorithms/relate.hpp>
#include <boost/geometry/algorithms/relation.hpp>
#include <boost/geometry/algorithms/touches.hpp>
#include <boost/geometry/algorithms/within.hpp>

template
<
    typename G1,
    typename G2,
    std::size_t Dim1 = bg::topological_dimension<G1>::value,
    std::size_t Dim2 = bg::topological_dimension<G2>::value
>
struct call_equals
{
    template <typename S>
    static void apply(G1 const& , G2 const& , S const& ) {}
};

template <typename G1, typename G2, std::size_t Dim>
struct call_equals<G1, G2, Dim, Dim>
{
    template <typename S>
    static void apply(G1 const& g1, G2 const& g2, S const& s)
    {
        bg::equals(g1, g2, s);
    }
};

template
<
    typename G1,
    typename G2,
    std::size_t Dim1 = bg::topological_dimension<G1>::value,
    std::size_t Dim2 = bg::topological_dimension<G2>::value
>
struct call_overlaps
{
    template <typename S>
    static void apply(G1 const& , G2 const& , S const& ) {}
};

template <typename G1, typename G2, std::size_t Dim>
struct call_overlaps<G1, G2, Dim, Dim>
{
    template <typename S>
    static void apply(G1 const& g1, G2 const& g2, S const& s)
    {
        bg::overlaps(g1, g2, s);
    }
};

template
<
    typename G1,
    typename G2,
    std::size_t Dim1 = bg::topological_dimension<G1>::value,
    std::size_t Dim2 = bg::topological_dimension<G2>::value
>
struct call_touches
{
    template <typename S>
    static void apply(G1 const& g1, G2 const& g2, S const& s)
    {
        bg::touches(g1, g2, s);
    }
};

template <typename G1, typename G2>
struct call_touches<G1, G2, 0, 0>
{
    template <typename S>
    static void apply(G1 const& , G2 const& , S const& ) {}
};

template
<
    typename G1,
    typename G2,
    std::size_t Dim1 = bg::topological_dimension<G1>::value,
    std::size_t Dim2 = bg::topological_dimension<G2>::value
>
struct call_crosses
{
    template <typename S>
    static void apply(G1 const& g1, G2 const& g2, S const& s)
    {
        bg::crosses(g1, g2, s);
    }
};

template <typename G1, typename G2>
struct call_crosses<G1, G2, 0, 0>
{
    template <typename S>
    static void apply(G1 const& , G2 const& , S const& ) {}
};

template <typename G1, typename G2>
struct call_crosses<G1, G2, 2, 2>
{
    template <typename S>
    static void apply(G1 const& , G2 const& , S const& ) {}
};

template <typename G1, typename G2, typename S>
inline void rel(G1 const& g1, G2 const& g2, S const& s)
{
    bg::relation(g1, g2, s);
    bg::relate(g1, g2, bg::de9im::mask("*********"), s);
    bg::covered_by(g1, g2, s);
    call_crosses<G1, G2>::apply(g1, g2, s);
    bg::disjoint(g1, g2, s);
    call_equals<G1, G2>::apply(g1, g2, s);
    bg::intersects(g1, g2, s);
    call_overlaps<G1, G2>::apply(g1, g2, s);
    call_touches<G1, G2>::apply(g1, g2, s);
    bg::within(g1, g2, s);
}

template <typename G1, typename G2>
inline void rel_pp(G1 const& g1, G2 const& g2)
{
    ::rel(g1, g2, bg::strategy::within::cartesian_point_point());
    ::rel(g1, g2, bg::strategy::within::spherical_point_point());
}

template <typename G1, typename G2>
inline void rel_ps(G1 const& g1, G2 const& g2)
{
    typedef typename bg::point_type<G1>::type point;
    ::rel(g1, g2, bg::strategy::within::cartesian_winding<point>());
    ::rel(g1, g2, bg::strategy::within::spherical_winding<point>());
    ::rel(g1, g2, bg::strategy::within::geographic_winding<point>());
}

template <typename G1, typename G2>
inline void rel_ss(G1 const& g1, G2 const& g2)
{
    ::rel(g1, g2, bg::strategy::intersection::cartesian_segments<>());
    ::rel(g1, g2, bg::strategy::intersection::spherical_segments<>());
    ::rel(g1, g2, bg::strategy::intersection::geographic_segments<>());
}

#endif // BOOST_GEOMETRY_TEST_CS_UNDEFINED_TEST_RELOPS_HPP


#ifndef _IEMBEDDINGMODEL_H
#define _IEMBEDDINGMODEL_H

#include <vector>
#include <string>
#include <Eigen/Dense>

/**
 * @breif An interface for various types of word embedding models
 */

class IEmbeddingModel
{
    public:
    virtual void GetVocab(std::vector<std::string> &vocabulary) = 0;
    virtual void GetEmbeddingMatrix(Eigen::MatrixXf &Embeddings) = 0;
    virtual void GetEmbeddingMatrix(std::vector<Eigen::VectorXf> &Embeddings) = 0;
};

#endif


#include "interval.h"
#include "thread_pool.h"
#include <boost/property_tree/xml_parser.hpp>
#include <iostream>
#include <unordered_set>

#define NUM_ARGS 2

void parse_intervals(const char *filename, std::vector<Interval> *intervals);
void parse_primes(std::vector<Interval> *intervals, ThreadPool *tp, std::unordered_set<uint64_t> *primes, std::mutex *mutex);
void _parse_primes_in_interval(Interval interval, std::unordered_set<uint64_t> *primes, std::mutex *mutex);
bool _isprime(int64_t digit);

int main(int argc, char **argv)
{
    if(argc != NUM_ARGS)
    {
        std::cerr << "Usage: " << argv[0] << " <file>\n";
        exit(EXIT_FAILURE);
    }

    std::vector<Interval> intervals;
    parse_intervals(argv[1], &intervals);

    ThreadPool tp;
    std::unordered_set<uint64_t> primes;
    std::mutex mutex;
    parse_primes(&intervals, &tp, &primes, &mutex);
    tp.join_workers();

    std::cout << primes.size() << std::endl;

    return 0;
}

void
parse_intervals(const char *filename, std::vector<Interval> *intervals)
{
    boost::property_tree::ptree pt;
    try {
        boost::property_tree::read_xml(filename, pt);
    } catch(boost::property_tree::xml_parser_error &e) {
        std::cerr << e.what() << std::endl;
        exit(EXIT_FAILURE);
    }
    for(auto &kv : pt.get_child(INTERVALS_TAG))
        intervals->emplace_back(kv.second.get<int64_t>(INTERVALS_LEFT_TAG, 0),
                                kv.second.get<int64_t>(INTERVALS_RIGHT_TAG, 0));
}

void
parse_primes(std::vector<Interval> *intervals,
             ThreadPool *tp,
             std::unordered_set<uint64_t> *primes,
             std::mutex *mutex)
{
    for(auto &interval : *intervals)
        tp->add_task(std::bind(_parse_primes_in_interval, interval, primes, mutex));
}

void
_parse_primes_in_interval(Interval interval,
                          std::unordered_set<uint64_t> *primes,
                          std::mutex *mutex)
{
    for(int64_t digit = interval.left_bound;
        digit < interval.right_bound;
        ++digit)
    {
        if(_isprime(digit))
        {
            std::lock_guard<std::mutex> lock(*mutex);
            primes->insert(digit);
        }
    }
}

bool
_isprime(int64_t digit)
{
    if(digit <= 1)
        return false;
    if(digit <= 3)
        return true;
    for(int64_t j = 2; j*j <= digit; ++j)
        if(digit % j == 0)
            return false;

    return true;
}


// SPDX-License-Identifier: Apache-2.0
//
// Copyright 2011-2017 Ryan Curtin (http://www.ratml.org/)
// Copyright 2017 National ICT Australia (NICTA)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// ------------------------------------------------------------------------

#include <armadillo>

#include "catch.hpp"

using namespace arma;

TEST_CASE("fn_max_subview_test")
  {
  // We will assume subview.at() works and returns points within the bounds of
  // the matrix, so we just have to ensure the results are the same as
  // Mat.max()...
  for (size_t r = 50; r < 150; ++r)
    {
    mat x;
    x.randu(r, r);

    uword x_max;
    uword x_subview_max1;
    uword x_subview_max2;
    uword x_subview_max3;

    const double mval = x.max(x_max);
    const double mval1 = x.submat(0, 0, r - 1, r - 1).max(x_subview_max1);
    const double mval2 = x.cols(0, r - 1).max(x_subview_max2);
    const double mval3 = x.rows(0, r - 1).max(x_subview_max3);

    REQUIRE( x_max == x_subview_max1 );
    REQUIRE( x_max == x_subview_max2 );
    REQUIRE( x_max == x_subview_max3 );

    REQUIRE( mval == Approx(mval1) );
    REQUIRE( mval == Approx(mval2) );
    REQUIRE( mval == Approx(mval3) );
    }
  }

TEST_CASE("fn_max_subview_col_test")
  {
  for (size_t r = 10; r < 50; ++r)
    {
    vec x;
    x.randu(r);

    uword x_max;
    uword x_subview_max1;
    uword x_subview_max2;

    const double mval = x.max(x_max);
    const double mval1 = x.submat(0, 0, r - 1, 0).max(x_subview_max1);
    const double mval2 = x.rows(0, r - 1).max(x_subview_max2);

    REQUIRE( x_max == x_subview_max1 );
    REQUIRE( x_max == x_subview_max2 );

    REQUIRE( mval == Approx(mval1) );
    REQUIRE( mval == Approx(mval2) );
    }
  }

TEST_CASE("fn_max_subview_row_test")
  {
  for (size_t r = 10; r < 50; ++r)
    {
    rowvec x;
    x.randu(r);

    uword x_max;
    uword x_subview_max1;
    uword x_subview_max2;

    const double mval = x.max(x_max);
    const double mval1 = x.submat(0, 0, 0, r - 1).max(x_subview_max1);
    const double mval2 = x.cols(0, r - 1).max(x_subview_max2);

    REQUIRE( x_max == x_subview_max1 );
    REQUIRE( x_max == x_subview_max2 );

    REQUIRE( mval == Approx(mval1) );
    REQUIRE( mval == Approx(mval2) );
    }
  }

TEST_CASE("fn_max_incomplete_subview_test")
  {
  for (size_t r = 50; r < 150; ++r)
    {
    mat x;
    x.randu(r, r);

    uword x_max;
    uword x_subview_max1;
    uword x_subview_max2;
    uword x_subview_max3;

    const double mval = x.max(x_max);
    const double mval1 = x.submat(1, 1, r - 2, r - 2).max(x_subview_max1);
    const double mval2 = x.cols(1, r - 2).max(x_subview_max2);
    const double mval3 = x.rows(1, r - 2).max(x_subview_max3);

    uword row, col;
    x.max(row, col);

    if (row != 0 && row != r - 1 && col != 0 && col != r - 1)
      {
      uword srow, scol;

      srow = x_subview_max1 % (r - 2);
      scol = x_subview_max1 / (r - 2);
      REQUIRE( x_max == (srow + 1) + r * (scol + 1) );
      REQUIRE( x_max == x_subview_max2 + r );

      srow = x_subview_max3 % (r - 2);
      scol = x_subview_max3 / (r - 2);
      REQUIRE( x_max == (srow + 1) + r * scol );

      REQUIRE( mval == Approx(mval1) );
      REQUIRE( mval == Approx(mval2) );
      REQUIRE( mval == Approx(mval3) );
      }
    }
  }

TEST_CASE("fn_max_incomplete_subview_col_test")
  {
  for (size_t r = 10; r < 50; ++r)
    {
    vec x;
    x.randu(r);

    uword x_max;
    uword x_subview_max1;
    uword x_subview_max2;

    const double mval = x.max(x_max);
    const double mval1 = x.submat(1, 0, r - 2, 0).max(x_subview_max1);
    const double mval2 = x.rows(1, r - 2).max(x_subview_max2);

    if (x_max != 0 && x_max != r - 1)
      {
      REQUIRE( x_max == x_subview_max1 + 1 );
      REQUIRE( x_max == x_subview_max2 + 1 );

      REQUIRE( mval == Approx(mval1) );
      REQUIRE( mval == Approx(mval2) );
      }
    }
  }

TEST_CASE("fn_max_cx_subview_row_test")
  {
  for (size_t r = 10; r < 50; ++r)
    {
    cx_rowvec x;
    x.randu(r);

    uword x_max;
    uword x_subview_max1;
    uword x_subview_max2;

    const std::complex<double> mval = x.max(x_max);
    const std::complex<double> mval1 = x.submat(0, 0, 0, r - 1).max(x_subview_max1);
    const std::complex<double> mval2 = x.cols(0, r - 1).max(x_subview_max2);

    REQUIRE( x_max == x_subview_max1 );
    REQUIRE( x_max == x_subview_max2 );

    REQUIRE( mval.real() == Approx(mval1.real()) );
    REQUIRE( mval.imag() == Approx(mval1.imag()) );
    REQUIRE( mval.real() == Approx(mval2.real()) );
    REQUIRE( mval.imag() == Approx(mval2.imag()) );
    }
  }

TEST_CASE("fn_max_cx_incomplete_subview_test")
  {
  for (size_t r = 50; r < 150; ++r)
    {
    cx_mat x;
    x.randu(r, r);

    uword x_max;
    uword x_subview_max1;
    uword x_subview_max2;
    uword x_subview_max3;

    const std::complex<double> mval = x.max(x_max);
    const std::complex<double> mval1 = x.submat(1, 1, r - 2, r - 2).max(x_subview_max1);
    const std::complex<double> mval2 = x.cols(1, r - 2).max(x_subview_max2);
    const std::complex<double> mval3 = x.rows(1, r - 2).max(x_subview_max3);

    uword row, col;
    x.max(row, col);

    if (row != 0 && row != r - 1 && col != 0 && col != r - 1)
      {
      uword srow, scol;

      srow = x_subview_max1 % (r - 2);
      scol = x_subview_max1 / (r - 2);
      REQUIRE( x_max == (srow + 1) + r * (scol + 1) );
      REQUIRE( x_max == x_subview_max2 + r );

      srow = x_subview_max3 % (r - 2);
      scol = x_subview_max3 / (r - 2);
      REQUIRE( x_max == (srow + 1) + r * scol );

      REQUIRE( mval.real() == Approx(mval1.real()) );
      REQUIRE( mval.imag() == Approx(mval1.imag()) );
      REQUIRE( mval.real() == Approx(mval2.real()) );
      REQUIRE( mval.imag() == Approx(mval2.imag()) );
      REQUIRE( mval.real() == Approx(mval3.real()) );
      REQUIRE( mval.imag() == Approx(mval3.imag()) );
      }
    }
  }

TEST_CASE("fn_max_cx_incomplete_subview_col_test")
  {
  for (size_t r = 10; r < 50; ++r)
    {
    cx_vec x;
    x.randu(r);

    uword x_max;
    uword x_subview_max1;
    uword x_subview_max2;

    const std::complex<double> mval = x.max(x_max);
    const std::complex<double> mval1 = x.submat(1, 0, r - 2, 0).max(x_subview_max1);
    const std::complex<double> mval2 = x.rows(1, r - 2).max(x_subview_max2);

    if (x_max != 0 && x_max != r - 1)
      {
      REQUIRE( x_max == x_subview_max1 + 1 );
      REQUIRE( x_max == x_subview_max2 + 1 );

      REQUIRE( mval.real() == Approx(mval1.real()) );
      REQUIRE( mval.imag() == Approx(mval1.imag()) );
      REQUIRE( mval.real() == Approx(mval2.real()) );
      REQUIRE( mval.imag() == Approx(mval2.imag()) );
      }
    }
  }

TEST_CASE("fn_max_cx_incomplete_subview_row_test")
  {
  for (size_t r = 10; r < 50; ++r)
    {
    cx_rowvec x;
    x.randu(r);

    uword x_max;
    uword x_subview_max1;
    uword x_subview_max2;

    const std::complex<double> mval = x.max(x_max);
    const std::complex<double> mval1 = x.submat(0, 1, 0, r - 2).max(x_subview_max1);
    const std::complex<double> mval2 = x.cols(1, r - 2).max(x_subview_max2);

    if (x_max != 0 && x_max != r - 1)
      {
      REQUIRE( x_max == x_subview_max1 + 1 );
      REQUIRE( x_max == x_subview_max2 + 1 );

      REQUIRE( mval.real() == Approx(mval1.real()) );
      REQUIRE( mval.imag() == Approx(mval1.imag()) );
      REQUIRE( mval.real() == Approx(mval2.real()) );
      REQUIRE( mval.imag() == Approx(mval2.imag()) );
      }
    }
  }

TEST_CASE("fn_max_weird_operation_test")
  {
  mat a(10, 10);
  mat b(25, 10);
  a.randn();
  b.randn();

  mat output = a * b.t();

  uword real_max;
  uword operation_max;

  const double mval = output.max(real_max);
  const double other_mval = (a * b.t()).max(operation_max);

  REQUIRE( real_max == operation_max );
  REQUIRE( mval == Approx(other_mval) );
  }

TEST_CASE("fn_max_weird_sparse_operation_test")
  {
  sp_mat a(10, 10);
  sp_mat b(25, 10);
  a.sprandn(10, 10, 0.3);
  b.sprandn(25, 10, 0.3);

  sp_mat output = a * b.t();

  uword real_max;
  uword operation_max;

  const double mval = output.max(real_max);
  const double other_mval = (a * b.t()).max(operation_max);

  REQUIRE( real_max == operation_max );
  REQUIRE( mval == Approx(other_mval) );
  }

TEST_CASE("fn_max_spsubview_test")
  {
  // We will assume subview.at() works and returns points within the bounds of
  // the matrix, so we just have to ensure the results are the same as
  // Mat.max()...
  for (size_t r = 50; r < 150; ++r)
    {
    sp_mat x;
    x.sprandn(r, r, 0.3);

    uword x_max;
    uword x_subview_max1;
    uword x_subview_max2;
    uword x_subview_max3;

    const double mval = x.max(x_max);
    const double mval1 = x.submat(0, 0, r - 1, r - 1).max(x_subview_max1);
    const double mval2 = x.cols(0, r - 1).max(x_subview_max2);
    const double mval3 = x.rows(0, r - 1).max(x_subview_max3);

    if (mval != 0.0)
      {
      REQUIRE( x_max == x_subview_max1 );
      REQUIRE( x_max == x_subview_max2 );
      REQUIRE( x_max == x_subview_max3 );

      REQUIRE( mval == Approx(mval1) );
      REQUIRE( mval == Approx(mval2) );
      REQUIRE( mval == Approx(mval3) );
      }
    }
  }

TEST_CASE("fn_max_spsubview_col_test")
  {
  for (size_t r = 10; r < 50; ++r)
    {
    sp_vec x;
    x.sprandn(r, 1, 0.3);

    uword x_max;
    uword x_subview_max1;
    uword x_subview_max2;

    const double mval = x.max(x_max);
    const double mval1 = x.submat(0, 0, r - 1, 0).max(x_subview_max1);
    const double mval2 = x.rows(0, r - 1).max(x_subview_max2);

    if (mval != 0.0)
      {
      REQUIRE( x_max == x_subview_max1 );
      REQUIRE( x_max == x_subview_max2 );

      REQUIRE( mval == Approx(mval1) );
      REQUIRE( mval == Approx(mval2) );
      }
    }
  }

TEST_CASE("fn_max_spsubview_row_test")
  {
  for (size_t r = 10; r < 50; ++r)
    {
    sp_rowvec x;
    x.sprandn(1, r, 0.3);

    uword x_max;
    uword x_subview_max1;
    uword x_subview_max2;

    const double mval = x.max(x_max);
    const double mval1 = x.submat(0, 0, 0, r - 1).max(x_subview_max1);
    const double mval2 = x.cols(0, r - 1).max(x_subview_max2);

    if (mval != 0.0)
      {
      REQUIRE( x_max == x_subview_max1 );
      REQUIRE( x_max == x_subview_max2 );

      REQUIRE( mval == Approx(mval1) );
      REQUIRE( mval == Approx(mval2) );
      }
    }
  }

TEST_CASE("fn_max_spincompletesubview_test")
  {
  for (size_t r = 50; r < 150; ++r)
    {
    sp_mat x;
    x.sprandn(r, r, 0.3);

    uword x_max;
    uword x_subview_max1;
    uword x_subview_max2;
    uword x_subview_max3;

    const double mval = x.max(x_max);
    const double mval1 = x.submat(1, 1, r - 2, r - 2).max(x_subview_max1);
    const double mval2 = x.cols(1, r - 2).max(x_subview_max2);
    const double mval3 = x.rows(1, r - 2).max(x_subview_max3);

    uword row, col;
    x.max(row, col);

    if (row != 0 && row != r - 1 && col != 0 && col != r - 1 && mval != 0.0)
      {
      uword srow, scol;

      srow = x_subview_max1 % (r - 2);
      scol = x_subview_max1 / (r - 2);
      REQUIRE( x_max == (srow + 1) + r * (scol + 1) );
      REQUIRE( x_max == x_subview_max2 + r );

      srow = x_subview_max3 % (r - 2);
      scol = x_subview_max3 / (r - 2);
      REQUIRE( x_max == (srow + 1) + r * scol );

      REQUIRE( mval == Approx(mval1) );
      REQUIRE( mval == Approx(mval2) );
      REQUIRE( mval == Approx(mval3) );
      }
    }
  }

TEST_CASE("fn_max_spincompletesubview_col_test")
  {
  for (size_t r = 10; r < 50; ++r)
    {
    sp_vec x;
    x.sprandu(r, 1, 0.3);

    uword x_max;
    uword x_subview_max1;
    uword x_subview_max2;

    const double mval = x.max(x_max);
    const double mval1 = x.submat(1, 0, r - 2, 0).max(x_subview_max1);
    const double mval2 = x.rows(1, r - 2).max(x_subview_max2);

    if (x_max != 0 && x_max != r - 1 && mval != 0.0)
      {
      REQUIRE( x_max == x_subview_max1 + 1 );
      REQUIRE( x_max == x_subview_max2 + 1 );

      REQUIRE( mval == Approx(mval1) );
      REQUIRE( mval == Approx(mval2) );
      }
    }
  }

TEST_CASE("fn_max_spincompletesubview_row_test")
  {
  for (size_t r = 10; r < 50; ++r)
    {
    sp_rowvec x;
    x.sprandn(1, r, 0.3);

    uword x_max;
    uword x_subview_max1;
    uword x_subview_max2;

    const double mval = x.max(x_max);
    const double mval1 = x.submat(0, 1, 0, r - 2).max(x_subview_max1);
    const double mval2 = x.cols(1, r - 2).max(x_subview_max2);

    if (mval != 0.0 && x_max != 0 && x_max != r - 1)
      {
      REQUIRE( x_max == x_subview_max1 + 1 );
      REQUIRE( x_max == x_subview_max2 + 1 );

      REQUIRE( mval == Approx(mval1) );
      REQUIRE( mval == Approx(mval2) );
      }
    }
  }

TEST_CASE("fn_max_cx_spsubview_test")
  {
  // We will assume subview.at() works and returns points within the bounds of
  // the matrix, so we just have to ensure the results are the same as
  // Mat.max()...
  for (size_t r = 50; r < 150; ++r)
    {
    sp_cx_mat x;
    x.sprandn(r, r, 0.3);

    uword x_max;
    uword x_subview_max1;
    uword x_subview_max2;
    uword x_subview_max3;

    const std::complex<double> mval = x.max(x_max);
    const std::complex<double> mval1 = x.submat(0, 0, r - 1, r - 1).max(x_subview_max1);
    const std::complex<double> mval2 = x.cols(0, r - 1).max(x_subview_max2);
    const std::complex<double> mval3 = x.rows(0, r - 1).max(x_subview_max3);

    if (mval != std::complex<double>(0.0))
      {
      REQUIRE( x_max == x_subview_max1 );
      REQUIRE( x_max == x_subview_max2 );
      REQUIRE( x_max == x_subview_max3 );

      REQUIRE( mval.real() == Approx(mval1.real()) );
      REQUIRE( mval.imag() == Approx(mval1.imag()) );
      REQUIRE( mval.real() == Approx(mval2.real()) );
      REQUIRE( mval.imag() == Approx(mval2.imag()) );
      REQUIRE( mval.real() == Approx(mval3.real()) );
      REQUIRE( mval.imag() == Approx(mval3.imag()) );
      }
    }
  }

TEST_CASE("fn_max_cx_spsubview_col_test")
  {
  for (size_t r = 10; r < 50; ++r)
    {
    sp_cx_vec x;
    x.sprandn(r, 1, 0.3);

    uword x_max;
    uword x_subview_max1;
    uword x_subview_max2;

    const std::complex<double> mval = x.max(x_max);
    const std::complex<double> mval1 = x.submat(0, 0, r - 1, 0).max(x_subview_max1);
    const std::complex<double> mval2 = x.rows(0, r - 1).max(x_subview_max2);

    if (mval != std::complex<double>(0.0))
      {
      REQUIRE( x_max == x_subview_max1 );
      REQUIRE( x_max == x_subview_max2 );

      REQUIRE( mval.real() == Approx(mval1.real()) );
      REQUIRE( mval.imag() == Approx(mval1.imag()) );
      REQUIRE( mval.real() == Approx(mval2.real()) );
      REQUIRE( mval.imag() == Approx(mval2.imag()) );
      }
    }
  }

TEST_CASE("fn_max_cx_spsubview_row_test")
  {
  for (size_t r = 10; r < 50; ++r)
    {
    sp_cx_rowvec x;
    x.sprandn(1, r, 0.3);

    uword x_max;
    uword x_subview_max1;
    uword x_subview_max2;

    const std::complex<double> mval = x.max(x_max);
    const std::complex<double> mval1 = x.submat(0, 0, 0, r - 1).max(x_subview_max1);
    const std::complex<double> mval2 = x.cols(0, r - 1).max(x_subview_max2);

    if (mval != std::complex<double>(0.0))
      {
      REQUIRE( x_max == x_subview_max1 );
      REQUIRE( x_max == x_subview_max2 );

      REQUIRE( mval.real() == Approx(mval1.real()) );
      REQUIRE( mval.imag() == Approx(mval1.imag()) );
      REQUIRE( mval.real() == Approx(mval2.real()) );
      REQUIRE( mval.imag() == Approx(mval2.imag()) );
      }
    }
  }

TEST_CASE("fn_max_cx_spincompletesubview_test")
  {
  for (size_t r = 50; r < 150; ++r)
    {
    sp_cx_mat x;
    x.sprandn(r, r, 0.3);

    uword x_max;
    uword x_subview_max1;
    uword x_subview_max2;
    uword x_subview_max3;

    const std::complex<double> mval = x.max(x_max);
    const std::complex<double> mval1 = x.submat(1, 1, r - 2, r - 2).max(x_subview_max1);
    const std::complex<double> mval2 = x.cols(1, r - 2).max(x_subview_max2);
    const std::complex<double> mval3 = x.rows(1, r - 2).max(x_subview_max3);

    uword row, col;
    x.max(row, col);

    if (row != 0 && row != r - 1 && col != 0 && col != r - 1 && mval != std::complex<double>(0.0))
      {
      uword srow, scol;

      srow = x_subview_max1 % (r - 2);
      scol = x_subview_max1 / (r - 2);
      REQUIRE( x_max == (srow + 1) + r * (scol + 1) );
      REQUIRE( x_max == x_subview_max2 + r );

      srow = x_subview_max3 % (r - 2);
      scol = x_subview_max3 / (r - 2);
      REQUIRE( x_max == (srow + 1) + r * scol );

      REQUIRE( mval.real() == Approx(mval1.real()) );
      REQUIRE( mval.imag() == Approx(mval1.imag()) );
      REQUIRE( mval.real() == Approx(mval2.real()) );
      REQUIRE( mval.imag() == Approx(mval2.imag()) );
      REQUIRE( mval.real() == Approx(mval3.real()) );
      REQUIRE( mval.imag() == Approx(mval3.imag()) );
      }
    }
  }

TEST_CASE("fn_max_cx_spincompletesubview_col_test")
  {
  for (size_t r = 10; r < 50; ++r)
    {
    sp_cx_vec x;
    x.sprandn(r, 1, 0.3);

    uword x_max;
    uword x_subview_max1;
    uword x_subview_max2;

    const std::complex<double> mval = x.max(x_max);
    const std::complex<double> mval1 = x.submat(1, 0, r - 2, 0).max(x_subview_max1);
    const std::complex<double> mval2 = x.rows(1, r - 2).max(x_subview_max2);

    if (x_max != 0 && x_max != r - 1 && mval != std::complex<double>(0.0))
      {
      REQUIRE( x_max == x_subview_max1 + 1 );
      REQUIRE( x_max == x_subview_max2 + 1 );

      REQUIRE( mval.real() == Approx(mval1.real()) );
      REQUIRE( mval.imag() == Approx(mval1.imag()) );
      REQUIRE( mval.real() == Approx(mval2.real()) );
      REQUIRE( mval.imag() == Approx(mval2.imag()) );
      }
    }
  }

TEST_CASE("fn_max_cx_spincompletesubview_row_test")
  {
  for (size_t r = 10; r < 50; ++r)
    {
    sp_cx_rowvec x;
    x.sprandn(1, r, 0.3);

    uword x_max;
    uword x_subview_max1;
    uword x_subview_max2;

    const std::complex<double> mval = x.max(x_max);
    const std::complex<double> mval1 = x.submat(0, 1, 0, r - 2).max(x_subview_max1);
    const std::complex<double> mval2 = x.cols(1, r - 2).max(x_subview_max2);

    if (x_max != 0 && x_max != r - 1 && mval != std::complex<double>(0.0))
      {
      REQUIRE( x_max == x_subview_max1 + 1 );
      REQUIRE( x_max == x_subview_max2 + 1 );

      REQUIRE( mval.real() == Approx(mval1.real()) );
      REQUIRE( mval.imag() == Approx(mval1.imag()) );
      REQUIRE( mval.real() == Approx(mval2.real()) );
      REQUIRE( mval.imag() == Approx(mval2.imag()) );
      }
    }
  }


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>
#include <ored/marketdata/marketimpl.hpp>
#include <ored/portfolio/enginedata.hpp>
#include <ored/portfolio/fxforward.hpp>
#include <ored/portfolio/fxoption.hpp>
#include <ored/portfolio/fxswap.hpp>
#include <oret/toplevelfixture.hpp>
#include <ql/termstructures/volatility/equityfx/blackconstantvol.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/time/daycounters/actualactual.hpp>

using namespace QuantLib;
using namespace boost::unit_test_framework;
using namespace std;
using namespace ore::data;
using namespace ore::data;
using namespace ore::data;

// FX Swap test
namespace {

class TestMarket : public MarketImpl {
public:
    TestMarket() {
        asof_ = Date(3, Feb, 2015);

        // build discount
        yieldCurves_[make_tuple(Market::defaultConfiguration, YieldCurveType::Discount, "EUR")] = flatRateYts(0.02);
        yieldCurves_[make_tuple(Market::defaultConfiguration, YieldCurveType::Discount, "USD")] = flatRateYts(0.03);
        yieldCurves_[make_tuple(Market::defaultConfiguration, YieldCurveType::Discount, "CHF")] = flatRateYts(0.04);
        yieldCurves_[make_tuple(Market::defaultConfiguration, YieldCurveType::Discount, "GBP")] = flatRateYts(0.05);

        // add fx rates
        fxSpots_[Market::defaultConfiguration].addQuote("EURUSD", Handle<Quote>(boost::make_shared<SimpleQuote>(1.2)));
        fxSpots_[Market::defaultConfiguration].addQuote("EURCHF", Handle<Quote>(boost::make_shared<SimpleQuote>(1.3)));
        fxSpots_[Market::defaultConfiguration].addQuote("EURGBP", Handle<Quote>(boost::make_shared<SimpleQuote>(1.4)));

        // build fx vols
        fxVols_[make_pair(Market::defaultConfiguration, "EURUSD")] = flatRateFxv(0.10);
        fxVols_[make_pair(Market::defaultConfiguration, "EURCHF")] = flatRateFxv(0.20);
        fxVols_[make_pair(Market::defaultConfiguration, "EURGBP")] = flatRateFxv(0.20);
    }

private:
    Handle<YieldTermStructure> flatRateYts(Real forward) {
        boost::shared_ptr<YieldTermStructure> yts(new FlatForward(4, NullCalendar(), forward, ActualActual()));
        return Handle<YieldTermStructure>(yts);
    }
    Handle<BlackVolTermStructure> flatRateFxv(Volatility forward) {
        boost::shared_ptr<BlackVolTermStructure> fxv(new BlackConstantVol(0, NullCalendar(), forward, ActualActual()));
        return Handle<BlackVolTermStructure>(fxv);
    }
};
} // namespace

namespace {

void test(string nearDate, string farDate, string nearBoughtCurrency, double nearBoughtAmount, string nearSoldCurrency,
          double nearSoldAmount, double farBoughtAmount, double farSoldAmount) {

    // build market
    boost::shared_ptr<Market> market = boost::make_shared<TestMarket>();
    Settings::instance().evaluationDate() = market->asofDate();
    // fxswap's npv should equal that of two separate fxforwards
    // build first fxforward
    Envelope env1("FxForward1");

    boost::shared_ptr<EngineData> engineData = boost::make_shared<EngineData>();
    engineData->model("FxForward") = "DiscountedCashflows";
    engineData->engine("FxForward") = "DiscountingFxForwardEngine";
    boost::shared_ptr<EngineFactory> engineFactory = boost::make_shared<EngineFactory>(engineData, market);
    // this trade has buyer and seller switched so that it returns it's npv in the same currency as the second forward
    // fxswap_npv= - fxfor1_npv + fxfor2_npv
    FxForward fxFor1(env1, nearDate, nearSoldCurrency, nearSoldAmount, nearBoughtCurrency, nearBoughtAmount);

    fxFor1.build(engineFactory);

    // build second fxforward
    Envelope env2("FxForward2");
    FxForward fxFor2(env2, farDate, nearSoldCurrency, farBoughtAmount, nearBoughtCurrency, farSoldAmount);
    fxFor2.build(engineFactory);

    // build fx swap

    Envelope env3("FxSwap");

    FxSwap fxswap(env3, nearDate, farDate, nearBoughtCurrency, nearBoughtAmount, nearSoldCurrency, nearSoldAmount,
                  farBoughtAmount, farSoldAmount);
    fxswap.build(engineFactory);
    fxswap.instrument()->NPV();
    Real fx1 = fxFor1.instrument()->NPV();
    Real fx2 = fxFor2.instrument()->NPV();
    Real npvForward = fx2 - fx1;
    if (fxswap.instrument()->NPV() != npvForward)
        BOOST_FAIL("The FxSwap has NPV: " << fxswap.instrument()->NPV()
                                          << ", which does not equal the sum of two Fxforwards: " << npvForward);
}
} // namespace

BOOST_FIXTURE_TEST_SUITE(OREDataTestSuite, ore::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(FXSwapTests)

BOOST_AUTO_TEST_CASE(testFXSwap) {

    BOOST_TEST_MESSAGE("Testing FXSwap...");

    string nearDate = "2015-10-27";
    string farDate = "2015-11-03";
    string nearBoughtCurrency = "EUR";
    Real nearBoughtAmount = 224557621.490000;
    string nearSoldCurrency = "USD";
    Real nearSoldAmount = 250000000.000000;
    Real farBoughtAmount = 250018000.00;
    Real farSoldAmount = 224552207.77;

    test(nearDate, farDate, nearBoughtCurrency, nearBoughtAmount, nearSoldCurrency, nearSoldAmount, farBoughtAmount,
         farSoldAmount);

    nearDate = "2015-07-14";
    farDate = "2015-11-16";
    nearBoughtCurrency = "CHF";
    nearBoughtAmount = 97000000;
    nearSoldCurrency = "USD";
    nearSoldAmount = 103718911.06;
    farBoughtAmount = 103923787.150000;
    farSoldAmount = 96737000.000000;

    test(nearDate, farDate, nearBoughtCurrency, nearBoughtAmount, nearSoldCurrency, nearSoldAmount, farBoughtAmount,
         farSoldAmount);

    nearDate = "2015-08-04";
    farDate = "2015-11-30";
    nearBoughtCurrency = "GBP";
    nearBoughtAmount = 100227439.19;
    nearSoldCurrency = "USD";
    nearSoldAmount = 156000000;
    farBoughtAmount = 156148000.000000;
    farSoldAmount = 100400372.110000;

    test(nearDate, farDate, nearBoughtCurrency, nearBoughtAmount, nearSoldCurrency, nearSoldAmount, farBoughtAmount,
         farSoldAmount);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


#define BOOST_TEST_MODULE "test_3SPN2_BaseStacking_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/forcefield/3SPN2/ThreeSPN2BaseStackingPotential.hpp>

using parameter_set_to_test_d = std::tuple<
    mjolnir::ThreeSPN2BaseStackingPotentialParameter<double>,
    mjolnir::ThreeSPN2CBaseStackingPotentialParameter<double>
>;
using parameter_set_to_test_f = std::tuple<
    mjolnir::ThreeSPN2BaseStackingPotentialParameter<float>,
    mjolnir::ThreeSPN2CBaseStackingPotentialParameter<float>
>;

BOOST_AUTO_TEST_CASE_TEMPLATE(f_3SPN2_BaseStacking_double,
        ParameterSet, parameter_set_to_test_d)
{
    mjolnir::LoggerManager::set_default_logger("test_3spn2_base_stacking_potential.log");
    using real_type = double;

    constexpr std::size_t N = 1000;
    constexpr real_type   h = 1e-6;
    constexpr real_type  pi = mjolnir::math::constants<real_type>::pi();

    mjolnir::ThreeSPN2BaseStackingPotential<real_type> potential(ParameterSet{});

    const real_type theta_0   = 0.5  * pi;
    const real_type theta_min = 0.01 * pi;
    const real_type theta_max = 0.99 * pi;
    const real_type dtheta    = (theta_max - theta_min) / N;

    for(std::size_t i=0; i<N; ++i)
    {
        const real_type theta = theta_min + i * dtheta;
        const real_type f1  = potential.f(theta + h, theta_0);
        const real_type f2  = potential.f(theta - h, theta_0);
        const real_type df_numeric  = (f1 - f2) / (2 * h);
        const real_type df_analytic = potential.df(theta, theta_0);

        if((f1 == 0.0 || f2 == 0.0) && !(f1 == 0.0 && f2 == 0.0))
        {
            // the numeric differentiation becomes unstable.
        }
        else
        {
            BOOST_TEST(df_numeric == df_analytic, boost::test_tools::tolerance(h));
        }
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(f_3SPN2_BaseStacking_float,
        ParameterSet, parameter_set_to_test_f)
{
    mjolnir::LoggerManager::set_default_logger("test_3spn2_base_stacking_potential.log");
    using real_type = float;

    constexpr std::size_t N = 100;
    constexpr real_type   h = 1e-3f;
    constexpr real_type  pi = mjolnir::math::constants<real_type>::pi();

    mjolnir::ThreeSPN2BaseStackingPotential<real_type> potential(ParameterSet{});

    const real_type theta_0  = 0.5  * pi;
    const real_type theta_min = 0.01 * pi;
    const real_type theta_max = 0.99 * pi;
    const real_type dtheta    = (theta_max - theta_min) / N;

    for(std::size_t i=0; i<N; ++i)
    {
        const real_type theta = theta_min + i * dtheta;
        const real_type f1  = potential.f(theta + h, theta_0);
        const real_type f2  = potential.f(theta - h, theta_0);
        const real_type df_numeric  = (f1 - f2) / (2 * h);
        const real_type df_analytic = potential.df(theta, theta_0);

        if((f1 == 0.0f || f2 == 0.0f) && !(f1 == 0.0f && f2 == 0.0f))
        {
            // the numeric differentiation becomes unstable.
        }
        else
        {
            BOOST_TEST(df_numeric == df_analytic, boost::test_tools::tolerance(h));
        }
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(U_rep_3SPN2_BaseStacking_double,
        ParameterSet, parameter_set_to_test_d)
{
    mjolnir::LoggerManager::set_default_logger("test_3spn2_base_stacking_potential.log");
    using real_type = double;
    using base_stack_kind = mjolnir::parameter_3SPN2::base_stack_kind;

    constexpr std::size_t N = 1000;
    constexpr real_type   h = 1e-6;

    mjolnir::ThreeSPN2BaseStackingPotential<real_type> potential(ParameterSet{});

    const real_type x_min = 2.0;
    const real_type x_max = 6.0; // outside of cutoff
    const real_type dx    = (x_max - x_min) / N;

    for(std::size_t i=0; i<N; ++i)
    {
        const real_type x  = x_min + i * dx;
        const real_type U1 = potential.U_rep(base_stack_kind::AA, x + h);
        const real_type U2 = potential.U_rep(base_stack_kind::AA, x - h);
        const real_type dU_numeric  = (U1 - U2) / (2 * h);
        const real_type dU_analytic = potential.dU_rep(base_stack_kind::AA, x);

        if(U1 != real_type(0.0) && U2 != real_type(0.0))
        {
            BOOST_TEST(dU_numeric == dU_analytic, boost::test_tools::tolerance(h));
        }
        // purely repulsive potential.
        BOOST_TEST(U1 <= U2); // U1 is for x+h, U2 is for x-h
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(U_rep_3SPN2_BaseStacking_float,
        ParameterSet, parameter_set_to_test_f)
{
    mjolnir::LoggerManager::set_default_logger("test_3spn2_base_stacking_potential.log");
    using real_type = float;
    using base_stack_kind = mjolnir::parameter_3SPN2::base_stack_kind;

    constexpr std::size_t N = 100;
    constexpr real_type   h = 1e-3f;

    mjolnir::ThreeSPN2BaseStackingPotential<real_type> potential(ParameterSet{});

    const real_type x_min = 2.0;
    const real_type x_max = 6.0; // outside of cutoff
    const real_type dx    = (x_max - x_min) / N;

    for(std::size_t i=0; i<N; ++i)
    {
        const real_type x  = x_min + i * dx;
        const real_type U1 = potential.U_rep(base_stack_kind::AA, x + h);
        const real_type U2 = potential.U_rep(base_stack_kind::AA, x - h);
        const real_type dU_numeric  = (U1 - U2) / (2 * h);
        const real_type dU_analytic = potential.dU_rep(base_stack_kind::AA, x);

        if(U1 != real_type(0.0) && U2 != real_type(0.0))
        {
            BOOST_TEST(dU_numeric == dU_analytic, boost::test_tools::tolerance(h));
        }
        // purely repulsive potential.
        BOOST_TEST(U1 <= U2); // U1 is for x+h, U2 is for x-h
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(U_attr_3SPN2_BaseStacking_double,
        ParameterSet, parameter_set_to_test_d)
{
    mjolnir::LoggerManager::set_default_logger("test_3spn2_base_stacking_potential.log");
    using real_type = double;
    using base_stack_kind = mjolnir::parameter_3SPN2::base_stack_kind;

    constexpr std::size_t N = 1000;
    constexpr real_type   h = 1e-6;

    mjolnir::ThreeSPN2BaseStackingPotential<real_type> potential(ParameterSet{});

    const real_type x_min =  3.0; // inside of flat region
    const real_type x_max = 12.0;
    const real_type dx    = (x_max - x_min) / N;

    for(std::size_t i=0; i<N; ++i)
    {
        const real_type x  = x_min + i * dx;
        const real_type U1 = potential.U_attr(base_stack_kind::AA, x + h);
        const real_type U2 = potential.U_attr(base_stack_kind::AA, x - h);
        const real_type dU_numeric  = (U1 - U2) / (2 * h);
        const real_type dU_analytic = potential.dU_attr(base_stack_kind::AA, x);

        if(U1 != potential.epsilon(base_stack_kind::AA) &&
           U2 == potential.epsilon(base_stack_kind::AA))
        {
            BOOST_TEST(dU_numeric == dU_analytic, boost::test_tools::tolerance(h));
        }
        // purely attractive potential
        BOOST_TEST(U2 <= U1);
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(U_attr_3SPN2_BaseStacking_float,
        ParameterSet, parameter_set_to_test_f)
{
    mjolnir::LoggerManager::set_default_logger("test_3spn2_base_stacking_potential.log");
    using real_type = float;
    using base_stack_kind = mjolnir::parameter_3SPN2::base_stack_kind;

    constexpr std::size_t N = 100;
    constexpr real_type   h = 1e-3f;

    mjolnir::ThreeSPN2BaseStackingPotential<real_type> potential(ParameterSet{});

    const real_type x_min =  3.0f;
    const real_type x_max = 12.0f;
    const real_type dx    = (x_max - x_min) / N;

    for(std::size_t i=0; i<N; ++i)
    {
        const real_type x  = x_min + i * dx;
        const real_type U1 = potential.U_attr(base_stack_kind::AA, x + h);
        const real_type U2 = potential.U_attr(base_stack_kind::AA, x - h);
        const real_type dU_numeric  = (U1 - U2) / (2 * h);
        const real_type dU_analytic = potential.dU_attr(base_stack_kind::AA, x);

        if(U1 != potential.epsilon(base_stack_kind::AA) &&
           U2 == potential.epsilon(base_stack_kind::AA))
        {
            BOOST_TEST(dU_numeric == dU_analytic, boost::test_tools::tolerance(h));
        }
        // purely attractive potential
        BOOST_TEST(U2 <= U1);

    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(U_dU_attr_3SPN2_BaseStacking_double,
        ParameterSet, parameter_set_to_test_d)
{
    mjolnir::LoggerManager::set_default_logger("test_3spn2_base_stacking_potential.log");
    using real_type = double;
    using base_stack_kind = mjolnir::parameter_3SPN2::base_stack_kind;

    constexpr std::size_t N   =  100;
    constexpr real_type   h   = 1e-6;
    constexpr real_type x_min =  3.0;
    constexpr real_type x_max = 12.0;
    constexpr real_type dx    = (x_max - x_min) / N;
    mjolnir::ThreeSPN2BaseStackingPotential<real_type> potential(ParameterSet{});

    for(std::size_t i=0; i<N; ++i)
    {
        const auto x    = x_min + i * dx;
        const auto U_dU = potential.U_dU_attr(base_stack_kind::AA, x);
        const auto U    = potential.U_attr(base_stack_kind::AA, x);
        const auto dU   = potential.dU_attr(base_stack_kind::AA, x);

        BOOST_TEST(U_dU.first  ==  U, boost::test_tools::tolerance(h));
        BOOST_TEST(U_dU.second == dU, boost::test_tools::tolerance(h));
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(U_dU_attr_3SPN2_BaseStacking_float,
        ParameterSet, parameter_set_to_test_f)
{
    mjolnir::LoggerManager::set_default_logger("test_3spn2_base_stacking_potential.log");
    using real_type = float;
    using base_stack_kind = mjolnir::parameter_3SPN2::base_stack_kind;

    constexpr std::size_t N   =  100;
    constexpr real_type   h   = 1e-3f;
    constexpr real_type x_min =  3.0f;
    constexpr real_type x_max = 12.0f;
    constexpr real_type dx    = (x_max - x_min) / N;
    mjolnir::ThreeSPN2BaseStackingPotential<real_type> potential(ParameterSet{});

    for(std::size_t i=0; i<N; ++i)
    {
        const auto x    = x_min + i * dx;
        const auto U_dU = potential.U_dU_attr(base_stack_kind::AA, x);
        const auto U    = potential.U_attr(base_stack_kind::AA, x);
        const auto dU   = potential.dU_attr(base_stack_kind::AA, x);

        BOOST_TEST(U_dU.first  ==  U, boost::test_tools::tolerance(h));
        BOOST_TEST(U_dU.second == dU, boost::test_tools::tolerance(h));
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(bs_kind_3SPN2_BaseStacking,
        ParameterSet, parameter_set_to_test_d)
{
    mjolnir::LoggerManager::set_default_logger("test_3spn2_base_stacking_potential.log");
    using real_type = double;
    using base_kind = mjolnir::parameter_3SPN2::base_kind;
    using base_stack_kind = mjolnir::parameter_3SPN2::base_stack_kind;

    mjolnir::ThreeSPN2BaseStackingPotential<real_type> potential(ParameterSet{});

    BOOST_TEST(base_stack_kind::AA == potential.bs_kind(base_kind::A, base_kind::A));
    BOOST_TEST(base_stack_kind::AT == potential.bs_kind(base_kind::A, base_kind::T));
    BOOST_TEST(base_stack_kind::AG == potential.bs_kind(base_kind::A, base_kind::G));
    BOOST_TEST(base_stack_kind::AC == potential.bs_kind(base_kind::A, base_kind::C));
    BOOST_TEST(base_stack_kind::TA == potential.bs_kind(base_kind::T, base_kind::A));
    BOOST_TEST(base_stack_kind::TT == potential.bs_kind(base_kind::T, base_kind::T));
    BOOST_TEST(base_stack_kind::TG == potential.bs_kind(base_kind::T, base_kind::G));
    BOOST_TEST(base_stack_kind::TC == potential.bs_kind(base_kind::T, base_kind::C));
    BOOST_TEST(base_stack_kind::GA == potential.bs_kind(base_kind::G, base_kind::A));
    BOOST_TEST(base_stack_kind::GT == potential.bs_kind(base_kind::G, base_kind::T));
    BOOST_TEST(base_stack_kind::GG == potential.bs_kind(base_kind::G, base_kind::G));
    BOOST_TEST(base_stack_kind::GC == potential.bs_kind(base_kind::G, base_kind::C));
    BOOST_TEST(base_stack_kind::CA == potential.bs_kind(base_kind::C, base_kind::A));
    BOOST_TEST(base_stack_kind::CT == potential.bs_kind(base_kind::C, base_kind::T));
    BOOST_TEST(base_stack_kind::CG == potential.bs_kind(base_kind::C, base_kind::G));
    BOOST_TEST(base_stack_kind::CC == potential.bs_kind(base_kind::C, base_kind::C));
}


/*
 *
 * reference.hpp
 * Header file for reference.cpp
 *
 */
#pragma once

#include <cstdint>
#include <cstddef>
#include <vector>
#include <string>
#include <tuple>
#include "robin_hood.h"

// #define ARMA_ALLOW_FAKE_GCC
// #define ARMA_DONT_USE_WRAPPER
// #include <armadillo>
#include "Eigen/Dense"

const size_t def_bbits = 14; // = log2(sketch size) where sketch size = 64 * sketchsize64
const size_t def_sketchsize64 = 156;

#include "sketch/sketch.hpp"

class RandomMC;

class Reference
{
public:
  Reference();
  // Read and run sketch
  Reference(const std::string &name,
            SeqBuf &sequence,
            const KmerSeeds &kmers,
            const size_t sketchsize64,
            const bool codon_phased,
            const bool use_rc,
            const uint8_t min_count,
            const bool exact);

  // For loading from DB
  Reference(const std::string &name,
            const size_t bbits,
            const size_t sketchsize64,
            const size_t seq_size,
            const std::vector<double> bases,
            const unsigned long int missing_bases);

  // Initialise from GPU sketch
  Reference(const std::string &name,
            robin_hood::unordered_map<int, std::vector<uint64_t>> &sketch,
            const size_t bbits,
            const size_t sketchsize64,
            const size_t seq_size,
            const BaseComp<double> &bases,
            const unsigned long int missing_bases,
            const bool use_rc,
            const bool densified);

  const std::vector<uint64_t> &get_sketch(const int kmer_len) const;
  void add_kmer_sketch(const std::vector<uint64_t> &sketch, const int kmer_len);
  void remove_kmer_sketch(const size_t kmer_len);
  double jaccard_dist(Reference &query, const int kmer_len, const double random_jaccard);
  double jaccard_dist(Reference &query, const int kmer_len, const RandomMC &random);
  template <typename T>
  std::tuple<float, float> core_acc_dist(Reference &query, const T &random);
  template <typename T>
  std::tuple<float, float> core_acc_dist(Reference &query, const Eigen::MatrixXf &kmers, const T &random);
  std::vector<size_t> kmer_lengths() const;

  std::string name() const { return _name; }
  size_t bbits() const { return _bbits; }
  size_t sketchsize64() const { return _sketchsize64; }
  size_t seq_length() const { return _seq_size; }
  bool densified() const { return _densified; }
  bool rc() const { return _use_rc; }
  std::vector<double> base_composition() const { return {_bases.a, _bases.c, _bases.g, _bases.t}; }
  unsigned long int missing_bases() const { return _missing_bases; }

  // For sorting, order by name
  friend bool operator<(Reference const &a, Reference const &b)
  {
    return a._name < b._name;
  }
  friend bool operator==(Reference const &a, Reference const &b)
  {
    return a._name == b._name;
  }

private:
  // Info
  std::string _name;
  size_t _bbits;
  size_t _sketchsize64;
  bool _use_rc;

  // Sequence statistics
  size_t _seq_size;
  unsigned long int _missing_bases;
  bool _densified;

  // Proportion of each base
  BaseComp<double> _bases;

  // sketch - map keys are k-mer length
  robin_hood::unordered_map<int, std::vector<uint64_t>> usigs;
};

template <class T>
Eigen::MatrixXf kmer2mat(const T &kmers);

// Defined in linear_regression.cpp
std::tuple<float, float> regress_kmers(Reference *r1,
                                       Reference *r2,
                                       const Eigen::MatrixXf &kmers,
                                       const std::vector<double> &random);
std::tuple<float, float> regress_kmers(Reference *r1,
                                       Reference *r2,
                                       const Eigen::MatrixXf &kmers,
                                       const RandomMC &random);


/*
 * model_fitting_demo.cpp
 *
 *  Created on: Jun 8, 2010
 *      Author: sturm
 */

#include <ros/ros.h>

#include "articulation_models/models/factory.h"

#include "articulation_msgs/ModelMsg.h"
#include "articulation_msgs/TrackMsg.h"
#include "articulation_msgs/ParamMsg.h"

using namespace std;
using namespace articulation_models;
using namespace articulation_msgs;

#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>

int main(int argc, char** argv) {
	ros::init(argc, argv, "model_fitting");
	ros::NodeHandle n;
	ros::Publisher model_pub = n.advertise<ModelMsg> ("model", 5);
	ros::Rate loop_rate(5);
	int count = 0;

	boost::normal_distribution<> nd(0.0, 0.01);
	boost::mt19937 rng;
	boost::variate_generator<boost::mt19937&, boost::normal_distribution<> >
			var_nor(rng, nd);

	MultiModelFactory factory;

	ModelMsg model_msg;
	model_msg.name = "rotational";
	ParamMsg sigma_param;
	sigma_param.name = "sigma_position";
	sigma_param.value = 0.02;
	sigma_param.type = ParamMsg::PRIOR;
	sigma_param.name = "sigma_orientation";
	sigma_param.value = M_PI*10;
	sigma_param.type = ParamMsg::PRIOR;
	model_msg.params.push_back(sigma_param);

	model_msg.track.header.stamp = ros::Time();
	model_msg.track.header.frame_id = "/";

	while (ros::ok()) {

		geometry_msgs::Pose pose;
		pose.position.x = cos(count / 30.0)+var_nor();
		pose.position.y = sin(count / 30.0)+var_nor();
		pose.position.z = var_nor();
		pose.orientation.x = 0;
		pose.orientation.y = 0;
		pose.orientation.z = 0;
		pose.orientation.w = 1;
		model_msg.track.pose.push_back(pose);

		if(model_msg.track.pose.size()<3) continue;

		cout <<"creating object"<<endl;
		GenericModelPtr model_instance = factory.restoreModel(model_msg);
		cout <<"fitting"<<endl;
		model_instance->fitModel();
		cout <<"evaluating"<<endl;
		model_instance->evaluateModel();
		cout <<"done"<<endl;

		cout << "model class = "<< model_instance->getModelName() << endl;
		cout << "	radius = "<<model_instance->getParam("rot_radius")<< endl;
		cout << "	center.x = "<<model_instance->getParam("rot_center.x")<< endl;
		cout << "	center.y = "<<model_instance->getParam("rot_center.y")<< endl;
		cout << "	center.z = "<<model_instance->getParam("rot_center.z")<< endl;
		cout << "	log LH = " << model_instance->getParam("loglikelihood")<< endl;

		ModelMsg fitted_model_msg = model_instance->getModel();
		model_pub.publish(fitted_model_msg);

		ros::spinOnce();
		loop_rate.sleep();
		++count;
	}
}


#include "OccupancyGrid/OccupancyGrid.h"
#include "OccupancyGrid/loadData.h"
#include "OccupancyGrid/dualdecomposition.hpp"
#include "OccupancyGrid/visualiser.h"
#include "OccupancyGrid/gtsam_adaptor.hpp"
#include "OccupancyGrid/slaveminimizer.hpp"
#include "OccupancyGrid/DDLaserFactor.hpp"
#include "OccupancyGrid/loadOccupancyGrid.h"
#include "OccupancyGrid/utility.hpp"

#include <gtsam/geometry/Pose2.h>
#include <boost/program_options.hpp>

Visualiser global_vis_;
Visualiser global_vis2_;

using namespace std;
using namespace boost;
using namespace gtsam;
using namespace occgrid;

namespace po = boost::program_options;

int main(int argc, const char *argv[])
{
  typedef double EnergyType;
  global_vis_.enable_show();
  global_vis2_.enable_show();
  // parse arguments ///////////////////////////////////////////
  // Declare the supported options.
  po::options_description desc("Run dual decomposition");
  desc.add_options()
    ("help", "produce help message")
    ("width", po::value<double>(), "Width")
    ("height", po::value<double>(), "Height")
    ("resolution", po::value<double>()->required(), "Size of square cell in the map")
    ("step", po::value<EnergyType>()->default_value(50), "step size for algorithm")
    ("dir", po::value<std::string>()->default_value("Data/player_sim_with_reflectance"), "Data directory")
    ("clock", po::value<double>()->default_value(400), "Max clock")
;

  po::positional_options_description pos;
  pos.add("resolution", 1);

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).options(desc).positional(pos).run(), vm);
  po::notify(vm);

  EnergyType step = vm["step"].as<EnergyType>();
  std::string directory = vm["dir"].as<std::string>();
  double max_clock = CLOCKS_PER_SEC * vm["clock"].as<double>();
  // end of parse arguments ////////////////////////////////////
  OccupancyGrid occupancyGrid = loadOccupancyGrid(vm);

  global_vis_.init(occupancyGrid.height(), occupancyGrid.width());
  global_vis2_.init(occupancyGrid.height(), occupancyGrid.width(), "d");

  typedef gtsam::Index SampleSpaceType;
  typedef gtsam::Index vertex_descriptor;
  typedef G<OccupancyGrid, EnergyType, SampleSpaceType, vertex_descriptor> OccupancyGridGraph;
  typedef hash_property_map< std::pair<vertex_descriptor, vertex_descriptor>, SampleSpaceType> MultiAssignment;
  typedef DisagreementTracker<OccupancyGridGraph, MultiAssignment> DisagreementMap;
  typedef hash_property_map< typename OccupancyGridGraph::MessageTypes::key_type, EnergyType> Messages;
  typedef SlaveMinimizer<OccupancyGridGraph,
          DDLaserFactor<DisagreementMap, Messages>, Messages, DisagreementMap> SlaveMinimizer_;
  OccupancyGridGraph ogg(occupancyGrid);
  SlaveMinimizer_ slvmin(ogg);
  typedef typename OccupancyGridGraph::SampleSpaceMap SampleSpaceMap;
  SampleSpaceMap ssm = get(sample_space_t(), ogg);
  Messages messages(0);
  MultiAssignment multiassign(0);
  DisagreementMap disagreement_map(ogg, multiassign);
  typedef SubgradientDualDecomposition<OccupancyGridGraph, SlaveMinimizer_,
          SampleSpaceMap, DisagreementMap, Messages, SampleSpaceType, EnergyType> SubgradientDualDecompositionType;
  SubgradientDualDecompositionType subg_dd(ogg, slvmin, ssm, disagreement_map, messages, step);
  iterate_dualdecomposition<OccupancyGridGraph, SubgradientDualDecompositionType, DisagreementMap, SampleSpaceType>(ogg, subg_dd, disagreement_map, 70, max_clock);
  std::stringstream ss;
  ss << directory << "/dualdecomposition.png";
  global_vis_.save(ss.str());
}


#include <vector>
#include <unordered_set>
#include <algorithm>

#include <rect_steiner_tree.h>
#include <config.h>

#include <omp.h>

#include <boost/sort/sort.hpp>

struct DeltaCostCompare
{
    inline bool operator()(const std::pair<std::ptrdiff_t, GridPoint>& lhs,
                           const std::pair<std::ptrdiff_t, GridPoint>& rhs) const
    {
        return lhs.first > rhs.first;
    }
};

RectSteinerTree::RectSteinerTree(const PointSet& points,
                                 const PointSet& hanan_grid_points)
    : mst(MinimumSpanningTree(points))
    , terminal_points(points)
    , rst_points(points)
{
    std::unordered_set<GridPoint, GridPointHasher> terminal_points_set(points.begin(), points.end());

    std::vector<std::pair<std::ptrdiff_t, GridPoint>> delta_costs(hanan_grid_points.size());

    std::unordered_set<GridPoint, GridPointHasher> rst_point_set(points.cbegin(), points.cend());

    const auto hanan_grid_size = hanan_grid_points.size();

    while (true)
    {
        #pragma omp parallel for
        for (std::size_t i = 0; i < hanan_grid_size; i++)
            if (rst_point_set.find(hanan_grid_points[i]) == rst_point_set.end())
            {
                auto hanan_mst = MinimumSpanningTree(mst, rst_points, hanan_grid_points[i]);
                delta_costs[i] = std::make_pair(delta_cost(mst, hanan_mst), hanan_grid_points[i]);
            }

        boost::sort::block_indirect_sort(delta_costs.begin(),
                                         delta_costs.end(),
                                         DeltaCostCompare(),
                                         config::ParallelConfig::get()->get_num_threads());

        if (delta_costs.empty() || delta_costs.front().first <= 0)
            break;

        for (std::size_t i = 0; i < hanan_grid_size; i++)
            if (delta_costs[i].first > 0 && rst_point_set.find(delta_costs[i].second) == rst_point_set.end())
            {
                auto tmp_mst = MinimumSpanningTree(mst, rst_points, delta_costs[i].second);
                auto tmp_cost = delta_cost(mst, tmp_mst);

                if (tmp_cost >= delta_costs[i].first)
                {
                    mst = std::move(tmp_mst);
                    rst_points.push_back(delta_costs[i].second);
                    rst_point_set.emplace(delta_costs[i].second);
                }
            }

        const auto prev_size = rst_point_set.size();

        for (auto it = rst_point_set.begin(); it != rst_point_set.end(); ) {
            if (terminal_points_set.find(*it) == terminal_points_set.end() && mst.get_vertex_degree(*it) <= 2)
                rst_point_set.erase(it++);
            else
                ++it;
        }

        if (prev_size != rst_point_set.size())
        {
            rst_points = std::vector<GridPoint>(rst_point_set.cbegin(), rst_point_set.cend());
            mst = MinimumSpanningTree(rst_points);
        }
    }
}


#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>

#include <scitbx/wigner/wigner3j.h>

namespace scitbx { namespace wigner {
namespace boost_python{

  struct wigner3j_fast_wrapper
  {
    typedef wigner3j_fast < double > w_t;
    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("wigner3j_fast", no_init)
        .def( init< int const&
                  >
             (( arg("max")
             ))
            )
        .def("compute", &w_t::compute)
      ;
    }
  };

  struct wigner3j_wrapper
  {
    typedef wigner3j < double > w_t;
    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("wigner3j", no_init)
        .def( init< int const&,
                    int const&,
                    int const&,
                    int const&,
                    int const&,
                    int const&
                  >
             (( arg("j1"),
                arg("j2"),
                arg("j3"),
                arg("m1"),
                arg("m2"),
                arg("m3")
             ))
            )
        .def("check", &w_t::check)
        .def("get_value", &w_t::get_value)
      ;
    }
  };

  void
  init_module()
  {
    wigner3j_fast_wrapper::wrap();
    wigner3j_wrapper::wrap();
  }

}}} // scitbx::wigner::boost_python

BOOST_PYTHON_MODULE(scitbx_wigner_ext)
{
  scitbx::wigner::boost_python::init_module();
}


#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/time.h>
#include <pcl/console/print.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/segmentation/organized_multi_plane_segmentation.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>

// Types
typedef pcl::PointXYZ PointT;
typedef pcl::PointNormal PointNT;
typedef pcl::PointCloud<PointT> PointCloudT;
typedef pcl::PointCloud<pcl::Normal> PointCloudN;
typedef pcl::FPFHSignature33 FeatureT;
typedef pcl::FPFHEstimationOMP<PointNT,PointNT,FeatureT> FeatureEstimationT;
typedef pcl::PointCloud<FeatureT> FeatureCloudT;
typedef pcl::visualization::PointCloudColorHandlerCustom<PointNT> ColorHandlerT;

// Align a rigid object to a scene with clutter and occlusions
int
main (int argc, char **argv)
{
  // Point clouds
  PointCloudT::Ptr object (new PointCloudT);
  PointCloudN::Ptr normals(new PointCloudN);
  FeatureCloudT::Ptr object_features (new FeatureCloudT);
  std::string filename;
  filename = "data/blade_data.ply";

  if (!pcl::io::loadPLYFile(filename, *object))
  {
	  printf("Cannot open ply file!");
  }

  //VoxelGrid Filter Downsampling
  pcl::VoxelGrid<PointT> grid;
  const float leaf = 1.00;
  grid.setLeafSize(leaf, leaf, leaf);
  grid.setInputCloud(object);
  grid.filter(*object);

  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
  ne.setInputCloud(object);
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
  ne.setSearchMethod(tree);
  ne.setRadiusSearch(1);
  ne.compute(*normals);

  printf("%d", normals->size());

  //pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
  //pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

  pcl::OrganizedMultiPlaneSegmentation<pcl::PointXYZ, pcl::Normal, pcl::Label>mps;
  mps.setMinInliers(1000);
  mps.setAngularThreshold(0.017453*2.0);
  mps.setDistanceThreshold(0.02);
  mps.setInputNormals(normals);
  mps.setInputCloud(object);
  std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
  //std::vector<pcl::ModelCoefficients> model_coefficients;
  //std::vector<pcl::PointIndices> inlier_indices;
  //pcl::PointCloud<pcl::Label>::Ptr labels(new pcl::PointCloud<pcl::Label>);
  //std::vector<pcl::PointIndices> label_indices;
  //std::vector<pcl::PointIndices> boundary_indices;

  mps.segmentAndRefine(regions);
  printf("%d", regions.size());

  boost::shared_ptr<pcl::visualization::PCLVisualizer> mpsvis;
  char name[1024];
  unsigned char red[6] = { 255,   0,   0, 255, 255,   0 };
  unsigned char grn[6] = { 0, 255,   0, 255,   0, 255 };
  unsigned char blu[6] = { 0,   0, 255,   0, 255, 255 };

  pcl::PointCloud<PointT>::Ptr contour(new pcl::PointCloud<PointT>);

  for (size_t i = 0; i < regions.size(); i++)
  {
	  Eigen::Vector3f centroid = regions[i].getCentroid();
	  Eigen::Vector4f model = regions[i].getCoefficients();
	  pcl::PointXYZ pt1 = pcl::PointXYZ(centroid[0], centroid[1], centroid[2]);
	  pcl::PointXYZ pt2 = pcl::PointXYZ(centroid[0] + (0.5f * model[0]),
		  centroid[1] + (0.5f * model[1]),
		  centroid[2] + (0.5f * model[2]));
	  sprintf(name, "normal_%d", unsigned(i));
	  mpsvis->addArrow(pt2, pt1, 1.0, 0, 0, false, name);

	  contour->points = regions[i].getContour();
	  sprintf(name, "plane_%02d", int(i));
	  pcl::visualization::PointCloudColorHandlerCustom <PointT> color(contour, red[i % 6], grn[i % 6], blu[i % 6]);
	  if (!mpsvis->updatePointCloud(contour, color, name))
		  mpsvis->addPointCloud(contour, color, name);
	  mpsvis->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, name);
  }
  mpsvis->spinOnce();

  system("pause");
  return (0);
}


#include <vector>
#include <cmath>
#include <random>
#include <limits>

#include <exception>
#include <iostream>
#include <fstream>
#include <string>
#include <utility>
#include <cstdlib>
#include <boost/program_options.hpp>
#include "../include/common.hpp"
#include "../include/configurations.hpp"
#include "../include/instance.hpp"
#include "../include/instance_creator.hpp"

// declare the random number generator
std::mt19937_64 rng;
std::string configuration_file = DEFAULT_CONFIG_FILE;

void read_parameters(int argc, char** argv, Configurations& con);

int main(int argc, char** argv){
	Configurations con;
	read_parameters(argc, argv, con);
	//printf("Seed: %ld\n", con.seed);
	rng = std::mt19937_64(con.seed);

	Instance inst;
	if(con.verbose >= 1) printf("Creating instance: getting locations, routing paths, and generating remaining data...\n");
	int ret = InstanceCreator::create(con, inst);
	if(ret == EXIT_FAILURE) return ret;
	else if(con.verbose >= 1) printf("DONE: Instance created.\n");

	if(con.verbose >= 1) printf("Writing instance to file %s...\n", con.inst_file.c_str());
	FileCode rwrite = Instance::write_instance_file(con.inst_file, inst);
	ret = rwrite == FileCode::WriteError ? EXIT_FAILURE : EXIT_SUCCESS;
	if(ret != EXIT_FAILURE && con.verbose >= 1) printf("DONE: Instance written to file.\n");

	return ret;
}

void read_parameters(int argc, char** argv, Configurations& con){
	size_t temp_seed = std::numeric_limits<size_t>::max();
	con.seed = 0;
	con.verbose = 1;
	con.inst_file = DEFAULT_INST_FILE;

	namespace po = boost::program_options;
	po::options_description desc("parameters");
	desc.add_options()
		("help,h", "print this help info")
		("configuration,c", po::value<std::string>(&configuration_file)->required(),
		 ("Configuration file to read the parameters."))
		("output,o", po::value<std::string>(&con.inst_file),
		 ("output file where the generated instance will be recorded. Default: " + DEFAULT_INST_FILE).c_str())
		("seed,s", po::value<size_t>(&temp_seed),
		 ("define the RNG seed. Default: 0"))
		("verbose,v", po::value<int>(&con.verbose),
		 ("verbosity level to print intermediate information. Values: 0 (none), 1 (some), 2 (full). Default: 1"))
		;

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	if (vm.count("help")) {
		std::cout << desc << std::endl;
		exit(EXIT_SUCCESS);
	}

	po::notify(vm);

	if(con.verbose >= 1) printf("Opening configuration file %s...\n", configuration_file.c_str());
	std::ifstream cfile(configuration_file);
	if(not cfile.is_open()){
		printf("Could not open file \"%s\"\n", configuration_file.c_str());
		exit(EXIT_FAILURE);
	}

	if(con.verbose >= 1){
		printf("DONE: configuration file opened.\n");
		printf("Reading configuration...\n");
	}
	if(Configurations::read_configurations(con, cfile) == FileCode::ReadError){
		printf("Error reading configuration from file %s\n", configuration_file.c_str());
		cfile.close();
		exit(EXIT_FAILURE);
	}
	if(con.verbose >= 1) printf("DONE: Configuration read.\n");

	cfile.close();
	if(temp_seed != std::numeric_limits<size_t>::max()){
		con.seed = temp_seed;
	}
}


#include <include/common.hpp>

#include <include/options.hpp>

#include <include/prob.hpp>
#include <include/util.hpp>

#include <vector>
#include <iostream>
#include <random>

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

// random number generator object for std random library
std::mt19937 gen;

// probability of equality of two calls of the SAME reference position
double p_equal_calls = 0.0;
double q_equal_calls = 0.0;

// probability of 'sequenced position' which represents the probability of a read
// starting at a given position of the reference
double p_read_start = 0.0;

// geometric distribution for inter-read distance
std::geometric_distribution<> geom;

// lookup tables for probability of the form
// p^x * (1-p)^{m-s}
double *power_peq_lookup = NULL; // p^i
double *power_qeq_lookup = NULL; // q^i = (1-p)^i

/* lookup tables for the approximated expected score defined as:
                         4^s * tildeI(s, peq)
     tildeE(A,B,s) =  ----------------------------
                      4^s * tildeI(s, peq) + N - 1
 */
double *approxExpScoreNum = NULL;
double *approxExpScoreDen = NULL;

void initProbabilities() {

    std::random_device rd;
    gen = std::mt19937(rd());

    // convenience variables
    double p_err = Options::opts.pe;
    double q_err = 1.0 - p_err;

    // init p_equal_calls and q_equal_call
    p_equal_calls = q_err * q_err + (1.0 / 3.0) * (p_err * p_err);
    q_equal_calls = (1.0 - p_equal_calls) / 3.0;

    // init p_read_start
    p_read_start = (double) Options::opts.M / (double) Options::opts.N;

    // init online inter-read distance distribution (i.e., geomtric)
    geom = std::geometric_distribution<>(p_read_start);

    // lookup tables
    int m = Options::opts.m;
    int N = Options::opts.N;
    power_peq_lookup = new double[m + 1];
    power_qeq_lookup = new double[m + 1];
    approxExpScoreNum = new double[m + 1];
    approxExpScoreDen = new double[m + 1];
    for (int s = 0; s <= m; ++s) {
        power_peq_lookup[s] = pow(p_equal_calls, s);
        power_qeq_lookup[s] = pow(q_equal_calls, s);
        double tmp = p_equal_calls * p_equal_calls + q_equal_calls * (1 - p_equal_calls);
        double tildeI = pow(tmp, s);
        approxExpScoreNum[s] = pow(4, s) * tildeI;
        approxExpScoreDen[s] = (pow(4, s) * tildeI) + N - 1;

    }
}

void clearProbabilities() {
    delete[] approxExpScoreNum;
    delete[] approxExpScoreDen;
    delete[] power_qeq_lookup;
    delete[] power_peq_lookup;

}

double indicatorErr(const std::string &r1, const std::string &r2, size_t s) {
    if (s > 0) {
        size_t hamm_d = prefixSuffixHammingDistance(r1, r2, s);
        return (power_peq_lookup[s - hamm_d] * power_qeq_lookup[hamm_d]);
    }
    return 1.0;

}

double indicatorNoErr(const std::string &r1, const std::string &r2, size_t s) {
    return (prefixSuffixHammingDistance(r1, r2, s) == 0);
}

double probabilityReads(const std::string &s1, const std::string &s2, size_t d) {

    // probability of D = d distance between reads
    double p_s = p_read_start * pow(1.0 - p_read_start, d);
    // no overlap
    if (d >= Options::opts.m) {
        return pow(4, 2 * Options::opts.m) * p_s;
    }

    size_t dh = prefixSuffixHammingDistance(s1, s2, Options::opts.m - d);
    // probability of dh unequal bases on the common part
    double p_err = pow(p_equal_calls, (Options::opts.m - d - dh)) * pow(1.0 - p_equal_calls, dh);
    // probability of indipendent parts
    double p_ind = pow(4, 2 * d);

    return p_s * p_err * p_ind;
}

/**
 * Compute
 *  \sum_{s}{I(A,B,s)4^s}
 */
double overlappingStringsSum(const std::string &s1, const std::string &s2) {
    size_t m = Options::opts.m;
    double sum = 0.0;
    for (size_t s = 1; s <= m - 1; ++s) {
        size_t indicator_ab = (prefixSuffixHammingDistance(s2, s1, s) == 0) ? 1 : 0;
        size_t indicator_ba = (prefixSuffixHammingDistance(s1, s2, s) == 0) ? 1 : 0;
        sum += power4_lookup[s] * (indicator_ab + indicator_ba);
    }
    size_t indic_m = (prefixSuffixHammingDistance(s2, s1, m) == 0) ? 1 : 0;
    sum += indic_m * power4_lookup[m];
    return sum;
}

double overlappingStringsSumWithErr(const std::string &s1, const std::string &s2) {
    double sum = 0.0;
    size_t m = Options::opts.m;
    for (size_t s = 1; s <= m - 1; ++s) {
        // WARNING: If needed here we can use lookup tables to make things faster
        double tab = indicatorErr(s1, s2, s);
        double tba = indicatorErr(s2, s1, s);
        sum += power4_lookup[s] * (tab + tba);
    }
    sum += power4_lookup[m] * indicatorErr(s1, s2, m);
    return sum;
}

/**
 * Computes probability of a random overlap of 's' bases with an hamming distance dh
 */
double randomReadsOverlapProbNoErr(const std::string &s1, const std::string &s2, size_t s) {
    size_t dh_for_s = prefixSuffixHammingDistance(s1, s2, s);
    if (dh_for_s != 0) {
        return 0;
    }
    double olap_p = overlappingStringsSum(s1, s2) + (double) Options::opts.N - 2 * (double) Options::opts.m + 1.0;
    return (pow(4, s) / (olap_p));
}

/**
 * Gets the distance until the next read
 */
size_t generateInterReadDistance() {
    return geom(gen);
}

EmpiricalDistribution::EmpiricalDistribution(double a, double b, size_t N)
        : f(N + 1, 0.0) {
    this->xa = a;
    this->xb = b;
    this->n = N + 1;
    this->step = (b - a) / (double) N;
    this->total = 0;
}

size_t EmpiricalDistribution::indexForSample(double x) const {
    return floor((x - xa) / step);
}

double EmpiricalDistribution::valueAtIndex(size_t i) const {
    return (f[i] / (double) total);
}

void EmpiricalDistribution::addSample(double x) {
    total++;
    size_t i = indexForSample(x);
    f[i] += 1;
}

void EmpiricalDistribution::getCDF(std::vector<double> &cdf) const {
    if (this->n == 0) {
        return;
    }
    cdf.reserve(this->n);
    cdf[0] = this->f[0];
    for (size_t i = 1; i < this->n; ++i) {
        cdf[i] = cdf[i - 1] + f[i];
    }
    // normalize between 0 and 1
    for (size_t i = 0; i < this->n; ++i) {
        cdf[i] /= cdf[this->n - 1];
    }
}

size_t percentileIndex(const std::vector<double> &cdf, double perc) {
    size_t idx = 0;
    size_t n = cdf.size();
    for (size_t i = 0; i < n; ++i) {
        if (cdf[i] >= perc) {
            break;
        }
        ++idx;
    }
    return idx;
}

double approximatedScore(size_t s, double *num_den) {
    num_den[0] = approxExpScoreNum[s];
    num_den[1] = approxExpScoreDen[s];
    return approxExpScoreNum[s] / approxExpScoreDen[s];
}

double approximatedScore(size_t s) {
    return approxExpScoreNum[s] / approxExpScoreDen[s];
}

double score(const std::string &r1, const std::string &r2, size_t s) {

    static double iidTerm = power4_lookup[0] *
                            ((double) Options::opts.N - 2.0 * (double) Options::opts.m + 1.0);

    double den = iidTerm + overlappingStringsSumWithErr(r1, r2);
    double num = indicatorErr(r1, r2, s) * power4_lookup[s];

    return num / den;
}

double scoreExt(const std::string &r1, const std::string &r2, size_t s, double *num_den) {

    static double iidTerm = power4_lookup[0] *
                            ((double) Options::opts.N - 2.0 * (double) Options::opts.m + 1.0);

    num_den[0] = iidTerm + overlappingStringsSumWithErr(r1, r2);
    num_den[1] = indicatorErr(r1, r2, s) * power4_lookup[s];

    return num_den[1] / num_den[0];
}

namespace lbio {
namespace prob {

//////////////////////////////////////////////////////////////////////
//               SamplingEstimationProcess CLASS
//////////////////////////////////////////////////////////////////////

SamplingEstimationProcess::SamplingEstimationProcess(size_t n_)
        : n{n_}, cumulativeSum{0}, cumulativeSumSquare{0}, k{0} {
    this->frequency = new size_t[n + 1];
    std::fill_n(this->frequency, n + 1, 0);
}

SamplingEstimationProcess::~SamplingEstimationProcess() {
    delete[] frequency;
}

void
SamplingEstimationProcess::newSample(size_t sample) {
    this->frequency[sample]++;
    this->cumulativeSum += sample;
    this->cumulativeSumSquare += (sample * sample);
    this->k++;
}

double
SamplingEstimationProcess::sampleMean() const {
    return ((double) cumulativeSum) / ((double) k);
}

double
SamplingEstimationProcess::sampleVariance() const {
    double sMean = sampleMean();
    double meanTerm = ((double) k) * sMean * sMean;
    return ((this->cumulativeSumSquare - meanTerm) / ((double) k - 1));
}

double
SamplingEstimationProcess::standardError() const {
    return std::sqrt(sampleMean() / sampleVariance());
}

size_t
SamplingEstimationProcess::medianForSampleDistribution() const {
    return medianFromFrequency<size_t>(frequency, n + 1);
}

size_t
SamplingEstimationProcess::sampleSize() const {
    return k;
}

void
SamplingEstimationProcess::writeFrequencyOnFile(const std::string &path) {
    std::ofstream os(path, std::ofstream::out);
    writeVectorOnStream<size_t>(frequency, n + 1, os);
    os.close();
}

SampleEstimates
SamplingEstimationProcess::toSampleEstimates() const {
    SampleEstimates est;
    est.sampleSize = k;
    est.sampleMean = sampleMean();
    est.sampleVariance = sampleVariance();
    return est;
}

}
} // namespaces


// libgp - Gaussian process library for Machine Learning
// Copyright (c) 2013, Manuel Blum <mblum@informatik.uni-freiburg.de>
// All rights reserved.

#include "gp.h"
#include "gp_utils.h"

#include <Eigen/Dense>
#include "cg.h"
#include "rprop.h"

#include <fstream>
using namespace libgp;

const int feature_dim = 17;
const int feature_start = 0;
const int feature_end = 15;
const int feature_used = 16;
const int y_dim = 6;

int train_index = 3;

const int feature_num_limit = 3000;
void normalize(std::vector<double> &feature) {
  if (feature.size() < feature_used) return;
  double sum = 0;
  for (int i = 0; i < feature_used; i++) {
    sum += feature[i];
  }
  if (sum < 0.000001) return;
  for (int i = 0; i < feature_used; i++) {
    feature[i] /= sum;
  }
}

void readFeature(std::string path, std::vector<std::vector<double>> &features) {
  std::ifstream fs_(path);
  std::vector<double> feature(feature_used + y_dim);

  double temp;
  int input_i = 0;
  int need_i = 0;
  while (fs_ >> temp) {
    if (input_i >= feature_start && input_i <= feature_end) {
      feature[need_i++] = temp;
    } else if (input_i >= feature_dim) {
      feature[need_i++] = temp;
    }
    input_i++;
    if (need_i >= feature_used + y_dim) {
      // normalize(feature);
      features.push_back(feature);
      input_i = 0;
      need_i = 0;
    }
  }
}

std::vector<std::vector<double>> randomSampleFeatures(
    const std::vector<std::vector<double>> &features, const int sample_limit = 2000) {
  int total_num = features.size();
  if (total_num < sample_limit * 1.05) return features;

  std::vector<std::vector<double>> result;
  for (const auto &feature : features) {
    if ((rand() % total_num) < sample_limit) result.push_back(feature);
  }
  return result;
}

int main(int argc, char const *argv[]) {
  std::string featurepath;
  if (argc == 2) {
    featurepath = argv[1];
  } else if (argc == 3) {
    featurepath = argv[1];
    train_index = std::stoi(argv[2]);
  }
  std::cout << train_index << " " << featurepath << std::endl;
  std::vector<std::vector<double>> features;
  readFeature(featurepath, features);
  std::cout << features.at(3).at(feature_used + train_index) << std::endl;
  // initialize Gaussian process for 2-D input using the squared exponential
  // covariance function with additive white noise.
  GaussianProcess gp(feature_used, "CovSum ( CovSEard, CovNoise)");
  // initialize hyper parameter vector
  Eigen::VectorXd params(gp.covf().get_param_dim());
  params.setOnes();

  // set parameters of covariance function
  gp.covf().set_loghyper(params);
  auto rand_features = randomSampleFeatures(features, feature_num_limit);
  // add training patterns
  for (int i = 2; i < rand_features.size(); ++i) {
    gp.add_pattern(rand_features.at(i).data(), rand_features.at(i).at(feature_used + train_index));
  }

  libgp::RProp rprop;
  rprop.init();
  rprop.maximize(&gp, 50, true);
  // gp.log_likelihood();
  gp.write("model.txt");
  // total squared error
  double tss = 0;
  std::ofstream fs_output("output.txt");
  Eigen::VectorXd gt(features.size());
  Eigen::VectorXd estimate(features.size());

  gt.setZero();
  estimate.setZero();

  for (int i = 2; i < features.size(); ++i) {
    double f = gp.f(features.at(i).data());
    double error = f - features.at(i).at(feature_used + train_index);
    estimate(i) = f;
    gt(i) = features.at(i).at(feature_used + train_index);

    double var = std::sqrt(gp.var(features.at(i).data()));
    fs_output << f << " " << features.at(i).at(feature_used + train_index) << " " << error << " "
              << var << std::endl;
    tss += error * error;
    // tss += std::fabs(error);
  }
  estimate.normalize();
  gt.normalize();
  std::cout << "estimate dot gt:" << estimate.transpose() * gt << std::endl;
  std::cout << "mse = " << std::sqrt(tss / (features.size() - 2)) << std::endl;
  return EXIT_SUCCESS;
}