/* -*- Mode: C++; indent-tabs-mode: nil; c-basic-offset: 2 -*- */
// This file is a part of ChASE.
// Copyright (c) 2015-2021, Simulation and Data Laboratory Quantum Materials,
//   Forschungszentrum Juelich GmbH, Germany. All rights reserved.
// License is 3-clause BSD:
// https://github.com/ChASE-library/ChASE

#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <vector>
#include <cmath>
#include <time.h>
#include <iomanip>
#include <chrono>
#include <cstdlib>

#include "algorithm/performance.hpp"
#include "ChASE-MPI/chase_mpi.hpp"

#include "ChASE-MPI/impl/chase_mpidla_blaslapack_seq.hpp"
#include "ChASE-MPI/impl/chase_mpidla_blaslapack_seq_inplace.hpp"
#include "ChASE-MPI/impl/chase_mpidla_blaslapack.hpp"

#ifdef DRIVER_BUILD_MGPU
#include "ChASE-MPI/impl/chase_mpidla_mgpu.hpp"
#endif

#include "scalapack_templates.hpp"

const int i_zero = 0, i_one = 1;
const std::size_t sze_one = 1;

typedef std::size_t DESC[ 9 ];

namespace po = boost::program_options;
namespace bf = boost::filesystem;

using namespace::chase;
using namespace::chase::mpi;

template <typename T>
std::vector<T> generateEyeMat(std::size_t N){
  int rank;
  MPI_Comm_rank(MPI_COMM_WORLD, &rank);
  if (rank == 0) std::cout << "]> Generating an Identity matrix as S" << std::endl;

  std::vector<T> S(N * N, T(0.0));
  for (std::size_t i = 0; i < N; i++) {
    S[i + i * N] = 1.0;
  }

  return S;
}

template <typename T>
std::vector<T> generateClementMat(std::size_t N){
  int rank;
  MPI_Comm_rank(MPI_COMM_WORLD, &rank);
  if (rank == 0) std::cout << "]> Generating a Clement matrix as H" << std::endl;

  std::vector<T> C(N * N, T(0.0));
  for (auto i = 0; i < N; ++i) {
    C[i + N * i] = 0;
    if (i != N - 1) C[i + 1 + N * i] = std::sqrt(i * (N + 1 - i));
    if (i != N - 1) C[i + N * (i + 1)] = std::sqrt(i * (N + 1 - i));
  }

  return C;
}

//write eigenvalues: path_out/eigenvalues_index.bin
//write eigenvetors: path_out/eigenvectors_index.bin
template <typename T>
void writeMatrix(T *H, std::string path_out, std::string prefix,
		       std::size_t index, std::string suffix, std::size_t size)
{
  std::ostringstream problem(std::ostringstream::ate);
  problem << path_out << prefix << "_" << index << suffix;

  std::cout << "]> writing ";
  std::cout << prefix << " of size into ";
  std::cout << problem.str() << std::endl;

  auto outfile = std::fstream(problem.str().c_str(), std::ios::out | std::ios::binary);

  outfile.write((char*)&H[0], size * sizeof(T));

  outfile.close();

}

//read eigenvalues: path_in/eigenvalues_index.bin
//read eigenvetors: path_in/eigenvectors_index.bin
template <typename T>
void readMatrix(T *H, std::string path_in, std::string prefix,
                       std::size_t index, std::string suffix, std::size_t size)
{
  std::ostringstream problem(std::ostringstream::ate);
  problem << path_in << prefix << "_" << index << suffix;

  std::cout << "]> reading ";
  std::cout << prefix << " of size from ";
  std::cout << problem.str() << std::endl;

  auto infile = std::fstream(problem.str().c_str(), std::ios::binary);

  infile.write((char*)&H[0], size * sizeof(T));

  infile.close();

}

template <typename T>
void readMatrix(T* H, std::string path_in, std::string prefix,
                std::size_t index, std::string suffix, std::size_t size,
		std::size_t m, std::size_t mblocks, std::size_t nblocks,
            	std::size_t* r_offs, std::size_t* r_lens, std::size_t* r_offs_l,
		std::size_t* c_offs, std::size_t* c_lens, std::size_t* c_offs_l)
{

  std::size_t N = std::sqrt(size);
  std::ostringstream problem(std::ostringstream::ate);

  int rank;
  MPI_Comm_rank(MPI_COMM_WORLD, &rank);
  problem << path_in << prefix << "  1 " << std::setw(2) << index << suffix;
  if (rank == 0) std::cout << "]> Loading " << problem.str() << std::endl;

  std::size_t file_size = bf::file_size(problem.str().c_str());

  try{
    if(size * sizeof(T) != file_size ){
      throw std::logic_error(std::string("The given file : ") +
                               problem.str() + std::string(" of size ") + std::to_string(file_size) +
                               std::string(" doesn't equals to the required size of matrix of size ") + std::to_string(size * sizeof(T)));
    }
  }
  catch(std::exception &e)
  {
    std::cerr << "Caught " << typeid( e ).name( ) << " : "<< e.what( ) << std::endl;
    return ;
  }

  std::ifstream input(problem.str().c_str(), std::ios::binary);

  if (!input.is_open()) {
    throw new std::logic_error(std::string("error reading file: ") +
                               problem.str());
  }

  for(std::size_t j = 0; j < nblocks; j++){
      for(std::size_t i = 0; i < mblocks; i++){
          for(std::size_t q = 0; q < c_lens[j]; q++){
	      input.seekg(((q + c_offs[j]) * N + r_offs[i])* sizeof(T));
	      input.read(reinterpret_cast<char*>(H + (q + c_offs_l[j]) * m + r_offs_l[i]), r_lens[i] * sizeof(T));
	  }
      }
  }
}

using T = std::complex<double>;

#ifdef DRIVER_BUILD_MGPU
typedef ChaseMpi<ChaseMpiDLAMultiGPU, T> CHASE;
#else
typedef ChaseMpi<ChaseMpiDLABlaslapack, T> CHASE;
#endif

struct ChASE_DriverProblemConfig {
  std::size_t N;    // Size of the Matrix
  std::size_t nev;  // Number of sought after eigenvalues
  std::size_t nex;  // Extra size of subspace
  std::size_t deg;  // initial degree
  std::size_t bgn;  // beginning of sequence
  std::size_t end;  // end of sequence

  double tol;     // desired tolerance
  bool sequence;  // handle this as a sequence?

  std::string path_in;    // path to the matrix input files
  std::string mode;       // Approx or Random mode
  std::string opt;        // enable optimisation of degree
  std::string arch;       // ??
  std::string path_eigp;  // TODO
  std::string path_out;
  std::string path_name;

  bool complex;
  bool isdouble;

  std::size_t mbsize;
  std::size_t nbsize;
  int dim0;
  int dim1;
  int irsrc;
  int icsrc;
  std::string major;
};

