#pragma once
#ifndef CANNON_GRAPHICS_PROJECTION_H
#define CANNON_GRAPHICS_PROJECTION_H

/*!
 * \file cannon/graphics/projection.hpp
 * \brief File containing utility free functions for computing projections.
 */

#include <Eigen/Dense>

using namespace Eigen;

namespace cannon {
  namespace graphics {

    /*!
     * Function to convert degrees to radians.
     *
     * \param degrees Angle representation in degrees.
     *
     * \returns Angle representation in radians.
     */
    float to_radians(float degrees);

    /*!
     * Compute perspective matrix corresponding to a view frustum specified by
     * the inputs.
     *
     * \param left Left extent of the frustum.
     * \param right Right extent of the frustum.
     * \param bottom Lower extent of the frustum.
     * \param top Upper extent of the frustum.
     * \param near Depth of the near clipping plane.
     * \param far Depth of the far clipping plane.
     *
     * \returns The corresponding perspective matrix.
     */
    Matrix4f make_perspective_frustum(float left, float right, float bottom,
        float top, float near, float far);

    /*!
     * Compute perspective matrix corresponding to a view frustum with a given
     * field of view and aspect ratio.
     *
     * \param fov The field of view (in radians) of the frustum.
     * \param aspect The aspect ratio of the frustum.
     * \param near Depth of the near clipping plane.
     * \param far Depth of the far clipping plane.
     *
     * \returns The corresponding perspective matrix.
     */
    Matrix4f make_perspective_fov(float fov, float aspect, float near, float far);

    /*!
     * Compute orthographic projection matrix corresponding to a view frustum
     * specified by the inputs.
     *
     * \param left Left extent of the frustum.
     * \param right Right extent of the frustum.
     * \param bottom Lower extent of the frustum.
     * \param top Upper extent of the frustum.
     * \param near Depth of the near clipping plane.
     * \param far Depth of the far clipping plane.
     *
     * \returns The corresponding projection matrix.
     */
    Matrix4f make_orthographic(float left, float right, float bottom, float
        top, float near, float far);

  } // namespace graphics
} // namespace cannon

#endif /* ifndef CANNON_GRAPHICS_PROJECTION_H */


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

#include <boost/test/unit_test.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/weighted_sum.hpp>
#include <boost/accumulators/statistics/variates/covariate.hpp>

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

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    accumulator_set<int, stats<tag::weighted_sum, tag::weighted_sum_of_variates<int, tag::covariate1> >, int> acc;

    acc(1, weight = 2, covariate1 = 3);
    BOOST_CHECK_EQUAL(2, weighted_sum(acc));
    BOOST_CHECK_EQUAL(6, weighted_sum_of_variates(acc));

    acc(2, weight = 3, covariate1 = 6);
    BOOST_CHECK_EQUAL(8, weighted_sum(acc));
    BOOST_CHECK_EQUAL(24, weighted_sum_of_variates(acc));

    acc(4, weight = 6, covariate1 = 9);
    BOOST_CHECK_EQUAL(32, weighted_sum(acc));
    BOOST_CHECK_EQUAL(78, weighted_sum_of_variates(acc));
}

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

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

    return test;
}


#ifndef __CAMERAMODEL_HPP__
#define __CAMERAMODEL_HPP__

#include <memory>

#include <Eigen/Dense>

#include "lvr2/types/MatrixTypes.hpp"

namespace lvr2
{

template<typename T>
struct PinholeModel
{
    double fx = 0;
    double fy = 0;
    double cx = 0;
    double cy = 0;
    unsigned width = 0;
    unsigned height = 0;
    std::vector<T> k;
    std::string distortionModel = "unknown";
};

template<typename T>
using PinholeModelPtr = std::shared_ptr<PinholeModel<T>>;

using PinholeModeld = PinholeModel<double>;
using PinholeModelf = PinholeModel<float>;

} // namespace lvr2

#endif


/* bitops_test.cc
   Jeremy Barnes, 20 February 2007
   Copyright (c) 2007 Jeremy Barnes.  All rights reserved.

   Test of the bit operations class.
*/

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK

#include "jml/arch/bitops.h"
#include "jml/arch/tick_counter.h"
#include "jml/arch/demangle.h"
#include "jml/math/xdiv.h"

#include <boost/test/unit_test.hpp>
#include <boost/test/auto_unit_test.hpp>
#include <vector>
#include <stdint.h>
#include <iostream>

using namespace ML;
using namespace std;

using boost::unit_test::test_suite;

template<class X>
void test1_type()
{
    cerr << "testing type " << demangle(typeid(X).name()) << endl;

    BOOST_CHECK_EQUAL(highest_bit((X)0),       -1);
    BOOST_CHECK_EQUAL(highest_bit((X)1),        0);
    BOOST_CHECK_EQUAL(highest_bit((X)2),        1);
    BOOST_CHECK_EQUAL(highest_bit((X)3),        1);
    BOOST_CHECK_EQUAL(highest_bit((X)-1),       sizeof(X) * 8 - 1);
    BOOST_CHECK_EQUAL(highest_bit((X)63),       5);
    BOOST_CHECK_EQUAL(highest_bit((X)64),       6);
    BOOST_CHECK_EQUAL(highest_bit((X)255),      7);

    BOOST_CHECK_EQUAL(lowest_bit((X)0),       -1);
    BOOST_CHECK_EQUAL(lowest_bit((X)1),        0);
    BOOST_CHECK_EQUAL(lowest_bit((X)2),        1);
    BOOST_CHECK_EQUAL(lowest_bit((X)3),        0);
    BOOST_CHECK_EQUAL(lowest_bit((X)-1),       0);
    BOOST_CHECK_EQUAL(lowest_bit((X)63),       0);
    BOOST_CHECK_EQUAL(lowest_bit((X)64),       6);
    BOOST_CHECK_EQUAL(lowest_bit((X)255),      0);
    BOOST_CHECK_EQUAL(lowest_bit((X)128),      7);

    if (sizeof(X) == 1) return;
    BOOST_CHECK_EQUAL(highest_bit((X)256),      8);
    BOOST_CHECK_EQUAL(lowest_bit((X)256),      8);

    if (sizeof(X) == 2) return;
    BOOST_CHECK_EQUAL(highest_bit((X)131071),  16);
    BOOST_CHECK_EQUAL(highest_bit((X)131072),  17);
    BOOST_CHECK_EQUAL(highest_bit((X)131073),  17);

    BOOST_CHECK_EQUAL(lowest_bit((X)131071),  0);
    BOOST_CHECK_EQUAL(lowest_bit((X)131072),  17);
    BOOST_CHECK_EQUAL(lowest_bit((X)131073),  0);
}

BOOST_AUTO_TEST_CASE( test1 )
{
    test1_type<uint8_t>();
    test1_type<int8_t>();
    test1_type<uint16_t>();
    test1_type<int16_t>();
    test1_type<uint32_t>();
    test1_type<int32_t>();
    test1_type<uint64_t>();
    test1_type<int64_t>();
}

uint64_t rand64()
{
    uint64_t high = rand(), low = rand();
    return (high << 32) | low;
}

template<class X>
int fake_highest_bit(X x)
{
    return x;
}

template<class X>
void profile_type(const vector<uint64_t> & vals_)
{
    vector<X> vals(vals_.begin(), vals_.end());

    cerr << "profiling for " << sizeof(X) * 8 << " bits, "
         << ((X)-1 > 0 ? "unsigned" : "signed")
         <<  " with " << vals.size() << " vals" << endl;

    /* Measure the overhead */
    double overhead = 0;

    int trials = 10;

    int total = 0;

    for (unsigned t = 0;  t < trials;  ++t) {
        uint64_t tbefore = ticks();
        for (unsigned i = 0;  i < vals.size();  ++i)
            total += fake_highest_bit(vals[i]);
        uint64_t tafter = ticks();
        overhead += tafter - tbefore - ticks_overhead;

    }

    cerr << "measurement overhead is " << overhead << endl;

    double measured = 0;

    for (unsigned t = 0;  t < trials;  ++t) {

        uint64_t tbefore = ticks();
        for (unsigned i = 0;  i < vals.size();  ++i)
            total += highest_bit(vals[i]);

        uint64_t tafter = ticks();

        measured += tafter - tbefore - ticks_overhead;
    }

    srand(total);  // use it so it doesn't get optimized out

    cerr << "measured " << measured << " total ticks" << endl;

    double cost = xdiv<double>(measured - overhead, trials * vals.size());

    cerr << "cost: " << cost << " ticks/call" << endl;
}

BOOST_AUTO_TEST_CASE( profile )
{
    vector<uint64_t> vals;
    for (unsigned i = 0;  i < 1000;  ++i)
        vals.push_back(rand64());

    profile_type<uint8_t>(vals);
    profile_type<int8_t>(vals);
    profile_type<uint16_t>(vals);
    profile_type<int16_t>(vals);
    profile_type<uint32_t>(vals);
    profile_type<int32_t>(vals);
    profile_type<uint64_t>(vals);
    profile_type<int64_t>(vals);
}

