/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2016 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/**
 * @file
 * @ingroup tests
 * @brief  Test with simple heat transfer equation, 3D.
 *
 * Testprogram for Kaskade: tests wether the error between calculated and exact solution behaves as expected.
 * This test was built by by using the stationary heat transfer example.
 * Boundary conditions and f are adapted in statHeatTransfer.hh.
 *
 * The test does not work with the following preconditioners: PrecondType::NONE, PrecondType::ARMS, PrecondType::ADDITIVESCHWARZ, PrecondType::HB, PrecondType::ICC.
 * The test works with: PrecondType::JACOBI, PrecondType::ILUT, PrecondType::ILUK,PrecondType::BOOMERAMG, PrecondType::EUCLID, PrecondType::ICC0 and with direct solvers DirectType::MUMPS, DirectType::UMFPACK and DirectType::UMFPACK3264.
 * Uses ContinuousHierachicMapper.
 */

#include <iostream>

#include <boost/timer/timer.hpp>

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

#include "fem/assemble.hh"
#include "fem/embedded_errorest.hh"
#include "fem/norms.hh"
//#include "fem/lagrangespace.hh"
#include "fem/hierarchicspace.hh"
#include "linalg/trivialpreconditioner.hh"
#include "linalg/partialDirectPreconditioner.hh"
#include "linalg/direct.hh"
#include "linalg/iccprecond.hh"
#include "linalg/icc0precond.hh"
#include "linalg/hyprecond.hh"       // BoomerAMG
#include "linalg/iluprecond.hh"
#include "linalg/additiveschwarz.hh"
#include "linalg/jacobiPreconditioner.hh"
#include "linalg/cg.hh"
#include "mg/hb.hh"
#include "utilities/enums.hh"
#include "utilities/kaskopt.hh"

#include "cubus.hh"

using namespace Kaskade;
#include "statHeatTransfer.hh"
#undef max

/**
 * @brief  This represents the known exact solution.
 * It is \f$ u = x(x-1)\exp (-(x-0.5)^2) \f$.
 */
struct StatHeatTransferSolution
{
   using Scalar = double;
   static int const components = 1;
   using ValueType = Dune::FieldVector<Scalar,components>;

   template <class Cell> int order(Cell const&) const { return std::numeric_limits<int>::max(); }

   template <class Cell>
   ValueType value(Cell const& cell,Dune::FieldVector<typename Cell::Geometry::ctype,Cell::dimension> const& localCoordinate) const
   {
     Dune::FieldVector<typename Cell::Geometry::ctype,Cell::Geometry::coorddimension> x = cell.geometry().global(localCoordinate);
     return x[0]*(x[0]-1) * std::exp(-(x[0]-0.5)*(x[0]-0.5));
   }
};