template <typename T>
int do_chase_gev(ChASE_DriverProblemConfig& conf) {

  T alpha = 0.5;
  std::size_t N = conf.N;
  std::size_t nev = conf.nev;
  std::size_t nex = conf.nex;
  std::size_t deg = conf.deg;
  std::size_t bgn = conf.bgn;
  std::size_t end = conf.end;

  double tol = conf.tol;
  bool sequence = conf.sequence;

  std::string path_in = conf.path_in;
  std::string mode = conf.mode;
  std::string opt = conf.opt;
  std::string arch;
  std::string path_eigp = conf.path_eigp;
  std::string path_out = conf.path_out;
  std::string path_name = conf.path_name;

  std::size_t mbsize = conf.mbsize;
  std::size_t nbsize = conf.nbsize;
  int dim0 = conf.dim0;
  int dim1 = conf.dim1;
  int irsrc = conf.irsrc;
  int icsrc = conf.irsrc;
  std::string major = conf.major;

  if(dim0 == 0 || dim1 == 0){
    int dims[2];
    dims[0] = dims[1] = 0;
    int gsize;
    MPI_Comm_size(MPI_COMM_WORLD, &gsize);
    MPI_Dims_create(gsize, 2, dims);
    dim0 = dims[0];
    dim1 = dims[1];
  }

  int rank, size;

  MPI_Comm_rank(MPI_COMM_WORLD, &rank);
  MPI_Comm_size(MPI_COMM_WORLD, &size);

  std::cout << std::setprecision(16);

  if (rank == 0){
    std::cout << "\nChASE example driver for a sequence of Generalized "
	<<"Eigenproblems."
	<< std::endl;
  }

  // Timing variables
  std::chrono::high_resolution_clock::time_point st, ed, st_loc;
  std::size_t prb_nb = end - bgn + 1;
  std::chrono::duration<double> elapsed[6][prb_nb];

  //Scalapack part
  //Initalize Scalapack environment
  int myproc, nprocs;
  blacs_pinfo( &myproc, &nprocs );

  int ictxt;
  int val;
  blacs_get( &ictxt, &i_zero, &val );

  blacs_gridinit( &ictxt, major.at(0), &dim0, &dim1 );
  int myrow, mycol;
  blacs_gridinfo( &ictxt, &dim0, &dim1, &myrow, &mycol);

  //get local size of matrix = N_loc_r x N_loc_c
  std::size_t N_loc_r, N_loc_c;

  N_loc_r = numroc( &N, &mbsize, &myrow, &irsrc, &dim0 );
  N_loc_c = numroc( &N, &nbsize, &mycol, &icsrc, &dim1 );

  //for column major matrix, the leading dimension
  std::size_t lld_loc = std::max(N_loc_r, (std::size_t)1);

  //construct scalapack matrix descriptor
  DESC   desc;
  int    info;

  descinit( desc, &N, &N, &mbsize, &nbsize, &irsrc, &irsrc, &ictxt, &lld_loc, &info );

  //ChASE part
  //eigenpairs of standard eigenproblem
  auto V__ = std::unique_ptr<T[]>(new T[N * (nev + nex)]);
  auto Lambda__ = std::unique_ptr<Base<T>[]>(new Base<T>[(nev + nex)]);

  T* V = V__.get();
  Base<T>* Lambda = Lambda__.get();

  //Setup ChASE environment for a standard eigenproblem
  CHASE single(new ChaseMpiProperties<T>(N, mbsize, nbsize, nev, nex, dim0,
	dim1, const_cast<char*>(major.c_str()), irsrc, icsrc, MPI_COMM_WORLD), V, Lambda);

  ChaseConfig<T>& config = single.GetConfig();
  config.SetTol(tol);
  config.SetDeg(deg);
  config.SetOpt(opt == "S");
  config.SetMaxIter(100);

  if (rank == 0)
    std::cout << "\n"
              << config;

  std::mt19937 gen(1337.0);
  std::normal_distribution<> d;

  T* matrix = single.GetMatrixPtr();

  // Using ChASE-MPI functionalities to get some additional information
  // on the block cyclic data layout which faciliates the implementation
  //local block number = mblocks x nblocks
  std::size_t mblocks = single.get_mblocks();
  std::size_t nblocks = single.get_nblocks();

  //local matrix size = m x n
  std::size_t m = single.get_m(); // should = N_loc_r
  std::size_t n = single.get_n(); // should = N_loc_c

  //global and local offset/length of each block of block-cyclic data
  std::size_t *r_offs, *c_offs, *r_lens, *c_lens, *r_offs_l, *c_offs_l;

  single.get_offs_lens(r_offs, r_lens, r_offs_l, c_offs, c_lens, c_offs_l);

  Base<T> scale; //for t_psy(he)gst in Scalapack

  // GEV: H * X = LAMBDA * S * X, in which H and S are local matrices
  T *H = new T [N_loc_r * N_loc_c];
  T *S = new T [N_loc_r * N_loc_c];

  //sequence of problem
  for(auto idx = bgn; idx <= end; ++idx){

    if (rank == 0) {
      std::cout << "]> Starting Problem #" << idx << "\n";
    }

    st = std::chrono::high_resolution_clock::now();
    st_loc = std::chrono::high_resolution_clock::now();

    // if the path of matrices are empty, generate H as a Clement matrix
    // and generate S as an Identity matrix
    if(path_in.empty())
    {
      //Generate Clement matrix
      std::vector<T> HH = generateClementMat<T>(N);
      //Generate an Identify matrix
      std::vector<T> SS = generateEyeMat<T>(N);
      //redistribute into HH and SS into block cyclic layout
      for(std::size_t j = 0; j < nblocks; j++){
        for(std::size_t i = 0; i < mblocks; i++){
          for(std::size_t q = 0; q < c_lens[j]; q++){
	    for(std::size_t p = 0; p < r_lens[i]; p++){
	      H[(q + c_offs_l[j]) * m + p + r_offs_l[i]] = HH[(q + c_offs[j]) * N + p + r_offs[i]];
              S[(q + c_offs_l[j]) * m + p + r_offs_l[i]] = SS[(q + c_offs[j]) * N + p + r_offs[i]];
	    }
	  }
        }
      }
    }else //if path of matrices are given, load them into H and S by parallel IO
    {
      //read matrix H from local
      readMatrix(H, path_in, "hmat", idx, ".bin", N * N, m, mblocks, nblocks,
         r_offs, r_lens, r_offs_l, c_offs, c_lens, c_offs_l);
      //read matrix S from local
      readMatrix(S, path_in, "smat", idx, ".bin", N * N, m, mblocks, nblocks,
         r_offs, r_lens, r_offs_l, c_offs, c_lens, c_offs_l);
    }

    ed = std::chrono::high_resolution_clock::now();
    elapsed[0][idx - 1] = std::chrono::duration_cast<std::chrono::duration<double>>(ed - st_loc);

    st_loc = std::chrono::high_resolution_clock::now();

    // Transform to standard problem using SCALAPACK
    // Cholesky Factorization of S = L * L^T, S is overwritten by L
    t_ppotrf<T>('U', N, S, sze_one, sze_one, desc);

    ed = std::chrono::high_resolution_clock::now();
    elapsed[1][idx - 1] = std::chrono::duration_cast<std::chrono::duration<double>>(ed - st_loc);

    st_loc = std::chrono::high_resolution_clock::now();

    // Reduce H * X = eig * S ( X to the standard from H' * X' = eig * X'
    // with H' = L^{-1} * H * (L^T)^{-1}
    t_psyhegst<T>(i_one, 'U', N, H, sze_one, sze_one, desc, S, sze_one, sze_one, desc, &scale);

    // ensure H' to be exact Symmetric/Hermtian by H' = 0.5*H'+0.5*transpose(H')
    T *tmpH = new T [m * n];
    std::memcpy(tmpH, H, m * n * sizeof(T));
    t_geadd<T>('T', N, N, alpha, tmpH, sze_one, sze_one, desc, alpha, H, sze_one, sze_one, desc);

    ed = std::chrono::high_resolution_clock::now();
    elapsed[2][idx - 1] = std::chrono::duration_cast<std::chrono::duration<double>>(ed - st_loc);

    // Copy H into single.matrix()
    std::memcpy(matrix, H, m * n * sizeof(T));

    // for the first problem, generate randomly the initial guess of V
    if (idx == bgn) {
      config.SetApprox(false);
      //random generated initial guess of V
      for (std::size_t i = 0; i < N * (nev + nex); ++i) {
        V[i] = T(d(gen), d(gen));
      }
    }else{
      //use the eigenpairs from last problem
      config.SetApprox(true);
    }

    PerformanceDecoratorChase<T> performanceDecorator(&single);

    MPI_Barrier(MPI_COMM_WORLD);

    st_loc = std::chrono::high_resolution_clock::now();

    // ChASE to solve the standard eigenproblem H' * X' = eig * X'
    chase::Solve(&performanceDecorator);

    MPI_Barrier(MPI_COMM_WORLD);

    ed = std::chrono::high_resolution_clock::now();
    elapsed[3][idx - 1] = std::chrono::duration_cast<std::chrono::duration<double>>(ed - st_loc);

    if (rank == 0) {
      std::cout << "\n]> Output of ChASE for Problem #" << idx << "\n";
      performanceDecorator.GetPerfData().print();
      Base<T>* resid = single.GetResid();
      std::cout << "]> Printing first 5 eigenvalues and residuals\n";
      std::cout
          << "| Index |       Eigenvalue      |         Residual      |\n"
          << "|-------|-----------------------|-----------------------|\n";
      std::size_t width = 20;
      std::cout << std::setprecision(12);
      std::cout << std::setfill(' ');
      std::cout << std::scientific;
      std::cout << std::right;
      for (auto i = 0; i < std::min(std::size_t(5), nev); ++i)
        std::cout << "|  " << std::setw(4) << i + 1 << " | " << std::setw(width)
                  << Lambda[i] << "  | " << std::setw(width) << resid[i]
                  << "  |\n";
      std::cout << "\n";
    }

    //Scalapack part
    //In ChASE, the eigenvectors V is stored rebundantly on each proc
    //In order to recover the generalized eigenvectors by Scalapack, it should be
    //redistributed into block-cyclic format. We re-use H to restore V.
    //this part is in parallel implicitly

    st_loc = std::chrono::high_resolution_clock::now();

    for(std::size_t j = 0; j < nblocks; j++){
      for(std::size_t i = 0; i < mblocks; i++){
        for(std::size_t q = 0; q < c_lens[j]; q++){
	  for(std::size_t p = 0; p < r_lens[i]; p++){
	    if((q + c_offs[j]) * N + p + r_offs[i] < (nev + nex) * N){
	      H[(q + c_offs_l[j]) * m + p + r_offs_l[i]] = V[(q + c_offs[j]) * N + p + r_offs[i]];
	    }
	  }
	}
      }
    }

    //Now the first (nev+nex) columns of H (a global view) is overwritten by V
    //Recover the genealized eigenvectors X by solving X' = L^T * X

    t_ptrtrs<T>('U','N','N', N, nev + nex, S, sze_one, sze_one, desc, H, sze_one, sze_one, desc);

    ed = std::chrono::high_resolution_clock::now();
    elapsed[4][idx - 1] = std::chrono::duration_cast<std::chrono::duration<double>>(ed - st_loc);
    elapsed[5][idx - 1] = std::chrono::duration_cast<std::chrono::duration<double>>(ed - st);

    //copy V_dist -> V;
    //restore eigenvectors from scalapack form to ChASE form

    if (rank == 0) {
      std::cout << "]> Finished Problem #" << idx << "\n";
      std::cout << "**********************"  << std::endl;
    }
  }

  MPI_Barrier(MPI_COMM_WORLD);

  if(rank == 0){
    std::cout << "\nSUMMARY : Time (s)" << "\n";
    std::cout
          << "---------------------------------------------------------------------------------------------------------------------------------------------------------------\n"
          << "|  Index|             Parallel IO|  Cholesky Factorization|    Transfer to Standard|            ChASE Solver|          Back Transform|                     All|\n"
          << "|-------|------------------------|------------------------|------------------------|------------------------|------------------------|------------------------|";
    std::cout << std::endl;

    std::cout << std::setprecision(6);
    std::cout << std::setfill(' ');
    std::cout << std::scientific;
    for(auto idx = bgn - 1; idx < end; ++idx){
      std::cout << "|" << std::setw(7) << idx + 1 << "|";
      for(auto i = 0; i < 6; i++){
	std::cout << std::setw(24) << elapsed[i][idx].count() << "|";
      }
      std::cout << std::endl;
    }

    std::cout
            << "---------------------------------------------------------------------------------------------------------------------------------------------------------------"
	    << std::endl;
  }

  return 0;
}

int main(int argc, char* argv[]) {

  MPI_Init(&argc, &argv);

  ChASE_DriverProblemConfig conf;

  po::options_description desc("ChASE Options");

  desc.add_options()(                                                     //
      "help,h",                                                           //
      "show this message"                                                 //
      )(                                                                  //
      "n", po::value<std::size_t>(&conf.N)->required(),                   //
      "Size of the Input Matrix"                                          //
      )(                                                                  //
      "double", po::value<bool>(&conf.isdouble)->default_value(true),     //
      "Is matrix complex double valued, false indicates the single type"  //
      )(                                                                  //
      "complex", po::value<bool>(&conf.complex)->default_value(true),     //
      "Matrix is complex valued"                                          //
      )(                                                                  //
      "nev", po::value<std::size_t>(&conf.nev)->required(),               //
      "Wanted Number of Eigenpairs"                                       //
      )(                                                                  //
      "nex", po::value<std::size_t>(&conf.nex)->default_value(25),        //
      "Extra Search Dimensions"                                           //
      )(                                                                  //
      "deg", po::value<std::size_t>(&conf.deg)->default_value(20),        //
      "Initial filtering degree"                                          //
      )(                                                                  //
      "bgn", po::value<std::size_t>(&conf.bgn)->default_value(1),         //
      "Start ell"                                                         //
      )(                                                                  //
      "end", po::value<std::size_t>(&conf.end)->default_value(1),         //
      "End ell"                                                           //
      )(                                                                  //
      "tol", po::value<double>(&conf.tol)->default_value(1e-10),          //
      "Tolerance for Eigenpair convergence"                               //
      )(                                                                  //
      "path_in", po::value<std::string>(&conf.path_in)->default_value(""),//
      "Path to the input matrix/matrices"                                 //
      )(                                                                  //
      "mode", po::value<std::string>(&conf.mode)->default_value("A"),     //
      "valid values are R(andom) or A(pproximate)"                        //
      )(                                                                  //
      "opt", po::value<std::string>(&conf.opt)->default_value("S"),       //
      "Optimi(S)e degree, or do (N)ot optimise"                           //
      )(                                                                  //
      "path_eigp", po::value<std::string>(&conf.path_eigp),               //
      "Path to approximate solutions, only required when mode"            //
      "is Approximate, otherwise not used"                                //
      )(                                                                  //
      "sequence", po::value<bool>(&conf.sequence)->default_value(false),  //
      "Treat as sequence of Problems. Previous ChASE solution is used,"   //
      "when available"                                                    //
      )(                                                                  //
      "mbsize", po::value<std::size_t>(&conf.mbsize)->default_value(50),  //
      "block size for the row"                                            //
      )(                                                                  //
      "nbsize", po::value<std::size_t>(&conf.nbsize)->default_value(50),  //
      "block size for the column"                                         //
      )(                                                                  //
      "dim0", po::value<int>(&conf.dim0)->default_value(0),               //
      "row number of MPI proc grid"                                       //
      )(                                                                  //
      "dim1", po::value<int>(&conf.dim1)->default_value(0),               //
      "column number of MPI proc grid"                                    //
      )(								  //
      "irsrc", po::value<int>(&conf.irsrc)->default_value(0),             //
      "The process row over which the first row of matrix is"             //
      "distributed."                                                      //
      )(                                                                  //
      "icsrc", po::value<int>(&conf.icsrc)->default_value(0),             //
      "The process column over which the first column of the array A is"  //
      "distributed."                                                      //
      )(                                                                  //
      "major", po::value<std::string>(&conf.major)->default_value("C"),    //
      "Major of MPI proc grid, valid values are R(ow) or C(olumn)"        //
      );

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);

  // print help
  if (vm.count("help")) {
    std::cout << desc << std::endl;
    return 1;
  }

  po::notify(vm);
  conf.mode = toupper(conf.mode.at(0));
  conf.opt = toupper(conf.opt.at(0));

  if (conf.bgn > conf.end) {
    std::cout << "Begin must be smaller than End!" << std::endl;
    return -1;
  }

  if (conf.mode != "R" && conf.mode != "A") {
    std::cout << "Illegal value for mode: \"" << conf.mode << "\"" << std::endl
              << "Legal values are R or A" << std::endl;
    return -1;
  }

  if (conf.opt != "N" && conf.opt != "S") {
    std::cout << "Illegal value for opt: " << conf.opt << std::endl
              << "Legal values are N, S" << std::endl;
    return -1;
  }

  if (conf.path_eigp.empty() && conf.mode == "A") {
    std::cout << "eigp is required when mode is " << conf.mode << std::endl;
    return -1;
  }

  if (conf.isdouble) {
    do_chase_gev<std::complex<double>>(conf);
  } else {
    std::cout << "single not implemented\n";
  }

  MPI_Finalize();

}