BOOST_AUTO_TEST_CASE(test_rotate)
{
    BOOST_CHECK_EQUAL(rotate_right<uint8_t>(0, 0), 0);
    BOOST_CHECK_EQUAL(rotate_right<uint8_t>(0, 1), 0);
    BOOST_CHECK_EQUAL(rotate_right<uint8_t>(0, 8), 0);

    BOOST_CHECK_EQUAL(rotate_left<uint8_t>(0, 0), 0);
    BOOST_CHECK_EQUAL(rotate_left<uint8_t>(0, 1), 0);
    BOOST_CHECK_EQUAL(rotate_left<uint8_t>(0, 8), 0);

    BOOST_CHECK_EQUAL(rotate_right<uint8_t>(1, 0), 1);
    BOOST_CHECK_EQUAL(rotate_right<uint8_t>(1, 1), 128);
    BOOST_CHECK_EQUAL(rotate_right<uint8_t>(1, 7), 2);
    BOOST_CHECK_EQUAL(rotate_right<uint8_t>(1, 8), 1);
    BOOST_CHECK_EQUAL(rotate_right<uint8_t>(1, 16), 1);
    BOOST_CHECK_EQUAL(rotate_right<uint8_t>(1, 17), 128);

    BOOST_CHECK_EQUAL(rotate_left<uint8_t>(1, 0), 1);
    BOOST_CHECK_EQUAL(rotate_left<uint8_t>(1, 1), 2);
    BOOST_CHECK_EQUAL(rotate_left<uint8_t>(1, 7), 128);
    BOOST_CHECK_EQUAL(rotate_left<uint8_t>(1, 8), 1);
    BOOST_CHECK_EQUAL(rotate_left<uint8_t>(1, 16), 1);
    BOOST_CHECK_EQUAL(rotate_left<uint8_t>(1, 17), 2);

    BOOST_CHECK_EQUAL(rotate_right<uint8_t>(0, 0), 0);
    BOOST_CHECK_EQUAL(rotate_right<uint8_t>(0, 1), 0);
    BOOST_CHECK_EQUAL(rotate_right<uint8_t>(0, 8), 0);

    BOOST_CHECK_EQUAL(rotate_left<int8_t>(0, 0), 0);
    BOOST_CHECK_EQUAL(rotate_left<int8_t>(0, 1), 0);
    BOOST_CHECK_EQUAL(rotate_left<int8_t>(0, 8), 0);

    BOOST_CHECK_EQUAL(rotate_right<int8_t>(1, 0), 1);
    BOOST_CHECK_EQUAL(rotate_right<int8_t>(1, 1), -128);
    BOOST_CHECK_EQUAL(rotate_right<int8_t>(1, 7), 2);
    BOOST_CHECK_EQUAL(rotate_right<int8_t>(1, 8), 1);
    BOOST_CHECK_EQUAL(rotate_right<int8_t>(1, 16), 1);
    BOOST_CHECK_EQUAL(rotate_right<int8_t>(1, 17), -128);

    BOOST_CHECK_EQUAL(rotate_left<int8_t>(1, 0), 1);
    BOOST_CHECK_EQUAL(rotate_left<int8_t>(1, 1), 2);
    BOOST_CHECK_EQUAL(rotate_left<int8_t>(1, 7), -128);
    BOOST_CHECK_EQUAL(rotate_left<int8_t>(1, 8), 1);
    BOOST_CHECK_EQUAL(rotate_left<int8_t>(1, 16), 1);
    BOOST_CHECK_EQUAL(rotate_left<int8_t>(1, 17), 2);
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2012 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#include <iostream>
#include <iomanip>
#include <cmath>

#include <boost/timer/timer.hpp>

#include "dune/grid/config.h"
#include "dune/grid/uggrid.hh"
#include "dune/istl/preconditioners.hh"

#include "fem/assemble.hh"
#include "fem/norms.hh"
#include "fem/lagrangespace.hh"
#include "fem/hierarchicspace.hh"
#include "fem/embedded_errorest.hh"
#include "fem/istlinterface.hh"
#include "fem/functional_aux.hh"
#include "fem/iterate_grid.hh"
#include "fem/hierarchicErrorEstimator.hh"
#include "linalg/trivialpreconditioner.hh"
#include "linalg/jacobiPreconditioner.hh"
#include "linalg/direct.hh"
#include "linalg/triplet.hh"
#include "linalg/mumps_solve.hh"

#include "io/vtk.hh"
#include "io/amira.hh"

#include "mg/pcg.hh"
#include "linalg/apcg.hh"

#include "utilities/enums.hh"
#include "utilities/kaskopt.hh"
#include "utilities/geometric_sequence.hh"

using namespace Kaskade;
#include "poisson.hh"
#include "createGrid.hh"

int problemNo = 1;

bool compareAbs(const double x1, const double  x2)
  {
    return fabs(x1)>fabs(x2);
  }

// Implements a test problem for the cascadic multigrid algorithm as of
// Deuflhard/Weiser chapter 7.

int main(int argc, char *argv[])
{
  int verbosity = 1;
  bool dump = false;
  std::unique_ptr<boost::property_tree::ptree> pt = getKaskadeOptions(argc, argv, verbosity, dump);

  std::cout << "Start cascadic multigrid test program" << std::endl;

  int  direct, refs, order, onlyLowerTriangle = false, maxAdaptSteps, gebiet;
  DirectType directType;
  IterateType iterateType = IterateType::PCG;
  MatrixProperties property;
  std::string empty, problem, geometry, functional;

  problem = getParameter(pt, "problem", empty);
  refs = getParameter(pt, problem+".refs", 0),
  order =  getParameter(pt, problem+".order", 1),
  maxAdaptSteps = getParameter(pt, problem+".maxAdaptSteps", 10);
  geometry = getParameter(pt, problem+".geometry", empty);
  functional = getParameter(pt, problem+".functional", empty);
  gebiet = getParameter(pt, "names.geometry."+geometry, 1);
  problemNo = getParameter(pt, "names.functional."+functional, 1);
  std::cerr << "selected problem " << problem << ", functional=" <<
  functional << "(" << problemNo <<  "), geometry=" <<
  geometry << "(" << gebiet << ")" << std::endl;

  std::string s("names.type.");
  s += getParameter(pt, "solver.type", empty);
  std::cerr << "s = " << s << std::endl;
  direct = getParameter(pt, s, 0);
  std::cerr << "s = " << s << std::endl;

  s = "names.direct." + getParameter(pt, "solver.direct", empty);
  std::cerr << "s = " << s << std::endl;
  directType = static_cast<DirectType>(getParameter(pt, s, 2));
  std::cerr << "s = " << s << std::endl;

  s = "names.iterate." + getParameter(pt, "solver.iterate", empty);
  iterateType = static_cast<IterateType>(getParameter(pt, s, 0));

  property = MatrixProperties::POSITIVEDEFINITE;

  if (((property == MatrixProperties::SYMMETRIC)||(property == MatrixProperties::POSITIVEDEFINITE))&&
    ((directType == DirectType::MUMPS)||(directType == DirectType::PARDISO)))
  {
    onlyLowerTriangle = true;
  }

  int const dim2=2;
  typedef Dune::UGGrid<dim2> Grid;
  Dune::GridFactory<Grid> factory;

  switch (gebiet)
  {
    case 0:
    {
      createUnitSquare<Grid,dim2>(factory);
    }
    break;
    case 1:
    {
      createUnitCrack<Grid,dim2>(factory);
    }
    break;
    case 2:
    {
      createOriginalBoDD<Grid,dim2>(factory);
    }
    break;
    case 3:
    {
      createAreaGrid<Grid,dim2>(factory);
    }
    break;
    default:
      std::cout << "Unknown gebiet" << std::endl;
      exit(2);
  }

  std::auto_ptr<Grid> grid( factory.createGrid() );
  // the coarse grid will be refined refs times
  grid->globalRefine(refs);

  // some information on the refined mesh
  std::cout << "Grid: " << grid->size(0) << " triangles, " << std::endl;
  std::cout << "      " << grid->size(1) << " edges, " << std::endl;
  std::cout << "      " << grid->size(2) << " points" << std::endl;

  // a gridmanager is constructed
  GridManager<Grid> gridManager(std::move(grid));

  typedef Grid::LeafGridView LeafView;
  // construction of finite element space for the scalar solution T
  typedef FEFunctionSpace<ContinuousHierarchicMapper<double,LeafView> > H1Space;

  H1Space temperatureSpace(gridManager,gridManager.grid().leafGridView(),order);
  typedef boost::fusion::vector<H1Space const*> Spaces;
  Spaces spaces(&temperatureSpace);
  // VariableDescription<int spaceId, int components, int Id>
  // spaceId: number of associated FEFunctionSpace
  // components: number of components in this variable
  // Id: number of this variable
  typedef boost::fusion::vector<VariableDescription<0,1,0> >
  VariableDescriptions;
  std::string varNames[1] = { "T" };
  typedef VariableSetDescription<Spaces,VariableDescriptions> VariableSet;
  VariableSet variableSet(spaces,varNames);

  typedef VariableSet::CoefficientVectorRepresentation<>::type CoefficientVector;

  // Define a higher order space for transfering the error estimate
  H1Space temperatureSpace2(gridManager,gridManager.grid().leafView(),order+1);

  // Define the variational functional
  typedef PoissonFunctional<double,VariableSet> Functional;
  Functional F;
  typedef VariationalFunctionalAssembler<LinearizationAt<Functional> > Assembler;
  typedef Assembler::RhsArray Rhs;
  Assembler assembler(gridManager,spaces);

  typedef FEFunctionSpace<ContinuousHierarchicExtensionMapper<double,LeafView> > H1ExSpace;
  H1ExSpace spaceEx(gridManager,gridManager.grid().leafView(), order+1);

  typedef boost::fusion::vector<H1Space const*,H1ExSpace const*> H1ExSpaces;
  H1ExSpaces exSpaces(&temperatureSpace,&spaceEx);

  typedef boost::fusion::vector<VariableDescription<1,1,0> > ExVariableDescriptions;
  typedef VariableSetDescription<H1ExSpaces,ExVariableDescriptions> ExVariableSet;
  std::string exVarNames[1] = { "e"};
  ExVariableSet exVariableSet(exSpaces, exVarNames);

  typedef ExVariableSet::CoefficientVectorRepresentation<>::type ExCoefficientVector;

  typedef HierarchicErrorEstimator<LinearizationAt<Functional>,ExVariableSet> ErrorEstimator;
  typedef VariationalFunctionalAssembler<ErrorEstimator> EstGOP;

  EstGOP estGop(gridManager,exSpaces);

  typedef VariableSet::Grid::Traits::LeafIndexSet IS ;
  IS const& is = gridManager.grid().leafIndexSet();

  VariableSet::VariableSet x(variableSet), dx(variableSet);

  double const TOL = getParameter(pt, problem+".TOL", 1.0-2),
  minRefine = getParameter(pt, problem+".minRefine", 0.0);
  std::cerr << "TOL = " << TOL << ", minRefine = " << minRefine << std::endl ;
  assert(TOL>0);

  IoOptions options;
  options.outputType = IoOptions::ascii;

  constexpr int neq = Functional::TestVars::noOfVariables;
  constexpr int nvars = Functional::AnsatzVars::noOfVariables;
  int refSteps = 0;
  bool accurate = false;
  int iteSteps = getParameter(pt, "solver.iteMax", 1000);
  int verbose = getParameter(pt, "solver.verbose", 1);
  double iteEps = getParameter(pt, "solver.iteEps", 1.0e-9);
  int lookahead = getParameter(pt,"solver.APCG.lookahead",6);
  double eefactor = getParameter(pt,"ccg.eefactor",0.0);
  Dune::InverseOperatorResult result;
  double errNorm = -1;

  size_t size = variableSet.degreesOfFreedom(0,1);

  double const gamma = iterateType==IterateType::APCG? 1.0: 0.5;   // Deuflhard/Weiser (7.60)
  int const d = 2;                                    // two dimensional problem
  assert(d*gamma>1);                                  // Deuflhard/Weiser Satz 7.36
  double const beta = 1.0/sqrt(d*gamma);              // Deuflhard/Weiser (7.67)
  double const alpha = (d*gamma-1.0)/(d*(1.0+gamma)); // Deuflhard/Weiser S. 303
  double q = 2;                                       // Deuflhard/Weiser (7.62)
  long N = size;
  double zk = 0.0;
  double const requested = sqrt(1-beta*beta)*TOL;     // Deuflhard/Weiser Algorithmus 7.37
  double yk = pow(N,alpha);
  double pcgerr = -1;
  double pcgerr_fact = -1;
  int required = 0;

  std::vector<VariableSet::VariableSet> solutions;

  do {
    // Zero coefficient vectors for initialization
    ExCoefficientVector exZero(ExVariableSet::CoefficientVectorRepresentation<>::init(exVariableSet)); exZero = 0;
    CoefficientVector zero(VariableSet::CoefficientVectorRepresentation<>::init(variableSet)); zero = 0;

    std::cerr << "----------------------------------------\nStarting round\n";
    CoefficientVector solution(zero), hilfe(zero);
    assembler.assemble(linearization(F,x));
    Rhs rhs(assembler.rhs());
    AssembledGalerkinOperator<Assembler,0,neq,0,nvars> A(assembler, onlyLowerTriangle);

    if (direct)
      directInverseOperator(A,directType,property).applyscaleadd(-1.0,rhs,solution);
    else  {
      switch (iterateType) {
	case IterateType::CG: {
	  JacobiPreconditioner<AssembledGalerkinOperator<Assembler,0,neq,0,nvars> > jacobi(A,1.0);
	  Dune::CGSolver<CoefficientVector> cg(A,jacobi,iteEps,iteSteps,verbose);
	  cg.apply(hilfe,rhs,result);
	  break;
	}
	case IterateType::PCG: {
	  JacobiPreconditioner<AssembledGalerkinOperator<Assembler,0,neq,0,nvars> > jacobi(A,1.0);
	  NMIIIPCGSolver<CoefficientVector> pcg(A,jacobi,iteEps,iteSteps,verbose);
	  pcg.apply(hilfe,rhs,result);
	  break;
	}
	case IterateType::APCG: {
	  JacobiPreconditioner<AssembledGalerkinOperator<Assembler,0,neq,0,nvars> > jacobiPCG(A,1.0);
	  DefaultDualPairing<CoefficientVector,CoefficientVector> dp;
	  PCGEnergyErrorTerminationCriterion<double> terminate(iteEps,iteSteps);
	  terminate.lookahead(lookahead);
	  Pcg<CoefficientVector,CoefficientVector> apcg(A,jacobiPCG,dp,terminate,verbose);
	  apcg.apply(hilfe,rhs,result);
	  pcgerr = terminate.error();

	  CoefficientVector hilfe2(zero);
	  PCGEnergyErrorTerminationCriterion<double> terminate2(iteEps/1000,iteSteps);
	  Pcg<CoefficientVector,CoefficientVector> apcg2(A,jacobiPCG,dp,terminate2,verbose);
          Dune::InverseOperatorResult result2;
	  apcg2.apply(hilfe2,rhs,result2);
    std::ostringstream fn;
    fn << "cmg-gamma-";
    fn.width(3);
    fn.fill('0');
    fn.setf(std::ios_base::right,std::ios_base::adjustfield);
    fn << refSteps;
    fn.flush();
    std::ofstream out(fn.str().c_str());
    terminate.clear();
    required = 0;
    for (int i=0; i<terminate2.gamma2().size(); ++i) {
      terminate.step(terminate2.gamma2()[i]);
      out << terminate2.gamma2()[i] << ' ' << terminate.error() << ' ' << std::sqrt(std::accumulate(terminate2.gamma2().begin()+i,terminate2.gamma2().end(),0.0)) << '\n';
      if (std::sqrt(std::accumulate(terminate2.gamma2().begin()+i,terminate2.gamma2().end(),0.0)) >= iteEps)
	required = i;
    }

// 	  PCGEnergyErrorTerminationCriterion<double> terminate3(0.0,required+1);
// 	  Pcg<CoefficientVector,CoefficientVector> apcg3(A,jacobiPCG,dp,terminate3,verbose);
// 	  apcg3.apply(hilfe,rhs,result);
// 	  pcgerr = terminate3.error();
// 	  //pcgstep = std::sqrt(std::accumulate(terminate3.gamma2().begin(),terminate3.gamma2().end(),0.0));

	  CoefficientVector dhilfe(hilfe2); dhilfe -= hilfe;
	  CoefficientVector Adhilfe(zero);
	  A.apply(dhilfe,Adhilfe);
	  pcgerr_fact = std::sqrt(dhilfe*Adhilfe);

	  break;
	}
// 	case IterateType::SGS: {
// 	  int addedIterations = 3;
// 	  typedef Dune::BlockVector<Dune::FieldVector<double,1> > NakedCoefficientVector;
// 	  Dune::SeqSSOR<MatrixAsTriplet<double>,NakedCoefficientVector,NakedCoefficientVector> sgs(A.getmat(),1,1.0);
// 	  sgs.pre(boost::fusion::at_c<0>(hilfe.data),boost::fusion::at_c<0>(rhs.data));
// 	  CoefficientVector y(zero), z(zero), r(rhs);
// 	  boost::circular_buffer<double> gamma(addedIterations);
// 	  for (int i=0; i<iteSteps; ++i) {
// 	    sgs.apply(boost::fusion::at_c<0>(y.data),boost::fusion::at_c<0>(r.data));
// 	    A.apply(y,z);
// 	    r -= z;
// 	    hilfe += y;
// 	    result.iterations = i+1;
// 	    gamma.push_back(std::sqrt(y*z));
// 	    if (i>=addedIterations && std::accumulate(gamma.begin(),gamma.end(),0.0)<iteEps)
// 	      break;
// 	  }
// 	  std::pair<double,double> geoest = estimateGeometricSequence(gamma.begin(),gamma.end());
// 	  std::cerr << "geometric estimate: c=" << geoest.first << " q=" << geoest.second << " delta=" << iteEps << '\n';
// 	  std::copy(gamma.begin(),gamma.end(),std::ostream_iterator<double>(std::cerr," ")); std::cerr << '\n';
// 	  break;
// 	}
	default:
	  std::cerr << "Solver " << iterateType << " not available" << std::endl;
	  throw -111;
      }
      solution.axpy(-1,hilfe);
    }
    dx.data = solution.data;

    // Do hierarchical error estimation. Remember to provide the very same underlying problem to the
    // error estimator functional as has been used to compute dx (do not modify x!).
    {

    estGop.assemble(ErrorEstimator(LinearizationAt<Functional>(F,x),dx));

      // iterative solution of error estimator
      AssembledGalerkinOperator<EstGOP> E(estGop);
      Dune::InverseOperatorResult estRes;
      ExCoefficientVector const estRhside(estGop.rhs()) ;
      ExCoefficientVector estSol(exZero) ;
      JacobiPreconditioner<EstGOP> jprec(estGop, 1.0);
      jprec.apply(estSol,estRhside); //single Jacobi iteration

      // Represent error estimator as FE function
      H1ExSpace::Element<1>::type erroresttmp(spaceEx);
      erroresttmp = boost::fusion::at_c<0>(estSol.data);
      H1Space::Element<1>::type errorest(temperatureSpace2);
      errorest = erroresttmp;

      // Transfer error indicators to cells.
      std::vector<double> errorDistribution(is.size(0));
      typedef VariableSet::GridView::Codim<0>::Iterator CellIterator ;
      double maxErr = 0.0;
      for (CellIterator ci=variableSet.gridView.begin<0>(); ci!=variableSet.gridView.end<0>(); ++ci) {
	typedef H1ExSpace::Mapper::GlobalIndexRange GIR;
	double err = 0;
	GIR gix = spaceEx.mapper().globalIndices(*ci);
	for (GIR::iterator j=gix.begin(); j!=gix.end(); ++j)
	  err += boost::fusion::at_c<0>(estSol.data)[*j] * boost::fusion::at_c<0>(estRhside.data)[*j]; // remember that what adds up here is positive (due to diagonal solve)
	assert(err>=0);
	errorDistribution[is.index(*ci)] = std::sqrt(err); // Deuflhard/Weiser (6.37)
	if (err>maxErr) maxErr = err;
      }

      // Select refinement threshold: Refine all elements which
      // - have at least an error of half of the maximum error or
      // - are among the minRefine fraction with largest error
      double errLevel = maxErr/(order+1);
      if (minRefine>0.0) {
	std::vector<double> eSort(errorDistribution);
	std::sort(eSort.begin(),eSort.end(),std::greater<double>()); // sort decreasingly
	int minRefineIndex = minRefine*(eSort.size()-1);
	errLevel = std::min(errLevel,eSort[minRefineIndex]+1.0e-14);
      }

      // Compute the error estimator norm according to Deuflhard/Weiser (6.36)
      errNorm = 0;
      for (int i=0; i<boost::fusion::at_c<0>(estSol.data).N(); ++i)
	errNorm += boost::fusion::at_c<0>(estSol.data)[i] * boost::fusion::at_c<0>(estRhside.data)[i]; // remember that what adds up here is positive (due to diagonal solve)
      errNorm = std::sqrt(errNorm);

      // Termination check
      if (errNorm<requested) {
	accurate = true;

	for (int i=0; i<solutions.size()-1; ++i) {
          solutions[i] -= solutions.back();

	  CoefficientVector du(zero), Adu(zero);
	  boost::fusion::at_c<0>(du.data) = boost::fusion::at_c<0>(solutions[i].data).coefficients();
	  A.apply(du,Adu);
	  std::cerr << "i= " << i << " err= " << std::sqrt(du*Adu) << '\n';
	}
      } else {
	// Refine mesh.
	int count = 0;
	for (CellIterator ci=variableSet.gridView.begin<0>(); ci!=variableSet.gridView.end<0>(); ++ci)
	  if (errorDistribution[is.index(*ci)] >= errLevel) {
	    gridManager.mark(1,*ci);
	    ++count;
	  }
	accurate = !gridManager.adaptAtOnce();

    std::ostringstream fn;
    fn << "graph/cmg-dx";
    fn.width(3);
    fn.fill('0');
    fn.setf(std::ios_base::right,std::ios_base::adjustfield);
    fn << refSteps;
    fn.flush();
    writeVTKFile(gridManager.grid().leafView(),variableSet,dx,fn.str(),options,order);

      }
    // apply the Newton correction here
    x += dx;

    std::ostringstream fn;
    fn << "graph/cmg-sol";
    fn.width(3);
    fn.fill('0');
    fn.setf(std::ios_base::right,std::ios_base::adjustfield);
    fn << refSteps;
    fn.flush();
    writeVTKFile(gridManager.grid().leafView(),variableSet,x,fn.str(),options,order);
    // add coarse grid error estimator to fine grid prolongation
    errorest *= eefactor;
    boost::fusion::at_c<0>(x.data) -= errorest;
    }

    // store solution for later checks
    solutions.push_back(x);

    std::cout << "step= " << refSteps << " [err]= " << errNorm << " zk= " << zk << " q= " << q << " alpha= " << alpha
              << " N= " << N << " yk= " << yk << " iter= " << result.iterations << " delta= " << iteEps << " pcgerr= " << pcgerr << " pcgerr_fact= " << pcgerr_fact
              << " required= " << required << '\n';
    std::cout.flush();

        refSteps++;

    // Modify tolerances according to Deuflhard/Weiser Algorithmus 7.37
    q = variableSet.degreesOfFreedom(0,1)/static_cast<double>(N); // DOF growth factor
    N = variableSet.degreesOfFreedom(0,1);                        // new DOF
    yk += pow(N,alpha);                                           // Deuflhard/Weiser S. 303
    zk = pow(N,alpha)*(pow(errNorm/requested,d*alpha)-pow(q,alpha))/(pow(q,alpha)-1.0); // Deuflhard/Weiser S. 304
    iteEps = beta*TOL*yk/(yk+zk);

    if (getParameter(pt, "strategy", 1) == 0)
      iteEps = beta*TOL;
    if (getParameter(pt, "strategy", 1) == 2)
      iteEps = errNorm*std::pow(q,1.0/d);

    if (refSteps>maxAdaptSteps) {
      std::cout << "number of steps " << refSteps << " > maxAdaptSteps (" << maxAdaptSteps << ")\n";
      break;
    }

    std::ostringstream fn;
    fn << "graph/cmg-grid";
    fn.width(3);
    fn.fill('0');
    fn.setf(std::ios_base::right,std::ios_base::adjustfield);
    fn << refSteps-1;
    fn.flush();
    writeVTKFile(gridManager.grid().leafView(),variableSet,x,fn.str(),options,order);

  } while (!accurate);

std::cout << "End cmgtest" << std::endl;
}


//
//  TDF SDK
//
//  Created by Sujan Reddy on 2020/04/20.
//  Copyright 2020 Virtru Corporation
//

#define BOOST_TEST_MODULE test_ec_key_pair

#include "ec_key_pair.h"
#include "crypto_utils.h"
#include "bytes.h"
#include "gcm_encryption.h"
#include "gcm_decryption.h"
#include "nanotdf/ecc_mode.h"

#include <iostream>

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

using namespace virtru;
using namespace virtru::crypto;

void testNanoTDFKeyMangement(const std::string& curveName, unsigned compressedPubKeySize) {
    constexpr auto KIvSize = 3;
    constexpr auto KAuthTagSize = 8;

    const std::string kPlainText = "Virtru!!";

    // -----------------------------------------------------------------
    // (SDK-Side) Generate an EC key pair
    // -----------------------------------------------------------------
    auto sdkECKeyPair = ECKeyPair::Generate(curveName);
    auto sdkPrivateKeyForEncrypt = sdkECKeyPair->PrivateKeyInPEMFormat();
    auto sdkPublicKeyForEncrypt = sdkECKeyPair->PublicKeyInPEMFormat();

    std::cout << "SDK private key for encrypt: " << sdkPrivateKeyForEncrypt <<'\n';
    std::cout << "SDK public key for encrypt: " << sdkPublicKeyForEncrypt <<'\n';

    // -----------------------------------------------------------------
    // (KAS-Side) Generate an EC key pair
    // -----------------------------------------------------------------
    auto kasECKeyPair = ECKeyPair::Generate(curveName);
    auto kasPrivateKey = kasECKeyPair->PrivateKeyInPEMFormat();
    auto kasPublicKey = kasECKeyPair->PublicKeyInPEMFormat();

    std::cout << "kasPrivateKey: " << kasPrivateKey <<'\n';
    std::cout << "kasPublicKey: " << kasPublicKey <<'\n';

    // -----------------------------------------------------------------
    // (SDK-Side) Generate the shared secret using sdk private key, and kas public-key
    //
    // NOTE: The kas public-key will be in PEM format and we get this from
    // entity object.
    //  -----------------------------------------------------------------
    std::vector<gsl::byte> wrappedKeyOnEncrypt;
    wrappedKeyOnEncrypt = ECKeyPair::ComputeECDHKey(kasPublicKey, sdkPrivateKeyForEncrypt);
    auto base64WrappedKeyOnEncrypt = base64Encode(toBytes(wrappedKeyOnEncrypt));
    std::cout << "Base64 wrapped key on Encrypt: " << base64WrappedKeyOnEncrypt << std::endl;

    // -----------------------------------------------------------------
    // (SDK-Side) Encrypt the plain data with wrapped key(TDF blob)
    // -----------------------------------------------------------------
    auto TDFBlobSize = kPlainText.size() + KIvSize + KAuthTagSize;
    std::vector<gsl::byte> TDFBlob(TDFBlobSize);
    {
        auto encryptedData = toWriteableBytes(TDFBlob);

        ByteArray<KAuthTagSize> tag;
        ByteArray<KIvSize> iv = symmetricKey<KIvSize>();
        const auto bufferSpan = encryptedData;

        auto encryptedDataSize = 0;
        const auto final = finalizeSize(encryptedData, encryptedDataSize);

        // Adjust the span to add the IV vector at the start of the buffer
        auto encryptBufferSpan = bufferSpan.subspan(KIvSize);

        auto encoder = GCMEncryption::create(toBytes(wrappedKeyOnEncrypt), iv);
        encoder->encrypt(toBytes(kPlainText), encryptBufferSpan);

        auto authTag = WriteableBytes{tag};
        encoder->finish(authTag);

        // Copy IV at start
        std::copy(iv.begin(), iv.end(), encryptedData.begin());

        // Copy tag at end
        std::copy(tag.begin(), tag.end(), encryptedData.begin() + KIvSize + kPlainText.size());

        // Final size.
        encryptedDataSize = TDFBlobSize;

        auto base64TDFBlob = base64Encode(encryptedData);
        std::cout << "Base64 of TDFBlob of data: " << base64TDFBlob << std::endl;
    }

    // -----------------------------------------------------------------
    // (SDK-Side) Store SDK public key as (33 Bytes, compressed ) in nanoTDF for 256-bit curve
    // -----------------------------------------------------------------
    std::vector<gsl::byte> compressedPubKey = ECKeyPair::CompressedECPublicKey(sdkPublicKeyForEncrypt);
    std::cout << "Compressed public key size: " << compressedPubKey.size() << std::endl;
    BOOST_TEST(compressedPubKey.size() == compressedPubKeySize,  "Checking the compressed public key size");

    auto pemPub = ECKeyPair::GetPEMPublicKeyFromECPoint(toBytes(compressedPubKey), curveName);
    BOOST_TEST(pemPub == sdkPublicKeyForEncrypt,  "Checking the sdk public is same after compression");

    ///******************************** ENCRYPTION COMPLETED ON SDK *******************************///

    // SDK side key-pair for rewrap
    sdkECKeyPair = ECKeyPair::Generate(curveName);
    auto sdkPrivateKeyForDecrypt = sdkECKeyPair->PrivateKeyInPEMFormat();
    auto sdkPublicKeyForDecrypt = sdkECKeyPair->PublicKeyInPEMFormat();

    // -----------------------------------------------------------------
    // (KAS-Side) Rewrap
    //
    // SDK ---> 'sdkPublicKeyForDecrypt' and 'compressed 33 bytes key from TDF' ---> KAS
    //
    // -----------------------------------------------------------------

    // -----------------------------------------------------------------
    // (KAS-Side) KAS computing the symmetric key.
    // -----------------------------------------------------------------
    std::vector<gsl::byte> symmetricKeyOnKas;
    symmetricKeyOnKas = ECKeyPair::ComputeECDHKey(sdkPublicKeyForEncrypt, kasPrivateKey);
    auto base64SymmetricKeyOnKas = base64Encode(toBytes(symmetricKeyOnKas));
    std::cout << "Base64 symmetric key on KAS: " << base64SymmetricKeyOnKas << std::endl;
    BOOST_TEST(base64SymmetricKeyOnKas == base64WrappedKeyOnEncrypt,  "symmetric key is same on SDK and KAS");

    kasECKeyPair = ECKeyPair::Generate(curveName);
    auto kasRewrapEphemeralPrivateKey = kasECKeyPair->PrivateKeyInPEMFormat();
    auto kasRewrapEphemeralPublicKey = kasECKeyPair->PublicKeyInPEMFormat();

    // Generate a session key on KAS
    auto sessionKeyOnKAS = ECKeyPair::ComputeECDHKey(sdkPublicKeyForDecrypt, kasRewrapEphemeralPrivateKey);
    auto base64SessionKeyOnKAS = base64Encode(toBytes(sessionKeyOnKAS));
    std::cout << "Base64 session key on KAS: " << base64SessionKeyOnKAS << std::endl;

    // Encrypt symmetricKeyOnKas with sessionKey on Kas -> payload KEK
    auto payloadKeKBufferSize = symmetricKeyOnKas.size() + KIvSize + KAuthTagSize;
    std::vector<gsl::byte> payloadKeK(payloadKeKBufferSize);
    {
        auto encryptedData = toWriteableBytes(payloadKeK);

        ByteArray<KAuthTagSize> tagForKek;
        ByteArray<KIvSize> iv = symmetricKey<KIvSize>();
        const auto bufferSpan = encryptedData;

        auto encryptedDataSize = 0;
        const auto final = finalizeSize(encryptedData, encryptedDataSize);

        // Adjust the span to add the IV vector at the start of the buffer
        auto encryptBufferSpan = bufferSpan.subspan(KIvSize);

        auto encoder = GCMEncryption::create(toBytes(sessionKeyOnKAS), iv);
        encoder->encrypt(toBytes(symmetricKeyOnKas), encryptBufferSpan);
        auto authTag = WriteableBytes{tagForKek};
        encoder->finish(authTag);

        // Copy IV at start
        std::copy(iv.begin(), iv.end(), encryptedData.begin());

        // Copy tag at end
        std::copy(tagForKek.begin(), tagForKek.end(), encryptedData.begin() + KIvSize + symmetricKeyOnKas.size());

        // Final size.
        encryptedDataSize = payloadKeKBufferSize;

        auto base64PayLoadKek = base64Encode(encryptedData);
        std::cout << "Base64 of payloadKEK: " << base64PayLoadKek << std::endl;
    }

    // -----------------------------------------------------------------
    // (SDK-Side) Rewrap
    //
    // KAS ---> 'payload KEK' and 'RewrapEphemeralPublic' ---> SDK
    //
    // -----------------------------------------------------------------

    // Generate a session key on SDK.
    auto sessionKeyOnSDK = ECKeyPair::ComputeECDHKey(kasRewrapEphemeralPublicKey, sdkPrivateKeyForDecrypt);
    auto base64SessionKeyOnSDK = base64Encode(toBytes(sessionKeyOnSDK));
    std::cout << "Base64 session key on SDK: " << base64SessionKeyOnSDK << std::endl;
    BOOST_TEST(base64SessionKeyOnKAS == base64SessionKeyOnSDK,  "session key should be same on SDK and KAS");

    // ON SDK - Decrypt 'payload KEK' with sessionKey to get a symmetricKey to decrypt.
    std::vector<gsl::byte> wrappedKeyOnRewrap;
    wrappedKeyOnRewrap.resize(payloadKeK.size() - KIvSize - KAuthTagSize);
    {
        // payloadKeK is data
        auto data = toBytes(payloadKeK);

        // Copy the auth tag from the data buffer.
        ByteArray<KAuthTagSize> tag;
        std::copy_n(data.last(KAuthTagSize).data(), KAuthTagSize, begin(tag));

        // Update the input buffer size after the auth tag is copied.
        auto inputSpan = data.first(data.size() - KAuthTagSize);
        auto decoder = GCMDecryption::create(toBytes(sessionKeyOnSDK), inputSpan.first(KIvSize));

        // Update the input buffer size after the IV is copied.
        inputSpan = inputSpan.subspan(KIvSize);

        // decrypt
        auto decryptedData = toWriteableBytes(wrappedKeyOnRewrap);
        decoder->decrypt(inputSpan, decryptedData);

        auto authTag = WriteableBytes{tag};
        decoder->finish(authTag);
    }

    auto base64wrappedKeyOnRewrap = base64Encode(toBytes(wrappedKeyOnRewrap));
    std::cout << "Base64 symmetric key on KAS: " << base64wrappedKeyOnRewrap << std::endl;
    BOOST_TEST(base64WrappedKeyOnEncrypt == base64wrappedKeyOnRewrap,  "symmetric key is same on encrypt and decrypt");

    // -----------------------------------------------------------------
    // (SDK-Side) TDFBlob decrypt to get a plain text.
    // -----------------------------------------------------------------
    std::vector<gsl::byte> TDFBlobDecrypt;
    TDFBlobDecrypt.resize(kPlainText.size());
    {
        // TDFBlob is data to be decrypt
        auto data = toBytes(TDFBlob);

        // Copy the auth tag from the data buffer.
        ByteArray<KAuthTagSize> tag;
        std::copy_n(data.last(KAuthTagSize).data(), KAuthTagSize, begin(tag));

        // Update the input buffer size after the auth tag is copied.
        auto inputSpan = data.first(data.size() - KAuthTagSize);
        auto decoder = GCMDecryption::create(toBytes(wrappedKeyOnRewrap), inputSpan.first(KIvSize));

        // Update the input buffer size after the IV is copied.
        inputSpan = inputSpan.subspan(KIvSize);

        // decrypt
        auto decryptedData = toWriteableBytes(TDFBlobDecrypt);
        decoder->decrypt(inputSpan, decryptedData);

        auto authTag = WriteableBytes{tag};
        decoder->finish(authTag);
    }
    std::string decryptedTDF(reinterpret_cast<const char *>(&TDFBlobDecrypt[0]), TDFBlobDecrypt.size());
    BOOST_TEST(kPlainText == decryptedTDF,  "TDF data decrypted successfully.");

}

BOOST_AUTO_TEST_SUITE(test_ec_key_pair_suite)

    BOOST_AUTO_TEST_CASE(ec_key_pair_curve_secp521r1)
    {
        const std::string curveName = "secp521r1";
        auto eckeyPair = ECKeyPair::Generate(curveName);
        auto privateKey = eckeyPair->PrivateKeyInPEMFormat();
        auto publicKey = eckeyPair->PublicKeyInPEMFormat();

        std::cout << "Private key: " << privateKey <<'\n';
        std::cout << "Public key: " << publicKey <<'\n';

        unsigned int secp521KeySize = 521;
        BOOST_TEST(eckeyPair->KeySize() == secp521KeySize, "Checking EC key length - key size 521 bits");
        BOOST_TEST(eckeyPair->CurveName() == curveName,  "Checking the curve name - secp521r1");
    }

    BOOST_AUTO_TEST_CASE(ec_key_pair_test_nano_tdf_crypto)
    {
        using namespace virtru::nanotdf;

        testNanoTDFKeyMangement(ECCMode::GetEllipticCurveName(EllipticCurve::SECP256R1),
                                ECCMode::GetECCompressedPubKeySize(EllipticCurve::SECP256R1));

        testNanoTDFKeyMangement(ECCMode::GetEllipticCurveName(EllipticCurve::SECP384R1),
                                ECCMode::GetECCompressedPubKeySize(EllipticCurve::SECP384R1));

        testNanoTDFKeyMangement(ECCMode::GetEllipticCurveName(EllipticCurve::SECP521R1),
                                ECCMode::GetECCompressedPubKeySize(EllipticCurve::SECP521R1));
    }

    BOOST_AUTO_TEST_CASE(test_ECDSA_signature_and_verify_with_only_private_key)
    {
        using namespace virtru::nanotdf;

        const std::string kPlainText = "Virtru!!";
        auto digest = calculateSHA256(toBytes(kPlainText));

        std::string supportedCurve[] = {
                ECCMode::GetEllipticCurveName(EllipticCurve::SECP256R1),
                ECCMode::GetEllipticCurveName(EllipticCurve::SECP384R1),
                ECCMode::GetEllipticCurveName(EllipticCurve::SECP521R1)};

        for (const auto& curveName: supportedCurve) {

            auto signerECKeyPair = ECKeyPair::Generate(curveName);
            auto signerPrivateKey = signerECKeyPair->PrivateKeyInPEMFormat();

            // Generate public key from private key
            auto publicKey = ECKeyPair::GetPEMPublicKeyFromPrivateKey(signerPrivateKey, curveName);

            // Calculate signature with signer private key
            auto signature =  ECKeyPair::ComputeECDSASig(toBytes(digest), signerPrivateKey);

            bool result = ECKeyPair::VerifyECDSASignature(toBytes(digest), toBytes(signature), publicKey);
            BOOST_TEST(result);
        }
    }

BOOST_AUTO_TEST_SUITE_END()


// Copyright (c) 2011-2014 The Bitcoin Core developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "main.h"
#include "miner.h"
#include "uint256.h"
#include "util.h"

#include <boost/test/unit_test.hpp>
// miners obey an exponential distribution
#include <boost/random/exponential_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/function.hpp>

static boost::mt19937 prng;

int VersionForAlgo(int algo)
{
    switch (algo)
    {
        case ALGO_SCRYPT:
            return BLOCK_VERSION_DEFAULT;
        case ALGO_SHA256D:
            return BLOCK_VERSION_DEFAULT | BLOCK_VERSION_SHA256D;
        case ALGO_GROESTL:
            return BLOCK_VERSION_DEFAULT | BLOCK_VERSION_GROESTL;
        case ALGO_SKEIN:
            return BLOCK_VERSION_DEFAULT | BLOCK_VERSION_SKEIN;
        case ALGO_QUBIT:
            return BLOCK_VERSION_DEFAULT | BLOCK_VERSION_QUBIT;
    }
    assert(false);
    return 0;
}

double BitsToDifficulty(int nBits)
{
    CBigNum target;
    target.SetCompact(nBits);
    // approximate
    return pow(2, 256-32)/target.getuint256().getdouble();
}

void SimulateNextBlock(CBlockIndex*& tip, double* hashRates)
{
    double recTime = 1e9;
    CBlockIndex* newTip = new CBlockIndex;
    newTip->pprev = tip;
    newTip->nHeight = tip->nHeight+1;
    for (int i=0; i<NUM_ALGOS; i++)
    {
        if (hashRates[i] == 0) continue;
        CBigNum target;
        unsigned int nBits = GetNextWorkRequired(tip, NULL, i, false);
        target.SetCompact(nBits);
        double dtarget = target.getuint256().getdouble()/pow(2,256);
        double mean = 1/(hashRates[i]*dtarget);
        boost::exponential_distribution<double> distr(1/mean);
        double received = distr(prng);
        if (received < recTime)
        {
            recTime = received;
            newTip->nVersion = VersionForAlgo(i);
            newTip->nTime = tip->nTime + received;
            newTip->nBits=nBits;
        }
    }
    assert(recTime < 1e9);
    tip = newTip;
    return;
}

void SimulationSetup(CBlockIndex*& tip, double* hashRates, int height)
{
    assert(tip==NULL);
    // give it a perfect setup
    for(int h=0, c=20*NUM_ALGOS-1; h<20; h++)
    for(int i=0; i<NUM_ALGOS; i++, c--)
    {
        CBlockIndex* newTip = new CBlockIndex;
        newTip->pprev = tip;
        newTip->nHeight = height - c;
        newTip->nTime = newTip->nHeight * 30;
        newTip->nVersion = VersionForAlgo(i);
        double dtarget = pow(2, 256)/hashRates[i] / 150;
        // no base_uint::setdouble()? no problem
        uint256 target;
        for(int b=255; b>=0; b--)
        {
            uint256 newTarget = target + (uint256(1)<<b);
            if (newTarget.getdouble() < dtarget)
                target = newTarget;
        }
        newTip->nBits = CBigNum(target).GetCompact();
        tip = newTip;
    }
}

double ConstantRate(const CBlockIndex* pBlock)
{
    return pow(2,32);
}

double MultipoolRate(const CBlockIndex* pBlock)
{
    if (!pBlock || pBlock->nHeight < 400500)
        return pow(2,32);
    if (pBlock->nHeight < 401000)
        return 10*pow(2,32);
    return pow(2,32);
}

void RunSimulation(boost::function<double(const CBlockIndex*)>* hashRate, int startHeight, int iterations)
{
    CBlockIndex* tip = NULL;
    double rate[5];
    for(int i=0; i<5; i++)
        rate[i]=hashRate[i](tip);
    SimulationSetup(tip, rate, startHeight);
    printf("begin simulation:\n");
    for(int it=0; it<iterations; it++)
    {
        for(int i=0; i<5; i++)
            rate[i]=hashRate[i](tip);
        SimulateNextBlock(tip, rate);

        double diffsum=0;
        for(int i=0; i<5; i++)
            diffsum+=BitsToDifficulty(GetNextWorkRequired(tip, NULL, i, false));

        printf("height %d algo %7s time %3d diff %8.3f, avg next diff %7.3f\n",
            tip->nHeight,
            GetAlgoName(GetAlgo(tip->nVersion)).c_str(),
            tip->nTime - tip->pprev->nTime,
            BitsToDifficulty(tip->nBits), diffsum/5);
    }
}

BOOST_AUTO_TEST_SUITE(difficulty_simulation)

// constant hashrate test
BOOST_AUTO_TEST_CASE(constant_rate)
{
    boost::function<double(const CBlockIndex*)> hashRate[5] = {
        ConstantRate,
        ConstantRate,
        ConstantRate,
        ConstantRate,
        ConstantRate,
    };
    RunSimulation(hashRate, 400000, 1000);
}

// a multipool hops on sha at block 500, then off at block 1000
BOOST_AUTO_TEST_CASE(multipool)
{
    boost::function<double(const CBlockIndex*)> hashRate[5] = {
        MultipoolRate,
        ConstantRate,
        ConstantRate,
        ConstantRate,
        ConstantRate,
    };
    RunSimulation(hashRate, 400000, 1500);
}

BOOST_AUTO_TEST_SUITE_END()


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

#include <OpenTissue/core/math/big/big_types.h>
#include <OpenTissue/core/math/big/big_generate_PD.h>
#include <OpenTissue/core/math/big/big_generate_PSD.h>
#include <OpenTissue/core/math/big/big_generate_random.h>
#include <OpenTissue/core/math/optimization/optimization_non_smooth_newton.h>
#include <OpenTissue/core/math/big/big_gmres.h>
#include <OpenTissue/core/math/big/big_svd.h>

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

template <typename vector_type>
class BoundFunction
  {
  public:

    typedef typename vector_type::size_type          size_type;
    typedef typename vector_type::value_type         real_type;
    typedef OpenTissue::math::ValueTraits<real_type> value_traits;

    bool m_is_lower;

  public:

    BoundFunction(bool const & is_lower)
    : m_is_lower(is_lower)
    {}

    real_type operator()(vector_type const & x, size_type const & i) const
    {
      size_type r = (i%3);

      if(r==0)
        return m_is_lower ? value_traits::zero() : value_traits::infinity();

      real_type mu_i = value_traits::one();
      size_type j = i - r;
      return m_is_lower ?  -mu_i*x(j) : mu_i*x(j);
    }

    class vector_iterator
    {
    protected:

      bool m_end;
      size_type m_idx;
      real_type m_mu;

    public:

      vector_iterator()
      : m_end(true)
      , m_idx(0)
      , m_mu(value_traits::zero())
      {}

      vector_iterator(vector_iterator const & i)
      {
        *this = i;
      }

      vector_iterator(size_type const & idx, real_type const & mu)
      : m_end(false)
      , m_idx(idx)
      , m_mu(mu)
      {}

      bool const operator==(vector_iterator const & i) const     {      return this->m_end == i.m_end;     }
      bool const operator!=(vector_iterator const & i) const {  return !( (*this) == i); }

      size_t const index() const { return m_idx; }

      real_type operator*() const { return m_mu; }

      vector_iterator & operator=(vector_iterator const & i)
      {
        this->m_end = i.m_end;
        this->m_idx = i.m_idx;
        this->m_mu = i.m_mu;
        return *this;
      }

      vector_iterator const & operator++()
      {
        m_end = true;
        return *this;
      }
    };

    vector_iterator partial_begin(size_type const & idx) const
    {
      size_type r = (idx%3);

      if(r==0)
        return vector_iterator();

      real_type mu_i = value_traits::one();
      size_type j = idx - r;
      return m_is_lower ? vector_iterator(j,-mu_i) :  vector_iterator(j, mu_i);
    }

    vector_iterator partial_end(size_type const & idx) const {  return vector_iterator(); }

  };

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

  BoundFunction<vector_type> l(true);
  BoundFunction<vector_type> u(false);

  size_type max_iterations       = 100;
  real_type absolute_tolerance   = boost::numeric_cast<real_type>(1e-6);
  real_type relative_tolerance   = boost::numeric_cast<real_type>(0.000000001);
  real_type stagnation_tolerance = boost::numeric_cast<real_type>(0.000000001);
  size_t status = 0;
  size_type iteration = 0;
  real_type accuracy = boost::numeric_cast<real_type>(0.0);
  real_type alpha = boost::numeric_cast<real_type>(0.0001);
  real_type beta = boost::numeric_cast<real_type>(0.5);
  bool use_shur = true;

  OpenTissue::math::optimization::non_smooth_newton(
                                                    A, b, l , u, x
                                                    , max_iterations
                                                    , absolute_tolerance
                                                    , relative_tolerance
                                                    , stagnation_tolerance
                                                    , status
                                                    , iteration
                                                    , accuracy
                                                    , alpha
                                                    , beta
                                                    , &OpenTissue::math::big::svd<matrix_type, vector_type>
                                                    , use_shur
                                                    );

  cnt_status[status]++;

  if(status==OpenTissue::math::optimization::ABSOLUTE_CONVERGENCE)
  {
    BOOST_CHECK( accuracy < absolute_tolerance );
    BOOST_CHECK( iteration <= max_iterations );
  }
  std::cout << "absolute " << accuracy << " iterations " << iteration  << " status " << OpenTissue::math::optimization::get_error_message(status) << std::endl;
}