int main(int argc, char *argv[])
{
  using namespace boost::fusion;

  std::cout << "Start heat transfer test program" << std::endl;

  boost::timer::cpu_timer totalTimer;

  int verbosityOpt = 0;
  bool dump = false;
  constexpr int dim=3;
  constexpr int heapSize=1024;
  using Grid = Dune::UGGrid<dim>;
  using LeafView = Grid::LeafGridView;
  //using H1Space = FEFunctionSpace<ContinuousLagrangeMapper<double,LeafView> >;
  using H1Space = FEFunctionSpace<ContinuousHierarchicMapper<double,LeafView> >;
  using Spaces = boost::fusion::vector<H1Space const*>;
  using VariableDescriptions = boost::fusion::vector<Variable<SpaceIndex<0>,Components<1>,VariableId<0> > >;
  using VariableSet = VariableSetDescription<Spaces,VariableDescriptions>;
  using Functional = StatHeatTransferFunctional<double,VariableSet>;
  constexpr int nvars = Functional::AnsatzVars::noOfVariables;
  constexpr int neq = Functional::TestVars::noOfVariables;
  using Assembler = VariationalFunctionalAssembler<LinearizationAt<Functional> >;
  using CoefficientVectors = VariableSet::CoefficientVectorRepresentation<0,1>::type;
  using LinearSpace = VariableSet::CoefficientVectorRepresentation<0,1>::type;

  std::unique_ptr<boost::property_tree::ptree> pt = getKaskadeOptions(argc, argv, verbosityOpt, dump);

  constexpr int orderLength = 5;
  //ansatz orders to be tested
  int order[orderLength] = {1,2,3,4,5};
  int  maxRefSteps[orderLength] = {3,3,1,1,1};
  //error values for corresponding ansatz order and refinement
  double errorsI[2][4] = {{9e-3,3.3e-3,9.8e-4,2.5e-4}, //order 1
                          {3.2e-3,2.7e-4,2.4e-5,2.4e-6}}; //order 2
  double errorsII[3] = {4.5e-5, //order3
                        9.7e-7, //order4
                        5.9e-8}; //order5
  bool valid = true;
  std::stringstream message("Test succeeded", std::stringstream::out);

  int verbosity   = getParameter(pt, "verbosity", 1);
  // if true, then the test result will be written in a file
  bool result = getParameter(pt, "result",0);
  int  direct, onlyLowerTriangle = false;

  DirectType directType;
  MatrixProperties property = MatrixProperties::SYMMETRIC;
  PrecondType precondType = PrecondType::NONE;
  std::string empty;

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

  std::string directSolver = getParameter(pt, "solver.direct", empty);
  s = "names.direct." + directSolver;
  directType = static_cast<DirectType>(getParameter(pt, s, 0));

  std::string preconditioner = getParameter(pt, "solver.preconditioner", empty);
  s = "names.preconditioner." + preconditioner;
  precondType = static_cast<PrecondType>(getParameter(pt, s, 0));

  int blocks = getParameter(pt,"blocks",40);
  int nthreads = getParameter(pt,"threads",4);
  double rowBlockFactor = getParameter(pt,"rowBlockFactor",2.0);

  property = MatrixProperties::SYMMETRIC;

  if ( (directType == DirectType::MUMPS)||(directType == DirectType::PARDISO) || (precondType == PrecondType::ICC) )
  {
    onlyLowerTriangle = true;
    if(verbosity > 0) {
      std::cout <<
        "Note: direct solver MUMPS/PARADISO or PrecondType::ICC preconditioner ===> onlyLowerTriangle is set to true!"
        << std::endl;
    }
  }

  //  three-dimensional space: dim=3
  //  - more complex: 1 cube defined by 48 tetrahedra, provided in cubus.hh

  // definition of a more complex mesh using cubus.hh

  //iterate over the ansatz orders
  for(int i = 0;i < orderLength && valid;i++) {
    if(verbosity > 0) {
      std::cout << "original mesh shall be refined : " << maxRefSteps[i] << " times" << std::endl;
      std::cout << "discretization order           : " << order[i] << std::endl;
      std::cout << "output level (verbosity)       : " << verbosity << std::endl;
    }

     std::unique_ptr<Grid> grid( RefineGrid<Grid>(0, heapSize) );

    // some information on the refined mesh
    if(verbosity > 0) {
      std::cout << std::endl << "Grid: " << grid->size(0) << " tetrahedra, " << std::endl;
      std::cout << "      " << grid->size(1) << " triangles, " << std::endl;
      std::cout << "      " << grid->size(dim-1) << " edges, " << std::endl;
      std::cout << "      " << grid->size(dim) << " points" << std::endl;
    }
    // a gridmanager is constructed
    // as connector between geometric and algebraic information
    GridManager<Grid> gridManager(std::move(grid));

    // construction of finite element space for the scalar solution T.
    H1Space temperatureSpace(gridManager,gridManager.grid().leafGridView(),order[i]);

    Spaces spaces(&temperatureSpace);

    // construct variable list.
    // VariableDescription<int spaceId, int components, int Id>
    // spaceId: number of associated FEFunctionSpace
    // components: number of components in this variable
    // Id: number of this variable

    std::string varNames[1] = { "u" };

    VariableSet variableSet(spaces,varNames);

    // construct variational functional

    double kappa = getParameter(pt, "kappa", 1.0);
    double q = getParameter(pt, "q", 1.0);
    Functional F(kappa,q);
    if(verbosity > 0) {
      std::cout << std::endl << "no of variables = " << nvars << std::endl;
      std::cout << "no of equations = " << neq   << std::endl;
      size_t dofs = variableSet.degreesOfFreedom(0,nvars);
      std::cout << "number of degrees of freedom = " << dofs   << std::endl;
    }

    //construct Galerkin representation
    Assembler assembler(gridManager,spaces);

    gridManager.enforceConcurrentReads(true);
    assembler.setNSimultaneousBlocks(blocks);
    assembler.setRowBlockFactor(rowBlockFactor);

    //keep mesh sizes
    //double hs[maxRefSteps+1];
    //keep errors
    //double ress[maxRefSteps+1];
    for(int refSteps = 0;refSteps<maxRefSteps[i]+1;refSteps++) {
      if(refSteps>0) {
        gridManager.globalRefine(1);
      } else {
        if(i>=2) continue;
      }

      boost::timer::cpu_timer assembTimer;
      VariableSet::VariableSet u(variableSet);

      size_t nnz = assembler.nnz(0,1,0,1,onlyLowerTriangle);
      if(verbosity > 0) {
        std::cout << "number of nonzero elements in the stiffness matrix: " << nnz << std::endl << std::endl;
      }

      CoefficientVectors solution(VariableSet::CoefficientVectorRepresentation<0,1>::init(spaces));
      solution = 0;

      assembler.assemble(linearization(F,u),assembler.MATRIX|assembler.RHS|assembler.VALUE,nthreads,verbosity);
      if(verbosity > 0) {
        std::cout << "computing time for assemble: " << boost::timer::format(assembTimer.elapsed()) << "\n";
      }

      CoefficientVectors rhs(assembler.rhs());
      AssembledGalerkinOperator<Assembler,0,1,0,1> A(assembler, onlyLowerTriangle);

      if (direct)
      {
        solverTypeII = directSolver;
        boost::timer::cpu_timer directTimer;
        directInverseOperator(A,directType,property).applyscaleadd(-1.0,rhs,solution);
        u.data = solution.data;
        if(verbosity > 0) {
          std::cout << "computing time for direct solve: " << boost::timer::format(directTimer.elapsed()) << "\n";
        }
      }
      else
      {
        solverTypeII = "CG, " + preconditioner;
        boost::timer::cpu_timer iteTimer;
        int iteSteps = getParameter(pt, "solver.iteMax", 1000);
        double iteEps = getParameter(pt, "solver.iteEps", 1.0e-10);
        Dune::InverseOperatorResult res;
        const DefaultDualPairing<LinearSpace,LinearSpace> defaultScalarProduct{};
        StrakosTichyPTerminationCriterion<double> termination(iteEps,iteSteps);
        int lookAhead;
        switch (precondType)
        {
          case PrecondType::NONE:
          case PrecondType::HB:   lookAhead=50; break;
          default:                lookAhead=3; break;
        }
        lookAhead = getParameter(pt, "solver.lookAhead", lookAhead);
        termination.setLookAhead(lookAhead);

        switch (precondType)
        {
          case PrecondType::NONE:
          {
            std::cout << "!!!!!!!!!!!!!!!!!!!!!!" << std::endl
                      << "ATTENTION: Test does not work without preconditioner." << std::endl
                      << "!!!!!!!!!!!!!!!!!!!!!!!" << std::endl;
            if(verbosity > 0) {
              std::cout << "selected preconditioner: NONE" << std::endl;
            }
            TrivialPreconditioner<AssembledGalerkinOperator<Assembler,0,1,0,1> > trivial;
            CG<LinearSpace,LinearSpace> cg(A,trivial,defaultScalarProduct,termination,verbosity);
            cg.apply(solution,rhs,res);
          }
          break;
          case PrecondType::ADDITIVESCHWARZ:
          {
            std::cout << "!!!!!!!!!!!!!!!!!!!!!!" << std::endl << "ATTENTION: Test does not work with preconditioner PrecondType::ADDITIVESCHWARZ." << std::endl << "!!!!!!!!!!!!!!!!!!!!!!!" << std::endl;
            if(verbosity > 0) {
              std::cout << "selected preconditioner: ADDITIVESCHWARZ" << std::endl;
            }
            std::pair<size_t,size_t> idx = temperatureSpace.mapper().globalIndexRange(gridManager.grid().leafIndexSet().geomTypes(dim)[0]);
            AdditiveSchwarzPreconditioner<AssembledGalerkinOperator<Assembler,0,1,0,1> > addschwarz(A,idx.first,idx.second,verbosity);
            CG<LinearSpace,LinearSpace> cg(A,addschwarz,defaultScalarProduct,termination,verbosity);
            cg.apply(solution,rhs,res);
          }
          break;
          case PrecondType::ILUT:
          {
            if(verbosity > 0) {
              std::cout << "selected preconditioner: ILUT" << std::endl;
              std::cout << "Note that this preconditioner combined with the BICGSTAB solver" << std::endl;
              std::cout << "needs matrix.property = GENERAL" << std::endl;
            }
            int lfil = getParameter(pt, "solver.ILUT.lfil", 140);
            double dropTol = getParameter(pt, "solver.ILUT.dropTol", 0.01);
            ILUTPreconditioner<AssembledGalerkinOperator<Assembler,0,1,0,1> > ilut(A,lfil,dropTol,verbosity);
            Dune::BiCGSTABSolver<LinearSpace> cg(A,ilut,iteEps,iteSteps,verbosity);
            cg.apply(solution,rhs,res);
          }
          break;
          case PrecondType::ILUK:
          {
            if(verbosity > 0) {
              std::cout << "selected preconditioner: ILUK" << std::endl;
              std::cout << "Note that this preconditioner combined with the BICGSTAB solver" << std::endl;
              std::cout << "needs matrix.property = GENERAL" << std::endl;
            }
            int fill_lev = getParameter(pt, "solver.ILUK.fill_lev", 3);
            ILUKPreconditioner<AssembledGalerkinOperator<Assembler,0,1,0,1> > iluk(A,fill_lev,verbosity);
            Dune::BiCGSTABSolver<LinearSpace> cg(A,iluk,iteEps,iteSteps,verbosity);
            cg.apply(solution,rhs,res);
          }
          break;
          case PrecondType::ICC:
          {
            if(verbosity > 0) {
              std::cout << "selected preconditioner: ICC" << std::endl;
            }
            if (property != MatrixProperties::SYMMETRIC)
            {
              std::cout << "PrecondType::ICC preconditioner of TAUCS lib has to be used with matrix.property==MatrixProperties::SYMMETRIC\n";
              std::cout << "i.e., call the executable with option --solver.property MatrixProperties::SYMMETRIC\n\n";
            }
            double dropTol = getParameter(pt, "solver.ICC.dropTol", 0.01);;
            ICCPreconditioner<AssembledGalerkinOperator<Assembler,0,1,0,1> > icc(A,dropTol);
            CG<LinearSpace,LinearSpace> cg(A,icc,defaultScalarProduct,termination,verbosity);
            cg.apply(solution,rhs,res);
          }
          break;
          case PrecondType::ICC0:
          {
            if(verbosity > 0) {
              std::cout << "selected preconditioner: ICC0" << std::endl;
            }
            ICC_0Preconditioner<AssembledGalerkinOperator<Assembler,0,1,0,1> > icc0(A);
            CG<LinearSpace,LinearSpace> cg(A,icc0,defaultScalarProduct,termination,verbosity);
            cg.apply(solution,rhs,res);
          }
          break;
          case PrecondType::HB:
          {
            std::cout << "!!!!!!!!!!!!!!!!!!!!!!" << std::endl << "ATTENTION: Test does not work with preconditioner PrecondType::HB." << std::endl << "!!!!!!!!!!!!!!!!!!!!!!!" << std::endl;
            if(verbosity > 0) {
              std::cout << "selected preconditioner: HB" << std::endl;
            }
            HierarchicalBasisPreconditioner<Grid,AssembledGalerkinOperator<Assembler,0,1,0,1>::range_type, AssembledGalerkinOperator<Assembler,0,1,0,1>::range_type > hb(gridManager.grid());
            CG<LinearSpace,LinearSpace> cg(A,hb,defaultScalarProduct,termination,verbosity);
            cg.apply(solution,rhs,res);
          }
          break;
          case PrecondType::ARMS:
          {
            if(verbosity > 0) {
              std::cout << "selected preconditioner: ARMS" << std::endl;
            }
            std::cout << "!!!!!!!!!!!!!!!!!!!!!!" << std::endl
              << "ATTENTION: Test does not work with preconditioner PrecondType::ARMS." << std::endl
              << "!!!!!!!!!!!!!!!!!!!!!!!" << std::endl;
            int lfil = getParameter(pt, "solver.ARMS.lfil", 140);
            int lev_reord = getParameter(pt, "solver.ARMS.lev_reord", 1);
            double dropTol = getParameter(pt, "solver.ARMS.dropTol", 0.01);
            double tolind = getParameter(pt, "solver.ARMS.tolind", 0.2);
            ARMSPreconditioner<AssembledGalerkinOperator<Assembler,0,1,0,1> > iluk(A,lfil,dropTol,lev_reord,tolind,verbosity);
            CG<LinearSpace,LinearSpace> cg(A,iluk,defaultScalarProduct,termination,verbosity);
            cg.apply(solution,rhs,res);
          }
          break;
          case PrecondType::BOOMERAMG:
          {
            int steps = getParameter(pt, "solver.BOOMERAMG.steps", iteSteps);
            int coarsentype = getParameter(pt, "solver.BOOMERAMG.coarsentype", 21);
            int interpoltype = getParameter(pt, "solver.BOOMERAMG.interpoltype", 0);
            int cycleType = getParameter(pt, "solver.BOOMERAMG.cycleType", 1);
            int relaxType = getParameter(pt, "solver.BOOMERAMG.relaxType", 3);
            int variant = getParameter(pt, "solver.BOOMERAMG.variant", 0);
            int overlap = getParameter(pt, "solver.BOOMERAMG.overlap", 1);
            double tol = getParameter(pt, "solver.BOOMERAMG.tol", iteEps);
            double strongThreshold = getParameter(pt, "solver.BOOMERAMG.strongThreshold", (dim==2)?0.25:0.6);
            BoomerAMG<AssembledGalerkinOperator<Assembler,0,1,0,1> >
                      BoomerAMGPrecon(A,steps,coarsentype,interpoltype,tol,cycleType,relaxType,
                      strongThreshold,variant,overlap,1,verbosity);
            CG<LinearSpace,LinearSpace> cg(A,BoomerAMGPrecon,defaultScalarProduct,termination,verbosity);
            //Dune::LoopSolver<LinearSpace> cg(A,BoomerAMGPrecon,iteEps,iteSteps,verbosity);
            cg.apply(solution,rhs,res);
          }
          break;
          case PrecondType::EUCLID:
          {
            if(verbosity > 0) {
              std::cout << "selected preconditioner: EUCLID" << std::endl;
            }
            int level      = getParameter(pt, "solver.EUCLID.level",1);
            double droptol = getParameter(pt, "solver.EUCLID.droptol",0.01);
            int printlevel = 0;
            if (verbosity>2) printlevel=verbosity-2;
            printlevel = getParameter(pt,"solver.EUCLID.printlevel",printlevel);
            int bj = getParameter(pt, "solver.EUCLID.bj",0);
            Euclid<AssembledGalerkinOperator<Assembler,0,1,0,1> > EuclidPrecon(A,level,droptol,printlevel,bj,verbosity);
            CG<LinearSpace,LinearSpace> cg(A,EuclidPrecon,defaultScalarProduct,termination,verbosity);
            cg.apply(solution,rhs,res);
          }
          break;
          case PrecondType::JACOBI:
          default:
          {
            if(verbosity > 0) {
              std::cout << "selected preconditioner: JACOBI" << std::endl;
            }
            JacobiPreconditioner<AssembledGalerkinOperator<Assembler,0,1,0,1> > jacobi(A,1.0);
            CG<LinearSpace,LinearSpace> cg(A,jacobi,defaultScalarProduct,termination,verbosity);
            cg.apply(solution,rhs,res);
          }
          break;
        }
        solution *= -1.0;
        u.data = solution.data;

        if(verbosity > 0) {
          std::cout << "iterative solve eps= " << iteEps << ": "
                    << (res.converged?"converged":"failed") << " after "
                    << res.iterations << " steps, rate="
                    << res.conv_rate << ", computing time=" << (double)(iteTimer.elapsed().user)/1e9 << "s\n";
        }
      }

      //calculate error in l2norm
      VariableSet::VariableSet func( variableSet ) ;
      interpolateGloballyWeak<PlainAverage>(boost::fusion::at_c<0>(func.data),StatHeatTransferSolution());
      u -= func;
      L2Norm l2 ;
      double nrm2 = l2( boost::fusion::at_c<0>(u.data) ) ;
      if(verbosity > 0) {
        std::cout << "error in l2norm: " << nrm2 << std::endl;
      }

      // query whether error is low enough
      if(i<2 && !(nrm2<=errorsI[i][refSteps])) {
        valid = false;
        message << "Test failed: The error after " << refSteps << " refinements was too high at the test with ansatz functions of order " << order[i] << ".";
      } else if(i>=2 && !(nrm2<=errorsII[i-2])) {
        valid = false;
        message << "Test failed: The error after 1 refinement was too high at the test with ansatz functions of order " << order[i] << ".";
      }
      //ress[refSteps] = nrm2;
      //calculate mesh size
      //hs[refSteps] = 1.414213562/pow(2,refSteps+1);
    }
  }

  std::cout << "total computing time: " << boost::timer::format(totalTimer.elapsed()) << "\n";
  std::cout << message.str() << std::endl;
  if(result) {
    std::string description = "Test with example stationary heat transfer, 3D. Used " + solverTypeI + " solver " + solverTypeII + ":";
    std::ofstream outfile("../testResult.txt", std::ofstream::out | std::ofstream::app);
    outfile << description << std::endl << message.str() << std::endl << std::endl;
    outfile.close();
  }
}