//  Copyright John Maddock 2007.
//  Copyright Paul A. Bristow 2007
//  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_STATS_PARETO_HPP
#define BOOST_STATS_PARETO_HPP

// http://en.wikipedia.org/wiki/Pareto_distribution
// http://www.itl.nist.gov/div898/handbook/eda/section3/eda3661.htm
// Also:
// Weisstein, Eric W. "Pareto Distribution."
// From MathWorld--A Wolfram Web Resource.
// http://mathworld.wolfram.com/ParetoDistribution.html

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/distributions/complement.hpp>
#include <boost/math/distributions/detail/common_error_handling.hpp>
#include <boost/math/special_functions/powm1.hpp>

#include <utility> // for BOOST_CURRENT_VALUE?

namespace boost
{
  namespace math
  {
    namespace detail
    { // Parameter checking.
      template <class RealType, class Policy>
      inline bool check_pareto_location(
        const char* function,
        RealType location,
        RealType* result, const Policy& pol)
      {
        if((boost::math::isfinite)(location))
        { // any > 0 finite value is OK.
          if (location > 0)
          {
            return true;
          }
          else
          {
            *result = policies::raise_domain_error<RealType>(
              function,
              "Location parameter is %1%, but must be > 0!", location, pol);
            return false;
          }
        }
        else
        { // Not finite.
          *result = policies::raise_domain_error<RealType>(
            function,
            "Location parameter is %1%, but must be finite!", location, pol);
          return false;
        }
      } // bool check_pareto_location

      template <class RealType, class Policy>
      inline bool check_pareto_shape(
        const char* function,
        RealType shape,
        RealType* result, const Policy& pol)
      {
        if((boost::math::isfinite)(shape))
        { // Any finite value > 0 is OK.
          if (shape > 0)
          {
            return true;
          }
          else
          {
            *result = policies::raise_domain_error<RealType>(
              function,
              "Shape parameter is %1%, but must be > 0!", shape, pol);
            return false;
          }
        }
        else
        { // Not finite.
          *result = policies::raise_domain_error<RealType>(
            function,
            "Shape parameter is %1%, but must be finite!", shape, pol);
          return false;
        }
      } // bool check_pareto_shape(

      template <class RealType, class Policy>
      inline bool check_pareto_x(
        const char* function,
        RealType const& x,
        RealType* result, const Policy& pol)
      {
        if((boost::math::isfinite)(x))
        { //
          if (x > 0)
          {
            return true;
          }
          else
          {
            *result = policies::raise_domain_error<RealType>(
              function,
              "x parameter is %1%, but must be > 0 !", x, pol);
            return false;
          }
        }
        else
        { // Not finite..
          *result = policies::raise_domain_error<RealType>(
            function,
            "x parameter is %1%, but must be finite!", x, pol);
          return false;
        }
      } // bool check_pareto_x

      template <class RealType, class Policy>
      inline bool check_pareto( // distribution parameters.
        const char* function,
        RealType location,
        RealType shape,
        RealType* result, const Policy& pol)
      {
        return check_pareto_location(function, location, result, pol)
           && check_pareto_shape(function, shape, result, pol);
      } // bool check_pareto(

    } // namespace detail

    template <class RealType = double, class Policy = policies::policy<> >
    class pareto_distribution
    {
    public:
      typedef RealType value_type;
      typedef Policy policy_type;

      pareto_distribution(RealType location = 1, RealType shape = 1)
        : m_location(location), m_shape(shape)
      { // Constructor.
        RealType result;
        detail::check_pareto("boost::math::pareto_distribution<%1%>::pareto_distribution", location, shape, &result, Policy());
      }

      RealType location()const
      { // AKA Xm and b
        return m_location;
      }

      RealType shape()const
      { // AKA k and a
        return m_shape;
      }
    private:
      // Data members:
      RealType m_location;  // distribution location (xm)
      RealType m_shape;  // distribution shape (k)
    };

    typedef pareto_distribution<double> pareto; // Convenience to allow pareto(2., 3.);

    template <class RealType, class Policy>
    inline const std::pair<RealType, RealType> range(const pareto_distribution<RealType, Policy>& /*dist*/)
    { // Range of permissible values for random variable x.
      using boost::math::tools::max_value;
      return std::pair<RealType, RealType>(0, max_value<RealType>()); // location zero to + infinity.
    } // range

    template <class RealType, class Policy>
    inline const std::pair<RealType, RealType> support(const pareto_distribution<RealType, Policy>& dist)
    { // Range of supported values for random variable x.
      // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
      using boost::math::tools::max_value;
      return std::pair<RealType, RealType>(dist.location(), max_value<RealType>() ); // location to + infinity.
    } // support

    template <class RealType, class Policy>
    inline RealType pdf(const pareto_distribution<RealType, Policy>& dist, const RealType& x)
    {
      BOOST_MATH_STD_USING  // for ADL of std function pow.
      static const char* function = "boost::math::pdf(const pareto_distribution<%1%>&, %1%)";
      RealType location = dist.location();
      RealType shape = dist.shape();
      RealType result;
      if(false == (detail::check_pareto_x(function, x, &result, Policy())
         && detail::check_pareto(function, location, shape, &result, Policy())))
         return result;
      if (x < location)
      { // regardless of shape, pdf is zero.
        return 0;
      }

      result = shape * pow(location, shape) / pow(x, shape+1);
      return result;
    } // pdf

    template <class RealType, class Policy>
    inline RealType cdf(const pareto_distribution<RealType, Policy>& dist, const RealType& x)
    {
      BOOST_MATH_STD_USING  // for ADL of std function pow.
      static const char* function = "boost::math::cdf(const pareto_distribution<%1%>&, %1%)";
      RealType location = dist.location();
      RealType shape = dist.shape();
      RealType result;

      if(false == (detail::check_pareto_x(function, x, &result, Policy())
         && detail::check_pareto(function, location, shape, &result, Policy())))
         return result;

      if (x <= location)
      { // regardless of shape, cdf is zero.
        return 0;
      }

      // result = RealType(1) - pow((location / x), shape);
      result = -boost::math::powm1(location/x, shape, Policy()); // should be more accurate.
      return result;
    } // cdf

    template <class RealType, class Policy>
    inline RealType quantile(const pareto_distribution<RealType, Policy>& dist, const RealType& p)
    {
      BOOST_MATH_STD_USING  // for ADL of std function pow.
      static const char* function = "boost::math::quantile(const pareto_distribution<%1%>&, %1%)";
      RealType result;
      RealType location = dist.location();
      RealType shape = dist.shape();
      if(false == (detail::check_probability(function, p, &result, Policy())
           && detail::check_pareto(function, location, shape, &result, Policy())))
      {
        return result;
      }
      if (p == 0)
      {
        return location; // x must be location (or less).
      }
      if (p == 1)
      {
        return tools::max_value<RealType>(); // x = + infinity.
      }
      result = location /
        (pow((1 - p), 1 / shape));
      // K. Krishnamoorthy,  ISBN 1-58488-635-8 eq 23.1.3
      return result;
    } // quantile

    template <class RealType, class Policy>
    inline RealType cdf(const complemented2_type<pareto_distribution<RealType, Policy>, RealType>& c)
    {
       BOOST_MATH_STD_USING  // for ADL of std function pow.
       static const char* function = "boost::math::cdf(const pareto_distribution<%1%>&, %1%)";
       RealType result;
       RealType x = c.param;
       RealType location = c.dist.location();
       RealType shape = c.dist.shape();
       if(false == (detail::check_pareto_x(function, x, &result, Policy())
           && detail::check_pareto(function, location, shape, &result, Policy())))
         return result;

       if (x <= location)
       { // regardless of shape, cdf is zero, and complement is unity.
         return 1;
       }
       result = pow((location/x), shape);

       return result;
    } // cdf complement

    template <class RealType, class Policy>
    inline RealType quantile(const complemented2_type<pareto_distribution<RealType, Policy>, RealType>& c)
    {
      BOOST_MATH_STD_USING  // for ADL of std function pow.
      static const char* function = "boost::math::quantile(const pareto_distribution<%1%>&, %1%)";
      RealType result;
      RealType q = c.param;
      RealType location = c.dist.location();
      RealType shape = c.dist.shape();
      if(false == (detail::check_probability(function, q, &result, Policy())
           && detail::check_pareto(function, location, shape, &result, Policy())))
      {
        return result;
      }
      if (q == 1)
      {
        return location; // x must be location (or less).
      }
      if (q == 0)
      {
        return tools::max_value<RealType>(); // x = + infinity.
      }
      result = location / (pow(q, 1 / shape));
      // K. Krishnamoorthy,  ISBN 1-58488-635-8 eq 23.1.3
      return result;
    } // quantile complement

    template <class RealType, class Policy>
    inline RealType mean(const pareto_distribution<RealType, Policy>& dist)
    {
      RealType result;
      static const char* function = "boost::math::mean(const pareto_distribution<%1%>&, %1%)";
      if(false == detail::check_pareto(function, dist.location(), dist.shape(), &result, Policy()))
      {
        return result;
      }
      if (dist.shape() > RealType(1))
      {
        return dist.shape() * dist.location() / (dist.shape() - 1);
      }
      else
      {
        using boost::math::tools::max_value;
        return max_value<RealType>(); // +infinity.
      }
    } // mean

    template <class RealType, class Policy>
    inline RealType mode(const pareto_distribution<RealType, Policy>& dist)
    {
      return dist.location();
    } // mode

    template <class RealType, class Policy>
    inline RealType median(const pareto_distribution<RealType, Policy>& dist)
    {
      RealType result;
      static const char* function = "boost::math::median(const pareto_distribution<%1%>&, %1%)";
      if(false == detail::check_pareto(function, dist.location(), dist.shape(), &result, Policy()))
      {
        return result;
      }
      BOOST_MATH_STD_USING
      return dist.location() * pow(RealType(2), (1/dist.shape()));
    } // median