BOOST_AUTO_TEST_SUITE(opentissue_math_big_non_smooth_newton);

BOOST_AUTO_TEST_CASE(random_test_case)
{

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

  size_t cnt_status[7] = {0, 0, 0, 0, 0, 0, 0};

  size_type N = 10;

  matrix_type A;
  vector_type x;
  vector_type b;
  vector_type y;

  A.resize(N,N,false);
  x.resize(N,false);
  b.resize(N,false);
  y.resize(N,false);

  for(size_type tst=0;tst<100;++tst)
  {
    OpenTissue::math::big::fast_generate_PD(N,A);
    OpenTissue::math::big::generate_random( N, y);
    b.assign(-y);
    OpenTissue::math::big::generate_random( N, y);
    x.clear();

    test(A,x,b,y, &cnt_status[0]);
  }

  for(size_t i=0;i<7;++i)
    std::cout << cnt_status[i] << "\t:\t"<< OpenTissue::math::optimization::get_error_message( i ) << std::endl;
  std::cout << std::endl;
}

BOOST_AUTO_TEST_SUITE_END();


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 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_EULER_FUNCTIONS_SIMD_COMMON_ERFC_HPP_INCLUDED
#define NT2_EULER_FUNCTIONS_SIMD_COMMON_ERFC_HPP_INCLUDED