// ****************************************************************************
// test/raw_simd_performance.h
//
// Part of the test harness for entity.
//
// Copyright Chris Glover 2014
//
// 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 <iostream>
#include <vector>
#include <algorithm>
#include <immintrin.h>
#include "performance_common.hpp"

static const int kNumEntities = TEST_SIZE;
static const float kTestLength = 10.0f;
static const float kFrameTime = 0.016f;

#define BOOST_TEST_MODULE Performance
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE( raw_simd )
{
#if USE_EXPLICIT_LOADS
	std::vector<unsigned int> ids;
	std::vector<float> positions;
	std::vector<float> velocities;
	std::vector<float> accels;

	ids.resize(kNumEntities);
	positions.resize(kNumEntities);
	velocities.resize(kNumEntities);
	accels.resize(kNumEntities);

	std::clog << "Created Pools\n";

	for(std::size_t i = 0; i < positions.size(); ++i)
	{
		ids[i] = i;
		accels[i] = 9.8f;
		velocities[i] = 0;
		positions[i] = 0;
	}

	std::clog << "Created Components, simulating...";

	// Simulate over some seconds using a fixed step.
	{
		float time_remaining = kTestLength;
		while(time_remaining > 0)
		{
			for(std::size_t i = 0; i < accels.size(); i += 4)
			{
				// Add a little to accel each frame.
				//if(ids[i] == i)
				{
					__m128 increment = _mm_set1_ps(0.001f);
					__m128 a = _mm_loadu_ps(&accels[i]);
					a += increment;
					_mm_stream_ps(&accels[i], a);
				}
			}

			for(std::size_t i = 0; i < velocities.size(); i += 4)
			{
				//if(ids[i] == i)
				{
					// Compute new velocity.
					__m128 divisor = _mm_set1_ps(2.f);
					__m128 frame_time_sq = _mm_set1_ps(kFrameTime * kFrameTime);
					__m128 a = _mm_loadu_ps(&accels[i]);
					__m128 v = _mm_loadu_ps(&velocities[i]);
					v += (a/divisor) * frame_time_sq;
					_mm_stream_ps(&velocities[i], v);
				}
			}

			for(std::size_t i = 0; i < positions.size(); i += 4)
			{
				//if(ids[i] == i)
				{
					// Compute new position.
					__m128 frame_time = _mm_set1_ps(kFrameTime);
					__m128 v = _mm_loadu_ps(&velocities[i]);
					__m128 p = _mm_loadu_ps(&positions[i]);
					p += v * frame_time;
					_mm_stream_ps(&positions[i], p);
				}
			}

			time_remaining -= kFrameTime;
		}
	}

	std::clog << "done.\n";

   	std::clog << "Positions: " << positions[0] << std::endl;
   	std::clog << "Velocities: " << velocities[0] << std::endl;
#else
   	std::vector<int> ids;
	std::vector<__m128> positions;
	std::vector<__m128> velocities;
	std::vector<__m128> accels;

	ids.resize(kNumEntities * 4);
	positions.resize(kNumEntities);
	velocities.resize(kNumEntities);
	accels.resize(kNumEntities);

	std::clog << "Created Pools\n";

	for(std::size_t i = 0; i < positions.size(); ++i)
	{
		ids[i] = i;
		accels[i] = _mm_set1_ps(9.8f);
		velocities[i] = _mm_setzero_ps();
		positions[i] = _mm_setzero_ps();
	}

	std::clog << "Created Components, simulating...";

	// Simulate over some seconds using a fixed step.
	{
		float time_remaining = kTestLength;
		while(time_remaining > 0)
		{
			for(std::size_t i = 0; i < accels.size(); ++i)
			{
				//if(ids[i] == i)
				{
					// Add a little to accel each frame.
					__m128 increment = _mm_set1_ps(0.001f);
					accels[i] += increment;
				}
			}

			for(std::size_t i = 0; i < velocities.size(); ++i)
			{
				//if(ids[i] == i)
				{
					// Compute new velocity.
					__m128 divisor = _mm_set1_ps(2.f);
					__m128 frame_time_sq = _mm_set1_ps(kFrameTime * kFrameTime);
					velocities[i] += (accels[i]/divisor) * frame_time_sq;
				}
			}

			for(std::size_t i = 0; i < positions.size(); ++i)
			{
				//if(ids[i] == i)
				{
					// Compute new position.
					__m128 frame_time = _mm_set1_ps(kFrameTime);
					positions[i] += velocities[i] * frame_time;
				}
			}

			time_remaining -= kFrameTime;
		}
	}

	std::clog << "done.\n";

   	std::clog << "Positions: " << _mm_cvtss_f32(positions[0]) << std::endl;
    std::clog << "Velocities: " << _mm_cvtss_f32(velocities[0]) << std::endl;
#endif

	return 0;
}


//
// Created by mmath on 5/28/17.
//

#ifndef PYSCAN_POINT_HPP
#define PYSCAN_POINT_HPP
#include <string>
#include <cstring>
#include <cmath>
#include <vector>
#include <iostream>
#include <array>
#include <cassert>
#include <sstream>
#include <functional>
#include <unordered_map>
#include <queue>
#include <optional>

#include "Utilities.hpp"

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

namespace pyscan {

    template<typename tuple_t>
    constexpr auto get_array_from_tuple(tuple_t&& tuple) {
        constexpr auto get_array = [](auto&& ... x){
            return std::array{std::forward<decltype(x)>(x) ... , 1.0};
        };
        return std::apply(get_array, std::forward<tuple_t>(tuple));
    }

    // Coordiates in homgenous space.
    template<int dim = 2>
    class Point {
    public:

        template <class ...Coords, std::enable_if_t<(sizeof...(Coords) == dim + 1)>* = nullptr>
        explicit Point(Coords... rest) : coords{rest...} {
            static_assert(sizeof...(rest) == dim + 1, "Wrong number of arguments for the point type.");
        }

        template <class ...Coords, std::enable_if_t<(sizeof...(Coords) == dim)>* = nullptr>
        explicit Point(std::tuple<Coords...> c) : coords(get_array_from_tuple(c)) {}

       /* Point<dim>& operator=(Point<dim> const& pt) {
            std::copy(coords.begin(), coords.end(), pt.coords.begin());
            return *this;
        }*/

        Point() {
            coords.fill(0.0);
        }
        virtual ~Point() = default;

        friend std::ostream &operator<<(std::ostream &os, Point const &pt) {
            os << "pyscan::Point<" << dim << ">(";
            for (size_t d = 0; d < dim; d++ ) {
                os << pt.coords[d] << ", ";
            }
            os << pt.coords[dim];
            os << ")";
            return os;
        }

        friend Point<dim> operator*(Point<dim> pt, double val) {
            Point<dim> res;
            for (size_t i = 0; i < dim; ++i) {
                res.coords[i] = pt.coords[i] * val;
            }
            res.coords[dim] = pt.coords[dim];
            return res;
        }

        friend Point<dim> operator*(double val, Point<dim> pt) {
            return operator*(pt, val);
        }

        Point<dim> flip_orientation() const {
            //Flips the orientation
            Point<dim> p_out;
            for (size_t j = 0; j < dim + 1; j++) {
                p_out.coords[j] = -coords[j];
            }
            return p_out;
        }

        Point<dim> orient_up(int i) const {
            // Always orient these down by the second to last coordinate.
            // In 2d this ensures that y is oriented down.
            // In 3d this ensures that z is oriented down.
            Point<dim> p_out;
            assert(i < dim + 1);
            double orientation = std::copysign(1.0, coords[i]);
            for (size_t j = 0; j < dim + 1; j++) {
                p_out.coords[j] = orientation * coords[j];
            }
            return p_out;
        }

        Point<dim> orient_down(int i) const {
            // Always orient these down by the second to last coordinate.
            // In 2d this ensures that y is oriented down.
            // In 3d this ensures that z is oriented down.
            Point<dim> p_out;
            assert(i < dim + 1);
            double orientation = std::copysign(1.0, coords[i]);
            for (size_t j = 0; j < dim + 1; j++) {
                p_out.coords[j] = -orientation * coords[j];
            }
            return p_out;
        }

        inline double& operator[](size_t i) {
            assert(i < dim + 1);
            return coords[i];
        }

        inline double get_coord(size_t i) const {
            return coords[i];
        }

        inline double operator[](size_t i) const {
            assert(i < dim + 1);
            return coords[i];
        }

        inline double operator()(size_t i) const {
            assert(i < dim);
            return coords[i] / coords[dim];
        }

        virtual std::string str() const {
            std::stringstream ss;
            ss << *this;
            return ss.str();
        }

        bool operator==(const Point<dim> &pt) const {
            for (size_t i = 0; i < dim + 1; ++i) {
                if (coords[i] != pt.coords[i])
                    return false;
            }
            return true;
        }

        inline double evaluate(const Point<dim> &other) const {
            double res = 0.0;
            for (size_t i = 0; i < dim + 1; ++i) {
                res += coords[i] * other.coords[i];
            }
            return res; //* std::copysign(1.0, other[dim]);
        }

        inline Point<dim> direction(const Point<dim> & other) const {
            auto pt = other.operator-(*this);
            return pt.normalize();
        }

        inline double square_dist(const Point<dim> &p) const {
            double res = 0.0;
            for (size_t i = 0; i < dim; ++i) {
                double tmp = (p[i] / p[dim] - coords[i] / coords[dim]);
                res += tmp * tmp;
            }
            return res;
        }

        inline Point<dim> on_segment(Point<dim> const& p2, double alpha) {
            /*
             * Returns a point between this point and the second point that is alpha this + (1 - alpha) p2;
             */
            return alpha * (*this) + (1 - alpha) * p2;
        }

        inline double dist(const Point<dim> &p) const {
            return sqrt(square_dist(p));
        }

        Point<dim> normalize() const {
            Point<dim> res;
            double magnitude = sqrt(pdot(*this)) / coords[dim];
            for (size_t i = 0; i < dim; ++i) {
                res.coords[i] = coords[i] / magnitude;
            }
            res.coords[dim] = 1.0;
            return res;
        }