    template <class RealType, class Policy>
    inline RealType variance(const pareto_distribution<RealType, Policy>& dist)
    {
      RealType result;
      RealType location = dist.location();
      RealType shape = dist.shape();
      static const char* function = "boost::math::variance(const pareto_distribution<%1%>&, %1%)";
      if(false == detail::check_pareto(function, location, shape, &result, Policy()))
      {
        return result;
      }
      if (shape > 2)
      {
        result = (location * location * shape) /
         ((shape - 1) *  (shape - 1) * (shape - 2));
      }
      else
      {
        result = policies::raise_domain_error<RealType>(
          function,
          "variance is undefined for shape <= 2, but got %1%.", dist.shape(), Policy());
      }
      return result;
    } // variance

    template <class RealType, class Policy>
    inline RealType skewness(const pareto_distribution<RealType, Policy>& dist)
    {
      BOOST_MATH_STD_USING
      RealType result;
      RealType shape = dist.shape();
      static const char* function = "boost::math::pdf(const pareto_distribution<%1%>&, %1%)";
      if(false == detail::check_pareto(function, dist.location(), shape, &result, Policy()))
      {
        return result;
      }
      if (shape > 3)
      {
        result = sqrt((shape - 2) / shape) *
          2 * (shape + 1) /
          (shape - 3);
      }
      else
      {
        result = policies::raise_domain_error<RealType>(
          function,
          "skewness is undefined for shape <= 3, but got %1%.", dist.shape(), Policy());
      }
      return result;
    } // skewness

    template <class RealType, class Policy>
    inline RealType kurtosis(const pareto_distribution<RealType, Policy>& dist)
    {
      RealType result;
      RealType shape = dist.shape();
      static const char* function = "boost::math::pdf(const pareto_distribution<%1%>&, %1%)";
      if(false == detail::check_pareto(function, dist.location(), shape, &result, Policy()))
      {
        return result;
      }
      if (shape > 4)
      {
        result = 3 * ((shape - 2) * (3 * shape * shape + shape + 2)) /
          (shape * (shape - 3) * (shape - 4));
      }
      else
      {
        result = policies::raise_domain_error<RealType>(
          function,
          "kurtosis_excess is undefined for shape <= 4, but got %1%.", shape, Policy());
      }
      return result;
    } // kurtosis

    template <class RealType, class Policy>
    inline RealType kurtosis_excess(const pareto_distribution<RealType, Policy>& dist)
    {
      RealType result;
      RealType shape = dist.shape();
      static const char* function = "boost::math::pdf(const pareto_distribution<%1%>&, %1%)";
      if(false == detail::check_pareto(function, dist.location(), shape, &result, Policy()))
      {
        return result;
      }
      if (shape > 4)
      {
        result = 6 * ((shape * shape * shape) + (shape * shape) - 6 * shape - 2) /
          (shape * (shape - 3) * (shape - 4));
      }
      else
      {
        result = policies::raise_domain_error<RealType>(
          function,
          "kurtosis_excess is undefined for shape <= 4, but got %1%.", dist.shape(), Policy());
      }
      return result;
    } // kurtosis_excess

    } // namespace math
  } // namespace boost

  // This include must be at the end, *after* the accessors
  // for this distribution have been defined, in order to
  // keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_STATS_PARETO_HPP


/********************************************************************************
 *
 * This file is part of the Geneva library collection. The following license
 * applies to this file:
 *
 * ------------------------------------------------------------------------------
 * 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.
 * ------------------------------------------------------------------------------
 *
 * Note that other files in the Geneva library collection may use a different
 * license. Please see the licensing information in each file.
 *
 ********************************************************************************
 *
 * Geneva was started by Dr. Rüdiger Berlich and was later maintained together
 * with Dr. Ariel Garcia under the auspices of Gemfony scientific. For further
 * information on Gemfony scientific, see http://www.gemfomy.eu .
 *
 * The majority of files in Geneva was released under the Apache license v2.0
 * in February 2020.
 *
 * See the NOTICE file in the top-level directory of the Geneva library
 * collection for a list of contributors and copyright information.
 *
 ********************************************************************************/

#pragma once

// Global checks, defines and includes needed for all of Geneva
#include "common/GGlobalDefines.hpp"

// Standard header files go here
#include <iostream>
#include <cmath>
#include <sstream>
#include <vector>
#include <tuple>
#include <type_traits>

// Boost header files go here
#include <boost/math/constants/constants.hpp>

// Geneva header files go here
#include "common/GParserBuilder.hpp"
#include "common/GCommonMathHelperFunctions.hpp"
#include "hap/GRandomT.hpp"
#include "geneva/GDoubleCollection.hpp"
#include "geneva/GConstrainedDoubleCollection.hpp"
#include "geneva/GDoubleObjectCollection.hpp"
#include "geneva/GConstrainedDoubleObjectCollection.hpp"
#include "geneva/GConstrainedDoubleObject.hpp"
#include "geneva/GDoubleGaussAdaptor.hpp"
#include "geneva/GDoubleBiGaussAdaptor.hpp"
#include "geneva/GParameterSet.hpp"
#include "geneva/GParameterSetMultiConstraint.hpp"
#include "geneva/GParameterSetFactory.hpp"

namespace Gem {
namespace Geneva {

/******************************************************************************/
////////////////////////////////////////////////////////////////////////////////
/******************************************************************************/
/**
 * This enum denotes the possible demo function types
 */
enum class solverFunction : Gem::Common::ENUMBASETYPE {
	 PARABOLA = 0,
	 NOISYPARABOLA = 1,
	 ROSENBROCK = 2,
	 ACKLEY = 3,
	 RASTRIGIN = 4,
	 SCHWEFEL = 5,
	 SALOMON = 6,
	 NEGPARABOLA = 7
};

const solverFunction MAXDEMOFUNCTION = solverFunction::NEGPARABOLA;

// Make sure solverFunction can be streamed
/** @brief Puts a Gem::Geneva::solverFunction into a stream. Needed also for boost::lexical_cast<> */
G_API_INDIVIDUALS std::ostream &operator<<(std::ostream &, const Gem::Geneva::solverFunction &);

/** @brief Reads a Gem::Geneva::solverFunction from a stream. Needed also for boost::lexical_cast<> */
G_API_INDIVIDUALS std::istream &operator>>(std::istream &, Gem::Geneva::solverFunction &);

/**
 * This enum describes different parameter types that may be used to fill the object with data
 */
enum class parameterType : Gem::Common::ENUMBASETYPE {
	 USEGDOUBLECOLLECTION = 0,
	 USEGCONSTRAINEDOUBLECOLLECTION = 1,
	 USEGDOUBLEOBJECTCOLLECTION = 2,
	 USEGCONSTRAINEDDOUBLEOBJECTCOLLECTION = 3,
	 USEGCONSTRAINEDDOUBLEOBJECT = 4
};

// Make sure parameterType can be streamed
/** @brief Puts a Gem::Geneva::parameterType into a stream. Needed also for boost::lexical_cast<> */
G_API_INDIVIDUALS std::ostream &operator<<(std::ostream &, const Gem::Geneva::parameterType &);

/** @brief Reads a Gem::Geneva::parameterType from a stream. Needed also for boost::lexical_cast<> */
G_API_INDIVIDUALS std::istream &operator>>(std::istream &, Gem::Geneva::parameterType &);

/**
 * This enum describes several ways of initializing the data collections
 */
enum class initMode : Gem::Common::ENUMBASETYPE {
	 INITRANDOM = 0 // random values for all variables
	 , INITPERIMETER = 1 // Uses a parameter set on the perimeter of the allowed or common value range
};

// Make sure initMode can be streamed
/** @brief Puts a Gem::Geneva::initMode into a stream. Needed also for boost::lexical_cast<> */
G_API_INDIVIDUALS std::ostream &operator<<(std::ostream &, const Gem::Geneva::initMode &);

/** @brief Reads a Gem::Geneva::initMode from a stream. Needed also for boost::lexical_cast<> */
G_API_INDIVIDUALS std::istream &operator>>(std::istream &, Gem::Geneva::initMode &);

/******************************************************************************/
// A number of default settings for the factory
const double GFI_DEF_ADPROB = 1.0;
const double GFI_DEF_ADAPTADPROB = 0.1;
const double GFI_DEF_MINADPROB = 0.05;
const double GFI_DEF_MAXADPROB = 1.;
const std::uint32_t GFI_DEF_ADAPTIONTHRESHOLD = 1;
const bool GFI_DEF_USEBIGAUSSIAN = false;
const double GFI_DEF_SIGMA1 = 0.025;
const double GFI_DEF_SIGMASIGMA1 = 0.2;
const double GFI_DEF_MINSIGMA1 = 0.001;
const double GFI_DEF_MAXSIGMA1 = 1;
const double GFI_DEF_SIGMA2 = 0.025;
const double GFI_DEF_SIGMASIGMA2 = 0.2;
const double GFI_DEF_MINSIGMA2 = 0.001;
const double GFI_DEF_MAXSIGMA2 = 1;
const double GFI_DEF_DELTA = 0.05;
const double GFI_DEF_SIGMADELTA = 0.2;
const double GFI_DEF_MINDELTA = 0.001;
const double GFI_DEF_MAXDELTA = 1.;
const std::size_t GFI_DEF_PARDIM = 2;
const double GFI_DEF_MINVAR = -10.;
const double GFI_DEF_MAXVAR = 10.;
const bool GFI_DEF_USECONSTRAINEDDOUBLECOLLECTION = false;
const parameterType GFI_DEF_PARAMETERTYPE = parameterType::USEGCONSTRAINEDDOUBLEOBJECT;
const initMode GFI_DEF_INITMODE = initMode::INITPERIMETER;
const solverFunction GO_DEF_EVALFUNCTION = solverFunction::PARABOLA;
const double GFI_DEF_CROSSOVERPROB = 0.5;

/******************************************************************************/
// Forward declaraion
class GFunctionIndividualFactory;

/******************************************************************************/
/**
 * This individual searches for a minimum of a number of predefined functions, each capable
 * of processing their input in multiple dimensions.
 */
class GFunctionIndividual : public GParameterSet
{
	 ///////////////////////////////////////////////////////////////////////
	 friend class boost::serialization::access;

	 template<class Archive>
	 void serialize(Archive &ar, const unsigned int) {
		 ar
		 & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GParameterSet)
		 & BOOST_SERIALIZATION_NVP(demoFunction_);
	 }

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

public:
	 using FACTORYTYPE = GFunctionIndividualFactory;

	 /** @brief The default constructor */
	 G_API_INDIVIDUALS GFunctionIndividual() = default;
	 /** @brief Initialization with the desired demo function */
	 explicit G_API_INDIVIDUALS GFunctionIndividual(const solverFunction &);
	 /** @brief A standard copy constructor */
	 G_API_INDIVIDUALS GFunctionIndividual(const GFunctionIndividual& cp) = default;

	 /** @brief The standard destructor */
	 G_API_INDIVIDUALS ~GFunctionIndividual() override = default;

	 /** @brief Allows to set the demo function */
	 G_API_INDIVIDUALS void setDemoFunction(solverFunction);
	 /** @brief Allows to retrieve the current demo function */
	 G_API_INDIVIDUALS solverFunction getDemoFunction() const;

	 /** @brief Allows to cross check the parameter size */
	 G_API_INDIVIDUALS std::size_t getParameterSize() const;