#include <nt2/euler/functions/erfc.hpp>
#include <nt2/euler/functions/details/erf_kernel.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/twothird.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/functions/simd/abs.hpp>
#include <nt2/include/functions/simd/divides.hpp>
#include <nt2/include/functions/simd/exp.hpp>
#include <nt2/include/functions/simd/if_else.hpp>
#include <nt2/include/functions/simd/inbtrue.hpp>
#include <nt2/include/functions/simd/is_less.hpp>
#include <nt2/include/functions/simd/is_ltz.hpp>
#include <nt2/include/functions/simd/logical_andnot.hpp>
#include <nt2/include/functions/simd/minus.hpp>
#include <nt2/include/functions/simd/multiplies.hpp>
#include <nt2/include/functions/simd/oneminus.hpp>
#include <nt2/include/functions/simd/oneplus.hpp>
#include <nt2/include/functions/simd/plus.hpp>
#include <nt2/include/functions/simd/splat.hpp>
#include <nt2/include/functions/simd/sqr.hpp>
#include <nt2/include/functions/simd/unary_minus.hpp>
#include <nt2/sdk/meta/as_logical.hpp>
#include <nt2/sdk/meta/cardinal_of.hpp>

#include <boost/simd/sdk/config.hpp>

#ifndef BOOST_SIMD_NO_INFINITIES
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/functions/simd/if_zero_else.hpp>
#include <nt2/include/functions/simd/is_equal.hpp>
#endif

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( erfc_, tag::cpu_
                              , (A0)(X)
                              , ((simd_<double_<A0>,X>))
                              )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
    {
      typedef typename meta::as_logical<A0>::type bA0;

      A0 x =  nt2::abs(a0);
      A0 xx =  nt2::sqr(x);
      A0 lim1 = nt2::splat<A0>(0.65);
      A0 lim2 = nt2::splat<A0>(2.2);
      bA0 test0 = nt2::is_ltz(a0);
      bA0 test1 = nt2::lt(x, lim1);
      A0 r1 = nt2::Zero<A0>();
      std::size_t nb = nt2::inbtrue(test1);
      if(nb > 0)
      {
        r1 = nt2::oneminus(x*details::erf_kernel<A0>::erf1(xx));
        if (nb >= meta::cardinal_of<A0>::value)
          return nt2::if_else(test0, nt2::Two<A0>()-r1, r1);
      }
      bA0 test2 = nt2::lt(x, lim2);
      bA0 test3 = nt2::logical_andnot(test2, test1);
      A0 ex = nt2::exp(-xx);

      std::size_t nb1 = nt2::inbtrue(test3);
      if(nb1 > 0)
      {
        A0 z = ex*details::erf_kernel<A0>::erfc2(x);
        r1 = nt2::if_else(test1, r1, z);
        nb += nb1;
        if (nb >= meta::cardinal_of<A0>::value)
          return nt2::if_else(test0, Two<A0>()-r1, r1);
      }
      A0 z =  ex*details::erf_kernel<A0>::erfc3(x);
      r1 = nt2::if_else(test2, r1, z);
      #ifndef BOOST_SIMD_NO_INFINITIES
      r1 = if_zero_else( eq(x, Inf<A0>()), r1);
      #endif
      return  nt2::if_else(test0, nt2::Two<A0>()-r1, r1);
      }
  };

  BOOST_DISPATCH_IMPLEMENT  ( erfc_, tag::cpu_
                              , (A0)(X)
                              , ((simd_<single_<A0>,X>))
                              )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
    {
      typedef typename meta::as_logical<A0>::type bA0;

      A0 x =  nt2::abs(a0);
      bA0 test0 = nt2::is_ltz(a0);
      A0 r1 = nt2::Zero<A0>();
      bA0 test1 = nt2::lt(x, Twothird<A0>());
      A0 z = x/oneplus(x);

      std::size_t nb = nt2::inbtrue(test1);
      if(nb > 0)
      {
        r1 = details::erf_kernel<A0>::erfc3(z);
        if (nb >= meta::cardinal_of<A0>::value)
          return nt2::if_else(test0, nt2::Two<A0>()-r1, r1);
      }
      z -= nt2::splat<A0>(0.4);
      A0 r2 = exp(-sqr(x))*details::erf_kernel<A0>::erfc2(z);
      r1 = if_else(test1, r1, r2);
      #ifndef BOOST_SIMD_NO_INFINITIES
      r1 = if_zero_else( eq(x, Inf<A0>()), r1);
      #endif
      return nt2::if_else(test0, nt2::Two<A0>()-r1, r1);
    }
  };
} }