        Point<dim> operator-(const Point<dim>& other) const {
            Point<dim> res;
            for (size_t i = 0; i < dim; ++i) {
                res.coords[i] = coords[i] * other[dim] - other[i] * coords[dim];
            }
            res.coords[dim] = other[dim] * coords[dim];
            return res;
        }

        Point<dim> operator+(const Point<dim>& other) const {
            Point<dim> res;
            for (size_t i = 0; i < dim; ++i) {
                res.coords[i] = coords[i] * other[dim] + other[i] * coords[dim];
            }
            res.coords[dim] = other[dim] * coords[dim];
            return res;
        }

        inline double square_dist(const Point<dim>& begin, const Point<dim>& end) const {
            auto v = end - begin;
            auto w = *this - begin;
            double c1 = w.pdot(v);
            if (util::alte(c1, 0.0)) return square_dist(begin);
            double c2 = v.pdot(v);
            if (util::alte(c2, c1)) return square_dist(end);
            double b = c1 / c2;
            auto pb = begin + v * b;
            return square_dist(pb);
        }

        inline double pdot(const Point<dim>& other) const {
            /*
             * Takes the dot product between these points as if they are vectors
             */
            double res = 0.0;
            for (size_t i = 0; i < dim; ++i) {
                res += coords[i] * other.coords[i];
            }
            //If this is approximately 0 then the points exist at infinity and this operation doesn't make sense.
            assert(!util::aeq(coords[dim] * other[dim], 0.0));
            return res / (coords[dim] * other[dim]);
        }

        inline bool approx_eq(Point<dim> const& p) const {

            double res = 0.0;
            for (size_t i = 0; i < dim; ++i) {
                double tmp = (p[i] * coords[dim] - coords[i] * p[dim]);
                res += std::abs(tmp);
            }
            return util::aeq(res, 0.0);
        }

        inline bool above_closed(const Point<dim> &p) const {
            return util::alte(0.0, evaluate(p));
        }

        inline bool below_closed(const Point<dim> &p) const {
            return util::alte(evaluate(p), 0.0);
        }

        inline bool above(const Point<dim> &p) const {
            return util::alt(0.0, evaluate(p));
        }

        inline bool below(const Point<dim> &p) const {
            return util::alt(evaluate(p), 0.0);
        }

        inline bool crosses( const Point<dim>& p1, const Point<dim>& p2) const {
            /*
             * Checks if this line is crossed by this line segment between p1
             */
            auto or1 = p1.orient_up(dim);
            auto or2 = p2.orient_up(dim);
            return (above(or1) && below(or2)) || (below(or1) && above(or2));
        }

        inline bool parallel_lte( const Point<dim>& l1) const {
            /*
             * Checks to see if this line is less than or equal to the line l1 assuming that l1 is parallel to
             * this.
             */
            double norm = 1.0;
            for (size_t i = 0; i < dim; ++i) {
                if (!util::aeq(l1[i], 0)) {
                    norm = coords[i] / l1[i];
                    break;
                }
            }
            return util::alte(l1[dim] * norm, coords[dim]);
        }

        inline bool parallel_lt( const Point<dim>& l1) const {
            /*
             * Checks to see if this line is less than or equal to the line l1 assuming that l1 is parallel to
             * this.
             */
            double norm = 1.0;
            for (size_t i = 0; i < dim; ++i) {
                if (!util::aeq(l1[i], 0)) {
                    norm = coords[i] / l1[i];
                    break;
                }
            }
            return util::alt(l1[dim] * norm, coords[dim]);
        }

    protected:
        std::array<double, dim + 1> coords;
    };

    template<int dim = 2>
    class WPoint : public Point<dim> {
    public:
        template<typename ...Coords>
        explicit WPoint(double weight, Coords... rest)
                : Point<dim>(rest...), weight(weight) {
                }

        WPoint()
                : Point<dim>(), weight(0.0) {}
        virtual ~WPoint() = default;

        friend std::ostream &operator<<(std::ostream &os, WPoint const &pt) {
            os << "WPoint(" << pt.get_weight() << ", ";
            for (auto &el: pt.coords) {
                os << el << ", ";
            }
            os << ")";
            return os;
        }

        inline double get_weight() const {
            return weight;
        }

        virtual void set_weight(double w) {
            weight = w;
        }

    protected:
        double weight;
    };

    template<int dim = 2>
    class LPoint : public WPoint<dim> {
    public:
        template<typename ...Coords>
        LPoint(size_t label, double weight, Coords... rest)
                : WPoint<dim>(weight, rest...), label(label) {}

        LPoint()
                : WPoint<dim>(), label(0) {}

        virtual ~LPoint() = default;

        inline size_t get_label() const {
            return label;
        }

        friend std::ostream &operator<<(std::ostream &os, LPoint const &pt) {
            os << "LPoint(" << pt.label << ", " << pt.get_weight() << ", ";
            for (auto &el: pt.coords) {
                os << el << ", ";
            }
            os << ")";
            return os;
        }

        virtual void set_label(size_t l) {
            label = l;
        }

    protected:
        size_t label;
    };

    Point<2> correct_orientation(const Point<2>& pivot, const Point<2>& p);

    Point<2> intersection(const Point<2> &p1, const Point<2> &p2);

    std::tuple<double, double, double> normal(const Point<3> &p1, const Point<3> &p2, const Point<3> &p3);

    bool is_parallel(const Point<2> &l1, const Point<2> &l2);

    Point<3> cross_product(const Point<3>& p1, const Point<3>& p2);

    bool crosses_segment(const Point<2> &p1, const Point<2> &p2, const Point<2> &q1, const Point<2> &q2);

    //Return the two points on the line that are equidistance to some other point.
//    std::tuple<Point<2>, Point<2>> chord_pts(const Point<2> &line, const Point<2> &origin, double dist);

    using pt2_t = Point<2>;
    using wpt2_t = WPoint<2>;
    using lpt2_t = LPoint<2>;
    using point_list_t = std::vector<pt2_t>;
    using point_it_t = point_list_t::iterator;
    using cpoint_it_t = point_list_t::const_iterator;

    using weight_list_t = std::vector<double>;
    using weight_it_t = weight_list_t::iterator;
    using pt3_t = Point<3>;
    using wpt3_t = WPoint<3>;
    using lpt3_t = LPoint<3>;
    using point3_list_t = std::vector<pt3_t>;
    using point3_it_t = point3_list_t::iterator;
    using label_list_t = std::vector<size_t>;
    using wpoint_list_t = std::vector<WPoint<2>>;
    using wpoint_it_t = wpoint_list_t::iterator;
    using cwpoint_it_t = wpoint_list_t::const_iterator;

    using lpoint_list_t = std::vector<LPoint<2>>;
    using lpoint_it_t = lpoint_list_t::iterator;
    using clpoint_it_t = lpoint_list_t::const_iterator;

    using wpoint3_list_t = std::vector<WPoint<3>>;
    using lpoint3_list_t = std::vector<LPoint<3>>;
    using lpoint3_it_t = lpoint3_list_t::iterator;

    using discrepancy_func_t = std::function<double(double, double, double, double)>;

    template <typename T>
    void remove_duplicates(std::vector<T>& pts) {

        auto pts_end = std::remove_if(pts.begin(), pts.end(), [] (T const& pt){
            return util::aeq(pt[2], 0.0) || std::isnan(pt(0)) || std::isnan(pt(1)) || std::isinf(pt(0)) || std::isinf(pt(1));
        });
        std::sort(pts.begin(), pts_end, [](T const& p1, T const& p2){
            return p1(0) < p2(0);
        });

        //This is a version of unique that should be defined for non transitive relationships.
        auto new_end = pts.end() - 1;
        if (pts.size() <= 1) {
            return;
        }
        for (auto pt_b = pts.end() - 2; ;pt_b--) {
            if ((pt_b + 1)->approx_eq(*pt_b)) {
                std::swap(*(pt_b + 1), *pt_b);
                std::swap(*(pt_b + 1), *new_end);
                new_end--;
            }
            if (pt_b == pts.begin()) {
                break;
            }
        }
        pts.erase(new_end + 1, pts.end());
    }

    inline bool cmpX(Point<2> const& p1, Point<2> const& p2) {
        return p1(0) < p2(0);
    }

    inline bool cmpY(Point<2> const& p1, Point<2> const& p2) {
        return p1(1) < p2(1);
    }

    using bbox_t = std::tuple<double, double, double, double>;

    template<typename Pt>
    std::optional<bbox_t> bbox(std::vector<Pt> const& pts) {
        if (pts.empty()) {
            return std::nullopt;
        }
        auto [mnx, mxx] = std::minmax_element(pts.begin(), pts.end(), cmpX);
        auto [mny, mxy] = std::minmax_element(pts.begin(), pts.end(), cmpY);
        return std::make_tuple((*mnx)(0), (*mny)(1), (*mxx)(0), (*mxy)(1));
    }

    template<typename Pt, typename ...Args>
    std::optional<bbox_t> bbox(std::vector<Pt> const& pts, Args ...rest) {
        if (!pts.empty()) {
            auto opt_bbox = bbox(rest...);
            auto opt_bbox2 = bbox(pts);
            if (opt_bbox.has_value() && opt_bbox2.has_value()) {
                auto[mnx1, mny1, mxx1, mxy1] = opt_bbox.value();
                auto[mnx2, mny2, mxx2, mxy2] = opt_bbox2.value();
                return std::make_tuple(std::min(mnx1, mnx2), std::min(mny1, mny2), std::max(mxx1, mxx2),
                                       std::max(mxy1, mxy2));
            } else if (opt_bbox.has_value()) {
                return opt_bbox;
            } else {
                return opt_bbox2;
            }
        } else {
            return bbox(rest...);
        }
    }

//    template <int dim, class URNG>
//    wpoint_list_t weighted_random_sample_wor(wpoint_list_t& arr, URNG&& g, size_t sample_size) {
//        std::priority_queue<double, WPoint<dim>> queue_els;
//
//        sample_size = std::min(arr.size(), sample_size);
//        for (size_t i = 0; i < sample_size; ++i) {
//            std::uniform_int_distribution<decltype(i)> d(i, arr.size() - 1);
//            std::swap (arr[i], arr[d(g)]);
//        }
//        return Vec(arr.begin(), arr.begin() + sample_size);
//    }

}

#endif //PYSCAN_POINT_HPP


#pragma once
#include <Eigen/Core>
#include <vector>

// stupid but useful stuff for debugging with visual studio's immediate window