	 //---------------------------------------------------------------------------
	 /**
	  * This function converts the function id to a string representation. This is a convenience
	  * function that is mostly used in GArgumentParser.cpp of various Geneva examples.
	  *
	  * @param df The id of the desired function individual
	  * @return A string representing the name of the current function
	  */
	 static G_API_INDIVIDUALS std::string getStringRepresentation(const solverFunction &df) {
		 std::string result;

		 // Set up a single function individual, depending on the expected function type
		 switch (df) {
			 case solverFunction::PARABOLA:
				 result = "Parabola";
				 break;
			 case solverFunction::NOISYPARABOLA:
				 result = "Berlich noisy parabola";
				 break;
			 case solverFunction::ROSENBROCK:
				 result = "Rosenbrock";
				 break;
			 case solverFunction::ACKLEY:
				 result = "Ackley";
				 break;
			 case solverFunction::RASTRIGIN:
				 result = "Rastrigin";
				 break;
			 case solverFunction::SCHWEFEL:
				 result = "Schwefel";
				 break;
			 case solverFunction::SALOMON:
				 result = "Salomon";
				 break;
			 case solverFunction::NEGPARABOLA:
				 result = "Negative parabola";
				 break;
		 }

		 return result;
	 }

	 //---------------------------------------------------------------------------
	 /**
	  * Retrieves a string in ROOT format (see http://root.cern.ch) of the 2D version of a
	  * given function.
	  *
	  * @param df The id of the desired function individual
	  * @return A string suitable for plotting a 2D version of this function with the ROOT analysis framework
	  */
	 static G_API_INDIVIDUALS std::string get2DROOTFunction(const solverFunction &df) {
		 std::string result;

		 // Set up a single function individual, depending on the expected function type
		 switch (df) {
			 case solverFunction::PARABOLA:
				 result = "x^2 + y^2";
				 break;
			 case solverFunction::NOISYPARABOLA:
				 result = "(cos(x^2 + y^2) + 2.) * (x^2 + y^2)";
				 break;
			 case solverFunction::ROSENBROCK:
				 result = "100.*(x^2 - y)^2 + (1 - x)^2";
				 break;
			 case solverFunction::ACKLEY:
				 result = "exp(-0.2)*sqrt(x^2 + y^2) + 3.*(cos(2.*x) + sin(2.*y))";
				 break;
			 case solverFunction::RASTRIGIN:
				 result = "20.+(x^2 - 10.*cos(2*pi*x)) + (y^2 - 10.*cos(2*pi*y))";
				 break;
			 case solverFunction::SCHWEFEL:
				 result = "-0.5*(x*sin(sqrt(abs(x))) + y*sin(sqrt(abs(y))))";
				 break;
			 case solverFunction::SALOMON:
				 result = "-cos(2.*pi*sqrt(x^2 + y^2)) + 0.1*sqrt(x^2 + y^2) + 1.";
				 break;
			 case solverFunction::NEGPARABOLA:
				 result = "-(x^2 + y^2)";
				 break;
		 }

		 return result;
	 }

	 //---------------------------------------------------------------------------
	 /**
	  * Retrieves the minimum x-value(s) of a given (2D) demo function
	  *
	  * @param df The id of the desired function individual
	  * @return The x-coordinate(s) of the global optimium in 2D
	  */
	 static G_API_INDIVIDUALS std::vector<double> getXMin(const solverFunction &df) {
		 std::vector<double> result;

		 // Set up a single function individual, depending on the expected function type
		 switch (df) {
			 case solverFunction::PARABOLA:
				 result.push_back(0.);
				 break;
			 case solverFunction::NOISYPARABOLA:
				 result.push_back(0.);
				 break;
			 case solverFunction::ROSENBROCK:
				 result.push_back(1.);
				 break;
			 case solverFunction::ACKLEY:
				 // two global optima
				 result.push_back(-1.5096201);
				 result.push_back(1.5096201);
				 break;
			 case solverFunction::RASTRIGIN:
				 result.push_back(0.);
				 break;
			 case solverFunction::SCHWEFEL:
				 result.push_back(420.968746);
				 break;
			 case solverFunction::SALOMON:
				 result.push_back(0.);
				 break;
			 case solverFunction::NEGPARABOLA:
				 result.push_back(0.);
				 break;
		 }

		 return result;
	 }

	 //---------------------------------------------------------------------------
	 /**
	  * Retrieves the minimum y-value(s) of a given (2D) demo function
	  *
	  * @param df The id of the desired function individual
	  * @return The y-coordinate(s) of the global optimium in 2D
	  */
	 static G_API_INDIVIDUALS std::vector<double> getYMin(const solverFunction &df) {
		 std::vector<double> result;

		 // Set up a single function individual, depending on the expected function type
		 switch (df) {
			 case solverFunction::PARABOLA:
				 result.push_back(0.);
				 break;
			 case solverFunction::NOISYPARABOLA:
				 result.push_back(0.);
				 break;
			 case solverFunction::ROSENBROCK:
				 result.push_back(1.);
				 break;
			 case solverFunction::ACKLEY:
				 result.push_back(-0.7548651);
				 break;
			 case solverFunction::RASTRIGIN:
				 result.push_back(0.);
				 break;
			 case solverFunction::SCHWEFEL:
				 result.push_back(420.968746);
				 break;
			 case solverFunction::SALOMON:
				 result.push_back(0.);
				 break;
			 case solverFunction::NEGPARABOLA:
				 result.push_back(0.);
				 break;
		 }

		 return result;
	 }

protected:
	 //---------------------------------------------------------------------------
	 /** @brief Adds local configuration options to a GParserBuilder object */
	 G_API_INDIVIDUALS void addConfigurationOptions_(Gem::Common::GParserBuilder &) override;
	 /** @brief Loads the data of another GFunctionIndividual */
	 G_API_INDIVIDUALS void load_(const GObject *) final;

	/** @brief Allow access to this classes compare_ function */
	friend void Gem::Common::compare_base_t<GFunctionIndividual>(
		GFunctionIndividual const &
		, GFunctionIndividual const &
		, Gem::Common::GToken &
	);

	/** @brief Searches for compliance with expectations with respect to another object of the same type */
	G_API_INDIVIDUALS void compare_(
		const GObject & // the other object
		, const Gem::Common::expectation & // the expectation for this object, e.g. equality
		, const double & // the limit for allowed deviations of floating point types
	) const final;

	 /** @brief The actual value calculation takes place here */
	 G_API_INDIVIDUALS double fitnessCalculation() final;

	 //---------------------------------------------------------------------------

	 /** @brief Applies modifications to this object. */
	 G_API_INDIVIDUALS bool modify_GUnitTests_() override;
	 /** @brief Performs self tests that are expected to succeed. */
	 G_API_INDIVIDUALS void specificTestsNoFailureExpected_GUnitTests_() override;
	 /** @brief Performs self tests that are expected to fail. */
	 G_API_INDIVIDUALS void specificTestsFailuresExpected_GUnitTests_() override;

private:
	 //---------------------------------------------------------------------------
	 /** @brief Creates a deep clone of this object */
	 G_API_INDIVIDUALS GObject *clone_() const final;

	 //---------------------------------------------------------------------------
	 // Data

	 solverFunction demoFunction_ = solverFunction::PARABOLA; ///< Specifies which demo function should be used
};

/******************************************************************************/
/**
 * Provide an easy way to print the individual's content
 */
G_API_INDIVIDUALS std::ostream &operator<<(std::ostream &, const Gem::Geneva::GFunctionIndividual &);

G_API_INDIVIDUALS std::ostream &operator<<(std::ostream &, std::shared_ptr <Gem::Geneva::GFunctionIndividual>);

/******************************************************************************/
////////////////////////////////////////////////////////////////////////////////
/******************************************************************************/
/**
 * A factory for GFunctionIndividual objects
 */
class GFunctionIndividualFactory
	: public GParameterSetFactory
{
	 ///////////////////////////////////////////////////////////////////////
	 friend class boost::serialization::access;

	 template<class Archive>
	 void serialize(Archive &ar, const unsigned int) {
		 ar
		 & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GParameterSetFactory)
		 & BOOST_SERIALIZATION_NVP(adProb_)
		 & BOOST_SERIALIZATION_NVP(adaptAdProb_)
		 & BOOST_SERIALIZATION_NVP(minAdProb_)
		 & BOOST_SERIALIZATION_NVP(maxAdProb_)
		 & BOOST_SERIALIZATION_NVP(adaptionThreshold_)
		 & BOOST_SERIALIZATION_NVP(useBiGaussian_)
		 & BOOST_SERIALIZATION_NVP(sigma1_)
		 & BOOST_SERIALIZATION_NVP(sigmaSigma1_)
		 & BOOST_SERIALIZATION_NVP(minSigma1_)
		 & BOOST_SERIALIZATION_NVP(maxSigma1_)
		 & BOOST_SERIALIZATION_NVP(sigma2_)
		 & BOOST_SERIALIZATION_NVP(sigmaSigma2_)
		 & BOOST_SERIALIZATION_NVP(minSigma2_)
		 & BOOST_SERIALIZATION_NVP(maxSigma2_)
		 & BOOST_SERIALIZATION_NVP(delta_)
		 & BOOST_SERIALIZATION_NVP(sigmaDelta_)
		 & BOOST_SERIALIZATION_NVP(minDelta_)
		 & BOOST_SERIALIZATION_NVP(maxDelta_)
		 & BOOST_SERIALIZATION_NVP(parDim_)
		 & BOOST_SERIALIZATION_NVP(minVar_)
		 & BOOST_SERIALIZATION_NVP(maxVar_)
		 & BOOST_SERIALIZATION_NVP(pT_)
		 & BOOST_SERIALIZATION_NVP(iM_);
	 }

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

public:
	 /** @brief The standard constructor */
	 explicit G_API_INDIVIDUALS GFunctionIndividualFactory(boost::filesystem::path const&);
	 /** @brief The copy constructor */
	 G_API_INDIVIDUALS GFunctionIndividualFactory(const GFunctionIndividualFactory &cp) = default;

	 /** @brief The destructor */
	 G_API_INDIVIDUALS ~GFunctionIndividualFactory() override = default;

	 //---------------------------------------------------------------------------
	 // Getters and setters

	 /** @brief Allows to retrieve the adaptionThreshold_ variable */
	 G_API_INDIVIDUALS std::uint32_t getAdaptionThreshold() const;
	 /** @brief Set the value of the adaptionThreshold_ variable */
	 G_API_INDIVIDUALS void setAdaptionThreshold(std::uint32_t adaptionThreshold);

	 /** @brief Allows to retrieve the adProb_ variable */
	 G_API_INDIVIDUALS double getAdProb() const;
	 /** @brief Set the value of the adProb_ variable */
	 G_API_INDIVIDUALS void setAdProb(double adProb);

	 /** @brief Allows to retrieve the iM_ variable */
	 G_API_INDIVIDUALS initMode getIM() const;
	 /** @brief Set the value of the iM_ variable */
	 G_API_INDIVIDUALS void setIM(initMode im);

	 /** @brief Allows to retrieve the parDim_ variable */
	 G_API_INDIVIDUALS std::size_t getParDim() const;
	 /** @brief (Re-)Set the dimension of the function */
	 G_API_INDIVIDUALS void setParDim(std::size_t);

	 /** @brief Allows to retrieve the pT_ variable */
	 G_API_INDIVIDUALS parameterType getPT() const;
	 /** @brief Set the value of the pT_ variable */
	 G_API_INDIVIDUALS void setPT(parameterType pt);

	 /** @brief Allows to retrieve the useBiGaussian_ variable */
	 G_API_INDIVIDUALS bool getUseBiGaussian() const;
	 /** @brief Set the value of the useBiGaussian_ variable */
	 G_API_INDIVIDUALS void setUseBiGaussian(bool useBiGaussian);