#endif


#include <iostream>
#include <fstream>
#include <armadillo>
#include <cmath>
#include <string>
#include <unordered_map>
#include "scf.h"
using namespace std;
using namespace arma;
SCF::SCF(){
  //Read in the object name like H2O
  std::string path_data = "/home/xiuyiqin/GF3ES/Test_SCF/data/";
  ifstream iptcoor(path_data+"inputfile.com");
  //In the input file never try to turn on the ERI symmetry, it will cause errors
  std::string data;
  string judge1 = "Obj=";
  string judge2 = "OCC=";
  while(!iptcoor.eof()){
      iptcoor >> data;
        if (data.compare(judge1)==0){
            iptcoor>>data;
            objname = data;
        }
        if (data.compare(judge2)==0){
            iptcoor>>data;
            _numoccp = std::stoi(data,nullptr,10);
        }
  }
  if (_numoccp == 0){
      printf("Please put number of occupied orbits in inputfile.com");
      exit(EXIT_FAILURE);
  }
  //Read in the 3 input files
  std::string overlapfile = "_overlap.dat";
  std::string coreHfile = "_coreH.dat";
  std::string erifile = "_eri.dat";
  std::string enucfile = "_enuc_Nb.dat";
  ifstream iptovlap(path_data+objname+overlapfile);
  ifstream iptcoreH(path_data+objname+coreHfile);
  ifstream ipteri(path_data+objname+erifile);
  ifstream iptenuc(path_data+objname+enucfile);

  while(!iptenuc.eof()){
  iptenuc>>_nucrepul;
  iptenuc>>_numorbit;
  }
  //to set up ioff:
  _ioff.resize(pow(_numorbit,4));
  _ioff[0] = 0;
  for(int i=1; i < pow(_numorbit,4); i++)
  _ioff[i] = _ioff[i-1] + i;
  //set up matrix
  _ovlap = new arma::mat(_numorbit,_numorbit);
  _coreHam = new arma::mat(_numorbit,_numorbit);
  _Pini = new arma::mat(_numorbit,_numorbit);
  _Pnext = new arma::mat(_numorbit,_numorbit);
  _Fini = new arma::mat(_numorbit,_numorbit);
  _Fnext = new arma::mat(_numorbit,_numorbit);
  int i = 0, j = 0,k = 0, l = 0;
  double temp,temp2;
  printf("T3");
  /*
  to input overlap integral
  and core Hamiltonian
  and nuclear repulsive energy and No of orbits
  */
   // overlap and core Hamiltonian is same size so I put them in same while
  while(!iptovlap.eof()){
  iptovlap >> i >> j >> temp;
  iptcoreH >> i >> j >> temp2;
  (*_ovlap)(i -1, j -1) = temp;
  (*_ovlap)(j -1, i -1 ) = temp;
  (*_coreHam)(i -1, j -1) = temp2;
  (*_coreHam)(j -1, i -1 ) = temp2;
  }
  /*two electron integral */
  double temp4;
  while (!ipteri.eof()) {
    ipteri >> i >> j >> k >> l >> temp4;
    _twoelec[getijkl(i, j, k, l)] = temp4;
  }
  iptcoor.close();iptovlap.close();iptcoreH.close();
  ipteri.close();iptenuc.close();
}