namespace dbg {

#define KT84DBG_H_MAKE_EIGEN_MATRIX_FUNC_SUB1(Type, Size)\
    Type&                           at  (Eigen::Matrix<Type, Size, Size>& m, int i, int j);\
    Eigen::Matrix<Type, Size, 1>    row (Eigen::Matrix<Type, Size, Size>& m, int i, Eigen::Matrix<Type, Size, 1>& dst);\
    Eigen::Matrix<Type, Size, 1>    col (Eigen::Matrix<Type, Size, Size>& m, int i, Eigen::Matrix<Type, Size, 1>& dst);\
    Type                            det (Eigen::Matrix<Type, Size, Size>& m);\
    Eigen::Matrix<Type, Size, Size> add (Eigen::Matrix<Type, Size, Size>& m1, Eigen::Matrix<Type, Size, Size>& m2, Eigen::Matrix<Type, Size, Size>& dst);\
    Eigen::Matrix<Type, Size, Size> sub (Eigen::Matrix<Type, Size, Size>& m1, Eigen::Matrix<Type, Size, Size>& m2, Eigen::Matrix<Type, Size, Size>& dst);\
    Eigen::Matrix<Type, Size, Size> mul (Eigen::Matrix<Type, Size, Size>& m1, Eigen::Matrix<Type, Size, Size>& m2, Eigen::Matrix<Type, Size, Size>& dst);\
    Eigen::Matrix<Type, Size, Size> mul (Type s, Eigen::Matrix<Type, Size, Size>& m, Eigen::Matrix<Type, Size, Size>& dst);\
    Eigen::Matrix<Type, Size, Size> wsum(Eigen::Matrix<Type, Size, Size>& m1, Eigen::Matrix<Type, Size, Size>& m2, Type w1, Type w2, Eigen::Matrix<Type, Size, Size>& dst);\
    Eigen::Matrix<Type, Size, Size> wsum(Eigen::Matrix<Type, Size, Size>& m1, Eigen::Matrix<Type, Size, Size>& m2, Eigen::Matrix<Type, Size, Size>& m3, Type w1, Type w2, Type w3, Eigen::Matrix<Type, Size, Size>& dst);\
    Eigen::Matrix<Type, Size, Size> rand(Eigen::Matrix<Type, Size, Size>& m, int rows, int cols);\
    Eigen::Matrix<Type, Size, Size> zero(Eigen::Matrix<Type, Size, Size>& m, int rows, int cols);\
    Eigen::Matrix<Type, Size, Size> rand(Eigen::Matrix<Type, Size, Size>& m);\
    Eigen::Matrix<Type, Size, Size> zero(Eigen::Matrix<Type, Size, Size>& m);\

#define KT84DBG_H_MAKE_EIGEN_VECTOR_FUNC_SUB1(Type, Rows, Cols)\
    Type&                           at   (Eigen::Matrix<Type, Rows, Cols>& v, int i);\
    Type                            dot  (Eigen::Matrix<Type, Rows, Cols>& v1, Eigen::Matrix<Type, Rows, Cols>& v2);\
    double                          norm(Eigen::Matrix<Type, Rows, Cols>& v);\
    Eigen::Matrix<Type, Rows, Cols> normalize(Eigen::Matrix<Type, Rows, Cols>& v);\
    Eigen::Matrix<Type, Rows, Cols> cross(Eigen::Matrix<Type, Rows, Cols>& v1, Eigen::Matrix<Type, Rows, Cols>& v2, Eigen::Matrix<Type, Rows, Cols>& dst);\
    Eigen::Matrix<Type, Rows, Cols> add  (Eigen::Matrix<Type, Rows, Cols>& v1, Eigen::Matrix<Type, Rows, Cols>& v2, Eigen::Matrix<Type, Rows, Cols>& dst);\
    Eigen::Matrix<Type, Rows, Cols> sub  (Eigen::Matrix<Type, Rows, Cols>& v1, Eigen::Matrix<Type, Rows, Cols>& v2, Eigen::Matrix<Type, Rows, Cols>& dst);\
    Eigen::Matrix<Type, Rows, Cols> mul  (Type s, Eigen::Matrix<Type, Rows, Cols>& v, Eigen::Matrix<Type, Rows, Cols>& dst);\
    /* TODO: matrix-vector multiplication */\
    Eigen::Matrix<Type, Rows, Cols> wsum (Eigen::Matrix<Type, Rows, Cols>& v1, Eigen::Matrix<Type, Rows, Cols>& v2, Type w1, Type w2, Eigen::Matrix<Type, Rows, Cols>& dst);\
    Eigen::Matrix<Type, Rows, Cols> wsum (Eigen::Matrix<Type, Rows, Cols>& v1, Eigen::Matrix<Type, Rows, Cols>& v2, Eigen::Matrix<Type, Rows, Cols>& v3, Type w1, Type w2, Type w3, Eigen::Matrix<Type, Rows, Cols>& dst);\
    Eigen::Matrix<Type, Rows, Cols> rand(Eigen::Matrix<Type, Rows, Cols>& v, int size);\
    Eigen::Matrix<Type, Rows, Cols> zero(Eigen::Matrix<Type, Rows, Cols>& v, int size);\
    Eigen::Matrix<Type, Rows, Cols> rand(Eigen::Matrix<Type, Rows, Cols>& v);\
    Eigen::Matrix<Type, Rows, Cols> zero(Eigen::Matrix<Type, Rows, Cols>& v);\

#define KT84DBG_H_MAKE_EIGEN_VECTOR_FUNC_SUB2(Type, Size)\
    KT84DBG_H_MAKE_EIGEN_VECTOR_FUNC_SUB1(Type, Size, 1)\
    KT84DBG_H_MAKE_EIGEN_VECTOR_FUNC_SUB1(Type, 1, Size)\

#define KT84DBG_H_MAKE_EIGEN_FUNC_SUB1(Type, Size)\
    KT84DBG_H_MAKE_EIGEN_MATRIX_FUNC_SUB1(Type, Size)\
    KT84DBG_H_MAKE_EIGEN_VECTOR_FUNC_SUB2(Type, Size)\

#define KT84DBG_H_MAKE_EIGEN_FUNC_SUB2(Type)\
    KT84DBG_H_MAKE_EIGEN_FUNC_SUB1(Type,  2)\
    KT84DBG_H_MAKE_EIGEN_FUNC_SUB1(Type,  3)\
    KT84DBG_H_MAKE_EIGEN_FUNC_SUB1(Type,  4)\
    KT84DBG_H_MAKE_EIGEN_FUNC_SUB1(Type, -1)\

#define KT84DBG_H_MAKE_STL_FUNC_SUB1(Type)\
    Type& at(std::vector<Type>& c, int i);\

const int pool_size = 100;

#define KT84DBG_H_MAKE_VARIABLE_SUB1(Type, TypeSuffix, Size, SizeSuffix)\
    extern Eigen::Matrix<Type, Size, Size> mat##SizeSuffix##TypeSuffix [pool_size];\
    extern Eigen::Matrix<Type, Size, 1>    vec##SizeSuffix##TypeSuffix [pool_size];\
    extern Eigen::Matrix<Type, 1, Size>    cvec##SizeSuffix##TypeSuffix[pool_size];\

#define KT84DBG_H_MAKE_VARIABLE_SUB2(Type, TypeSuffix)\
    extern Type x##TypeSuffix[pool_size];\
    KT84DBG_H_MAKE_VARIABLE_SUB1(Type, TypeSuffix,  2, 2)\
    KT84DBG_H_MAKE_VARIABLE_SUB1(Type, TypeSuffix,  3, 3)\
    KT84DBG_H_MAKE_VARIABLE_SUB1(Type, TypeSuffix,  4, 4)\
    KT84DBG_H_MAKE_VARIABLE_SUB1(Type, TypeSuffix, -1, x)\

#define KT84DBG_H_MAKE_SUB1(Type, TypeSuffix)\
    KT84DBG_H_MAKE_EIGEN_FUNC_SUB2(Type)\
    KT84DBG_H_MAKE_STL_FUNC_SUB1(Type)\
    KT84DBG_H_MAKE_VARIABLE_SUB2(Type, TypeSuffix)\

KT84DBG_H_MAKE_SUB1(int   , i)
KT84DBG_H_MAKE_SUB1(double, d)
KT84DBG_H_MAKE_SUB1(float , f)

#define KT84DBG_H_EIGEN_COPY_SUB1(TypeSrc, TypeDst, Size)\
    void copy(Eigen::Matrix<TypeSrc, Size, Size>& src, Eigen::Matrix<TypeDst, Size, Size>& dst);\
    void copy(Eigen::Matrix<TypeSrc, Size, 1   >& src, Eigen::Matrix<TypeDst, Size, 1   >& dst);\
    void copy(Eigen::Matrix<TypeSrc, 1   , Size>& src, Eigen::Matrix<TypeDst, 1   , Size>& dst);\

#define KT84DBG_H_EIGEN_COPY_SUB2(TypeSrc, TypeDst)\
    KT84DBG_H_EIGEN_COPY_SUB1(TypeSrc, TypeDst,  2)\
    KT84DBG_H_EIGEN_COPY_SUB1(TypeSrc, TypeDst,  3)\
    KT84DBG_H_EIGEN_COPY_SUB1(TypeSrc, TypeDst,  4)\
    KT84DBG_H_EIGEN_COPY_SUB1(TypeSrc, TypeDst, -1)\

KT84DBG_H_EIGEN_COPY_SUB2(int   , int   )
KT84DBG_H_EIGEN_COPY_SUB2(int   , double)
KT84DBG_H_EIGEN_COPY_SUB2(int   , float )
KT84DBG_H_EIGEN_COPY_SUB2(double, int   )
KT84DBG_H_EIGEN_COPY_SUB2(double, double)
KT84DBG_H_EIGEN_COPY_SUB2(double, float )
KT84DBG_H_EIGEN_COPY_SUB2(float , int   )
KT84DBG_H_EIGEN_COPY_SUB2(float , double)
KT84DBG_H_EIGEN_COPY_SUB2(float , float )

}