	 /** @brief Allows to retrieve the minVar_ variable */
	 G_API_INDIVIDUALS double getMinVar() const;
	 /** @brief Allows to retrieve the maxVar_ variable */
	 G_API_INDIVIDUALS double getMaxVar() const;
	 /** @brief Extract the minimum and maximum boundaries of the variables */
	 G_API_INDIVIDUALS std::tuple<double, double> getVarBoundaries() const;
	 /** @brief Set the minimum and maximum boundaries of the variables */
	 G_API_INDIVIDUALS void setVarBoundaries(std::tuple<double, double>);

	 /** @brief Allows to retrieve the delta_ variable */
	 G_API_INDIVIDUALS double getDelta() const;
	 /** @brief Set the value of the delta_ variable */
	 G_API_INDIVIDUALS void setDelta(double delta);
	 /** @brief Allows to retrieve the minDelta_ variable */
	 G_API_INDIVIDUALS double getMinDelta() const;
	 /** @brief Allows to retrieve the maxDelta_ variable */
	 G_API_INDIVIDUALS double getMaxDelta() const;
	 /** @brief Allows to retrieve the allowed value range of delta */
	 G_API_INDIVIDUALS std::tuple<double, double> getDeltaRange() const;
	 /** @brief Allows to set the allowed value range of delta */
	 G_API_INDIVIDUALS void setDeltaRange(std::tuple<double, double>);

	 /** @brief Allows to retrieve the minSigma1_ variable */
	 G_API_INDIVIDUALS double getMinSigma1() const;
	 /** @brief Allows to retrieve the maxSigma1_ variable */
	 G_API_INDIVIDUALS double getMaxSigma1() const;
	 /** @brief Allows to retrieve the allowed value range of sigma1_ */
	 G_API_INDIVIDUALS std::tuple<double, double> getSigma1Range() const;
	 /** @brief Allows to set the allowed value range of sigma1_ */
	 G_API_INDIVIDUALS void setSigma1Range(std::tuple<double, double>);

	 /** @brief Allows to retrieve the minSigma2_ variable */
	 G_API_INDIVIDUALS double getMinSigma2() const;
	 /** @brief Allows to retrieve the maxSigma2_ variable */
	 G_API_INDIVIDUALS double getMaxSigma2() const;
	 /** @brief Allows to retrieve the allowed value range of sigma2_ */
	 G_API_INDIVIDUALS std::tuple<double, double> getSigma2Range() const;
	 /** @brief Allows to set the allowed value range of sigma2_ */
	 G_API_INDIVIDUALS void setSigma2Range(std::tuple<double, double>);

	 /** @brief Allows to retrieve the sigma1_ variable */
	 G_API_INDIVIDUALS double getSigma1() const;
	 /** @brief Set the value of the sigma1_ variable */
	 G_API_INDIVIDUALS void setSigma1(double sigma1);

	 /** @brief Allows to retrieve the sigma2_ variable */
	 G_API_INDIVIDUALS double getSigma2() const;
	 /** @brief Set the value of the sigma2_ variable */
	 G_API_INDIVIDUALS void setSigma2(double sigma2);

	 /** @brief Allows to retrieve the sigmaDelta_ variable */
	 G_API_INDIVIDUALS double getSigmaDelta() const;
	 /** @brief Set the value of the sigmaDelta_ variable */
	 G_API_INDIVIDUALS void setSigmaDelta(double sigmaDelta);

	 /** @brief Allows to retrieve the sigmaSigma1_ variable */
	 G_API_INDIVIDUALS double getSigmaSigma1() const;
	 /** @brief Set the value of the sigmaSigma1_ variable */
	 G_API_INDIVIDUALS void setSigmaSigma1(double sigmaSigma1);

	 /** @brief Allows to retrieve the sigmaSigma2_ variable */
	 G_API_INDIVIDUALS double getSigmaSigma2() const;
	 /** @brief Set the value of the sigmaSigma2_ variable */
	 G_API_INDIVIDUALS void setSigmaSigma2(double sigmaSigma2);

	 /** @brief Allows to retrieve the rate of evolutionary adaption of adProb_ */
	 G_API_INDIVIDUALS double getAdaptAdProb() const;
	 /** @brief Allows to specify an adaption factor for adProb_ (or 0, if you do not want this feature) */
	 G_API_INDIVIDUALS void setAdaptAdProb(double adaptAdProb);

	 /** @brief Allows to retrieve the allowed range for adProb_ variation */
	 G_API_INDIVIDUALS std::tuple<double, double> getAdProbRange() const;
	 /** @brief Allows to set the allowed range for adaption probability variation */
	 G_API_INDIVIDUALS void setAdProbRange(double minAdProb, double maxAdProb);

	 // End of public getters and setters
	 //--------------------------------------------------------------------------

	 /** @brief Loads the data of another GFunctionIndividualFactory object */
	 G_API_INDIVIDUALS void load(std::shared_ptr <Gem::Common::GFactoryT<GParameterSet>>) override;
	 /** @brief Creates a deep clone of this object */
	 G_API_INDIVIDUALS std::shared_ptr <Gem::Common::GFactoryT<GParameterSet>> clone() const override;

protected:
	 /** @brief Allows to describe local configuration options in derived classes */
	 G_API_INDIVIDUALS void describeLocalOptions_(Gem::Common::GParserBuilder &) override;
	 /** @brief Allows to act on the configuration options received from the configuration file */
	 G_API_INDIVIDUALS void postProcess_(std::shared_ptr<GParameterSet> &) override;

private:
     /** @brief Creates individuals of this type */
     G_API_INDIVIDUALS std::shared_ptr <GParameterSet> getObject_(Gem::Common::GParserBuilder &, const std::size_t &) override;

	 /** @brief Set the value of the minVar_ variable */
	 void setMinVar(double minVar);

	 /** @brief Set the value of the maxVar_ variable */
	 void setMaxVar(double maxVar);

	 /** @brief Set the value of the minDelta_ variable */
	 void setMinDelta(double minDelta);

	 /** @brief Set the value of the maxDelta_ variable */
	 void setMaxDelta(double maxDelta);

	 /** @brief Set the value of the minSigma1_ variable */
	 void setMinSigma1(double minSigma1);

	 /** @brief Set the value of the maxSigma1_ variable */
	 void setMaxSigma1(double maxSigma1);

	 /** @brief Set the value of the minSigma2_ variable */
	 void setMinSigma2(double minSigma2);

	 /** @brief Set the value of the maxSigma2_ variable */
	 void setMaxSigma2(double maxSigma2);

	 /** @brief The default constructor; Only needed for (de-)serialization purposes. */
	 GFunctionIndividualFactory();

	 Gem::Common::GOneTimeRefParameterT<double> adProb_{GFI_DEF_ADPROB};
	 Gem::Common::GOneTimeRefParameterT<double> adaptAdProb_{GFI_DEF_ADAPTADPROB};
	 Gem::Common::GOneTimeRefParameterT<double> minAdProb_{GFI_DEF_MINADPROB};
	 Gem::Common::GOneTimeRefParameterT<double> maxAdProb_{GFI_DEF_MAXADPROB};
	 Gem::Common::GOneTimeRefParameterT<std::uint32_t> adaptionThreshold_{GFI_DEF_ADAPTIONTHRESHOLD};
	 Gem::Common::GOneTimeRefParameterT<bool> useBiGaussian_{GFI_DEF_USEBIGAUSSIAN};
	 Gem::Common::GOneTimeRefParameterT<double> sigma1_{GFI_DEF_SIGMA1};
	 Gem::Common::GOneTimeRefParameterT<double> sigmaSigma1_{GFI_DEF_SIGMASIGMA1};
	 Gem::Common::GOneTimeRefParameterT<double> minSigma1_{GFI_DEF_MINSIGMA1};
	 Gem::Common::GOneTimeRefParameterT<double> maxSigma1_{GFI_DEF_MAXSIGMA1};
	 Gem::Common::GOneTimeRefParameterT<double> sigma2_{GFI_DEF_SIGMA2};
	 Gem::Common::GOneTimeRefParameterT<double> sigmaSigma2_{GFI_DEF_SIGMASIGMA2};
	 Gem::Common::GOneTimeRefParameterT<double> minSigma2_{GFI_DEF_MINSIGMA2};
	 Gem::Common::GOneTimeRefParameterT<double> maxSigma2_{GFI_DEF_MAXSIGMA2};
	 Gem::Common::GOneTimeRefParameterT<double> delta_{GFI_DEF_DELTA};
	 Gem::Common::GOneTimeRefParameterT<double> sigmaDelta_{GFI_DEF_SIGMADELTA};
	 Gem::Common::GOneTimeRefParameterT<double> minDelta_{GFI_DEF_MINDELTA};
	 Gem::Common::GOneTimeRefParameterT<double> maxDelta_{GFI_DEF_MAXDELTA};
	 Gem::Common::GOneTimeRefParameterT<std::size_t> parDim_{GFI_DEF_PARDIM};
	 Gem::Common::GOneTimeRefParameterT<double> minVar_{GFI_DEF_MINVAR};
	 Gem::Common::GOneTimeRefParameterT<double> maxVar_{GFI_DEF_MAXVAR};
	 Gem::Common::GOneTimeRefParameterT<parameterType> pT_{GFI_DEF_PARAMETERTYPE};
	 Gem::Common::GOneTimeRefParameterT<initMode> iM_{GFI_DEF_INITMODE};
};

/******************************************************************************/
////////////////////////////////////////////////////////////////////////////////
/******************************************************************************/
/**
 * A simple constraint checker searching for valid solutions that fulfill
 * a given constraint. Here, the sum of all double variables needs to be smaller
 * than a given constant.
 */
class GDoubleSumConstraint : public GParameterSetConstraint {
	 ///////////////////////////////////////////////////////////////////////
	 friend class boost::serialization::access;

	 template<typename Archive>
	 void serialize(Archive &ar, const unsigned int) {
		 using boost::serialization::make_nvp;
		 ar
		 & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GParameterSetConstraint)
		 & BOOST_SERIALIZATION_NVP(C_);
	 }
	 ///////////////////////////////////////////////////////////////////////
public:

	 /** @brief The default constructor */
	 G_API_INDIVIDUALS GDoubleSumConstraint() = default;
	 /** @brief Initialization with the constant */
	 explicit G_API_INDIVIDUALS GDoubleSumConstraint(const double &);
	 /** @brief The copy constructor */
	 G_API_INDIVIDUALS GDoubleSumConstraint(const GDoubleSumConstraint &cp) = default;

	 /** @brief The destructor */
	 G_API_INDIVIDUALS ~GDoubleSumConstraint() override = default;

protected:
	 G_API_INDIVIDUALS double check_(const GParameterSet *) const override;

	 /** @brief Adds local configuration options to a GParserBuilder object */
	 G_API_INDIVIDUALS void addConfigurationOptions_(Gem::Common::GParserBuilder&) override;
	 /** @brief Loads the data of another GParameterSetMultiConstraint */
	 G_API_INDIVIDUALS void load_(const GObject *) override;

	/** @brief Allow access to this classes compare_ function */
	friend void Gem::Common::compare_base_t<GDoubleSumConstraint>(
		GDoubleSumConstraint const &
		, GDoubleSumConstraint const &
		, Gem::Common::GToken &
	);

	/** @brief Searches for compliance with expectations with respect to another object of the same type */
	G_API_INDIVIDUALS void compare_(
		const GObject & // the other object
		, const Gem::Common::expectation & // the expectation for this object, e.g. equality
		, const double & // the limit for allowed deviations of floating point types
	) const final;