int SCF::getijkl(int i, int j, int k, int l){
  int ij = 0, kl = 0, ijkl = 0;
  ij = (i > j) ? _ioff[i] + j : _ioff[j] + i;
  kl = (k > l) ? _ioff[k] + l : _ioff[l] + k;
  ijkl = (ij > kl) ? _ioff[ij] + kl : _ioff[kl] + ij;
  return ijkl;
}

SCF::~SCF(){
  delete _ovlap;
  delete _coreHam;
}

void SCF::print(arma::mat ipt){
  for (size_t i = 0; i < _numorbit; i++) {
    printf("%s\n", " ");
    for (size_t j = 0; j < _numorbit; j++) {
    printf("%20.12f, %s", (ipt)(i, j), " ");
    }
  }
}

void SCF::calculation(){
  /*diagonlize overlap Matrix*/
  printf("T9");
  arma::vec Seigval;
  arma::mat Seigvec;
  arma::eig_sym(Seigval, Seigvec, *_ovlap);
  /*to get diagonlized eigenvalue matrix
  *arma::mat eigvalmat = eigvec.t() * (*_ovlap) * eigvec;//works good
  but the following one would be easier to use*/
  arma::mat Seigvalmat = arma::diagmat(Seigval);
  // to get Orthogonalization Matrix
  //So element-wise inverse and square-root ! except ij term in matrix!

  /*arma::mat et = 1/ sqrt(abs(eigvalmat));
  *arma::mat S_ihalf = arma::eye(_numorbit,_numorbit);
  for (size_t i = 0; i < _numorbit; i++) {
      S_ihalf(i, i) = sqrt(1.0 /eigvalmat(i,i));
  } this one works but the following one would be more neat
  */
  arma::mat lmd_sqrti = arma::sqrt(arma::inv(Seigvalmat));

  _Ssqrtinv = Seigvec * lmd_sqrti * Seigvec.t();//different from website But I believe it caused by different order of eigenvalue? No it is caused by wrong input file
  /*temporaly moving on
  * get the Fock matrix for inital guess from hailtonian
  * then transfer to orthogonal basis to get Fock prime
  * then diagonlize Fock! in the orthogonal basis!
  * then transfer back to original basis which is the atomic orbitals
  */
  *_Fini = *_coreHam;
  _Finiprime = _Ssqrtinv.t() * (*_coreHam) * _Ssqrtinv;
  //digonalize Fock Matrix
  arma::vec Feigval;
  arma::mat Feigvec;
  arma::eig_sym(Feigval, Feigvec, _Finiprime);
  arma::mat Coe_orig = _Ssqrtinv * Feigvec;
  //density matrix equals to C * C.t()
  *_Pini = Coe_orig.cols(0, _numoccp -1) *  Coe_orig.cols(0, _numoccp -1).t();
  //then get the Energy
  arma::mat Eini = (*_Pini) % ((*_coreHam) + *_Fini);
  _Eini = arma::accu(Eini);
  _Eini += _nucrepul;
  printf("%f\n", _Eini);

  /*iteration!
  */
  while(true){
  /*build new Fock ! by old density matrix */
  for (size_t i = 0; i < _numorbit; i++) {
      for (size_t j = 0; j < _numorbit; j++) {
        (*_Fnext)(i,j) = (*_coreHam)(i,j);
        for (size_t k = 0; k < _numorbit; k++) {
          for (size_t l = 0; l < _numorbit; l++) {
            (*_Fnext)(i,j) += (*_Pini)(k,l) * (2 * _twoelec[getijkl(i+1,j+1,k+1,l+1)] - _twoelec[getijkl(i+1,k+1,j+1,l+1)]);
          }
        }
      }
    }
    _Finiprime = _Ssqrtinv.t() * (*_Fnext) * _Ssqrtinv;
    /* to get density matrix and get Energy again..
    */
    arma::vec eigval;
    arma::mat eigvec;
    arma::eig_sym(eigval, eigvec, _Finiprime);
    arma::mat Coe = _Ssqrtinv * eigvec;
    //density matrix equals to C * C.t()
    *_Pnext = Coe.cols(0, _numoccp -1) *  Coe.cols(0, _numoccp -1).t();
    //print(*_Pnext);
    _Enext = arma::accu((*_Pnext) % ((*_coreHam) + *_Fnext)) + _nucrepul;
    printf("%20.12f\n", _Enext);
    if(abs(_Enext - _Eini) < 0.00000000001)
    break;
    _Eini = _Enext;
    *_Fini = *_Fnext;
    *_Pini = *_Pnext;
  }
}


// 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_PC_ILUT_INCLUDE
#define ITL_PC_ILUT_INCLUDE

#include <boost/numeric/mtl/vector/sparse_vector.hpp>
#include <boost/numeric/mtl/operation/invert_diagonal.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace itl { namespace pc {

struct ilut_factorizer
{

    template <typename Matrix, typename Para, typename L_type, typename U_type>
    ilut_factorizer(const Matrix &A, const Para& p, L_type& L, U_type& U)
    { factorize(A, p, L, U, mtl::traits::is_row_major<Matrix>()); }

    // column-major matrices are copied first
    template <typename Matrix, typename Para, typename L_type, typename U_type, bool B>
    void factorize(const Matrix &A, const Para& p, L_type& L, U_type& U, boost::mpl::bool_<B>)
    {
	typedef typename mtl::Collection<Matrix>::value_type      value_type;
	typedef typename mtl::Collection<Matrix>::size_type       size_type;
	typedef mtl::matrix::parameters<mtl::row_major, mtl::index::c_index, mtl::non_fixed::dimensions, false, size_type> para;
	typedef mtl::matrix::compressed2D<value_type, para>  LU_type;
	LU_type LU(A);
	factorize(LU, p, L, U, boost::mpl::true_());
    }

    // According Yousef Saad: ILUT, NLAA, Vol 1(4), 387-402 (1994)
#if 0
    template <typename Value, typename MPara, typename Para, typename L_type, typename U_type>
    factorize(const mtl::matrix::compressed2D<Value, MPara>& A, const Para& p, L_type& L, U_type& U, boost::mpl::true_)
#endif

    template <typename Matrix, typename Para, typename L_type, typename U_type>
    void factorize(const Matrix& A, const Para& p, L_type& L, U_type& U, boost::mpl::true_)

    {
	mtl::vampir_trace<5049> tracer;
	using std::abs; using mtl::traits::range_generator; using mtl::begin; using mtl::end;
	using namespace mtl::tag;
	MTL_THROW_IF(num_rows(A) != num_cols(A), mtl::matrix_not_square());

	typedef typename mtl::Collection<Matrix>::value_type      value_type;
	typedef typename mtl::Collection<Matrix>::size_type       size_type;
	typedef typename range_generator<row, Matrix>::type       cur_type;
	typedef typename range_generator<nz, cur_type>::type      icur_type;
	typename mtl::traits::col<Matrix>::type                   col(A);
	typename mtl::traits::const_value<Matrix>::type           value(A);

	size_type n= num_rows(A);
	L.change_dim(n, n);
	U.change_dim(n, n);
	{
	    mtl::matrix::inserter<L_type> L_ins(L, p.first);
	    mtl::matrix::inserter<U_type> U_ins(U, p.first + 1); // plus one for diagonal

	    mtl::vector::sparse_vector<value_type> vec(n); // corr. row in paper
	    cur_type ic= begin<row>(A); // , iend= end<row>(A);
	    for (size_type i= 0; i < n; ++i, ++ic) {

		for (icur_type kc= begin<nz>(ic), kend= end<nz>(ic); kc != kend; ++kc) // row= A[i][*]
		    vec.insert(col(*kc), value(*kc));
		// std::cerr << "vec_" << i << " = " << vec << std::endl;
		value_type tau_i= p.second * two_norm(vec); // threshold for i-th row
		// loop over non-zeros in vec; changes in vec considered
		for (size_type j= 0; j < vec.nnz() && vec.index(j) < i; j++) {
		    size_type k= vec.index(j);
		    value_type ukk= U_ins.value(k, k);
		    MTL_DEBUG_THROW_IF(ukk == value_type(0), mtl::missing_diagonal());
		    value_type vec_k= vec.value(j)/= ukk;
		    // std::cout << "vec after updating from U[" << k << "][" << k << "] is " << vec << '\n';
		    for (size_type j0= U_ins.ref_major()[k], j1= U_ins.ref_slot_ends()[k]; j0 < j1; j0++) { // U[k][k+1:n]
			size_type k1= U_ins.ref_minor()[j0];
			if (k1 > k)
			    vec[k1]-= vec_k * U_ins.ref_elements()[j0];
			// std::cout << "vec after updating from U[" << k << "][" << k1 << "] is " << vec << '\n';
		    }
		    // if (i > 1000 && i < 1010) std::cout << "vec before crop in row " << i << ", updating from row " << k << ": \n" << vec << "\n";
		    vec.crop(tau_i);
		    // if (i > 1000 && i < 1010) std::cout << "vec after crop: \n" << vec << "\n";
		}
		// std::cerr << "vec_" << i << " = " << vec << std::endl;
		vec.sort_on_data();
		// std::cerr << "vec_" << i << " sorted on data = " << vec << std::endl;

		// std::cout << "vec at " << i << " is " << vec << '\n';
		// mtl::vampir_trace<9904> tracer2;
		bool diag_found= false;
		for (size_type cntu= 0, cntl= 0, j= 0; j < vec.nnz() && (cntu < p.first || cntl < p.first); j++) {
		    size_type k= vec.index(j);
		    value_type v= vec.value(j);
		    // if (abs(v) < tau_i) break;
		    if (i == k) {
			U_ins[i][i] << v; diag_found= true;
		    } else if (i < k) {
			if (cntu++ < p.first)
			    U_ins[i][k] << v;
		    } else // i > k
			if (cntl++ < p.first)
			    L_ins[i][k] << v;
		}
		if (!diag_found) std::cerr << "Deleted diagonal!!!!\n";
		vec.make_empty();
	    }
	} // destroy inserters
	invert_diagonal(U);
    }
};

// Not usable yet !!!!!
template <typename Matrix, typename Value= typename mtl::Collection<Matrix>::value_type>
class ilut
  : public ilu<Matrix, ilut_factorizer, Value>
{
    typedef ilu<Matrix, ilut_factorizer, Value> base;
  public:
    ilut(const Matrix& A, std::size_t p, typename mtl::Collection<Matrix>::value_type tau)
      : base(A, std::make_pair(p, tau)) {}
};

}} // namespace itl::pc

#endif // ITL_PC_ILUT_INCLUDE


#define BOOST_TEST_MODULE matrix

#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/mpl/list.hpp>

#include <mla/matrix/all.h++>
#include <mla/vector/all.h++>
#include <mla/matrix/convert.h++>

#include <mla/solvers/umfpack.h++>

typedef boost::mpl::list<
	mla::matrix::SparseCCS<double>
> matrix_type_list;

BOOST_AUTO_TEST_SUITE(test_solvers)

BOOST_AUTO_TEST_CASE_TEMPLATE( solve_unit_1, MatrixType, matrix_type_list )
{
	size_t matrix_size = 6;

	typedef typename MatrixType::scalar_type Scalar;

	mla::matrix::DenseRowMajor<Scalar> from(matrix_size, matrix_size);
	MatrixType A(matrix_size, matrix_size);
	mla::vector::Dense<Scalar> x(matrix_size), b(matrix_size);

	Scalar value = 1.0f;

	for(unsigned int i = 0; i < matrix_size; i++)
	{
		from.setValue( i, i, (Scalar)1.0f);
		b.setValue( i, value);
	}

	mla::matrix::convert(from, A);

	mla::umfpack(A, x, b, NULL);

	for(unsigned int i = 0; i < matrix_size; i++)
	{
		BOOST_CHECK_CLOSE( x.getValue(i), 1.0f/b.getValue(i), 0.001f);
	}
}

BOOST_AUTO_TEST_CASE_TEMPLATE( solve_unit_2, MatrixType, matrix_type_list )
{
	size_t matrix_size = 6;

	typedef typename MatrixType::scalar_type Scalar;

	mla::matrix::DenseRowMajor<Scalar> from(matrix_size, matrix_size);
	MatrixType A(matrix_size, matrix_size);
	mla::vector::Dense<Scalar> x(matrix_size), b(matrix_size);

	Scalar value = 2.0f;

	for(unsigned int i = 0; i < matrix_size; i++)
	{
		from.setValue( i, i, (Scalar)1.0f);
		b.setValue( i, value);
	}

	mla::matrix::convert(from, A);

	mla::umfpack(A, x, b, NULL);

	for(unsigned int i = 0; i < matrix_size; i++)
	{
		BOOST_CHECK_CLOSE( x.getValue(i), b.getValue(i), 0.001f);
	}
}