/*

if you're compiling dbg.cpp with your project instead of linking against *.lib,
make sure that these functions are included in the binary by specifying the linker option:
    Linker->Optimization->References->No (/OPT:NOREF)

example code:

int main() {
    MatrixXd V = MatrixXd::Random(100, 3);
    auto vec = util::make_vector<int>(5, 1, 2, 3, 4, 5);
    dbg::at(V, 0, 0);           // <-- dummy call to any function in dbg::, otherwise the Immediate Window won't find symbols, weird...
    return 0;
}

we can work in the immediate window like this:

dbg::at(V, 0, 2)
-0.12662129581591230
dbg::at(vec, 3)
4
dbg::row(V, 0, dbg::vecxd[0])
[3] (dynamic row vector)
    [size]: 3
    [0]: -0.99749748222296819
    [1]: -0.70500808740501109
    [2]: -0.12662129581591230
    [Raw View]: 0x005df930 {...}
dbg::row(V, 1, dbg::vecxd[1])
[3] (dynamic row vector)
    [size]: 3
    [0]: 0.12717062898648024
    [1]: 0.89916684469130526
    [2]: -0.53514206366161077
    [Raw View]: 0x005df930 {...}
dbg::row(V, 2, dbg::vecxd[2])
[3] (dynamic row vector)
    [size]: 3
    [0]: -0.61339152195806756
    [1]: -0.71684926908169810
    [2]: 0.15573595385601369
    [Raw View]: 0x005df930 {...}
dbg::sub(dbg::vecxd[1], dbg::vecxd[0], dbg::vecxd[3])
[3] (dynamic row vector)
    [size]: 3
    [0]: 1.1246681112094485
    [1]: 1.6041749320963163
    [2]: -0.40852076784569846
    [Raw View]: 0x005df930 {...}
dbg::sub(dbg::vecxd[2], dbg::vecxd[0], dbg::vecxd[4])
[3] (dynamic row vector)
    [size]: 3
    [0]: 0.38410596026490063
    [1]: -0.011841181676687018
    [2]: 0.28235724967192599
    [Raw View]: 0x005df930 {...}
dbg::cross(dbg::vecxd[3], dbg::vecxd[4], dbg::vecxd[5])
[3] (dynamic row vector)
    [size]: 3
    [0]: 0.44811305318860389
    [1]: -0.47447345649634626
    [2]: -0.62949055215654492
    [Raw View]: 0x005df930 {...}
dbg::normalize(dbg::vecxd[5])
[3] (dynamic row vector)
    [size]: 3
    [0]: 0.49419897060183438
    [1]: -0.52327039373186424
    [2]: -0.69423012935177841
    [Raw View]: 0x005df930 {...}

*/


#ifndef SHASTA_ALIGN4_HPP
#define SHASTA_ALIGN4_HPP

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

Marker alignment of two sequences, each defined as a sequence of
marker KmerId's.

We work with Feature's defined as a sequence of m markers,
where m is the template parameter of class Align4.

Each of the two input marker sequences sequence0 and sequence1
is treated as a sequence of Features's.
So for example consider an input sequence
consisting of the following marker KmerId's:
45 58 106 17
If m=2, the sequence of Feature's representing this sequence is
(45,58) (58,106), (106,17).

We call x the index or position in the first sequence sequence0
and y the index or position in the second sequence sequence1.
sequence[x] is the Feature at position x of sequence0
and sequence1[y] is the feature at position y of sequence1.
In other Shasta code, x and y are often called ordinals or marker
ordinals - but here they refer to Feature's, not markers.
The number of features in sequence0 is nx
and the number of features in sequence1 is ny.
For any two positions x and y the following hold:
0 <= x <= nx-1
0 <= y <= ny-1

Note that the sequences of Feature's are shorter (by m-1)
than the original marker sequences.

The alignment matrix in feature space is sparse because of the
large alphabet. For example, with default options there are
about 8000 marker KmerId's and therefore about 64000000
distinct features.

The coordinates in the alignment matrix are x and y, with x
represented along the horizontal axis and increasing toward the right,
and y represented along the vertical axis and increasing toward
the bottom. The alignment matrix element at position (x,y)
exists if sequence0[x]=sequence1[y].

We also use coordinates X and Y defined as:
X = x + y
Y = y + (nx - 1 - x)

It can be verified that:
0 <= X <= nx + ny - 2
0 <= Y <= nx + ny - 2

X is a coordinate along the diagonal of the alignment matrix,
and Y is orthogonal to it and identifies the diagonal.
In (X,Y) coordinates the alignment matrix is a subset of
the square of size nx + ny -1. The alignment matrix is
rotated by 45 degrees relative to this square.

Consider two successive points (x0,y0) and (x1, y1) in
a hypothetical alignment, and corresponding (X0,Y0) and (X1,Y1).
For a good alignment, the "skip" along the diagonal, |X1-X0|,
must be sufficiently small, and the "diagonal drift",
|Y1-Y0| must also be sufficiently small.
We require
|X1-X0| <= deltaX
|Y1-Y0| <= deltaY
Note that deltaX is similar in spirit to Shasta maxSkip
when working in marker space (9as opposed to Feature space).
Similarly, deltaY is similaro to Shasta maxDrift.

We construct a sparse representation of this sparse
alignment matrix with a special structure that, given an
alignment matrix entry at (x0,y0), allows efficient
look up of entries (x1,y1) which satisfy the
deltaX and deltaY criteria.

To achieve this, we use rectangular cells in (X,Y) space of size
(deltaX, deltaY). If (iX,iY) are the indices of a cell
containing (x0,y0), to locate all possible (x1,y1) we must
only look in the same cell (iX, iY) plus all of its 8
immediate neighbors (including diagonal neighbors).

The AlignmentMatrix is stored as an unordered_multimap with key (iX, iY)
and values consisting of Feature's.

To construct candidate alignments, we consider each element
in the alignment matrix as a vertex in a directed graph.
There is an edge between the vertices at positions (x0,y0) and (x1,y1)
if x1>=x0 and y1>=y0 and the above conditions on deltaX, deltaY
are satisfied.

Each candidate alignment must have at least one vertex near the top or left
and one vertex near the bottom or right of the alignment matrix.

To avoid doing a BFS on the entire alignment matrix, we do a BFS
starting only with vertices near the top or left.
We only keep connected components that also contain at
least one vertex neat the bottom or right.

For performance, we don't explicitly construct edges of the graph.
We find neighbors as needed.

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

#include "hashArray.hpp"
#include "Marker.hpp"
#include "span.hpp"

#include <boost/graph/adjacency_list.hpp>

#include "array.hpp"
#include <unordered_map>
#include <set>
#include "string.hpp"
#include "utility.hpp"
#include "vector.hpp"

namespace shasta {
    template<uint64_t m> class Align4;
    class Align4Options;
    class Alignment;
    class AlignmentInfo;

    void align4(
        const span<const CompressedMarker>&,
        const span<const CompressedMarker>&,
        const Align4Options&,
        Alignment&,
        AlignmentInfo&,
        bool debug);

    template<uint64_t m> void align4(
        const span<const CompressedMarker>&,
        const span<const CompressedMarker>&,
        const Align4Options&,
        Alignment&,
        AlignmentInfo&,
        bool debug);
}

class shasta::Align4Options {
public:
    uint64_t m;
    uint64_t deltaX;
    uint64_t deltaY;
    int64_t matchScore;
    int64_t mismatchScore;
    int64_t gapScore;
};

template<uint64_t m> class shasta::Align4 {
public:

    using Sequence = span<const CompressedMarker>;

    // The constructor does all the work.
    Align4(
        const Sequence&,
        const Sequence&,
        const Align4Options&,
        Alignment&,
        AlignmentInfo&,
        bool debug);

private:

    // A Feature is a sequence of m markers.
    using Feature = array<KmerId, m>;

    // A FeatureMap gives the x (or y) where each Feature occurs in one of the
    // sequences being aligned.
    using FeatureMap = std::unordered_multimap<Feature, uint32_t, HashTuple<Feature> >;
    static void fillFeatureMap(const Sequence&, FeatureMap&);

    // Cell sizes in the X and Y direction.
    int32_t deltaX;
    int32_t deltaY;

    // An entry of the alignment matrix.
    using Coordinates = pair<int32_t, int32_t>;
    class AlignmentMatrixEntry {
    public:

        // The vertex_descriptor in the Graph defined below.
        // This is only valid after createGraph was called.
        // This happens after unreachable vertices are removed.
        uint64_t v = std::numeric_limits<uint64_t>::max();
        bool hasValidVertex() const {
            return v != std::numeric_limits<uint64_t>::max();
        }

        Coordinates xy;
        Coordinates XY;

        // Flags for elements near the boundary of the alignment matrix.

        // Set if x < deltaX.
        uint8_t isNearLeft : 1;

        // Set if y < deltaX.
        uint8_t isNearTop : 1;

        // Set if nx-1-x < deltaX.
        uint8_t isNearRight : 1;

        // Set if ny-1-y < deltaX.
        uint8_t isNearBottom : 1;

        // Set if there is a forward path to this feature starting
        // near the top or left of the alignment matrix.
        uint8_t isForwardReachableFromTopOrLeft : 1;

        // Set if there is a backward path to this feature starting
        // near the bottom or right of the alignment matrix.
        uint8_t isBackwardReachableFromBottomOrRight : 1;

        // Flag used during the BFS.
        bool wasDiscovered = false;

        bool isNeighbor(const AlignmentMatrixEntry&,
            int32_t deltaX,
            int32_t deltaY) const;

        // Return true if (*this) is a child of that.
        bool isChild(
            const AlignmentMatrixEntry& that,
            int32_t deltaX,
            int32_t deltaY) const;

        // Return true if (*this) is a parent of that.
        bool isParent(
            const AlignmentMatrixEntry& that,
            int32_t deltaX,
            int32_t deltaY) const;
    };

    // In the alignment matrix, the key has the (iX,iY) cell coordinates.
    using AlignmentMatrix = std::unordered_multimap<Coordinates, AlignmentMatrixEntry, HashTuple<Coordinates> >;
    AlignmentMatrix alignmentMatrix;
    void fillAlignmentMatrix(
        const FeatureMap& featureMap0,
        const Sequence& sequence1,
        int32_t nx,
        int32_t ny);
    void writeMatrixCsv(const string& fileName);
    void writeMatrixPng(
        uint32_t nx, uint32_t ny,
        const string& fileName);
    void clearDiscoveredFlags();

    // Compute the reachability flags in the alignment matrix.
    void computeReachability();

    // Remove alignment matrix entries that are not reachable in both directions.
    void removeUnreachable();

    // Find neighbors and flag them as discovered.
    void findAndFlagUndiscoveredNeighbors(
        typename AlignmentMatrix::iterator,
        vector<typename AlignmentMatrix::iterator>&
    );
    void findChildren(
        typename AlignmentMatrix::iterator,
        vector<typename AlignmentMatrix::iterator>&
    );
    void findAndFlagUndiscoveredChildren(
        typename AlignmentMatrix::iterator,
        vector<typename AlignmentMatrix::iterator>&
    );
    void findAndFlagUndiscoveredParents(
        typename AlignmentMatrix::iterator,
        vector<typename AlignmentMatrix::iterator>&
    );

    // Order AlignmentMatrix iterators by increasing X, then Y.
    class OrderByIncreasingXThenY {
    public:
        bool operator() (
            const typename AlignmentMatrix::iterator& it0,
            const typename AlignmentMatrix::iterator& it1) const
        {
            const AlignmentMatrixEntry& entry0 = it0->second;
            const AlignmentMatrixEntry& entry1 = it1->second;
            return entry0.XY < entry1.XY;
        }
    };

    // After unreachable vertices are removed from the alignment matrix,
    // we create the equivalent Boost graph.
    // Each vertex contains an AlignmentMatrixEntry.
    class AlignmentMatrixEntry;
    using Graph = boost::adjacency_list<
        boost::vecS,
        boost::vecS,
        boost::bidirectionalS,
        typename AlignmentMatrix::iterator,
        boost::property<boost::edge_weight_t, uint64_t> >;
    using vertex_descriptor = typename Graph::vertex_descriptor;
    using edge_descriptor = typename Graph::edge_descriptor;
    Graph graph;
    void createGraph();

    // The optimal paths in the graph.
    using Path = vector<vertex_descriptor>;
    vector<Path> paths;
    void findShortestPaths(bool debug);
    void writePaths(const string& fileName) const;

    // Information for each path we found.
    class PathInfo {
    public:

        // The number of features in the path.
        uint64_t featureCount;

        // Aligned fraction is number of features over half the total X span
        // (which equals the average span in x and y).
        double alignedFraction;

        // The mean square X skip.
        double dX;

        // The mean square Y skip.
        double dY;

        // The minimum and maximum Y values. They define the band
        // containing the alignment (in feature space).
        int32_t minY;
        int32_t maxY;
        int32_t bandWidth() const {
            return maxY - minY;
        }

    };
    vector<PathInfo> pathInfos;
    void computePathInfo(const Path&, PathInfo&);
    void computePathInfos();
    void writePathInfos(const string& fileName);
};