private:
	 /** @brief Creates a deep clone of this object */
	 G_API_INDIVIDUALS GObject *clone_() const override;

	 double C_ = 1.; ///< The constant that should not be exceeded by the sum of parameters
};

/******************************************************************************/
////////////////////////////////////////////////////////////////////////////////
/******************************************************************************/
/**
 * A constraint checker trying to enforce a condition x+y+z=C (note the equal
 * sign!) for double variables
 */
class GDoubleSumGapConstraint : public GParameterSetConstraint {
	 ///////////////////////////////////////////////////////////////////////
	 friend class boost::serialization::access;

	 template<typename Archive>
	 void serialize(Archive &ar, const unsigned int) {
		 using boost::serialization::make_nvp;
		 ar
		 & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GParameterSetConstraint)
		 & BOOST_SERIALIZATION_NVP(C_)
		 & BOOST_SERIALIZATION_NVP(gap_);
	 }
	 ///////////////////////////////////////////////////////////////////////
public:

	 /** @brief The default constructor */
	 G_API_INDIVIDUALS GDoubleSumGapConstraint() = default;
	 /** @brief Initialization with the constant */
	 G_API_INDIVIDUALS GDoubleSumGapConstraint(const double &, const double &);
	 /** @brief The copy constructor */
	 G_API_INDIVIDUALS GDoubleSumGapConstraint(const GDoubleSumGapConstraint& cp) = default;

	 /** @brief The destructor */
	 G_API_INDIVIDUALS ~GDoubleSumGapConstraint() override = default;

protected:
	 G_API_INDIVIDUALS double check_(const GParameterSet *) const override;

	 /** @brief Adds local configuration options to a GParserBuilder object */
	 G_API_INDIVIDUALS void addConfigurationOptions_(Gem::Common::GParserBuilder &) override;
	 /** @brief Loads the data of another GParameterSetMultiConstraint */
	 G_API_INDIVIDUALS void load_(const GObject *) override;

	/** @brief Allow access to this classes compare_ function */
	friend void Gem::Common::compare_base_t<GDoubleSumGapConstraint>(
		GDoubleSumGapConstraint const &
		, GDoubleSumGapConstraint const &
		, Gem::Common::GToken &
	);

	/** @brief Searches for compliance with expectations with respect to another object of the same type */
	G_API_INDIVIDUALS void compare_(
		const GObject & // the other object
		, const Gem::Common::expectation & // the expectation for this object, e.g. equality
		, const double & // the limit for allowed deviations of floating point types
	) const final;

private:
	 /** @brief Creates a deep clone of this object */
	 G_API_INDIVIDUALS GObject *clone_() const override;

	 double C_ = 1.; ///< The constant that should not be exceeded by the sum of parameters
	 double gap_ = 0.5; ///< A tolerance around C_ that is still considered to be valid
};

/******************************************************************************/
////////////////////////////////////////////////////////////////////////////////
/******************************************************************************/
/**
 * A simple constraint checker searching for valid solutions that fulfill
 * a given constraint. Here, valid solutions lie in a sphere around 0
 */
class GSphereConstraint : public GParameterSetConstraint {
	 ///////////////////////////////////////////////////////////////////////
	 friend class boost::serialization::access;

	 template<typename Archive>
	 void serialize(Archive &ar, const unsigned int) {
		 using boost::serialization::make_nvp;
		 ar
		 &BOOST_SERIALIZATION_BASE_OBJECT_NVP(GParameterSetConstraint);
	 }
	 ///////////////////////////////////////////////////////////////////////
public:

	 /** @brief The default constructor */
	 G_API_INDIVIDUALS GSphereConstraint() = default;
	 /** @brief Initialization with the diameter */
	 explicit G_API_INDIVIDUALS GSphereConstraint(const double &cp);
	 /** @brief The copy constructor */
	 G_API_INDIVIDUALS GSphereConstraint(const GSphereConstraint &) = default;

	 /** @brief The destructor */
	 G_API_INDIVIDUALS ~GSphereConstraint() override = default;

protected:
	 G_API_INDIVIDUALS double check_(const GParameterSet *) const override;

	 /** @brief Adds local configuration options to a GParserBuilder object */
	 G_API_INDIVIDUALS void addConfigurationOptions_(Gem::Common::GParserBuilder &) override;
	 /** @brief Loads the data of another GParameterSetMultiConstraint */
	 G_API_INDIVIDUALS void load_(const GObject *) override;

	/** @brief Allow access to this classes compare_ function */
	friend void Gem::Common::compare_base_t<GSphereConstraint>(
		GSphereConstraint const &
		, GSphereConstraint const &
		, Gem::Common::GToken &
	);

	/** @brief Searches for compliance with expectations with respect to another object of the same type */
	G_API_INDIVIDUALS void compare_(
		const GObject & // the other object
		, const Gem::Common::expectation & // the expectation for this object, e.g. equality
		, const double & // the limit for allowed deviations of floating point types
	) const final;

private:
	 /** @brief Creates a deep clone of this object */
	 G_API_INDIVIDUALS GObject *clone_() const override;

	 /** @brief The diameter of the sphere */
	 double diameter_ = 1.;
};

/******************************************************************************/
////////////////////////////////////////////////////////////////////////////////
/******************************************************************************/
} /* namespace Geneva */
} /* namespace Gem */

BOOST_CLASS_EXPORT_KEY(Gem::Geneva::GFunctionIndividual)
BOOST_CLASS_EXPORT_KEY(Gem::Geneva::GFunctionIndividualFactory)
BOOST_CLASS_EXPORT_KEY(Gem::Geneva::GDoubleSumConstraint)
BOOST_CLASS_EXPORT_KEY(Gem::Geneva::GDoubleSumGapConstraint)
BOOST_CLASS_EXPORT_KEY(Gem::Geneva::GSphereConstraint)


// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <signal.h>
#include <iostream>
#include <iomanip>
#include <string>
#include <fstream>
#include <sstream>
#include <algorithm>
#include <cassert>
#include <sstream>
#include "g2o/apps/g2o_cli/dl_wrapper.h"
#include "g2o/apps/g2o_cli/output_helper.h"
#include "g2o/apps/g2o_cli/g2o_common.h"

#include "g2o/core/estimate_propagator.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/factory.h"
#include "g2o/core/optimization_algorithm_factory.h"
#include "g2o/core/hyper_dijkstra.h"

#include "g2o/stuff/macros.h"
#include "g2o/stuff/color_macros.h"
#include "g2o/stuff/command_args.h"
#include "g2o/stuff/filesys_tools.h"
#include "g2o/stuff/string_tools.h"
#include "g2o/stuff/timeutil.h"

#include "edge_labeler.h"
#include "edge_creator.h"
#include "star.h"

#include "g2o/stuff/unscented.h"
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace g2o;
using namespace Eigen;

typedef SigmaPoint<VectorXd> MySigmaPoint;

void testMarginals(SparseOptimizer& optimizer){
  cerr << "Projecting marginals" << endl;
  std::vector<std::pair<int, int> > blockIndices;
  for (size_t i=0; i<optimizer.activeVertices().size(); i++) {
    OptimizableGraph::Vertex* v=optimizer.activeVertices()[i];
    if (v->hessianIndex()>=0){
      blockIndices.push_back(make_pair(v->hessianIndex(), v->hessianIndex()));
    }
    // if (v->hessianIndex()>0){
    //   blockIndices.push_back(make_pair(v->hessianIndex()-1, v->hessianIndex()));
    // }
  }
  SparseBlockMatrix<MatrixXd> spinv;
  if (optimizer.computeMarginals(spinv, blockIndices)) {
    for (size_t i=0; i<optimizer.activeVertices().size(); i++) {
      OptimizableGraph::Vertex* v=optimizer.activeVertices()[i];
      cerr << "Vertex id:" << v->id() << endl;
      if (v->hessianIndex()>=0){
        cerr << "increments block :" << v->hessianIndex() << ", " << v->hessianIndex()<< " covariance:" <<  endl;
        VectorXd mean(v->minimalEstimateDimension()); //HACK: need to set identity
        mean.fill(0);
        VectorXd oldMean(v->minimalEstimateDimension()); //HACK: need to set identity
        v->getMinimalEstimateData(&oldMean[0]);
        MatrixXd& cov= *(spinv.block(v->hessianIndex(), v->hessianIndex()));
        std::vector<MySigmaPoint, Eigen::aligned_allocator<MySigmaPoint> > spts;
        cerr << cov << endl;
        if (! sampleUnscented(spts,mean,cov) )
          continue;

        // now apply the oplus operator to the sigma points,
        // and get the points in the global space
        std::vector<MySigmaPoint, Eigen::aligned_allocator<MySigmaPoint> > tspts = spts;

        for (size_t j=0; j<spts.size(); j++) {
          v->push();
          // cerr << "v_before [" << j << "]" << endl;
          v->getMinimalEstimateData(&mean[0]);
          // cerr << mean << endl;
          // cerr << "sigma [" << j << "]" << endl;
          // cerr << spts[j]._sample << endl;
          v->oplus(&(spts[j]._sample[0]));
          v->getMinimalEstimateData(&mean[0]);
          tspts[j]._sample=mean;
          // cerr << "oplus [" << j << "]" << endl;
          // cerr << tspts[j]._sample << endl;
          v->pop();
        }
        MatrixXd cov2=cov;
        reconstructGaussian(mean, cov2, tspts);
        cerr << "global block :" << v->hessianIndex() << ", " << v->hessianIndex()<< endl;
        cerr << "mean: " << endl;
        cerr <<  mean << endl;
        cerr << "oldMean: " << endl;
        cerr <<  oldMean << endl;
        cerr << "cov: " << endl;
        cerr << cov2 << endl;

      }
      // if (v->hessianIndex()>0){
      //   cerr << "inv block :" << v->hessianIndex()-1 << ", " << v->hessianIndex()<< endl;
      //   cerr << *(spinv.block(v->hessianIndex()-1, v->hessianIndex()));
      //   cerr << endl;
      // }
    }
  }
}

int unscentedTest(){
  MatrixXd m=MatrixXd(6,6);
  for (int i=0; i<6; i++){
    for (int j=i; j<6; j++){
      m(i,j)=m(j,i)=i*j+1;
    }
  }
  m+=MatrixXd::Identity(6,6);
  cerr << m;
  VectorXd mean(6);
  mean.fill(1);

  std::vector<MySigmaPoint, Eigen::aligned_allocator<MySigmaPoint> > spts;
  sampleUnscented(spts,mean,m);
  for (size_t i =0; i<spts.size(); i++){
    cerr << "Point " << i << " " << endl << "wi=" << spts[i]._wi << " wp=" << spts[i]._wp << " " << endl;
    cerr << spts[i]._sample << endl;
  }

  VectorXd recMean(6);
  MatrixXd recCov(6,6);

  reconstructGaussian(recMean, recCov, spts);

  cerr << "recMean" << endl;
  cerr << recMean << endl;

  cerr << "recCov" << endl;
  cerr << recCov << endl;

  return 0;
}


#ifndef SEGMATCH_GRAPH_UTILITIES_HPP_
#define SEGMATCH_GRAPH_UTILITIES_HPP_

#include <iostream>
#include <fstream>

#include <boost/graph/connected_components.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/graphviz.hpp>
#include <glog/logging.h>