BOOST_AUTO_TEST_CASE_TEMPLATE( solve_unit_3, MatrixType, matrix_type_list )
{
	size_t matrix_size = 6;

	typedef typename MatrixType::scalar_type Scalar;

	mla::matrix::DenseRowMajor<Scalar> from(matrix_size, matrix_size);
	MatrixType A(matrix_size, matrix_size);
	mla::vector::Dense<Scalar> x(matrix_size), b(matrix_size);

	Scalar value = 2.0f;

	for(unsigned int i = 0; i < matrix_size; i++)
	{
		from.setValue( i, i, value);
		b.setValue( i, (Scalar)1.0f);
	}

	mla::matrix::convert(from, A);

	mla::umfpack(A, x, b, NULL);

	for(unsigned int i = 0; i < matrix_size; i++)
	{
		BOOST_CHECK_CLOSE( x.getValue(i), 1.0f/value, 0.001f);
	}
}

BOOST_AUTO_TEST_SUITE_END()


#include <map>
#include <sstream>
#include <fstream>
#include <iostream>
#include <chrono>
#include <unordered_map>

#define TINYOBJLOADER_IMPLEMENTATION
#include "tiny_obj_loader.h"

#include <glm/gtx/norm.hpp>
#include <boost/lexical_cast.hpp>

#include "mesh.hpp"

void genNormals(Mesh& mesh)
{
	std::vector<glm::vec3> normals(mesh.v.size());

	for (int i = 0; i < mesh.t.size(); i += 3)
	{
		auto v0 = mesh.v[mesh.t[i + 0]];
		auto v1 = mesh.v[mesh.t[i + 1]];
		auto v2 = mesh.v[mesh.t[i + 2]];

		auto n = glm::normalize(glm::cross(v1 - v0, v2 - v0));

		normals[mesh.t[i + 0]] += n;
		normals[mesh.t[i + 1]] += n;
		normals[mesh.t[i + 2]] += n;
	}

	for (auto& normal : normals)
	{
		normal = glm::normalize(normal);
	}

	mesh.n = normals;
}

void genRandomColors(Mesh& mesh) {
	for (int i = 0; i < mesh.v.size(); i++) {
		mesh.AddColor(glm::vec4(
			(float)rand() / (float)RAND_MAX,
			(float)rand() / (float)RAND_MAX,
			(float)rand() / (float)RAND_MAX,
			1.0f
		));
	}
}

Mesh genIcosahedron()
{
	Mesh mesh;

	const float t = (1.0f + std::sqrt(5.0f)) / 2.0f;

	mesh.AddVert(glm::vec3(-1.0f, t, 0.0f));
	mesh.AddVert(glm::vec3(1.0f, t, 0.0f));
	mesh.AddVert(glm::vec3(-1.0f, -t, 0.0f));
	mesh.AddVert(glm::vec3(1.0f, -t, 0.0f));
	mesh.AddVert(glm::vec3(0.0f, -1.0, t));
	mesh.AddVert(glm::vec3(0.0f, 1.0, t));
	mesh.AddVert(glm::vec3(0.0f, -1.0, -t));
	mesh.AddVert(glm::vec3(0.0f, 1.0, -t));
	mesh.AddVert(glm::vec3(t, 0.0f, -1.0f));
	mesh.AddVert(glm::vec3(t, 0.0f, 1.0f));
	mesh.AddVert(glm::vec3(-t, 0.0f, -1.0f));
	mesh.AddVert(glm::vec3(-t, 0.0f, 1.0f));

	mesh.AddTri(0, 11, 5);
	mesh.AddTri(0, 5, 1);
	mesh.AddTri(0, 1, 7);
	mesh.AddTri(0, 7, 10);
	mesh.AddTri(0, 10, 11);
	mesh.AddTri(1, 5, 9);
	mesh.AddTri(5, 11, 4);
	mesh.AddTri(11, 10, 2);
	mesh.AddTri(10, 7, 6);
	mesh.AddTri(7, 1, 8);
	mesh.AddTri(3, 9, 4);
	mesh.AddTri(3, 4, 2);
	mesh.AddTri(3, 2, 6);
	mesh.AddTri(3, 6, 8);
	mesh.AddTri(3, 8, 9);
	mesh.AddTri(4, 9, 5);
	mesh.AddTri(2, 4, 11);
	mesh.AddTri(6, 2, 10);
	mesh.AddTri(8, 6, 7);
	mesh.AddTri(9, 8, 1);

	mesh.Normalize();
	mesh.radius = 1.0f;

	return mesh;
}

struct Edge
{
	int v0;
	int v1;

	Edge(int v0, int v1)
		: v0(v0 < v1 ? v0 : v1), v1(v0 < v1 ? v1 : v0) {}

	bool operator <(const Edge& rhs) const
	{
		return v0 < rhs.v0 || (v0 == rhs.v0 && v1 < rhs.v1);
	}
};

int subdivideEdge(int f0, int f1, const glm::vec3& v0, const glm::vec3& v1, Mesh& io_mesh, std::map<Edge, int>& io_divisions)
{
	const Edge edge(f0, f1);
	auto it = io_divisions.find(edge);
	if (it != io_divisions.end())
	{
		return it->second;
	}

	const glm::vec3 v = normalize((v0 + v1) * 0.5f);
	const int f = io_mesh.v.size();
	io_mesh.v.emplace_back(v);
	io_divisions.emplace(edge, f);
	return f;
}

void SubdivideMesh(const Mesh& meshIn, Mesh& meshOut)
{
	meshOut.v = meshIn.v;

	std::map<Edge, int> divisions; // Edge -> new vertex

	for (uint32_t i = 0; i < meshIn.t.size() / 3; ++i)
	{
		const int f0 = meshIn.t[i * 3];
		const int f1 = meshIn.t[i * 3 + 1];
		const int f2 = meshIn.t[i * 3 + 2];

		const glm::vec3 v0 = meshIn.v[f0];
		const glm::vec3 v1 = meshIn.v[f1];
		const glm::vec3 v2 = meshIn.v[f2];

		const int f3 = subdivideEdge(f0, f1, v0, v1, meshOut, divisions);
		const int f4 = subdivideEdge(f1, f2, v1, v2, meshOut, divisions);
		const int f5 = subdivideEdge(f2, f0, v2, v0, meshOut, divisions);

		meshOut.AddTri(f0, f3, f5);
		meshOut.AddTri(f3, f1, f4);
		meshOut.AddTri(f4, f2, f5);
		meshOut.AddTri(f3, f4, f5);
	}
}

Mesh genIcosphere(int subdivisions)
{
	Mesh m = genIcosahedron();
	Mesh m2;

	for (int i = 0; i < subdivisions; i++)
	{
		SubdivideMesh (m, m2);
		m.v = m2.v;
		m.t = m2.t;
		m2.Clear();
	}
	return m;
}

Mesh genRing(const int points, const glm::vec4 color)
{
	Mesh m;
	for (int i = 0; i < points; i++)
	{
		m.AddVert({ std::cosf(2.f * PI * ((float)i / points)), 0.f, std::sinf(2 * PI * ((float)i / points)) });
		m.AddVert({ std::cosf(2.f * PI * (((float)i + 1) / points)), 0.f, std::sinf(2 * PI * (((float)i + 1) / points)) });
		m.AddColor(color);
		m.AddColor(color);
	}
	m.radius = 1.f;

	return m;
}

void genGrid(Mesh& mesh, const float dim, const int segments, const int subgrids, const glm::vec4 color)
{
	float start = -0.5f * dim;
	float pos = start;
	const float step = dim / segments;
	for (int i = 0; i <= segments; i++)
	{
		mesh.AddVert(glm::vec3(start, 0, pos));
		mesh.AddVert(glm::vec3(-start, 0, pos));
		mesh.AddVert(glm::vec3(pos, 0, start));
		mesh.AddVert(glm::vec3(pos, 0, -start));
		pos += step;

		for (int j = 0; j < 4; j++)
			mesh.AddColor(color);
	}

	if (subgrids > 0)
		genGrid(mesh, dim, segments * 10, subgrids - 1, glm::vec4(color[0], color[1], color[2], 0.5f * color[3]));
}

Mesh genGrid(const float dim, const int segments, const int subgrids, const glm::vec4 color)
{
	Mesh m;
	genGrid(m, dim, segments, subgrids, color);

	if (subgrids > 0)
	{
		genGrid(m,
			dim,
			segments * 10,
			subgrids - 1,
			glm::vec4(color[0], color[1], color[2], 0.5f * color[3])
		);
	}
	return m;
}

Mesh genVector(const glm::vec3 start, const glm::vec3 end, const glm::vec4 color)
{
	Mesh m;
	m.AddVert(start);
	m.AddVert(end);
	m.AddColor(color);
	m.AddColor(color);
	return m;
}

// seek till digit, sign or eof
// signless for int
void seekNextNumber(std::istringstream& iss)
{
	while (!std::isdigit(iss.peek()) && iss.peek() != '-' && iss.peek() != EOF)
		iss.ignore(1, EOF);
}

bool extractFloats(std::istringstream& iss, float* coord)
{
	for (int i = 0; i < 3; i++)
	{
		seekNextNumber(iss);

		char next = iss.peek();
		std::string s;
		std::getline(iss, s, ' ');
		try
		{
			coord[i] = boost::lexical_cast<float>(s);
		}
		catch (boost::bad_lexical_cast&)
		{
			std::cout << "Bad cast: " << s << " : '" << next << "', eof: " << (next == EOF) << std::endl;
		}
	}

	return true;
}

bool getInt(std::istringstream& iss, std::string& s)
{
	// Look ahead til we get to end of integer
	auto start = iss.tellg();
	while (std::isdigit(iss.peek()))
		iss.ignore(1, EOF);

	char end = iss.peek();

	// Seek back
	iss.clear();
	iss.seekg(start);
	std::getline(iss, s, end);

	// Seek till digit, space or eof
	while (!std::isdigit(iss.peek()) && !std::isspace(iss.peek()) && iss.peek() != EOF)
		iss.ignore(1, EOF);

	// Return whether next is a slash
	return end == '/';
}

bool extractInts(std::istringstream& iss, float* values)
{
	std::string s;
	bool proceed;

	for (int i = 0; i < 3; i++)
	{
		seekNextNumber(iss);
		proceed = true;
		if (proceed)
		{
			proceed = getInt(iss, s);
			values[i + 0] = boost::lexical_cast<float>(s);
		}
		if (proceed)
		{
			proceed = getInt(iss, s);
			values[i + 3] = boost::lexical_cast<float>(s);
		}
		if (proceed)
		{
			proceed = getInt(iss, s);
			values[i + 6] = boost::lexical_cast<float>(s);
		}
	}

	return true;
}

Mesh LoadOBJ(const std::string path, const glm::vec3 offset)
{
	Mesh m;

	auto start = std::chrono::system_clock::now();
	std::ifstream infile(path);

	int lineCount = 0;
	int lineTotal = std::count(std::istreambuf_iterator<char>(infile),
		std::istreambuf_iterator<char>(), '\n');

	infile.seekg(0, std::ios::beg);

	std::string line;
	glm::dvec3 avg(0.0);

	int v = 0;
	int t = 0;

	while (std::getline(infile, line))
	{
		std::istringstream iss(line);
		if (iss.peek() == 'v')
		{
			float coord[3];
			iss.ignore(1, EOF);

			if (iss.peek() == 'n')
			{
				extractFloats(iss, coord);
				// TODO: vertex normal support
			}
			else
			{
				extractFloats(iss, coord);
				glm::vec3 vec = glm::vec3(coord[0], coord[1], coord[2]);
				m.AddVert(vec);
				avg += vec;
				v++;
			}
		}
		else if (iss.peek() == 'f')
		{
			float values[9];
			extractInts(iss, values);
			m.AddTri(values[0] - 1, values[1] - 1, values[2] - 1);
			t++;
		}
	}

	avg = avg / static_cast<double>(v);

	glm::vec3 avgVec = glm::vec3(avg);
	float maxlen = 0;
	for (auto& vert : m.v)
	{
		// Center to mid
		vert = (vert - avgVec);
		// Find furthest vertice (mesh radius)
		if (glm::length2(vert) > maxlen) maxlen = glm::length2(vert);
	}

	m.radius = sqrtf(maxlen);
	m.mid = glm::vec3(0.f);// avgVec;

	auto end = std::chrono::system_clock::now();
	std::chrono::duration<double> elapsed_seconds = end - start;

	std::cout
		<< "Loaded OBJ mesh, vertices: "
		<< m.v.size() << "/" << v
		<< ", triangles: "
		<< m.t.size() / 3 << "/" << t
		<< ", time: "
		<< elapsed_seconds.count() << "s"
		<< std::endl;

	return m;
}

// For identical vertex matching
struct Vertex
{
	size_t vx, vy, vz;
	size_t nx, ny, nz;
	size_t tu, tv;