#endif


/******************************************************************************
 *
 *  filename: Surfaces.h
 *  author  : Do Won Cha
 *
 *****************************************************************************/

#pragma once
#ifndef _RAY_SURFACES_
#define _RAY_SURFACES_

#include <Eigen/Core>
#include <string>

namespace raytracer
{

using namespace Eigen;

class Surface;
class Ray;

/**
 *	Hit data is returned upon call to IntersectSurfaces.
 */
struct HitData
{
  Vector3f hit_point, hit_time;
  Vector3f normal;
  float t, tMax;
  Surface* hit_surface;

  HitData() :
    t(0),
    tMax(10000.0f),
    hit_surface(nullptr)
  { }
};

/**
 *  Object's that have a position.
 */
class Node
{
public:

  Node() : position_(0.0f) { }
  Node(Vector3f position) : position_(position) { }

  virtual ~Node() { }

  Vector3f position() const         { return position_; }
  void set_position(Vector3f position) { position_ = position; }
protected:
  Vector3f position_;
};

/**
 *  Abstract class for objects that require a position and a material.
 */
class Surface : public Node
{
public:
  Surface(Vector3f position, std::string material_name = "") :
    Node(position),
    material_name_(material_name)
  { }

  virtual ~Surface() { }

  virtual bool Intersect(const Ray& ray, HitData& hit) = 0;

  virtual Vector3f normal() const;

  void set_material(std::string material_name) { material_name_ = material_name; }
  std::string material() const { return material_name_; }
protected:
  std::string material_name_;
};

} // end of namespace raytracer

#endif // RAY_SURFACES end


/*
 * Copyright 2021 IBM Corporation
 *
 * 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/UtlTestCommon.h"

#include <SPL/Runtime/Function/BoostMatrixHelper.h>
#include <SPL/Runtime/Function/SPLFunctions.h>
#include <SPL/Runtime/Type/SPLType.h>
#include <UTILS/DistilleryApplication.h>

#include <boost/numeric/ublas/io.hpp>

#include <algorithm>

using namespace std;
using namespace SPL;
using namespace Distillery;

namespace SPL {

class MatrixConversionTest : public DistilleryApplication
{
  public:
    MatrixConversionTest() {}

    virtual int run(const std::vector<std::string>& /*remains*/)
    {
        list<list<double> > lm;
        lm.push_back(list<double>());
        lm.push_back(list<double>());
        lm[0].push_back(11);
        lm[0].push_back(12);
        lm[0].push_back(13);
        lm[1].push_back(21);
        lm[1].push_back(22);
        lm[1].push_back(23);

        ostringstream ostr;

        using namespace boost::numeric::ublas;

        // small matrix, from large list
        matrix<double> m(0, 0);
        SPL::Functions::Math::Compat::convertFromNestedListToUblasMatrix(m, lm);
        ostr.str("");
        ostr << m;
        FASSERT(ostr.str() == "[2,3]((11,12,13),(21,22,23))");

        // large matrix, from small list
        list<list<double> > lm1(lm);
        lm1.pop_back();
        SPL::Functions::Math::Compat::convertFromNestedListToUblasMatrix(m, lm1);
        ostr.str("");
        ostr << m;
        FASSERT(ostr.str() == "[1,3]((11,12,13))");
        lm1[0].pop_back();
        SPL::Functions::Math::Compat::convertFromNestedListToUblasMatrix(m, lm1);
        ostr.str("");
        ostr << m;
        FASSERT(ostr.str() == "[1,2]((11,12))");

        // back to original
        SPL::Functions::Math::Compat::convertFromNestedListToUblasMatrix(m, lm);

        // large matrix, to small list
        list<list<double> > lm2;
        SPL::Functions::Math::Compat::convertToNestedListFromUblasMatrix(lm2, m);
        ostr.str("");
        ostr << lm2;
        FASSERT(ostr.str() == "[[11,12,13],[21,22,23]]");

        // small matrix, to large list
        m.resize(2, 2);
        SPL::Functions::Math::Compat::convertToNestedListFromUblasMatrix(lm2, m);
        ostr.str("");
        ostr << lm2;
        FASSERT(ostr.str() == "[[11,12],[21,22]]");

        return EXIT_SUCCESS;
    }
};

};

MAIN_APP(SPL::MatrixConversionTest)


#include <cctbx/symmetry_search/translation_refinement.h>

#include <scitbx/array_family/boost_python/shared_flat_conversions.h>

#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/return_by_value.hpp>

namespace boost {
  template <>
  struct has_trivial_destructor<scitbx::af::tiny<std::complex<double>, 3> > {
    static const bool value = true;
  };
}

namespace cctbx { namespace symmetry_search { namespace boost_python {

  template <typename FloatType>
  struct symmetrised_shifted_structure_factors_wrapper
  {
    typedef symmetrised_shifted_structure_factors<FloatType> wt;
    typedef typename wt::real_type real_type;
    typedef typename wt::complex_type complex_type;
    typedef typename wt::complex_grad_type complex_grad_type;
    typedef typename wt::vector_type vector_type;

    static void wrap(char const *name) {
      using namespace boost::python;
      return_value_policy<return_by_value> rbv;
      af::boost_python::flat_shared_conversions<complex_grad_type>();
      class_<wt>(name, no_init)
        .def(init<sgtbx::space_group const &,
                  af::const_ref<miller::index<> > const &,
                  miller::f_calc_map<real_type> &,
                  vector_type const &,
                  bool>
             ((arg("space_group"), arg("indices"), arg("f_c"),
               arg("x"), arg("compute_gradient")=false)))
        .add_property("f_x", make_getter(&wt::f_x, rbv))
        .add_property("grad_f_x", make_getter(&wt::grad_f_x, rbv))
        ;
    }
  };

  template <typename FloatType>
  struct ls_with_scale_and_bias_wrapper
  {
    typedef ls_with_scale_and_bias<FloatType> wt;
    typedef typename wt::real_type real_type;
    typedef typename wt::complex_type complex_type;
    typedef typename wt::complex_grad_type complex_grad_type;

    static void wrap(char const *name) {
      using namespace boost::python;
      return_value_policy<return_by_value> rbv;
      class_<wt>(name, no_init)
        .def(init<af::const_ref<complex_type> const &,
                  af::const_ref<complex_grad_type> const &,
                  af::const_ref<real_type> const &,
                  af::const_ref<real_type> const &>
             ((arg("f_x"), arg("grad_f_x"), arg("f_o_sq"), arg("weight"))))
        .def_readonly("value", &wt::q)
        .def_readonly("correlation", &wt::c)
        .add_property("gradient", make_getter(&wt::grad_q, rbv))
        .def_readonly("scale", &wt::lambda)
        .def_readonly("bias", &wt::mu)
        ;
    }
  };

  void init_module() {
    using namespace boost::python;
    symmetrised_shifted_structure_factors_wrapper<double>::wrap(
      "symmetrised_shifted_structure_factors");
    ls_with_scale_and_bias_wrapper<double>::wrap("ls_with_scale_and_bias");
  }

}}} // boost_python

BOOST_PYTHON_MODULE(cctbx_symmetry_search_ext)
{
  cctbx::symmetry_search::boost_python::init_module();
}


// Copyright 2015 Will Oursler

#include <boost/config.hpp>
#include <boost/iterator/filter_iterator.hpp>
#include <boost/cstdlib.hpp>  // for exit_success
#include <stdio.h>
#include <random>
#include <iostream>
#include <vector>
#include <algorithm>
#include <functional>
#include <string>
#include <iterator>
#include <utility>

#include "bbox.h"
#include "rstore.h"
#include "naive_rstore.h"

class bbox_distribution {
 private:
  std::uniform_real_distribution<float> x_dist, y_dist, width_dist, height_dist;

 public:
  bbox_distribution(
    std::uniform_real_distribution<float> x_dist,
    std::uniform_real_distribution<float> y_dist,
    std::uniform_real_distribution<float> width_dist,
    std::uniform_real_distribution<float> height_dist )
    : x_dist(x_dist),
    y_dist(y_dist),
    width_dist(width_dist),
    height_dist(height_dist) {}

  template <class RNG>
  BBox operator()(RNG &gen);
};

template <class RNG>
BBox bbox_distribution::operator()(RNG &gen) {
  float x_center = x_dist(gen);
  float y_center = y_dist(gen);
  float width = width_dist(gen);
  float height = height_dist(gen);

  BBox rect;

  rect.left = x_center - (width / 2);
  rect.right  = x_center + (width / 2);
  rect.bottom = x_center - (height / 2);
  rect.top  = x_center + (height / 2);

  return rect;
}

void pprint(const BBox rect) {
  std::cout << "LEFT: ";
  if (!rect.l_inf) std::cout << rect.left << std::endl;
  else
    std::cout << "NegInf" << std::endl;

  std::cout << "RIGHT: ";
  if (!rect.r_inf) std::cout << rect.right << std::endl;
  else
    std::cout << "Inf" << std::endl;

  std::cout << "TOP: ";
  if (!rect.t_inf) std::cout << rect.top << std::endl;
  else
    std::cout << "Inf" << std::endl;

  std::cout << "BOTTOM: ";
  if (!rect.b_inf) std::cout << rect.bottom << std::endl;
  else
    std::cout << "NegInf" << std::endl;
}