namespace segmatch {

/// \brief Provide generic graph utility functions.
class GraphUtilities {
 public:
  /// \brief Prevent instantiation of static class.
  GraphUtilities() = delete;

  /// \brief Writes the specified graph to a .dot file that can be visualized in graphviz
  /// ( http://www.graphviz.org/ ).
  /// \param graph The graph that needs to be saved.
  /// \param file_name The destination file.
  template<typename Graph>
  static void saveGraphForGraphviz(const Graph& graph, const std::string& file_name) {
    std::ofstream output_file;
    output_file.open(file_name);
    if (output_file.is_open()) {
      boost::write_graphviz(output_file, graph);
    } else {
      LOG(ERROR) << "Unable to write graph to file: " << file_name;
    }
  }

  /// \brief Finds the vertices of a graph belonging to the a maximum clique. Only one maximum
  /// clique is returned.
  /// Closely follows the exact algorithm described in:
  /// "Fast Algorithms for the Maximum Clique Problem on Massive Sparse Graphs"
  /// Pattabiraman, Bharath Mostofa Ali Patwary, Md Gebremedhin, Assefaw H Liao, Wei-keng
  /// Choudhary, Alok ( https://arxiv.org/pdf/1209.5818.pdf )
  /// The algorithm is modified so that vertices are visited in increasing degeneracy order. This
  /// limits the search depth to the degeneracy of the graph.
  /// \param graph The input graph. The graph must be indirected and the underlying data structure
  /// must support random access.
  /// \param min_clique_size The minimum size of the maximum clique, smaller cliques will be
  /// ignored. Must be greater or equal 2.
  /// \returns Vector containing the vertices belonging to a maximum clique. If the vector is
  /// empty, no clique with the specified minimum size exists.
  template<typename Graph>
  static std::vector<typename boost::graph_traits<Graph>::vertex_descriptor> findMaximumClique(
      const Graph& graph, const size_t min_clique_size) {
    // Ensure that the graph type is supported and define type shortcuts.
    CHECK(min_clique_size >= 2);
    assertIsUndirectedAndRandomAccessGraph(graph);
    typedef boost::graph_traits<Graph> GraphTraits;
    typedef typename GraphTraits::vertex_descriptor Vertex;

    const size_t n_vertices = boost::num_vertices(graph);
    std::vector<Vertex> neighbors;
    neighbors.reserve(n_vertices);

    std::vector<Vertex> maximum_clique_tmp;
    std::vector<Vertex> maximum_clique;
    maximum_clique_tmp.reserve(n_vertices);
    size_t max_found_size = min_clique_size - 1u;

    // Use bin-sort to sort the vertex indices in increasing degree order.
    std::vector<size_t> bin_starts;
    std::vector<Vertex> sorted_vertices;
    std::vector<size_t> vertex_positions;
    std::vector<size_t> vertex_degrees;
    binSortVerticesByDegree(graph, bin_starts, sorted_vertices, vertex_positions, vertex_degrees);

    // Try to find a clique starting from each vertex.
    for (size_t i = 0u; i < sorted_vertices.size(); ++i) {
      const Vertex vertex = sorted_vertices[i];
      const size_t vertex_degree = vertex_degrees[vertex];

      // Skip the vertex if it doesn't have enough neighbors to be a maximum clique.
      if (vertex_degree >= max_found_size) {
        neighbors.clear();

        // Collect all the neighbors that have enough neighbors to be a maximum clique.
        typename GraphTraits::out_edge_iterator e_it, e_end;
        for (boost::tie(e_it, e_end) = boost::out_edges(vertex, graph); e_it != e_end; ++e_it) {
          const Vertex neighbor = boost::target(*e_it, graph);
          if(vertex_positions[neighbor] > vertex_positions[vertex] &&
              vertex_degrees[neighbor] >= max_found_size)
            neighbors.push_back(neighbor);
        }

        // Get the size of the maximum clique contained in the subgraph defined by the current vertex
        // and its neighbors.
        const size_t new_found_size = findMaximumCliqueSubset(graph, neighbors, vertex_degrees, 1u,
                                                              max_found_size, maximum_clique_tmp);

        // If a bigger clique is found, set it as the new maximum clique.
        if(new_found_size > max_found_size) {
          max_found_size = new_found_size;
          maximum_clique_tmp.push_back(vertex);
          maximum_clique = std::move(maximum_clique_tmp);
        } else {
          maximum_clique_tmp.clear();
        }
      }

      // Decrease the degree of neighbor vertices of higher degree. This is equivalent to removing
      // this vertex and the incident edges.
      typename GraphTraits::out_edge_iterator e_it, e_end;
      for (boost::tie(e_it, e_end) = boost::out_edges(sorted_vertices[i], graph);
           e_it != e_end; ++e_it) {
        const Vertex neighbor = boost::target(*e_it, graph);
        const size_t neighbor_degree = vertex_degrees[neighbor];
        if (neighbor_degree > vertex_degree) {
          const size_t neighbor_position = vertex_positions[neighbor];
          const size_t swapped_neighbor_position = bin_starts[neighbor_degree];
          const Vertex swapped_neighbor = sorted_vertices[swapped_neighbor_position];
          if (neighbor != swapped_neighbor) {
            vertex_positions[neighbor] = swapped_neighbor_position;
            vertex_positions[swapped_neighbor] = neighbor_position;
            sorted_vertices[neighbor_position] = swapped_neighbor;
            sorted_vertices[swapped_neighbor_position] = neighbor;
          }
          ++bin_starts[neighbor_degree];
          --vertex_degrees[neighbor];
        }
      }
    }

    return maximum_clique;
  }

  /// \brief Finds the vertex degrees and the maximum vertex degree in the graph.
  /// \param graph The input graph. The graph must be indirected and the underlying data structure
  /// must support random access.
  /// \param vertex_degrees Vector in which the vertex degrees will be stored.
  /// \returns Maximum vertex degree in the graph.
  template<typename Graph>
  static size_t getVertexDegreesAndGraphMaxDegree(const Graph& graph,
                                                  std::vector<size_t>& vertex_degrees) {
    // Ensure that the graph type is supported and define type shortcuts.
    assertIsUndirectedAndRandomAccessGraph(graph);

    // Get and store the vertex degrees.
    vertex_degrees.clear();
    vertex_degrees.resize(num_vertices(graph));
    size_t maximum_degree = 0u;
    typename boost::graph_traits<Graph>::vertex_iterator v_it, v_end;
    for (boost::tie(v_it, v_end) = boost::vertices(graph); v_it != v_end; ++v_it) {
      vertex_degrees[*v_it] = boost::out_degree(*v_it, graph);
      maximum_degree = std::max(maximum_degree, vertex_degrees[*v_it]);
    }
    return maximum_degree;
  }

 private:
  // Statically verify that a graph is undirected and based on data structures that allow random
  // access.
  template<typename Graph>
  static void assertIsUndirectedAndRandomAccessGraph(const Graph& graph) {
    BOOST_CONCEPT_ASSERT((boost::concepts::GraphConcept<Graph>));
    typedef boost::graph_traits<Graph> GraphTraits;
    typedef typename GraphTraits::vertex_descriptor Vertex;
    static_assert(std::is_same<typename GraphTraits::directed_category,
                               boost::undirected_tag>::value,
                  "GraphUtilities::findMaximumKCore only supports undirected graphs");
    static_assert(std::is_same<Vertex, size_t>::value,
                  "GraphUtilities::findMaximumKCore only supports graphs with vertex descriptors "
                  "of type size_t (usually graphs based on random access containers).");
  }

  // Sort the vertices of a graph in increasing vertex degree order using bin-sorting.
  template<typename Graph>
  static size_t binSortVerticesByDegree(
      const Graph& graph, std::vector<size_t>& bin_starts,
      std::vector<typename boost::graph_traits<Graph>::vertex_descriptor>& sorted_vertices,
      std::vector<size_t>& vertex_positions, std::vector<size_t>& vertex_degrees) {

    // Ensure that the graph type is supported.
    assertIsUndirectedAndRandomAccessGraph(graph);
    typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;

    // Get and store the vertex degrees.
    size_t maximum_degree = getVertexDegreesAndGraphMaxDegree(graph, vertex_degrees);

    // Use bin-sort to sort the vertex indices in increasing degree order.
    // 1) Find the size of each bin.
    std::vector<size_t> bin_sizes(maximum_degree + 1u);
    for (const auto degree : vertex_degrees) ++bin_sizes[degree];

    // 2) Find the starting index of each bin.
    bin_starts.resize(maximum_degree + 1u);
    size_t next_bin_start = 0u;
    for (size_t i = 0u; i < bin_sizes.size(); ++i) {
      bin_starts[i] = next_bin_start;
      next_bin_start += bin_sizes[i];
    }

    // 3) Sort vertex indices
    std::vector<size_t> bin_offsets(bin_starts);
    sorted_vertices.resize(boost::num_vertices(graph));
    vertex_positions.resize(boost::num_vertices(graph));
    typename boost::graph_traits<Graph>::vertex_iterator v_it, v_end;
    for (boost::tie(v_it, v_end) = boost::vertices(graph); v_it != v_end; ++v_it) {
      vertex_positions[*v_it] = bin_offsets[vertex_degrees[*v_it]]++;
      sorted_vertices[vertex_positions[*v_it]] = *v_it;
    }
  }

  // Helper recursive function for the findMaximumClique() function.
  template<typename Graph>
  static size_t findMaximumCliqueSubset(
      const Graph& graph,
      std::vector<typename boost::graph_traits<Graph>::vertex_descriptor>& subset,
      const std::vector<size_t>& vertex_degrees,
      const size_t clique_size, size_t max_found_size,
      std::vector<typename boost::graph_traits<Graph>::vertex_descriptor>& maximum_clique_tmp) {
    // Ensure that the graph type is supported and define type shortcuts.
    assertIsUndirectedAndRandomAccessGraph(graph);
    typedef boost::graph_traits<Graph> GraphTraits;
    typedef typename GraphTraits::vertex_descriptor Vertex;

    const size_t n_vertices = boost::num_vertices(graph);
    std::vector<Vertex> neighbors;
    neighbors.reserve(n_vertices);

    // Final step of the recursion: if there are no more vertices to process, the search is
    // complete.
    if(subset.empty()) {
      if(clique_size > max_found_size) {
        maximum_clique_tmp.clear();
        return clique_size;
      }
      return max_found_size;
    }

    // Process the given subset of vertices.
    while(!subset.empty()) {
      // Continue the search only if there are enough remaining candidates.
      if(clique_size + subset.size() <= max_found_size) break;
      Vertex vertex = subset.back();
      subset.pop_back();

      // Collect the vertices that have enough neighbors and are connected to the current vertex.
      for (const Vertex candidate : subset) {
        if (vertex_degrees[candidate] >= max_found_size &&
            boost::edge(vertex, candidate, graph).second)
            neighbors.push_back(candidate);
      }

      // Get the size of the maximum clique contained in the subgraph defined by the current vertex
      // and its neighbors.
      const size_t new_found_size = findMaximumCliqueSubset(graph, neighbors, vertex_degrees,
                                                            clique_size + 1u, max_found_size,
                                                            maximum_clique_tmp);

      // If a bigger clique is found, use the current vertex.
      if(new_found_size > max_found_size) {
        max_found_size = new_found_size;
        maximum_clique_tmp.push_back(vertex);
      }
      neighbors.clear();
    }

    return max_found_size;
  }
}; // class GraphUtilities

} // namespace segmatch

#endif // SEGMATCH_GRAPH_UTILITIES_HPP_