	bool operator==(const Vertex& other) const
	{
		return (
			this->vx == other.vx &&
			this->vy == other.vy &&
			this->vz == other.vz &&
			this->nx == other.nx &&
			this->ny == other.ny &&
			this->nz == other.nz &&
			+this->tu == other.tu &&
			this->tv == other.tv
			);
	}
};

// Hash function for Vertex struct (sum of powers of two)
namespace std {
	template<> struct hash<Vertex>
	{
		std::size_t operator()(const Vertex& v) const noexcept
		{
			return (
				(v.vx * v.vx) + (v.vy * v.vy) + (v.vz * v.vz) +
				(v.nx * v.nx) + (v.ny * v.ny) + (v.nz * v.nz) +
				(v.tu * v.tu) + (v.tv * v.tv)
				);
		}
	};
}

Mesh LoadOBJFast(const std::string filename, const std::string path)
{
	auto start = std::chrono::system_clock::now();

	tinyobj::attrib_t attrib;
	std::vector<tinyobj::shape_t> shapes;
	std::vector<tinyobj::material_t> materials;

	std::string warn;
	std::string err;

	Mesh m;
	glm::dvec3 avg(0.0);

	bool loaded = tinyobj::LoadObj(&attrib, &shapes, &materials, &warn, &err, filename.c_str(), path.c_str());

	if (!warn.empty()) std::cout << warn << std::endl;
	if (!err.empty()) std::cerr << err << std::endl;
	if (!loaded)
	{
		std::cerr << "Failed to load OBJ: " << path << std::endl;
		return m;
	}
	if (shapes.empty())
	{
		std::cerr << "No shapes in OBJ: " << path << std::endl;
		return m;
	}
	if (shapes[0].mesh.num_face_vertices.empty())
	{
		std::cerr << "No vertices in OBJ: " << path << std::endl;
		return m;
	}
	if (shapes[0].mesh.num_face_vertices[0] != 3)
	{
		std::cerr << "Unsupported non-triangle primitives in OBJ: " << path << std::endl;
		return m;
	}

	unsigned idx = 0u;
	unsigned offset = 0u;
	std::unordered_map<Vertex, int> previous;

	for (unsigned face = 0; face < shapes[0].mesh.num_face_vertices.size(); face++)
	{
		for (unsigned vertex = 0; vertex < 3; vertex++)
		{
			auto v_idx = shapes[0].mesh.indices[offset + vertex];
			Vertex v = {
				3 * v_idx.vertex_index + 0,
				3 * v_idx.vertex_index + 1,
				3 * v_idx.vertex_index + 2,
				3 * v_idx.normal_index + 0,
				3 * v_idx.normal_index + 1,
				3 * v_idx.normal_index + 2,
				2 * v_idx.texcoord_index + 0,
				2 * v_idx.texcoord_index + 1
			};

			auto match = previous.find(v);
			if (match == previous.end()) // New vertex
			{
				glm::vec3 vec = glm::vec3(
					attrib.vertices[v.vx],
					attrib.vertices[v.vy],
					attrib.vertices[v.vz]
				);
				m.v.emplace_back(vec);
				avg += vec;

				if (v_idx.normal_index != -1)
				{
					m.n.emplace_back(glm::vec3(
						attrib.normals[v.nx],
						attrib.normals[v.ny],
						attrib.normals[v.nz]
					));
				}

				if (v_idx.texcoord_index != -1)
				{
					m.tx.emplace_back(glm::vec2(
						attrib.texcoords[v.tu],
						attrib.texcoords[v.tv]
					));
				}
				size_t index = previous.size();
				previous.insert({ v, index });
				m.t.emplace_back(index);
			}
			else
			{
				m.t.emplace_back(match->second);
			}
		}
		offset += 3;
	}

	float maxlen = 0;
	avg = avg / static_cast<double>(m.v.size());
	for (auto& vert : m.v)
	{
		// Center to mid
		vert = (vert - glm::vec3(avg));
		// Find furthest vertice (mesh radius)
		if (glm::length2(vert) > maxlen) maxlen = glm::length2(vert);
	}
	m.radius = sqrtf(maxlen);
	m.mid = glm::vec3(0.f);

	auto end = std::chrono::system_clock::now();
	std::chrono::duration<double> elapsed_seconds = end - start;

	std::cout
		<< "Loaded OBJ mesh, vertices: " << m.v.size()
		<< ", triangles: " << (float)m.t.size() / 3.f
		<< ", normals: " << m.n.size()
		//<< ", texcoords: " << m.tx.size()
		<< ", colors: " << m.c.size()
		<< ", time: " << elapsed_seconds.count() << "s"
		<< std::endl;

	return m;
}


// The Reactive C++ Toolbox.
// Copyright (C) 2020 Reactive Markets Limited
//
// 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 "Utility.hpp"

#include "Histogram.hpp"
#include "Iterator.hpp"

#include <boost/io/ios_state.hpp>

#include <cmath>
#include <iomanip>
#include <ostream>

namespace toolbox {
inline namespace hdr {
using namespace std;
namespace {
int64_t get_count_at_percentile(const HdrHistogram& h, double percentile) noexcept
{
    if (percentile > 100.0) {
        percentile = 100.0;
    }
    const int count_at_percentile = (percentile * h.total_count() / 100) + 0.5;
    return std::max<int64_t>(count_at_percentile, 1);
}
} // namespace

int64_t min(const HdrHistogram& h) noexcept
{
    return h.min();
}

int64_t max(const HdrHistogram& h) noexcept
{
    return h.max();
}

int64_t value_at_percentile(const HdrHistogram& h, double percentile) noexcept
{
    const int64_t count_at_percentile{get_count_at_percentile(h, percentile)};

    int64_t total{0};
    HdrIterator iter{h};
    while (iter.next()) {
        total += iter.count();
        if (total >= count_at_percentile) {
            return h.highest_equivalent_value(iter.value());
        }
    }
    return 0;
}

double mean(const HdrHistogram& h) noexcept
{
    const auto total_count = h.total_count();

    int64_t total{0};
    HdrIterator iter{h};
    while (iter.next()) {
        if (iter.count() != 0) {
            total += iter.count() * h.median_equivalent_value(iter.value());
        }
    }
    return double(total) / total_count;
}

double stddev(const HdrHistogram& h) noexcept
{
    const int64_t total_count{h.total_count()};
    const double mean_val{mean(h)};

    double geometric_dev_total{0.0};
    HdrIterator iter{h};
    while (iter.next()) {
        if (iter.count() != 0) {
            const double dev{h.median_equivalent_value(iter.value()) - mean_val};
            geometric_dev_total += (dev * dev) * iter.count();
        }
    }
    return sqrt(geometric_dev_total / total_count);
}

ostream& operator<<(ostream& os, PutPercentiles pp)
{
    const auto sf = pp.h.significant_figures();
    boost::io::ios_all_saver all_saver{os};

    os << "       Value     Percentile TotalCount 1/(1-Percentile)\n\n";

    HdrPercentileIterator iter{pp.h, pp.ticks_per_half_distance};
    while (iter.next()) {
        const double value{iter.highest_equivalent_value() / pp.value_scale};
        const double percentile{iter.percentile() / 100.0};
        const int64_t total_count{iter.cumulative_count()};

        // clang-format off
        os << setw(12) << fixed << setprecision(sf) << value
           << setw(15) << fixed << setprecision(6) << percentile
           << setw(11) << total_count;
        // clang-format on

        if (percentile < 1.0) {
            const double inverted_percentile{(1.0 / (1.0 - percentile))};
            os << setw(15) << fixed << setprecision(2) << inverted_percentile;
        }
        os << '\n';
    }

    const double mean_val{mean(pp.h) / pp.value_scale};
    const double stddev_val{stddev(pp.h)};
    const double max_val{pp.h.max() / pp.value_scale};
    const int64_t total_val{pp.h.total_count()};

    // clang-format off
    return os
        << "#[Mean    = " << setw(12) << fixed << setprecision(sf) << mean_val
        << ", StdDeviation   = " << setw(12) << fixed << setprecision(sf) << stddev_val
        << "]\n"
        "#[Max     = " << setw(12) << fixed << setprecision(sf) << max_val
        << ", TotalCount     = " << setw(12) << total_val
        << "]\n"
        "#[Buckets = " << setw(12) << pp.h.bucket_count()
        << ", SubBuckets     = " << setw(12) << pp.h.sub_bucket_count()
        << "]";
    // clang-format on
}

} // namespace hdr
} // namespace toolbox


#pragma once
#include <iostream>

#include <utility>

#include <memory>
#include <tuple>
#include <iostream>
#include <Eigen/Sparse>
#include <Eigen/Dense>
#include <functional>
#include <vector>

namespace gsimp {
// geometric types
typedef std::vector<double> point_t;
typedef std::vector<size_t> cell_t;

// linear algebra types
typedef typename Eigen::SparseMatrix<double> matrix_t;
typedef typename Eigen::SparseVector<double> vector_t;

// chains
typedef typename std::pair<int, vector_t> chain_t;
typedef typename std::pair<int, std::vector<double>> chain_v;

int& chain_dim(chain_t& p) { return std::get<0>(p); }
vector_t& chain_rep(chain_t& p) { return std::get<1>(p); }
double& chain_val(chain_t& p, size_t i) { return std::get<1>(p).coeffRef(i); }
size_t chain_size(chain_t& p) {return std::get<1>(p).size();}

int& chain_dim(chain_v& p) { return std::get<0>(p); }
std::vector<double>& chain_rep_v(chain_v& p) { return std::get<1>(p); }
double& chain_val(chain_v& p, size_t i) { return std::get<1>(p)[i]; }
size_t chain_size(chain_v& p) {return std::get<1>(p).size();}

chain_t create_chain(int d,std::vector<double>& vec) {
    // no dimension checking
    // use at own risk
    std::cout << "creating the sparse vector\n";
    vector_t v(vec.size());
    std::cout << "creating the pair\n";
    chain_t c(d,v);
    std::cout << "made the pair starting conversion\n";
    for (size_t i = 0 ; i < vec.size(); ++i)
    {if (vec[i] != 0) chain_val(c,i) = vec[i];}
    return c;
}

chain_t add(chain_t chain1, chain_t chain2) {
    if (std::get<0>(chain1) == std::get<0>(chain2))
        return chain_t(std::get<0>(chain1),
                       std::get<1>(chain1) + std::get<1>(chain2));
    std::cout << "chains must have the same dimension to be added (+)";
    throw std::exception();
}

void add_to(chain_t chain1, chain_t chain2) {
    if (std::get<0>(chain1) == std::get<0>(chain2))
        std::get<1>(chain1) += std::get<1>(chain2);
    else {
        std::cout << "chains must have the same dimension to be added (+=)";
        throw std::exception();
    }
}

chain_t subtract(chain_t chain1, chain_t chain2) {
    if (std::get<0>(chain1) == std::get<0>(chain2))
        return chain_t(std::get<0>(chain1),
                       std::get<1>(chain1) - std::get<1>(chain2));
    std::cout << "chains must have the same dimension to be added (-)";
    throw std::exception();
}

void subtract_to(chain_t chain1, chain_t chain2) {
    if (std::get<0>(chain1) == std::get<0>(chain2))
        std::get<1>(chain1) -= std::get<1>(chain2);
    else {
        std::cout << "chains must have the same dimension to be added (-=)";
        throw std::exception();
    }
}

chain_t prod(double coef, chain_t chain) {
    return chain_t(std::get<0>(chain), std::get<1>(chain) * coef);
}

void prod_to(double coef, chain_t chain) { std::get<1>(chain) *= coef; }

Eigen::VectorXd point_to_eigen(point_t pt) {
    Eigen::VectorXd v0(pt.size());
    for (int j = 0; j < pt.size(); ++j) v0(j) = pt[j];
    return v0;
}

}  // namespace gsimp


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

#include <mpllibs/metamonad/list.hpp>
#include <mpllibs/metamonad/return_.hpp>
#include <mpllibs/metamonad/bind.hpp>

#include <mpllibs/metamonad/mempty.hpp>
#include <mpllibs/metamonad/mappend.hpp>
#include <mpllibs/metamonad/mconcat.hpp>

#include <boost/test/unit_test.hpp>

#include "common.hpp"

#include <boost/mpl/equal.hpp>
#include <boost/mpl/assert.hpp>

BOOST_AUTO_TEST_CASE(test_list_monoid)
{
  using mpllibs::metamonad::list_tag;
  using mpllibs::metamonad::mempty;
  using mpllibs::metamonad::mappend;
  using mpllibs::metamonad::mconcat;

  using boost::mpl::equal;
  using boost::mpl::list;

  typedef list<int, double> l_x;
  typedef list<char, long> l_y;
  typedef list<int*, int**, int***> l_z;

  // test_left_identity
  BOOST_MPL_ASSERT((
    equal<l_x, mappend<list_tag, mempty<list_tag>::type, l_x>::type>
  ));

  // test_right_identity
  BOOST_MPL_ASSERT((
    equal<l_x, mappend<list_tag, l_x, mempty<list_tag>::type>::type>
  ));

  // test_assoc
  BOOST_MPL_ASSERT((
    equal<
      mappend<list_tag, mappend<list_tag, l_x, l_y>::type, l_z>::type,
      mappend<list_tag, l_x, mappend<list_tag, l_y, l_z>::type>::type
    >
  ));
}