int main() {
  std::random_device rd;
  std::mt19937 mt(rd());

  std::uniform_real_distribution<float> x_dist(-100, 100);
  std::uniform_real_distribution<float> y_dist(-100, 100);
  std::uniform_real_distribution<float> width_dist(0.1, 50);
  std::uniform_real_distribution<float> height_dist(0.1, 50);

  bbox_distribution rect_dist(x_dist, y_dist, width_dist, height_dist);

  std::vector<std::pair<int, BBox>> entries;

  for (int i=0; i < 100; ++i) {
    entries.push_back(std::make_pair(i, rect_dist(mt)));
  }

  NaiveRStore<int> rstore(entries);

  BBox reference;

  reference.l_inf = true;
  reference.right  = 0;
  reference.bottom = -1;
  reference.top  = 1;
  pprint(reference);

  for (auto rect : rstore.overlapping(reference)) {
    pprint(rect.second);
    std::cout << std::endl;
  }

  return boost::exit_success;
}


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_REFINE_REC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_REFINE_REC_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!

    @ingroup group-arithmetic
    This function object performs a Newton-Raphson step to improve precision of a reciprocate estimate.

    This function can be used in conjunction with raw_(rec)
    to add more precision to the estimates if their default
    precision is not enough.

    @par Header <boost/simd/function/refine_rec.hpp>

    @par semantic:

    @code
    auto r =refine_rec(x, est);
    @endcode

    is similar to

    @code
    auto r = fma(fnms(est, a0, One<T>()), est, est);
    @endcode

    @see rec

  **/
  IEEEValue refine_rec(IEEEValue const& x, IEEEValue const& est);
} }
#endif

#include <boost/simd/function/scalar/refine_rec.hpp>
#include <boost/simd/function/simd/refine_rec.hpp>

#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_REC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_REC_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing rec capabilities

    Returns the inverse (reciprocal) of the entry.

    @par semantic:
    For any given value @c x of type @c T:

    @code
    T r = rec(x);
    @endcode

    The code is similar to:

    @code
    T r = T(1)/x;
    @endcode

    @par Note:
    Speed can be sometimes gained using rec(x, fast_) but fast means here
    that the computation is possibly done through some
    low precision intrinsic. The result can be not fully accurate.

  **/
  const boost::dispatch::functor<tag::rec_> rec = {};
} }
#endif

#include <boost/simd/function/scalar/rec.hpp>
#include <boost/simd/function/scalar/rec.hpp>
#include <boost/simd/function/simd/rec.hpp>

#endif


#include <boost/test/unit_test.hpp>
#include <chrono>
#include <iostream>
#include "regularizer.hpp"

using namespace deconvolution;

constexpr double epsilon = 0.0001;

BOOST_AUTO_TEST_SUITE(RegularizerTests)

    BOOST_AUTO_TEST_CASE(BasicInterface) {
        auto ep = TruncatedL1{0, 0};
        auto R = GridRegularizer<1, TruncatedL1>{std::vector<int>{10}, 2, 1, ep};

        BOOST_CHECK_EQUAL(R.numSubproblems(), 1);
        BOOST_CHECK_EQUAL(R.numLabels(), 2);
        for (int i = 0; i < 10; ++i) {
            BOOST_CHECK_EQUAL(R.getLabel(i, 0), 0);
            BOOST_CHECK_EQUAL(R.getLabel(i, 1), 1);
        }
    }

    BOOST_AUTO_TEST_CASE(SingleVariable) {
        auto ep = TruncatedL1{0, 0};
        auto R = GridRegularizer<1, TruncatedL1>{std::vector<int>{1}, 2, 1, ep};

        std::vector<double> lambda = { 1.0, 1.0 };
        std::vector<double> gradient = {0.0, 0.0};

        for (double t = 1024.0; t >= 1.0/1024.0; t /= 2) {
            gradient[0] = 0.0;
            gradient[1] = 0.0;
            auto result = R.evaluate(0, lambda.data(), t, gradient.data(), nullptr);

            BOOST_CHECK_CLOSE(result, -1.0-t*log(2), epsilon);
            BOOST_CHECK_CLOSE(gradient[0], -0.5, epsilon);
            BOOST_CHECK_CLOSE(gradient[1], -0.5, epsilon);
        }

        lambda[1] = 0.0;

        for (double t = 1024.0; t >= 1.0/1024.0; t /= 2) {
            gradient[0] = 0.0;
            gradient[1] = 0.0;
            auto result = R.evaluate(0, lambda.data(), t, gradient.data(), nullptr);

            BOOST_CHECK_CLOSE(result, -1.0-t*log(1+exp(-1.0/t)), epsilon);
            BOOST_CHECK_GE(result, -1.0-t*log(2));
            BOOST_CHECK_CLOSE(gradient[0], -1.0/(1+exp(-1.0/t)), epsilon);
            BOOST_CHECK_CLOSE(gradient[1], -exp(-1.0/t)/(1+exp(-1.0/t)), epsilon);
        }

    }

    BOOST_AUTO_TEST_CASE(MultipleVars) {
        for (int n : {10, 100, 1000}) {
            auto ep = TruncatedL1{0, 0};
            auto R = GridRegularizer<1, TruncatedL1>{std::vector<int>{n}, 2, 1, ep};
            std::vector<double> lambda(n*2, 1.0);
            std::vector<double> gradient(n*2, 0);

            //for (double t = 1024.0; t >= 1.0/1024.0; t /= 2) {
            double t = 1.0;
            {
                auto result = R.evaluate(0, lambda.data(), t, gradient.data(), nullptr);

                BOOST_CHECK_CLOSE(result, -n - t*n*log(2), epsilon);
                BOOST_CHECK_GE(result, -n - t*n*log(2)-epsilon);
                for (int i = 0; i < 2*n; ++i)
                    BOOST_CHECK_CLOSE(gradient[i], -0.5, epsilon);
            }
        }
    }

    BOOST_AUTO_TEST_CASE(Primal) {
        auto ep = TruncatedL1{3.0, 1.0};
        auto R = GridRegularizer<2, TruncatedL1>{std::vector<int>{3, 4}, 2, 1, ep};

        double x[] = {
            4.0, 1.0, 1.0, 7.0,
            3.0, 7.0, 7.0, 5.0,
            0.0,-2.0, 2.0, 4.0
        };

        auto primal = R.primal(x, nullptr);
        BOOST_CHECK_CLOSE(primal, 18 + 19, epsilon);
    }

    BOOST_AUTO_TEST_CASE(Timing) {
        int width = 200;
        int height = 200;
        int nLabels = 32;
        auto ep = TruncatedL1{0, 0};
        auto R = GridRegularizer<2, TruncatedL1>{std::vector<int>{width, height}, nLabels, 1, ep};

        std::vector<double> lambda(width*height*nLabels, 0.0);
        std::vector<double> gradient(width*height*nLabels, 0.0);

        auto startTime = std::chrono::system_clock::now();
        for (int subproblem : {0, 1}) {
            R.evaluate(subproblem, lambda.data(), 1.0, gradient.data(), nullptr);
        }
        double time = std::chrono::duration<double>{std::chrono::system_clock::now() - startTime}.count();
        std::cout << "Time: " << time << "\n";
    }

BOOST_AUTO_TEST_SUITE_END()


/* ----------------------------------------------------------------------- *//**
 *
 * @file EigenIntegration.hpp
 *
 *//* ----------------------------------------------------------------------- */

#ifndef MADLIB_DBAL_EIGEN_INTEGRATION_HPP
#define MADLIB_DBAL_EIGEN_INTEGRATION_HPP

// Plugin with methods to add to Eigen's MatrixBase
#define EIGEN_MATRIXBASE_PLUGIN <dbal/EigenIntegration/EigenPlugin.hpp>
#include <Eigen/Dense>

#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
#include <Eigen/Sparse>

namespace madlib {

namespace dbal {

namespace eigen_integration {

typedef Eigen::VectorXi IntegerVector;
typedef Eigen::VectorXd ColumnVector;
typedef Eigen::RowVectorXd RowVector;
typedef Eigen::VectorXcd VectorXcd;
typedef Eigen::MatrixXd Matrix;
typedef Eigen::MatrixXcd ComplexMatrix;
typedef EIGEN_DEFAULT_DENSE_INDEX_TYPE Index;
typedef Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, Index> PermutationMatrix;

typedef Eigen::SparseVector<double> SparseColumnVector;
typedef Eigen::SparseMatrix<double> SparseMatrix;

typedef Eigen::LDLT<Eigen::MatrixXd> MatrixLDLT;
typedef Eigen::HouseholderQR<Eigen::MatrixXd> HouseholderQR;
typedef Eigen::FullPivLU<Eigen::MatrixXd> FullPivLU;
typedef Eigen::JacobiSVD<Eigen::MatrixXd> JacobiSVD;

enum ViewMode {
    Lower = Eigen::Lower,
    Upper = Eigen::Upper,
    StrictlyLower = Eigen::StrictlyLower
};

enum DecompositionOptions {
    ComputeEigenvectors = Eigen::ComputeEigenvectors,
    EigenvaluesOnly = Eigen::EigenvaluesOnly,
    ComputeFullU = Eigen::ComputeFullU,
    ComputeFullV = Eigen::ComputeFullV
};

enum SPDDecompositionExtras {
    ComputePseudoInverse = 0x01
};

using Eigen::Success;

// In the following we make several definitions that allow certain object
// methods to be called as a plain functions. This makes using Eigen more
// similar to Armadillo.

template <typename Derived>
inline
typename Eigen::MatrixBase<Derived>::ConstTransposeReturnType
static trans(const Eigen::MatrixBase<Derived>& mat) {
    return mat.transpose();
}

template <typename Derived, typename OtherDerived>
inline
typename Eigen::internal::scalar_product_traits<
    typename Eigen::internal::traits<Derived>::Scalar,
    typename Eigen::internal::traits<OtherDerived>::Scalar
>::ReturnType
static dot(
    const Eigen::MatrixBase<Derived>& mat,
    const Eigen::MatrixBase<OtherDerived>& other) {
    return mat.dot(other);
}

template <typename Derived>
inline
typename Eigen::MatrixBase<Derived>::CoeffReturnType
static as_scalar(const Eigen::MatrixBase<Derived>& mat) {
    return mat.value();
}

template <ViewMode Mode, typename Derived>
inline
typename Eigen::MatrixBase<Derived>::template TriangularViewReturnType<Mode>::Type
static triangularView(Eigen::MatrixBase<Derived>& mat) {
    return mat.template triangularView<Mode>();
}

template <typename Derived>
bool
static isfinite(const Eigen::MatrixBase<Derived>& mat) {
    return mat.is_finite();
}

} // namespace eigen_integration

} // namespace dbal

} // namespace madlib

#include "HandleMap_proto.hpp"
#include "SymmetricPositiveDefiniteEigenDecomposition_proto.hpp"

#include "HandleMap_impl.hpp"
#include "SymmetricPositiveDefiniteEigenDecomposition_impl.hpp"

#endif // defined(MADLIB_EIGEN_INTEGRATION_HPP)