#include <cmath>
#include <fstream>
#include <string>

#include <boost/timer/timer.hpp>

#include "optimization.hh"
#include "adaptive_refinement.hh"
#include "linalg/linearsystem.hh"
#include "linalg/triplet.hh"
#include "./opt_aux/src/include/Fmin.h"
#include "modelCreator.hh"
#include "modelFunctions.hh"

#ifndef Cygwin
using std::to_string;
#else
std::string to_string(size_t n)
{
  std::stringstream s; s << n; return s.str();
}
#endif

namespace Kaskade
{
  OptimizationParameters::OptimizationParameters(double desiredAccuracy_, int maxSteps_) : desiredAccuracy(desiredAccuracy_), maxSteps(maxSteps_) {}

  void OptimizationParameters::setThetaAim(double theta) { ThetaAim = theta; ensureAdmissibleThetas(); }
  void OptimizationParameters::setThetaNormal(double theta) { ThetaNormal = theta; ensureAdmissibleThetas(); }
  void OptimizationParameters::setThetaMax(double theta) { ThetaMax = theta; ensureAdmissibleThetas(); }
  void OptimizationParameters::setThetas(double thetaNormal, double thetaAim, double thetaMax)
  {
    ThetaNormal = thetaNormal;
    ThetaAim = thetaAim;
    ThetaMax = thetaMax;
    ensureAdmissibleThetas();
  }
  void OptimizationParameters::setEps(double eps_)
  {
    eps = eps_;
    sqrtEps = sqrt(eps);
    thirdSqrtEps = pow(eps,1./3.);
  }

  double OptimizationParameters::getThetaAim() const { return ThetaAim; }
  double OptimizationParameters::getThetaNormal() const { return ThetaNormal; }
  double OptimizationParameters::getThetaMax() const { return ThetaMax; }
  double OptimizationParameters::getEps() const { return eps; }
  double OptimizationParameters::getSqrtEps() const { return sqrtEps; }
  double OptimizationParameters::getThirdSqrtEps() const { return thirdSqrtEps; }

  void OptimizationParameters::ensureAdmissibleThetas()
  {
    if( !(ThetaAim < ThetaMax) )
    {
      std::cout << "OPTIMIZATION PARAMETERS: Warning: Inconsistent algorithmic parameters. (ThetaMax <= ThetaAIm)" << std::endl;
      std::cout << "OPTIMIZATION PARAMETERS: Adjusting ThetaAim from " << ThetaAim << " to ";
      ThetaAim = 0.9*ThetaMax;
      std::cout << ThetaAim << std::endl;
    }
    if( !(ThetaNormal < ThetaAim) )
    {
      std::cout << "OPTIMIZATION PARAMETERS: Warning: Inconsistent algorithmic parameters. (ThetaAim <= ThetaNormal)" << std::endl;
      std::cout << "OPTIMIZATION PARAMETERS: Adjusting ThetaNormal from " << ThetaNormal << " to ";
      ThetaNormal = 0.9*ThetaAim;
      std::cout << ThetaNormal << std::endl;
    }
  }

  Optimization::~Optimization(){}

  Optimization::Optimization(AbstractScalarProduct& nL, AbstractScalarProduct& nC, AbstractChart const& chart_, OptimizationParameters const& p_,
                             AbstractCompositeStepErrorEstimator* errorEstimator_, int verbose_)
    : normL(nL), normC(nC), chart(chart_.clone()), p(p_), errorEstimator(errorEstimator_),
      verbose(verbose_),L(verbose), C(verbose)
  {}

  Optimization::Optimization(AbstractNormalDirection& normalSolver,
                             AbstractTangentialSpace& tangentSpace_,
                             AbstractScalarProduct& nL,
                             OptimizationParameters const& p_,
                             double omegaCinit, double omegaLinit, int verbose_):
    normL(nL),
    normC(nL),
    chart(new PrimalChart()),
    p(p_),
    normalDirection(&normalSolver),
    tangentSpace(&tangentSpace_),
    verbose(verbose_),
    L(verbose,omegaLinit), C(verbose,omegaCinit)
  {}

  Optimization::Optimization(AbstractNormalDirection& normalSolver,
                             AbstractScalarProduct& nL,
                             AbstractScalarProduct& nC,
                             AbstractChart const& chart_,
                             OptimizationParameters const& p_, int verbose_):
    normL(nL),
    normC(nC),
    chart(chart_.clone()),
    p(p_),
    normalDirection(&normalSolver),
    verbose(verbose_),
    L(verbose), C(verbose)
  {}

  /// Return true, if convergence is detected, false otherwise
  Convergence Optimization::convergenceTest(double nu, std::vector<double> const& tau, double normOfCorrection) const
  {
    if( verbose > 0 )
    {
      std::cout << csPre << "Desired accuracy: " << p.desiredAccuracy << ", ||x|| = " << normOfIterate << std::endl;
      std::cout << csPre << "||dx|| = " << normOfCorrection << std::endl;
    }

    //if( nu > 0.1 && normOfCorrection < p.getEps() ) return Convergence::Achieved;

    if( !tangentSpace->localConvergenceLikely() ) return Convergence::Missed;

    // no local convergence as long as at least one of the steps is damped
    if( !noDamping(nu) || !noDamping(tau) ) return Convergence::Missed;
    if( normOfCorrection < p.desiredAccuracy * normOfIterate ) return Convergence::Achieved;

    return Convergence::Missed;
  }

  bool Optimization::adaptiveMeshRefinement(LagrangeLinearization& lin, double nu, std::vector<double> tau, AbstractFunctionSpaceElement const& correction)
  {
    double safetyFactorForDesiredAccuracy = 0.9;

    if( !( noDamping(nu) && noDamping(tau) ) )
    {
      std::cout << csPre << "Damping factors != 1 -> Skipping error estimation." << std::endl;
      return false;
    }

    if(hbErrorEstimator->gridSize() > p.maxGridSize)
    {
      std::cout << csPre << "Grid size: " << hbErrorEstimator->gridSize() << " > " << p.maxGridSize << ". Skipping error estimation. " << std::endl;
      return false;
    }

      // estimate error
    (*hbErrorEstimator)(lin, *iterate, correction, step, *iterate);

    double absoluteError=hbErrorEstimator->estimatedAbsoluteError();
    double requiredAbsoluteError=std::max(p.requiredRelativeError*normOfLastCorrection,p.desiredAccuracy*safetyFactorForDesiredAccuracy*normL(*iterate));
    bool accuracyReached = absoluteError < requiredAbsoluteError;

    if(verbose > 0)
    {
      std::cout << csPre << "ERROR ESTIMATOR: err0=" << p.requiredRelativeError*normOfLastCorrection << ", err1=" << p.desiredAccuracy*safetyFactorForDesiredAccuracy << std::endl;
      std::cout << csPre << "ERROR ESTIMATOR: normOfAcceptedCorrection=" << normOfLastCorrection << ", p.desiredAccuracy=" << p.desiredAccuracy << std::endl;
      std::cout << csPre << "ERROR ESTIMATOR: absolute error: " << absoluteError << ", required error: " << requiredAbsoluteError << std::endl;
    }

    if(accuracyReached)
    {
      std::cout << "ERROR ESTIMATOR: Discretization accuracy reached!" << std::endl;
      return false;
    }

    hbErrorEstimator->refineGrid();
    return true;
  }

  int Optimization::runAlgorithm()
  {
    std::ofstream omegafile("omegas.log");
    std::ofstream coefffile("coeff.log");
    std::ofstream correctionfile("corrections.log");
    std::ofstream values("values.log");
    values.setf(std::ios::scientific,std::ios::floatfield);
    values.precision(16);
    std::ofstream steps("steps.log");

    double nu = 0;
    std::vector<double> tau(1,0);
    // Main loop
    for( step=1 ; step <= p.maxSteps; ++step )
    {
      if(verbose > 0) std::cout <<  "\n --------------- Optimization-Step: " << step << " ----------------------" << std::endl;

      /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
      /* * * * * * * * * * * * * * * * Normal Step * * * * * * * * * * * * * * * * */
      /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
      if( functionalN == nullptr ) std::cout << "no normal lineraization" << std::endl;
      normalLinearization = functionalN->getLinearization(*iterate);
      values << normalLinearization->eval() << std::endl;

      normC.setOrigin(*normalLinearization);
      normL.setOrigin(*normalLinearization);
      std::unique_ptr<AbstractFunctionSpaceElement> normalStepResidual(iterate->initZeroVector()), adjointResidual(iterate->initZeroVector()), undampedStep(iterate->initZeroVector());
      auto normalStepResult = computeNormalStep(normalStepResidual.get(),adjointResidual.get());

      std::unique_ptr<AbstractFunctionSpaceElement> normalStep(std::move(normalStepResult.first)), adjointCorrection(std::move(normalStepResult.second));
      undampedStep->axpy(1.0,*normalStep,"primal");
      double normOfUndampedCorrection = 0;

      double Lxdn_res = adjointResidual->applyAsDualTo(*normalStep,"primal")
                      - normalStepResidual->applyAsDualTo(*adjointCorrection,"primal")
                      + adjointResidual->applyAsDualTo(*normalStep,"dual");
      if( verbose > 1 ) std::cout << csPre << "Lxdn_res: " << Lxdn_res << std::endl;
      auto normNormal = normC( *normalStep );

      // Predict damping factor for normal step
      nu = (normNormal > 0) ? updateNormalStepDampingFactor(normNormal) : 1.;
      if( verbose > 0 ) printNormalStep(normNormal,nu);

      std::unique_ptr<AbstractFunctionSpaceElement> tmpIter(iterate->clone());
      tmpIter->axpy(nu,*normalStep,"primal");
      std::unique_ptr<AbstractLinearization> lin_x_nudn(functionalN->getLinearization(*tmpIter));

      std::unique_ptr<AbstractFunctionSpaceElement> dc_x0_dn ( iterate->initZeroVector() );
      normalLinearization->d2axpy(1.0,*dc_x0_dn, *normalStep, 2, 3, 0, 2);
      double p_dc_x0_dn = iterate->applyAsDualTo(*dc_x0_dn,"dual");
      double f_x1 = lin_x_nudn->eval();
      double f_x0 = normalLinearization->eval();

      double normalStepMonitor = ( ( std::fabs(f_x1 - f_x0) + std::fabs(p_dc_x0_dn) ) > 0 ) ? std::fabs( f_x1 - f_x0 + nu*p_dc_x0_dn - Lxdn_res ) / ( std::fabs(f_x1 - f_x0) + std::fabs(p_dc_x0_dn) ) : 0.;
      if( normNormal < p.getSqrtEps()*normOfIterate || step == 1 ) normalStepMonitor = 0;
      bool reliableQuadraticModel = true;//normalStepMonitor < 0.5;

      if( verbose > 1 )
      {
        std::cout << "f(x0) = " << f_x0 << ", f(x1) = " << f_x1 << ", nupc'(x0) = " << nu << "*" << p_dc_x0_dn << std::endl;
        std::cout << "f(x1) - f(x0) = " << f_x1 - f_x0 << std::endl;
      }
      if( verbose > 0 ) std::cout << csPre << "NORMAL STEP MONITOR: " << normalStepMonitor << ", is reliable: " << reliableQuadraticModel << std::endl;

      /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
      /* * * * * * * * * * * * * * * Tangential Step * * * * * * * * * * * * * * * */
      /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
      double normTangential = 0;
      normOfUndampedCorrection = normNormal;
      std::vector<std::shared_ptr<AbstractFunctionSpaceElement> > tangentialBasis(1,iterate->clone());

      std::unique_ptr<LagrangeLinearization> lagrangeLinearization(nullptr);

      if(reliableQuadraticModel)
      {
        if(functionalT!=nullptr) tangentialLinearization = functionalT->getLinearization(*iterate);
        else tangentialLinearization = nullptr;

        lagrangeLinearization.reset(new LagrangeLinearization(normalLinearization.get(),tangentialLinearization.get(),*iterate));

        tangentialBasis = computeTangentialStep(*lagrangeLinearization,*normalStep, nu, tau);
        for(size_t i=0; i<tangentialBasis.size(); ++i) undampedStep->axpy(1.0,*tangentialBasis[i],"primal");
        normTangential = normL(*tangentialBasis[0]);
        normOfUndampedCorrection = normC(*undampedStep);
      }
      else
      {
        *tangentialBasis[0] *= 0;
        tau[0] = 0;
      }
      if( verbose > 0 ) std::cout << csPre << "Tangential step length: " << normTangential << std::endl;
      /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
      /* * * * * * * * * * * Lipschitz constants and damping * * * * * * * * * * * */
      /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
      AcceptanceTest acceptanceTestResult = AcceptanceTest::Failed;
      std::unique_ptr<AbstractFunctionSpaceElement> trialIterate(iterate->initZeroVector()), secondOrderCorrected(iterate->clone()), correction(iterate->initZeroVector());
      double normCAtCorr = normNormal;
      while(acceptanceTestResult != AcceptanceTest::Passed)
      {
        double eta = 1;
        if( acceptanceTestResult == AcceptanceTest::LeftAdmissibleDomain ) nu *= 0.5;
        else nu = updateNormalStepDampingFactor(normNormal);
        if( verbose > 0 ) printNormalStep(normNormal,nu);
        if(!reliableQuadraticModel)
        {
          correction = createCorrection(nu,*normalStep, tau, tangentialBasis);
          normCAtCorr = normC(*correction);
          // Compute Trial Iterate: first candidate: trialIterate = chart_x(dx)
          *trialIterate *= 0;
          chart->addPerturbation(*trialIterate,*correction,*normalLinearization);

          std::unique_ptr<AbstractLinearization> lin_xplus(functionalN->getLinearization(*trialIterate));
          auto simplifiedNormalStep = computeSimplifiedNormalStep( *lin_xplus, normalStepResidual.get());
          simplifiedNormalStep->axpy( nu-1.0, *normalStep, "primal" );

          updateConstraintD1LipschitzConstant( normC( *simplifiedNormalStep ), normCAtCorr );
        }
        else
        {
          QuadraticModelCreator quadraticModelCreator(*normalStep,tangentialBasis,*lagrangeLinearization,Lxdn_res);
          NormModelCreator normModelCreatorL(*normalStep,tangentialBasis,normL);

          QuadraticFunction normLModel(normModelCreatorL.create(nu));
          QuadraticFunction quadraticModel(quadraticModelCreator.create(nu));
          L.setL_xx(quadraticModel.quadraticPart[0][0]);
          IsotropicCubicRegularization reg(normLModel,L.omega/6);
          RegularizedQuadraticFunction cubic(quadraticModel, reg);
          if( acceptanceTestResult == AcceptanceTest::LeftAdmissibleDomain ) tau[0] *= 0.5;
          else updateTangentialDampingFactor(nu, normNormal, normTangential, cubic, tau);

          // Compute Correction as a linear combination of normal step and basis vectors of tangent search space
          correction = createCorrection(nu,*normalStep,tau,tangentialBasis);
          auto normLAtCorr = sqrt(normLModel.d0(tau));
          normCAtCorr = normC(*correction);
          auto quadraticModelAtCorr = quadraticModel.d0(tau);
          // Compute Trial Iterate: first candidate: trialIterate = chart_x(dx)
          trialIterate = iterate->clone();
          chart->addPerturbation(*trialIterate,*correction,*lagrangeLinearization);
          if( !functionalN->inDomain(*trialIterate) || ( functionalT != nullptr && !functionalT->inDomain(*trialIterate) ) )
          {
            std::cout << csPre << "Iterate leaves admissible domain: rejecting step." << std::endl;
            acceptanceTestResult = AcceptanceTest::LeftAdmissibleDomain;
          }
          else
          {
            acceptanceTestResult = AcceptanceTest::Failed;
            std::unique_ptr<AbstractLinearization> lin_xplus(functionalN->getLinearization(*trialIterate));
            auto simplifiedNormalStep = computeSimplifiedNormalStep( *lin_xplus, normalStepResidual.get());
            simplifiedNormalStep->axpy( nu-1.0, *normalStep, "primal" );
            //for( size_t i = 0; i < tangentialBasis.size(); ++i ) simplifiedNormalStep->axpy( tau[i] - 1.0, *tangentialBasis[i], "primal" );

            updateConstraintD1LipschitzConstant( normC( *simplifiedNormalStep ), normCAtCorr );

            // Perform second order correction update secondOrderCorrected=chart_x(dx+ds)
            std::unique_ptr<AbstractFunctionSpaceElement> secondOrderCorrection(simplifiedNormalStep->clone());
            secondOrderCorrection->axpy(1.0,*correction,"primal");
            *secondOrderCorrected = *iterate->clone();
            chart->addPerturbation(*secondOrderCorrected,*secondOrderCorrection,*lagrangeLinearization);

            eta = updateLagrangianD2LipschitzConstant(*lagrangeLinearization, *secondOrderCorrected, normLAtCorr, quadraticModelAtCorr, cubic, nu, tau,*normalStep,*trialIterate,*simplifiedNormalStep,*correction,Lxdn_res);
          }
        }

        // Acceptance tests
        if( acceptanceTestResult != AcceptanceTest::LeftAdmissibleDomain ) acceptanceTestResult = acceptanceTest(eta,nu,tau,normCAtCorr);
        if(!regularityTest(nu,tau,reliableQuadraticModel)) return -1;

        if( reliableQuadraticModel && (acceptanceTestResult==AcceptanceTest::TangentialStepFailed) && L.omega < (1 + 0.25*(1 - p.etaMin))*L.oldOmega )
        {
          if( verbose > 0 ) std::cout << csPre << "Stagnating omegaL update. Accepting step." << std::endl;
          acceptanceTestResult = AcceptanceTest::Passed;
          if(eta < 0.1)
          {
            if( verbose > 0 ) std::cout << csPre << "Ignoring tangential step." << std::endl;
            for(size_t i=0; i<tangentialBasis.size(); ++i)
            {
              trialIterate->axpy(-tau[i],*tangentialBasis[i]);
              secondOrderCorrected->axpy(-tau[i],*tangentialBasis[i]);
            }
          }
        }

        if(acceptanceTestResult == AcceptanceTest::Passed)
        {
          normOfLastCorrection = normCAtCorr;
          normOfLastCorrection_Undamped = normOfUndampedCorrection;
          correctionfile << normOfLastCorrection << " " << normL(*normalStep) << " " << normL(*tangentialBasis[0]) << std::endl;
        }
      }

      // Error estimation
      bool refinedMesh = false;
      if( hbErrorEstimator != nullptr && tangentSpace->localConvergenceLikely()  && reliableQuadraticModel ) refinedMesh = adaptiveMeshRefinement(*lagrangeLinearization, nu,tau,*correction);

      // at this point, *trialIterate is the combined step
      // and *secondOrderCorrected is the second order corrected step
      // if Theta > 0.25, this might be worse w.r.t feasibility than the original step
      // use original step in this case
      if(normalDirection)
      {
        if(C.theta > 0.25 || !reliableQuadraticModel)
        {
          if(verbose > 1) std::cout << csPre << "Adding ordinary update" << std::endl;
        }
        else
        {
          *trialIterate = *secondOrderCorrected;
          if(verbose > 1) std::cout << csPre << "Adding second order correction" << std::endl;
        }
      }

      iterate->swap(*trialIterate);
      normOfIterate = normL(*iterate);
//      *trialIterate -= *iterate;

      /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
      /* * * * * * * * * * * * * * * * Write data  * * * * * * * * * * * * * * * * */
      /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
      if( verbose > 0)
      {
        std::cout << "\n" << csPre << "Accepted step" << std::endl;
        printNormalStep(normNormal, nu);
        printTangentialStep(normTangential, tau[0]);
      }

      if( verbose > 1 )
      {
        std::string name = std::string("iterate_") + to_string(step);
        iterate->writeToFile(name,false);
      }

      omegafile << " " << L.omega << " " << " " << C.omega << " " << std::endl;
      coefffile << nu << "  " << tau[0] << std::endl;

      if( !refinedMesh && convergenceTest(nu,tau,normL(*correction)) == Convergence::Achieved )
      {
        steps << step << std::endl;

        if( verbose > 1 )
        {
          std::cout << csPre << "overall normal step computation time: " << normalStepComputationTime << std::endl;
          std::cout << csPre << "overall tangential step computation time: " << tangentialStepComputationTime << std::endl;
        }
        return 1;
      }
    }
    return -2;
  }

  void Optimization::solve(AbstractFunctional &fN, AbstractFunctional& fT, AbstractFunctionSpaceElement &x) { solve(fN,&fT,x); }

  void Optimization::solve(AbstractFunctional& fN, AbstractFunctional* fT,AbstractFunctionSpaceElement& x)
  {
    functionalN = &fN;
    functionalT = fT;

    iterate = x.clone();
    normalStepComputationTime = tangentialStepComputationTime = 0;
    if( tangentSpace != nullptr ) tangentSpace->setEps(p.getEps());

    normOfLastCorrection = normOfLastCorrection_Undamped = -1;
    normOfIterate = 0;
    auto terminationFlag = algorithmWrapper();
    if(terminationFlag > 0 || terminationFlag == -2) x=*iterate;
  }

  void Optimization::solve(AbstractFunctional& fN, AbstractFunctional& fT, AbstractFunctionSpaceElement& x, AbstractHierarchicalErrorEstimator& hbErrorEstimator_)
  {
    hbErrorEstimator = &hbErrorEstimator_;
    solve(fN,fT,x);
  }

  void Optimization::solve(AbstractFunctional& f, AbstractFunctionSpaceElement& x, AbstractHierarchicalErrorEstimator& hbErrorEstimator_)
  {
    hbErrorEstimator = &hbErrorEstimator_;
    solve(f,nullptr,x);
  }

  bool Optimization::regularityTest(double nu, std::vector<double> const& tau, bool reliableQuadraticModel) const
  {
    bool passed = nu > p.minimalDampingFactor;
    if(reliableQuadraticModel) passed = passed && tau[0] > p.minimalDampingFactor;
    if(verbose > 0 && !passed) std::cout << csPre << "Regularity test failed!!!" << std::endl;
    return passed;
  }

  std::pair<std::unique_ptr<AbstractFunctionSpaceElement>,std::unique_ptr<AbstractFunctionSpaceElement> > Optimization::computeNormalStep(AbstractFunctionSpaceElement* normalStepResidual, AbstractFunctionSpaceElement* adjointResidual)
  {
    std::pair<std::unique_ptr<AbstractFunctionSpaceElement>,std::unique_ptr<AbstractFunctionSpaceElement> > result(iterate->clone(),iterate->clone());
    *(result.first) *= 0;
    *(result.second) *= 0;

    if(normalDirection) // constrained problem
    {
      // Compute normal step, compute update for Lagrange multiplier
      if( step > 1 && std::fabs(normOfLastCorrection_Undamped-normOfLastCorrection) < p.getEps() * normOfLastCorrection_Undamped) normalDirection->setRelativeAccuracy(std::max(std::min(p.minimalAccuracy, C.omega*normOfLastCorrection),p.desiredAccuracy));
      else normalDirection->setRelativeAccuracy(p.minimalAccuracy);
      boost::timer::cpu_timer timer;
      normalDirection->ordinaryAndAdjoint(*(result.first), *(result.second), *normalLinearization, normalStepResidual, adjointResidual);
      normalStepComputationTime += (double)timer.elapsed().wall*1e-9;

      if( verbose > 1 ) std::cout << "normal step computation time: " << boost::timer::format(timer.elapsed()) << std::endl;

      iterate->axpy(1.0,*(result.second),"dual");
    }
    else // unconstrained problem
      if(verbose > 0) std::cout << csPre << "Unconstrained Problem" << std::endl;

    return result;
  }

  std::vector<std::shared_ptr<AbstractFunctionSpaceElement> > Optimization::computeTangentialStep(LagrangeLinearization& lagrangeLinearization, AbstractFunctionSpaceElement const& normalStep, double nu, std::vector<double> const& tau)
  {
    std::vector<std::shared_ptr<AbstractFunctionSpaceElement> > tangentialBasis;
    tangentialBasis.resize(1,std::shared_ptr<AbstractFunctionSpaceElement>(iterate->clone())); *(tangentialBasis[0]) *= 0;

    if(!tangentSpace) return tangentialBasis;

    double relativeAccuracy = std::max(p.desiredAccuracy,std::min(p.minimalAccuracy, L.omega * std::fabs(normOfLastCorrection)));
    if(step > 1 && std::fabs(normOfLastCorrection_Undamped-normOfLastCorrection) < p.getEps() * normOfLastCorrection_Undamped && noDamping(nu))
    {
      if(tangentSpace) tangentSpace->setRelativeAccuracy(relativeAccuracy);
      if(verbose > 1)
      {
        std::cout << csPre << "desired contraction: " << p.getThetaAim() << std::endl;
      }
    }
    else tangentSpace->setRelativeAccuracy(p.minimalAccuracy);// * std::min(1.,normOfLastCorrection_Undamped));

    boost::timer::cpu_timer timer;
    tangentSpace->setLipschitzConstant(L.omega);
    tangentSpace->basis(tangentialBasis, lagrangeLinearization, normalStep, nu, nullptr);
    tangentialStepComputationTime += (double) timer.elapsed().wall * 1e-9;

    if( verbose > 1 ) std::cout << "tangential step computation time: " << boost::timer::format(timer.elapsed()) << std::endl;

    return tangentialBasis;
  }

  double Optimization::updateNormalStepDampingFactor(double normNormal) const
  {
    double nu = 1;
    if(normNormal > p.getEps() && std::fabs(normNormal*C.omega) > p.getEps()) nu = std::min(1.0,/*2.0*/p.getThetaNormal()/(C.omega*normNormal));
    if(noDamping(nu)) nu = 1;
    return nu;
  }

  void Optimization::updateTangentialDampingFactor(double nu, double normNormal, double normTangential, RegularizedQuadraticFunction const& cubic, std::vector<double> &tau) const
  {
    if( tau.size()==1 )
    {
      tau[0] = 1.0;

      if( normTangential < p.getSqrtEps() ) return;

      double maxTauMax = 1;
      double taumax = maxTauMax;
      if( C.omega != 0.0  )
      {
        if( ( pow(p.getThetaAim()/C.omega,2)-pow(nu*normNormal,2) ) > 0 && normTangential > 0) taumax = sqrt(pow(p.getThetaAim()/C.omega,2)/*-pow(nu*normNormal,2)*/)/normTangential;//std::min(1.,normTangential);

        if( taumax > maxTauMax && tangentSpace->localConvergenceLikely() )
        {
          if( verbose > 1 ) std::cout << csPre << "reducing taumax: " << taumax << " to " << maxTauMax << std::endl;
          taumax = maxTauMax;
        }
      }
      if( verbose > 1 ) std::cout << csPre << "taumax: " << taumax << std::endl;

      if( taumax > 0.0 )
      {
        CubicModel1dForFmin cubicmodel(cubic);
        double toleranceFmin = p.getThirdSqrtEps()*std::min(taumax,1.);
        double taumin = 0;
        tau[0] = Fmin(taumin,taumax,cubicmodel,toleranceFmin);
      }

      if( noDamping(tau) ) tau[0]=1.0;
    }
    else
    {
      if( tau.size() > 1 )
      {
        std::cout << csPre << "Multidimensional search space not implemented" << std::endl;
        exit(-1);
      }
    }

  }

  std::unique_ptr<AbstractFunctionSpaceElement> Optimization::computeSimplifiedNormalStep(AbstractLinearization const& lin_xplus, AbstractFunctionSpaceElement* normalStepResidual)
  {
    std::unique_ptr<AbstractFunctionSpaceElement> simplifiedNormalStep(iterate->initZeroVector());
    if(!normalDirection) return simplifiedNormalStep;

    normalDirection->simplified(*simplifiedNormalStep,lin_xplus,nullptr);
    return simplifiedNormalStep;
  }

  void Optimization::updateConstraintD1LipschitzConstant(double normSimplifiedNormal, double normCAtCorr)
  {
    if( verbose > 1 ) std::cout << csPre << "normCAtCorr = " << normCAtCorr << ", normSimplifiedNormal = " << normSimplifiedNormal << std::endl;
//    if( normCAtCorr > p.getEps() )
    {
      C.theta = normSimplifiedNormal/normCAtCorr; // contraction rate
      bool lock = (C.theta < 0.25 && (normCAtCorr < p.getSqrtEps()*normOfIterate || normSimplifiedNormal < p.getEps()*normOfIterate) );
      C.update(2*C.theta/normCAtCorr, lock);
    }
  }

  double Optimization::updateLagrangianD2LipschitzConstant(AbstractLinearization const& lin_x0, AbstractFunctionSpaceElement const& secondOrderCorrected, double normLAtCorr, double quadraticModelAtCorr, RegularizedQuadraticFunction const& cubic, double nu, std::vector<double> const& tau,
                                                           AbstractFunctionSpaceElement const& normalStep, AbstractFunctionSpaceElement const& trialIterate, AbstractFunctionSpaceElement const& sNormalStep, AbstractFunctionSpaceElement const& correction, double Lxdn_res)
  {
    double eta = 1;
    if( tangentSpace == nullptr ) return eta;

    std::unique_ptr<AbstractLinearization> lin_xplus(functionalN->getLinearization(trialIterate));
    auto f_x0 = lin_x0.eval();
    // compute: f(x_)-q(x)dx or alternative if high round-off is expected
    double deltaf(0.0);
    auto zero = tau;
    for(double& d : zero) d = 0;

//    if(normalDirection)
//    {
//      std::unique_ptr<AbstractFunctionSpaceElement> dc_x0_dn ( iterate->initZeroVector() ), lxxdn( iterate->initZeroVector() );
//      lin_x0.d2axpy(1.0, *lxxdn, normalStep, 0, 2, 0, 2);
//      lin_x0.d2axpy(1.0, *dc_x0_dn, normalStep, 2, 3, 0, 2);
//      double p_dc_x0_dn = iterate->applyAsDualTo(*dc_x0_dn,"dual");
//      double lxxdndn = normalStep.applyAsDualTo(*lxxdn);
      std::unique_ptr<AbstractLinearization> lin_xbar=functionalN->getLinearization(secondOrderCorrected);
      deltaf=lin_xbar->eval()-f_x0;
      double deltaf_cor = deltaf - cubic.d0(zero);
      if(std::fabs(cubic.d0(tau) - cubic.d0(zero)) > p.getSqrtEps()*normOfIterate/* && std::fabs(deltaf_cor) > p.getSqrtEps()*/) eta = deltaf_cor/( cubic.d0(tau) - cubic.d0(zero) ) ;
      else eta = 1;
      if( verbose > 1 )
      {
        std::cout << csPre << "predicted decrease: " << (cubic.d0(tau) - cubic.d0(zero)) << std::endl;
        std::cout << csPre << "actual decrease: " << deltaf_cor << std::endl;
        std::cout << csPre << " eta = " << eta << std::endl;
      }
      L.setFirstOrder( normLAtCorr, C.theta, deltaf - quadraticModelAtCorr );

//    }
//    else { assert("not implemented"); }

    if(L.highRoundOffError(true,p.getEps()*f_x0))
    {
      std::unique_ptr<AbstractFunctionSpaceElement> tmp1=iterate->clone();
      // tmp1 = chart_x(0)-chart_x(dx+ds)
      *tmp1 -= secondOrderCorrected;

      lin_xbar->evald(*tmp1);
      double p0_c_xbar=tmp1->applyAsDualTo(*iterate,"dual");  // p0 is dual component of *iterate

      double p0_c_x0(0.0);
      if(!noDamping(nu))
      {
        lin_x0.evald(*tmp1);
        p0_c_x0=tmp1->applyAsDualTo(*iterate,"dual");     // p0 is dual component of *iterate
      }

      // Lx_xplus_ds = L_x(x_+,p_0) ds
      double Lx_xplus_ds(0.0);
      lin_xplus->evald(*tmp1);
      Lx_xplus_ds=tmp1->applyAsDualTo(sNormalStep,"primal"); // delta s is primal component of *snormalStep

      // secondorderestimate [ (L_x(x_+,p0)-L_x(x0,p0)-L_xx(x0,p0)dx)dx ] + [ L_x(x_+,p0)ds ]-[ p0 c(xbar)-(1-nu)p0 c(x0) ]
      // each term in [...] is third order.
      // the summands in the first (...) are first order (close to solution), the difference is second order -> first order round-off error effects
      // close to a solution where nu = 1, the very last term vanishes -> then no other round-off error effects
      // if nu is very small, and c(x_0) approx c(xbar), then round-off error is expected (try alternative?)

      // tmp1 = L_x(x0,p0)+L_xx(x0,p0)dx
      lin_x0.evald(*tmp1);
      lin_x0.ddxpy(*tmp1,correction);

      // tmp2=L_x(x_+,p0)-L_x(x0,p0)-L_xx(x0,p0)dx
      std::unique_ptr<AbstractFunctionSpaceElement> tmp2=iterate->clone();
      lin_xplus->evald(*tmp2);
      (*tmp2)-=(*tmp1);

      L.setSecondOrder(0.5*tmp2->applyAsDualTo(correction,"primal")+0.5*Lx_xplus_ds-(p0_c_xbar-(1-nu)*(p0_c_x0-Lxdn_res)));
    }

    L.update( std::fabs( eta - 1. ) < (1.-p.etaLock) );

    if(verbose > 1)
    {
      std::cout << csPre << "f(x_0): " << f_x0 << " f(x): "  << f_x0+deltaf << "  |  Decrease: " << -deltaf << std::endl;
      std::cout << csPre << "Quadratic model q(x):" << f_x0+quadraticModelAtCorr << std::endl;
      std::cout << csPre << "q(x)-f(x_0): " << quadraticModelAtCorr << std::endl;
      std::cout << csPre << "q(x)-f(x): " << quadraticModelAtCorr-deltaf << std::endl;
    }

    return eta;
  }

  AcceptanceTest Optimization::acceptanceTest(double eta, double nu, std::vector<double> const& tau, double normOfCorrection) const
  {
    if( nu > 0.1 && normOfCorrection < p.getEps() ) return AcceptanceTest::Passed;
    if( noDamping(nu) && noDamping(tau) && (normOfCorrection) < p.getSqrtEps() ) return AcceptanceTest::Passed;

    if( eta < p.etaMin )
    {
      if( verbose > 0 ) std::cout << csPre << "Rejecting due to small eta: " << eta << " < " << p.etaMin << std::endl;
      return AcceptanceTest::TangentialStepFailed;
    }

    if(C.theta > p.getThetaMax())
    {
      if(verbose > 0) std::cout << csPre << "Insufficient contraction:" << C.theta << " > " << p.getThetaMax() << std::endl;
      return AcceptanceTest::NormalStepFailed;
    }

    return AcceptanceTest::Passed;
  }

  bool Optimization::noDamping(double d) const
  {
    return std::fabs(d-1.) < dampingFactorTolerance;
  }

  bool Optimization::noDamping(std::vector<double> const& tau) const
  {
    for(double d : tau) if(!noDamping(d)) return false;
    return true;
  }

  void Optimization::terminationMessage(int flag)
  {
    switch(flag)
    {
      case 1  : std::cout << csPre << "Desired accuracy reached!" << std::endl; break;
      case -1 : std::cout << csPre << "Regularity test failed!" << std::endl;; break;
      case -2 : std::cout << csPre << "Maximum number of iterations reached!" << std::endl; break;
      default : Algorithm::terminationMessage(flag);
    }
  }

  void Optimization::printNormalStep(double normNormal, double nu) const
  {
    std::cout << csPre << "Normal step: " << normNormal << std::endl;
    std::cout << csPre << "Damping factor: " << nu << std::endl << std::endl;
  }

  void Optimization::printTangentialStep(double normTangential, double tau) const
  {
    std::cout << csPre << "Tangential step: " << normTangential << std::endl;
    std::cout << csPre << "Damping factor: " << tau << std::endl << std::endl;

  }

  std::unique_ptr<AbstractFunctionSpaceElement> Optimization::createCorrection(double nu, const AbstractFunctionSpaceElement &normalStep, const std::vector<double> &tau, const std::vector<std::shared_ptr<AbstractFunctionSpaceElement> > &tangentialBasis) const
  {
    std::unique_ptr<AbstractFunctionSpaceElement> correction(normalStep.clone());
    *correction *= 0.0;
    correction->axpy(nu,normalStep,"primal");
    for(size_t i=0; i<tangentialBasis.size();++i) correction->axpy(tau[i],*tangentialBasis[i],"primal");
    return correction;
  }

  void Optimization::addCorrection(AbstractFunctionSpaceElement &v, double nu, const AbstractFunctionSpaceElement &normalStep, const std::vector<double> &tau, const std::vector<std::shared_ptr<AbstractFunctionSpaceElement> > &tangentialBasis) const
  {
    v.axpy(nu,normalStep,"primal");
    for(size_t i=0; i<tangentialBasis.size(); ++i) v.axpy(tau[i],*tangentialBasis[i],"primal");
  }
}  // namespace Kaskade


#pragma once
#include <Eigen/Core>
#include <Eigen/SVD>
#include <vector>
#include <unordered_set>
#include <numeric>
#include <iterator>
#include <queue>
#include <iostream>

template <int _Dims>
class Optimize {
public:
    typedef Eigen::Array<float,_Dims,1> vars_type;
    typedef std::function<float(vars_type)> funk_type;

    Optimize(funk_type funk, vars_type min_bounds = -vars_type::Ones(), vars_type max_bounds = vars_type::Ones());
    ~Optimize() {};

    void setNumObservations(int num_observations) {num_observations_ = num_observations;};
    void setMaxDepth(int max_depth) {max_depth_ = max_depth;};
    void setThreshold(float threshold) {threshold_ = threshold;};
    void setMaxIterations(int max_iterations) {max_iterations_=max_iterations;};

    Optimize<_Dims>::vars_type run();

private:

    static int powint(int x, int p)//https://stackoverflow.com/a/1505791
    {
        if (p == 0) return 1;
        if (p == 1) return x;
        int tmp = powint(x, p/2);
        if (p%2 == 0) return tmp * tmp;
        else return x * tmp * tmp;
    }

    struct Split {
        int dimension;
        int left_index;
        int right_index;
        bool left_right; //Keep left or right side of split
        Split(int dim, int left_idx, int right_idx, bool lr) : dimension(dim), left_index(left_idx), right_index(right_idx), left_right(lr){};
    };

    struct Partition {
        std::vector<Split> splits;
        std::vector<int> elements;

        bool getClass(const Eigen::Matrix<float,-1,_Dims+1> & data, float threshold){
            int low = 0;
            for (auto ind : elements) {
                if (data(ind,data.cols()-1) < threshold)
                    low++;
            }
            return ((float) low / (float) elements.size())<=0.5;
        };

        void getBounds(const Eigen::Matrix<float,-1,_Dims+1> & data, vars_type & min, vars_type & max){
            for (auto split : splits){
                float x_left = data(split.left_index,split.dimension);
                float x_right = data(split.right_index,split.dimension);
                float x_mid = (x_left+x_right)/2;
                if (split.left_right) {
                    if (min(split.dimension) < x_mid)
                        min(split.dimension) = x_mid;
                } else {
                    if (max(split.dimension) > x_mid)
                        max(split.dimension) = x_mid;
                }
            }
        };
    };

    funk_type funk_;
    vars_type min_bounds_;
    vars_type max_bounds_;

    int num_observations_ = 125;
    float threshold_ = 3;
    int max_depth_ = 10;
    int max_iterations_ = 10;
    float tolerance_ = 1e-6;

    Eigen::Matrix<float,-1,_Dims+1> data_;
    Eigen::Matrix<float,_Dims,_Dims> householder_ = Eigen::Matrix<float,_Dims,_Dims>::Identity();

    //Sample randomly from bounds
    Eigen::Matrix<float,-1,_Dims+1> randSample(int n);
    Eigen::Matrix<float,-1,_Dims+1> randSampleLow(int n, const std::vector<vars_type> & mins, const std::vector<vars_type> & maxs);

    //Generates observations from random samples
    void generateData() {
        data_ = randSample(num_observations_);
    };
    void generateDataLow(const std::vector<vars_type> & mins, const std::vector<vars_type> & maxs) {
        data_ = randSampleLow(num_observations_, mins, maxs);
    };

    //Sorts along each dimension and stores permutation
    void sortData();
    std::array<std::vector<int>,_Dims> permutations_;

    float calculateGini(const std::vector<bool> & classified, int start, int end);
    int findSplit(const Partition & main_part, Partition & left_part, Partition & right_part);

    bool withinUnion(const vars_type & x, const std::vector<vars_type> & mins, const std::vector<vars_type> & maxs);

};

// Implementation
template <int _Dims>
Optimize<_Dims>::Optimize(Optimize::funk_type funk, Optimize::vars_type min_bounds, Optimize::vars_type max_bounds) :
    funk_(funk),
    min_bounds_(min_bounds),
    max_bounds_(max_bounds)
{
}

template <int _Dims>
typename Optimize<_Dims>::vars_type Optimize<_Dims>::run()
{
    //place box
    generateData();

    //partition box
    std::vector<int> v(num_observations_);
    std::iota(v.begin(),v.end(),0);

    vars_type current_min = vars_type::Zero();
    float current_min_f = std::numeric_limits<float>::max();

    for (int k = 0; k < max_iterations_; k++){
        //PCA to find dominant direction of hyper-ellipse
        Eigen::JacobiSVD<Eigen::Matrix<float,-1,_Dims>> svd(data_.block(0,0,num_observations_,_Dims), Eigen::ComputeThinU | Eigen::ComputeThinV);
        svd.computeV();
        vars_type dominant = svd.matrixV().template block<1,_Dims>(0,0).transpose();
        //Get Householder transformation matrix
        vars_type e1 = vars_type::Zero();
        e1(0) = 1;
        vars_type u = e1 - dominant;
        u.matrix().normalize();
        householder_ = Eigen::Matrix<float,_Dims,_Dims>::Identity() - 2*u.matrix() * u.matrix().transpose();

        //Apply Householder transformation
        bool no_transform = false;
        if (!std::isnan(householder_.norm()) && !std::isnan(data_.block(0,0,num_observations_,_Dims).norm())){
            data_.block(0,0,num_observations_,_Dims) = data_.block(0,0,num_observations_,_Dims).matrix() * householder_.transpose();
        } else {
            no_transform = true;
        }
        sortData();

        std::queue<Partition> partition_queue;
        std::vector<Partition> terminal;

        Partition initial;
        initial.elements = v;
        partition_queue.push(initial);

        int depth = 0;
        while (depth < max_depth_){
            std::queue<Partition> temp_queue;
            while (!partition_queue.empty()){
                auto part = partition_queue.front();
                partition_queue.pop();

                //Check stopping conditions
                if (part.elements.size() <= 1){
                    //terminal
                    terminal.push_back(part);
                    continue;
                }

                Partition left_part, right_part;
                int split_ret = this->findSplit(part, left_part, right_part);

                if (!split_ret) {
                    terminal.push_back(part);
                } else if (depth == max_depth_-1){
                    terminal.push_back(left_part);
                    terminal.push_back(right_part);
                } else {
                    temp_queue.push(left_part);
                    temp_queue.push(right_part);
                }
            }
            partition_queue = temp_queue;
            depth++;
        }

        std::vector<vars_type> minimums;
        std::vector<vars_type> maximums;
        //Find bounding box in new coordinates
        //We have 2^_Dims corners
        vars_type new_min = householder_.transpose() * min_bounds_.matrix();
        vars_type new_max = householder_.transpose() * max_bounds_.matrix();
        for(int i = 0; i < powint(2,_Dims); i++) {
            vars_type corner;
            for (int j = 0; j < _Dims; j++){
                if (i & powint(2,j))
                    corner(j) = max_bounds_(j);
                else
                    corner(j) = min_bounds_(j);
            }
            vars_type corner_transformed = householder_.transpose() * corner.matrix();
            for (int d = 0; d < _Dims; d++){
                if (corner_transformed(d) < new_min(d))
                    new_min(d) = corner_transformed(d);
                if (corner_transformed(d) > new_max(d))
                    new_max(d) = corner_transformed(d);
            }
        }
        for (auto t : terminal) {
            if (!t.getClass(data_,threshold_)){ //classified as low
                vars_type min = new_min;
                vars_type max = new_max;
                t.getBounds(data_, min, max);
                minimums.push_back(min);
                maximums.push_back(max);
            }
        }

        //Randomly sample in union of low partitions
        generateDataLow(minimums,maximums);

        //Apply inverse Householder transformation
        if (!no_transform){
            data_.block(0,0,num_observations_,_Dims) = data_.block(0,0,num_observations_,_Dims).matrix() * householder_;
        }

        for (int i = 0; i < num_observations_; i++){
            //Calculate f for new batch of points
            data_(i,_Dims) = funk_(data_.template block<1,_Dims>(i,0));
            //Check if f is new minimum
            if (data_(i,_Dims) < current_min_f){
                current_min = data_.template block<1,_Dims>(i,0);
                current_min_f = data_(i,_Dims);
            }
        }
    }
    return current_min;
}

template <int _Dims>
Eigen::Matrix<float,-1,_Dims+1> Optimize<_Dims>::randSample(int n)
{
    Eigen::Matrix<float,-1,_Dims+1> samples;
    samples.resize(n,_Dims+1);
    for (int i = 0; i < n; i++){
        vars_type x = vars_type::Random();//between -1 and 1
        x += 1;
        x /= 2;
        x *= max_bounds_-min_bounds_;
        x += min_bounds_;
        samples.template block<1,_Dims>(i,0) = x;
        samples(i,_Dims) = funk_(x);
    }
    return samples;
}

template <int _Dims>
Eigen::Matrix<float,-1,_Dims+1> Optimize<_Dims>::randSampleLow(int n, const std::vector<vars_type> & mins, const std::vector<vars_type> & maxs)
{
    Eigen::Matrix<float,-1,_Dims+1> samples;
    samples.resize(n,_Dims+1);

    //Create a new bounding box on which to sample
    // vars_type new_min_bounds = mins[0];
    // vars_type new_max_bounds = maxs[0];
    vars_type new_min_bounds = mins[0];
    vars_type new_max_bounds = maxs[0];

    for (int i = 1; i < mins.size(); i++){
        for (int d = 0; d < _Dims; d++){
            if (mins[i](d) < new_min_bounds(d))
                new_min_bounds(d) = mins[i](d);
            if (maxs[i](d) > new_max_bounds(d))
                new_max_bounds(d) = maxs[i](d);
        }
    }

    for (int i = 0; i < n; i++){
        vars_type x = vars_type::Random();//between -1 and 1
        x += 1;
        x /= 2;
        x *= new_max_bounds-new_min_bounds;
        x += new_min_bounds;
        while (!withinUnion(x, mins, maxs)){
            x = vars_type::Random();//between -1 and 1
            x += 1;
            x /= 2;
            x *= new_max_bounds-new_min_bounds;
            x += new_min_bounds;
        }
        samples.template block<1,_Dims>(i,0)= x;
    }
    return samples;
}

template <int _Dims>
float Optimize<_Dims>::calculateGini(const std::vector<bool> & classified, int start, int end)
{
    int num_low_int=0;
    for (int i = start; i < end; i++){
        if (!classified[i])
            num_low_int++;
    }
    float num_low = num_low_int;
    float num_total = end-start;

    return 2*num_low/num_total * (1 - num_low/num_total);
}

template <int _Dims>
int Optimize<_Dims>::findSplit(const Optimize<_Dims>::Partition & main_part, Optimize<_Dims>::Partition & left_part, Optimize<_Dims>::Partition & right_part)
{
    auto indices = main_part.elements;

    std::vector<int> v(data_.rows());
    std::iota(v.begin(),v.end(),0);

    //classify indices
    std::unordered_set<int> low;
    std::unordered_set<int> high;

    for (int i : indices){
        if (data_(i,_Dims) < threshold_)
            low.insert(i);
        else
            high.insert(i);
    }

    //partition
    float delta = 0;
    for (int dim = 0; dim < _Dims; dim++){
        auto p = permutations_[dim];
        //keep only indices in current partition
        for (auto it = p.begin(); it != p.end(); ){
            if (low.find(*it)==low.end() && high.find(*it)==high.end())
                it = p.erase(it);
            else
                ++it;
        }
        //sort indices in non-decreasing order along current dimension
        std::vector<int> sorted_inds(indices.size());
        std::transform(p.begin(), p.end(), sorted_inds.begin(), [&](int i){ return v[i]; });
         //get class at each index
        std::vector<bool> classified;
        for (int ind : sorted_inds){
            if (low.find(ind) == low.end())
                classified.push_back(true); //high
            else
                classified.push_back(false); //low
        }
        //find best split
        for (int i = 0; i < classified.size() - 1; i++){
            float gini_left = calculateGini(classified, 0, i+1);
            float gini_right = calculateGini(classified, i+1, classified.size());
            float gini_all = calculateGini(classified, 0, classified.size());

            float size_left = i+1;
            float size_right = classified.size() - i - 1;
            float size_all = classified.size();

            float d = gini_all - gini_left*size_left/size_all - gini_right*size_right/size_all;

            if (d > delta){
                delta = d;
                left_part = main_part;
                right_part = main_part;

                left_part.elements =std::vector<int>(sorted_inds.begin(),std::next(sorted_inds.begin(),i+1));
                right_part.elements = std::vector<int>(std::next(sorted_inds.begin(),i+1),sorted_inds.end());

                left_part.splits.push_back(Split(dim,sorted_inds[i],sorted_inds[i+1],false));
                right_part.splits.push_back(Split(dim,sorted_inds[i],sorted_inds[i+1],true));
            }
        }
    }
    //Return 0 if stopping condition has been met
    if (delta < tolerance_){
        return 0;
    }
    return 1;
}

template <int _Dims>
void Optimize<_Dims>::sortData()
{
    //Each vector in permutations_ should contain the sorted indices along a dimension
    for (int dim = 0; dim < _Dims; dim++){
        std::vector<float> xi(data_.rows());
        for (int i = 0; i < data_.rows(); i++)
            xi[i] = data_(i,dim);
        //sort in non-decreasing order
        std::vector<int> p(xi.size());
        std::iota(p.begin(), p.end(), 0);
        std::sort(p.begin(), p.end(), [&](std::size_t i, std::size_t j){ return xi[i] < xi[j]; });
        //store permutation
        permutations_[dim] = p;
    }
}

template <int _Dims>
bool Optimize<_Dims>::withinUnion(const vars_type & x, const std::vector<vars_type> & mins, const std::vector<vars_type> & maxs)
{
    for (int i = 0; i < mins.size(); i++){
        if ((x-mins[i] < 0).count() == 0 && (maxs[i]-x < 0).count() == 0){
            return true;
        }
    }
    return false;
}


#include "modules/neuralNetwork/layer/linear/linear.hpp"
#include "modules/neuralNetwork/neuralNetwork.hpp"

#ifdef _KORALI_USE_CUDNN
  #include "auxiliar/cudaUtils.hpp"
#endif

#ifdef _KORALI_USE_ONEDNN
  #include "auxiliar/dnnUtils.hpp"
using namespace dnnl;
#endif

#include <Eigen/Dense>
using namespace Eigen;

namespace korali
{
namespace neuralNetwork
{
namespace layer
{
;

void Linear::initialize()
{
  // Checking Layer size
  if (_outputChannels == 0) KORALI_LOG_ERROR("Node count for layer (%lu) should be larger than zero.\n", _index);

  // Checking position
  if (_index == 0) KORALI_LOG_ERROR("Feed Forward layers cannot be the starting layer of the NN\n");
  if (_index == _nn->_layers.size() - 1) KORALI_LOG_ERROR("Feed Forward layers cannot be the last layer of the NN\n");
}

std::vector<float> Linear::generateInitialHyperparameters()
{
  std::vector<float> hyperparameters;

  // If this is not the initial layer, calculate hyperparameters for weight and bias operation
  if (_prevLayer != nullptr)
  {
    // Setting value for this layer's xavier constant
    float xavierConstant = std::sqrt(6.0f) / std::sqrt(_outputChannels + _prevLayer->_outputChannels);

    // Adding layer's weights hyperparameter values
    for (size_t i = 0; i < _outputChannels; i++)
      for (size_t j = 0; j < _prevLayer->_outputChannels; j++)
        hyperparameters.push_back(_weightScaling * xavierConstant * _nn->_uniformGenerator->getRandomNumber());

    // Adding layer's bias hyperparameter values
    for (size_t i = 0; i < _outputChannels; i++)
      hyperparameters.push_back(0.0f);
  }

  return hyperparameters;
}

void Linear::createHyperparameterMemory()
{
  // Checking Layer sizes
  ssize_t OC = _outputChannels;
  ssize_t IC = _prevLayer->_outputChannels;

  // Setting hyperparameter count
  _hyperparameterCount = IC * OC + OC;

  if (_nn->_engine == "Korali")
  {
    _weightValues = (float *)malloc(IC * OC * sizeof(float));
    _biasValues = (float *)malloc(OC * sizeof(float));
  }

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    memory::dims weightDims = {OC, IC};
    auto weightMemDesc = memory::desc(weightDims, memory::data_type::f32, memory::format_tag::ab);
    _weightsMem = memory(weightMemDesc, _nn->_dnnlEngine);

    auto biasMemDesc = memory::desc({OC}, memory::data_type::f32, memory::format_tag::a);
    _biasMem = memory(biasMemDesc, _nn->_dnnlEngine);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    cudnnErrCheck(cudnnCreateFilterDescriptor(&_weightsFilterDesc));
    cudnnErrCheck(cudnnSetFilter4dDescriptor(_weightsFilterDesc, CUDNN_DATA_FLOAT, CUDNN_TENSOR_NCHW, OC, IC, 1, 1));
    cudaErrCheck(cudaMalloc((void **)&_weightsFilter, IC * OC * sizeof(float)));

    cudnnErrCheck(cudnnCreateTensorDescriptor(&_biasTensorDesc));
    cudnnErrCheck(cudnnSetTensor4dDescriptor(_biasTensorDesc, CUDNN_TENSOR_NCHW, CUDNN_DATA_FLOAT, 1, OC, 1, 1));
    cudaErrCheck(cudaMalloc((void **)&_biasTensor, OC * sizeof(float)));
  }
#endif
}

void Linear::copyHyperparameterPointers(Layer *dstLayer)
{
  Linear *dstPtr = dynamic_cast<Linear *>(dstLayer);
  dstPtr->_hyperparameterCount = _hyperparameterCount;

  if (_nn->_engine == "Korali")
  {
    dstPtr->_weightValues = _weightValues;
    dstPtr->_biasValues = _biasValues;
  }

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    dstPtr->_weightsMem = _weightsMem;
    dstPtr->_biasMem = _biasMem;
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    dstPtr->_weightsFilterDesc = _weightsFilterDesc;
    dstPtr->_weightsFilter = _weightsFilter;
    dstPtr->_biasTensorDesc = _biasTensorDesc;
    dstPtr->_biasTensor = _biasTensor;
  }
#endif
}

void Linear::createForwardPipeline()
{
  // Calling base layer function
  Layer::createForwardPipeline();

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    // We create the inner product (Wx + b) operation
    auto inner_product_d = inner_product_forward::desc(_propKind, _prevLayer->_outputMem[0].get_desc(), _weightsMem.get_desc(), _biasMem.get_desc(), _outputMem[0].get_desc());

    // Create inner product primitive descriptor.
    dnnl::primitive_attr forwardPrimitiveAttributes;
    _forwardInnerProductPrimitiveDesc = inner_product_forward::primitive_desc(inner_product_d, forwardPrimitiveAttributes, _nn->_dnnlEngine);

    // Create the weights+bias primitive.
    _forwardInnerProductPrimitive = inner_product_forward(_forwardInnerProductPrimitiveDesc);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    // Creating convolution operator
    cudnnErrCheck(cudnnCreateConvolutionDescriptor(&_convolutionDesc));
    cudnnErrCheck(cudnnSetConvolution2dDescriptor(_convolutionDesc, 0, 0, 1, 1, 1, 1, CUDNN_CONVOLUTION, CUDNN_DATA_FLOAT));
    cudnnErrCheck(cudnnGetConvolutionForwardWorkspaceSize(_nn->_cuDNNHandle, _prevLayer->_outputTensorDesc, _weightsFilterDesc, _convolutionDesc, _outputTensorDesc, CUDNN_CONVOLUTION_FWD_ALGO_IMPLICIT_GEMM, &_convolutionWorkspaceSize));

    _convolutionWorkspace.resize(_nn->_timestepCount);
    for (size_t t = 0; t < _nn->_timestepCount; t++)
      cudaErrCheck(cudaMalloc((void **)&_convolutionWorkspace[t], _convolutionWorkspaceSize * sizeof(float)));
  }
#endif
}

void Linear::createBackwardPipeline()
{
  /*********************************************************************************
   *  Initializing memory objects and primitives for BACKWARD propagation
   *********************************************************************************/

  // Checking Layer sizes
  ssize_t OC = _outputChannels;
  ssize_t IC = _prevLayer->_outputChannels;

  // Calling base layer function
  Layer::createBackwardPipeline();

  if (_nn->_engine == "Korali")
  {
    _weightGradient = (float *)malloc(IC * OC * sizeof(float));
    _biasGradient = (float *)malloc(OC * sizeof(float));
  }

// Creating backward propagation primitives
#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    _weightsGradientMem = memory(_weightsMem.get_desc(), _nn->_dnnlEngine);
    _biasGradientMem = memory(_biasMem.get_desc(), _nn->_dnnlEngine);

    auto backwardDataDesc = inner_product_backward_data::desc(
      _prevLayer->_outputGradientMem[0].get_desc(),
      _weightsMem.get_desc(),
      _outputGradientMem[0].get_desc());

    // Create the primitive.
    auto backwardDataPrimitiveDesc = inner_product_backward_data::primitive_desc(backwardDataDesc, _nn->_dnnlEngine, _forwardInnerProductPrimitiveDesc);
    _backwardDataPrimitive = inner_product_backward_data(backwardDataPrimitiveDesc);

    auto backwardWeightsDesc = inner_product_backward_weights::desc(
      _prevLayer->_outputMem[0].get_desc(),
      _weightsMem.get_desc(),
      _biasMem.get_desc(),
      _outputGradientMem[0].get_desc());

    // Create the primitive.
    auto backwardWeightsPrimitiveDesc = inner_product_backward_weights::primitive_desc(backwardWeightsDesc, _nn->_dnnlEngine, _forwardInnerProductPrimitiveDesc);
    _backwardWeightsPrimitive = inner_product_backward_weights(backwardWeightsPrimitiveDesc);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    cudaErrCheck(cudaMalloc((void **)&_weightsGradientFilter, IC * OC * sizeof(float)));
    cudaErrCheck(cudaMalloc((void **)&_biasGradientTensor, OC * sizeof(float)));
  }
#endif
}

void Linear::forwardData(const size_t t)
{
  size_t N = _batchSize;
  size_t IC = _prevLayer->_outputChannels;
  size_t OC = _outputChannels;

  if (_nn->_engine == "Korali")
  {
    // Performing Wx computation
    Map<MatrixXf> matA(_weightValues, IC, OC);
    Map<MatrixXf> matB(_prevLayer->_outputValues, IC, N);
    Map<MatrixXf> matC(_outputValues, OC, N);

    matC = matA.transpose() * matB;

    // Adding Bias
    for (size_t i = 0; i < N; i++)
      for (size_t j = 0; j < OC; j++)
        _outputValues[i * OC + j] += _biasValues[j];
  }

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    // Arguments to the inner product operation
    std::unordered_map<int, dnnl::memory> forwardInnerProductArgs;
    forwardInnerProductArgs[DNNL_ARG_SRC] = _prevLayer->_outputMem[t];
    forwardInnerProductArgs[DNNL_ARG_WEIGHTS] = _weightsMem;
    forwardInnerProductArgs[DNNL_ARG_BIAS] = _biasMem;
    forwardInnerProductArgs[DNNL_ARG_DST] = _outputMem[t];

    _forwardInnerProductPrimitive.execute(_nn->_dnnlStream, forwardInnerProductArgs);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    float alpha1 = 1.0f;
    float alpha2 = 0.0f;
    cudnnErrCheck(cudnnConvolutionForward(
      _nn->_cuDNNHandle,
      &alpha1,
      _prevLayer->_outputTensorDesc,
      _prevLayer->_outputTensor[t],
      _weightsFilterDesc,
      _weightsFilter,
      _convolutionDesc,
      CUDNN_CONVOLUTION_FWD_ALGO_IMPLICIT_GEMM,
      _convolutionWorkspace[t],
      _convolutionWorkspaceSize,
      &alpha2,
      _outputTensorDesc,
      _outputTensor[t]));

    float alpha = 1.0f;
    float beta = 1.0f;
    cudnnAddTensor(_nn->_cuDNNHandle, &alpha, _biasTensorDesc, _biasTensor, &beta, _outputTensorDesc, _outputTensor[t]);
  }
#endif
}

void Linear::backwardData(const size_t t)
{
  int N = _batchSize;
  int IC = _prevLayer->_outputChannels;
  int OC = _outputChannels;

  if (_nn->_mode == "Inference")
    KORALI_LOG_ERROR("Requesting Layer backward data propagation but NN was configured for inference only.\n");

  if (_nn->_engine == "Korali")
  {
    // Backward propagating Wx+b operation
    Map<MatrixXf> matA(_weightValues, IC, OC);
    Map<MatrixXf> matB(_outputGradient, OC, N);
    Map<MatrixXf> matC(_prevLayer->_outputGradient, IC, N);

    matC = matA * matB;
  }

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    _backwardDataArgs[DNNL_ARG_DIFF_DST] = _outputGradientMem[t];             // Input
    _backwardDataArgs[DNNL_ARG_WEIGHTS] = _weightsMem;                        // Input
    _backwardDataArgs[DNNL_ARG_DIFF_SRC] = _prevLayer->_outputGradientMem[t]; // Output

    _backwardDataPrimitive.execute(_nn->_dnnlStream, _backwardDataArgs);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    float alpha = 1.0f;
    float beta = 0.0f;
    cudnnErrCheck(cudnnConvolutionBackwardData(
      _nn->_cuDNNHandle,
      &alpha,
      _weightsFilterDesc,
      _weightsFilter,
      _outputTensorDesc,
      _outputGradientTensor[t],
      _convolutionDesc,
      CUDNN_CONVOLUTION_BWD_DATA_ALGO_0,
      _convolutionWorkspace[t],
      _convolutionWorkspaceSize,
      &beta,
      _prevLayer->_outputTensorDesc,
      _prevLayer->_outputGradientTensor[t]));
  }
#endif
}

void Linear::backwardHyperparameters(size_t t)
{
  const size_t N = _batchSize;
  const size_t IC = _prevLayer->_outputChannels;
  const size_t OC = _outputChannels;

  if (_nn->_mode == "Inference")
    KORALI_LOG_ERROR("Requesting Layer hyperparameter gradient propagation but NN was configured for inference only.\n");

  if (_nn->_engine == "Korali")
  {
    // Performing Weight gradient calculation
    Map<MatrixXf> matA(_prevLayer->_outputValues, IC, N);
    Map<MatrixXf> matB(_outputGradient, OC, N);
    Map<MatrixXf> matC(_weightGradient, IC, OC);

    matC = matA * matB.transpose();

    // Setting the bias values to all minibatch inputs
    for (size_t j = 0; j < OC; j++) _biasGradient[j] = _outputGradient[0 * OC + j];
    for (size_t i = 1; i < N; i++)
      for (size_t j = 0; j < OC; j++) _biasGradient[j] += _outputGradient[i * OC + j];
  }

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    // Arguments for the backward propagation of the gradient wrt Weights and Biases
    std::unordered_map<int, dnnl::memory> backwardWeightsArgs;
    backwardWeightsArgs[DNNL_ARG_SRC] = _prevLayer->_outputMem[t];    // Input
    backwardWeightsArgs[DNNL_ARG_DIFF_DST] = _outputGradientMem[t];   // Input
    backwardWeightsArgs[DNNL_ARG_DIFF_WEIGHTS] = _weightsGradientMem; // Output
    backwardWeightsArgs[DNNL_ARG_DIFF_BIAS] = _biasGradientMem;       // Output

    _backwardWeightsPrimitive.execute(_nn->_dnnlStream, backwardWeightsArgs);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    float alpha = 1.0f;
    float beta = 0.0f;

    cudnnErrCheck(cudnnConvolutionBackwardBias(
      _nn->_cuDNNHandle,
      &alpha,
      _outputTensorDesc,
      _outputGradientTensor[t],
      &beta,
      _biasTensorDesc,
      _biasGradientTensor));

    cudnnErrCheck(cudnnConvolutionBackwardFilter(
      _nn->_cuDNNHandle,
      &alpha,
      _prevLayer->_outputTensorDesc,
      _prevLayer->_outputTensor[t],
      _outputTensorDesc,
      _outputGradientTensor[t],
      _convolutionDesc,
      CUDNN_CONVOLUTION_BWD_FILTER_ALGO_0,
      _convolutionWorkspace[t],
      _convolutionWorkspaceSize,
      &beta,
      _weightsFilterDesc,
      _weightsGradientFilter));
  }
#endif
}

void Linear::setHyperparameters(float *hyperparameters)
{
  size_t IC = _prevLayer->_outputChannels;
  size_t OC = _outputChannels;

  if (_nn->_engine == "Korali")
  {
    memcpy(_weightValues, &hyperparameters[0], IC * OC * sizeof(float));
    memcpy(_biasValues, &hyperparameters[IC * OC], OC * sizeof(float));
  }

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    write_to_dnnl_memory(&hyperparameters[0], _weightsMem);
    write_to_dnnl_memory(&hyperparameters[IC * OC], _biasMem);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    cudaErrCheck(cudaMemcpy(_weightsFilter, &hyperparameters[0], IC * OC * sizeof(float), cudaMemcpyHostToDevice));
    cudaErrCheck(cudaMemcpy(_biasTensor, &hyperparameters[IC * OC], OC * sizeof(float), cudaMemcpyHostToDevice));
  }
#endif
}

void Linear::getHyperparameters(float *hyperparameters)
{
  size_t IC = _prevLayer->_outputChannels;
  size_t OC = _outputChannels;

  if (_nn->_engine == "Korali")
  {
    memcpy(&hyperparameters[0], _weightValues, IC * OC * sizeof(float));
    memcpy(&hyperparameters[IC * OC], _biasValues, OC * sizeof(float));
  }

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    read_from_dnnl_memory(&hyperparameters[0], _weightsMem);
    read_from_dnnl_memory(&hyperparameters[IC * OC], _biasMem);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    cudaErrCheck(cudaMemcpy(&hyperparameters[0], _weightsFilter, IC * OC * sizeof(float), cudaMemcpyDeviceToHost));
    cudaErrCheck(cudaMemcpy(&hyperparameters[IC * OC], _biasTensor, OC * sizeof(float), cudaMemcpyDeviceToHost));
  }
#endif
}

void Linear::getHyperparameterGradients(float *gradient)
{
  size_t IC = _prevLayer->_outputChannels;
  size_t OC = _outputChannels;

  if (_nn->_engine == "Korali")
  {
    memcpy(&gradient[0], _weightGradient, IC * OC * sizeof(float));
    memcpy(&gradient[IC * OC], _biasGradient, OC * sizeof(float));
  }

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    read_from_dnnl_memory(&gradient[0], _weightsGradientMem);
    read_from_dnnl_memory(&gradient[IC * OC], _biasGradientMem);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    cudaErrCheck(cudaMemcpy(&gradient[0], _weightsGradientFilter, IC * OC * sizeof(float), cudaMemcpyDeviceToHost));
    cudaErrCheck(cudaMemcpy(&gradient[IC * OC], _biasGradientTensor, OC * sizeof(float), cudaMemcpyDeviceToHost));
  }
#endif
}

void Linear::setConfiguration(knlohmann::json& js)
{
 if (isDefined(js, "Results"))  eraseValue(js, "Results");

 Layer::setConfiguration(js);
 _type = "layer/linear";
 if(isDefined(js, "Type")) eraseValue(js, "Type");
 if(isEmpty(js) == false) KORALI_LOG_ERROR(" + Unrecognized settings for Korali module: linear: \n%s\n", js.dump(2).c_str());
}

void Linear::getConfiguration(knlohmann::json& js)
{

 js["Type"] = _type;
 Layer::getConfiguration(js);
}

void Linear::applyModuleDefaults(knlohmann::json& js)
{

 std::string defaultString = "{}";
 knlohmann::json defaultJs = knlohmann::json::parse(defaultString);
 mergeJson(js, defaultJs);
 Layer::applyModuleDefaults(js);
}

void Linear::applyVariableDefaults()
{

 Layer::applyVariableDefaults();
}

;

} //layer
} //neuralNetwork
} //korali
;


#include "aabb.h"
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace Eigen;
using namespace std;

aabb::aabb(const vec& low_bd, const vec& up_bd){
  low_bd_ = low_bd;
  up_bd_ = up_bd;
}
aabb::aabb(const aabb& other){
  low_bd_ = other.low_bd_;
  up_bd_ = other.up_bd_;
}
void aabb::merge(const aabb& other){
  for(size_t i = 0; i < 3; ++i){
    if(low_bd_(i) > other.low_bd_(i))
      low_bd_(i) = other.low_bd_(i);
    if(up_bd_(i) < other.up_bd_(i))
      up_bd_(i) = other.up_bd_(i);
  }

  {//check
    for(size_t i = 0; i < 3; ++i){
      assert(low_bd_(i) <= other.low_bd_(i));
      assert(up_bd_(i) >= other.up_bd_(i));
    }
  }
}

tri_aabb::tri_aabb(const size_t& id, const vec& p1, const vec& p2, const vec& p3):id_(id){
  p_.col(0) = p1;
  p_.col(1) = p2;
  p_.col(2) = p3;
  id_ = id;
  low_bd_ = p1;
  up_bd_ = p1;
  for(size_t i = 0; i < 3; ++i){
    if(low_bd_(i) > p2(i))
      low_bd_(i) = p2(i);
    else if (up_bd_(i) < p2(i))
      up_bd_(i) = p2(i);

    if(low_bd_(i) > p3(i))
      low_bd_(i) = p3(i);
    else if (up_bd_(i) < p3(i))
      up_bd_(i) = p3(i);
  }

  center = (p1 + p2 + p3) / 3.0;

  vec one_edge = p2 - p1, other_edge = p3 - p1;
  normal_ = one_edge.cross(other_edge);
  // normal_(0) = (p2(1) - p1(1)) * (p3(2) - p1(2)) - (p2(2) - p1(2)) * (p3(1) - p1(1));
  // normal_(1) = (p2(0) - p1(0)) * (p3(2) - p1(2)) - (p2(2) - p1(2)) * (p3(0) - p1(0));
  // normal_(2) = (p2(0) - p1(0)) * (p3(1) - p1(1)) - (p2(1) - p1(2)) * (p3(0) - p1(0));

  double norm  = normal_.norm();
  if(norm > 1e-6)
    normal_ = normal_ / norm;
  else
    normal_ = vec::Zero();
  d_ = -normal_.dot(p1) ;

}
tri_aabb::tri_aabb(const size_t& id, const tri& plane, const tri& n){
  id_ = id;
  p_ = plane;
  n_ = n;
  vec p1 = plane.col(0), p2 = plane.col(1), p3 = plane.col(2);
  low_bd_ = p1;
  up_bd_ = p1;
  for(size_t i = 0; i < 3; ++i){
    if(low_bd_(i) > p2(i))
      low_bd_(i) = p2(i);
    if (up_bd_(i) < p2(i))
      up_bd_(i) = p2(i);

    if(low_bd_(i) > p3(i))
      low_bd_(i) = p3(i);
    if (up_bd_(i) < p3(i))
      up_bd_(i) = p3(i);
  }

  center = (p1 + p2 + p3) / 3.0;
  vec one_edge = p2 - p1, other_edge = p3 - p1;
  normal_ = one_edge.cross(other_edge);
  // normal_(0) = (p2(1) - p1(1)) * (p3(2) - p1(2)) - (p2(2) - p1(2)) * (p3(1) - p1(1));
  // normal_(1) = (p2(0) - p1(0)) * (p3(2) - p1(2)) - (p2(2) - p1(2)) * (p3(0) - p1(0));
  // normal_(2) = (p2(0) - p1(0)) * (p3(1) - p1(1)) - (p2(1) - p1(2)) * (p3(0) - p1(0));

  double norm  = normal_.norm();
  if(norm > 1e-6)
    normal_ = normal_ / norm;
  else
    normal_ = vec::Zero();
  d_ = -normal_.dot(p1) ;
}

aabb merge_tri_aabbs(const std::vector<std::shared_ptr<tri_aabb>> tri_aabbs){
  if(tri_aabbs.empty())
    throw runtime_error("tri_aabbs is empty");

  bool bk =false;
  if(tri_aabbs.size() == 90)
    bk = true;
  size_t num = 0;
  aabb bd_box(tri_aabbs[0]->low_bd_, tri_aabbs[0]->up_bd_);
  for(auto& one_bdbox : tri_aabbs){
    bd_box.merge(*one_bdbox);
    ++num;
  }

  {//check
    for(size_t i = 0; i < tri_aabbs.size(); ++i){
      for(size_t j = 0; j < 3; ++j){
        assert(tri_aabbs[i]->low_bd_(j) >= bd_box.low_bd_(j));
        assert(tri_aabbs[i]->up_bd_(j) <= bd_box.up_bd_(j));

      }
    }

  }

  return bd_box;
}


#include <deal.II/base/function.h>
#include <deal.II/base/function_parser.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/parameter_handler.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/timer.h>
#include <deal.II/base/table_handler.h>

#include <deal.II/lac/constraint_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/petsc_parallel_sparse_matrix.h>
#include <deal.II/lac/petsc_parallel_vector.h>
#include <deal.II/lac/petsc_solver.h>
#include <deal.II/lac/petsc_precondition.h>
#include <deal.II/lac/sparsity_tools.h>
#include <deal.II/lac/vector.h>

#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>

#include <deal.II/distributed/grid_refinement.h>
#include <deal.II/distributed/tria.h>

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_values.h>
#include <deal.II/fe/fe_system.h>
#include <deal.II/fe/fe_q.h>

#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/error_estimator.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/vector_tools.h>

// #include <boost/program_options.hpp>

#include <mandy/function_tools.h>

#include <mandy/elastic_problem.h>
#include <mandy/piezoelectric_problem.h>

#include <fstream>
#include <iostream>

#include <algorithm>    // std::transform
#include <functional>   // std::plus

namespace aphex
{

  template <int dim>
  class Aphex
  {
  public:

    /**
     * Class constructor.
     */
    Aphex (const std::string &prm);

    /**
     * Class destructor.
     */
    ~Aphex ();

    /**
     * Run.
     */
    void run ();

  private:

    /**
     * MPI communicator.
     */
    MPI_Comm mpi_communicator;

    /**
     * A distributed grid on which all computations are done.
     */
    dealii::parallel::distributed::Triangulation<dim> triangulation;

    /**
     * Solution of the elastic problem.
     */
    dealii::PETScWrappers::MPI::Vector displacement;

    /**
     * Solution of the piezoelectric.
     */
    dealii::PETScWrappers::MPI::Vector piezoelectric_potential;

    /**
     * Parallel iostream.
     */
    dealii::ConditionalOStream pcout;

    /**
     * Stop clock.
     */
    dealii::TimerOutput timer;

  };

  template <int dim>
  Aphex<dim>::Aphex (const std::string &prm)
    :
    mpi_communicator (MPI_COMM_WORLD),
    triangulation (mpi_communicator,
		   typename dealii::Triangulation<dim>::MeshSmoothing
		   (dealii::Triangulation<dim>::smoothing_on_refinement |
		    dealii::Triangulation<dim>::smoothing_on_coarsening)),
    pcout (std::cout, (dealii::Utilities::MPI::this_mpi_process (mpi_communicator) == 0)),
    timer (mpi_communicator, pcout,
     	   dealii::TimerOutput::summary,
     	   dealii::TimerOutput::wall_times)
  {}

  template <int dim>
  Aphex<dim>::~Aphex ()
  {}

  template <int dim>
  void
  Aphex<dim>::run ()
  {

    try
      {

	dealii::GridGenerator::hyper_cube (triangulation, -10, 10);
	// triangulation.refine_global (parameters.get_integer ("Global mesh refinement steps"));
	triangulation.refine_global (2);

	{
	  dealii::TimerOutput::Scope time (timer, "material");
	  mandy::FunctionTools<3> material (triangulation, "material.prm");
	  material.run ();
	}

	{
	  dealii::TimerOutput::Scope time (timer, "elastic problem");
	  mandy::ElasticProblem<3> elastic_problem (triangulation, displacement,
						    mpi_communicator, "elastic.prm");
	  elastic_problem.run ();
	}

	{
	  dealii::TimerOutput::Scope time (timer, "piezoelectric problem");
	  mandy::PiezoelectricProblem<3> piezoelectric_problem (triangulation, displacement,
								mpi_communicator, "piezoelectric.prm");
	  piezoelectric_problem.run ();
	}

      }

    catch (std::exception &exc)
      {
	std::cerr << std::endl << std::endl
		  << "----------------------------------------------------"
		  << std::endl;
	std::cerr << "Exception on processing: " << std::endl
		  << exc.what() << std::endl
		  << "Aborting!" << std::endl
		  << "----------------------------------------------------"
		  << std::endl;
      }

    catch (...)
      {
	std::cerr << std::endl << std::endl
		  << "----------------------------------------------------"
		  << std::endl;
	std::cerr << "Unknown exception!" << std::endl
		  << "Aborting!" << std::endl
		  << "----------------------------------------------------"
		  << std::endl;
      }

  } // run ()

} // namespace aphex

/**
 * Main function: Initialise problem and run it.
 */
int main (int argc, char *argv[])
{

  // Initialise MPI
  dealii::Utilities::MPI::MPI_InitFinalize mpi_initialization (argc, argv, 1);

  try
    {
#ifdef INCLUDED_EXTERNAL_BOOST
      // Parse the commandline.
      boost::program_options::options_description description {"Options"};
      description.add_options ()
	("help", "Help screen")
	("prm", value<string> ()->default_value ("aphex.prm"), "Parameter file");

      boost::program_options::variables_map vmap;
      boost::program_options::store
	(boost::program_options::parse_command_line (argc, argv, description), vmap);

      if (vmap.count ("help"))
	std::cout << description
		  << std::endl;

      else if (vmap.count ("prm"))
	std::cout << "Parameter file: " << vmap["prm"].as<string>
		  << std::endl;

      // else if (...)
#endif

      std::vector<std::string> args (argv+1, argv+argc);

      AssertThrow (args.size ()>0, dealii::ExcMessage ("The number of input arguments must be greater than zero."));

      std::cout << std::endl << std::endl
		<< "----------------------------------------------------"
		<< std::endl
		<< "Caught arguments: ";
      for (unsigned int i=0; i<args.size (); ++i)
	std::cout << std::endl << "   " << args[i];
      std::cout  << std::endl
		 << "----------------------------------------------------"
		 << std::endl << std::endl;

      aphex::Aphex<3> aphex (args[1]);
      aphex.run ();
    }

  catch (std::exception &exc)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }

  catch (...)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <chrono>

using namespace std;
using namespace cv;

void feature_matching(const Mat& img1, const Mat& img2,
    std::vector<KeyPoint>& keypoints1,
    std::vector<KeyPoint>& keypoints2,
    std::vector< DMatch >& matches)
    {
        Mat descriptors_1, descriptors_2;
        Ptr<FeatureDetector> detector = ORB::create();
        Ptr<DescriptorExtractor> descriptor = ORB::create();

        // compute FAST corners
        detector->detect(img1, keypoints1);
        detector->detect(img2, keypoints2);

        // compute BRIEF descriptor
        descriptor->compute(img1, keypoints1, descriptors_1);
        descriptor->compute(img2, keypoints2, descriptors_2);

        Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create(4);
        /*
        FLANNBASED = 1,
        BRUTEFORCE = 2,
        BRUTEFORCE_L1 = 3,
        BRUTEFORCE_HAMMING = 4,
        BRUTEFORCE_HAMMINGLUT = 5,
        BRUTEFORCE_SL2 = 6 */

        vector<DMatch> allmatches;
        matcher->match(descriptors_1, descriptors_2, allmatches);

        // Filter all match points
        // Find maximum and minimum distance
        double min_dist = 1000000, max_dist = 0;
        for ( int i = 0; i < descriptors_1.rows; i++ )
        {
            double dist = allmatches[i].distance;
            if ( dist < min_dist ) min_dist = dist;
            if ( dist > max_dist ) max_dist = dist;
        }

        cout << "max distance: " << max_dist << endl;
        cout << "min distance: " << min_dist << endl;

        for ( int i = 0; i < descriptors_1.rows; i++ )
        {
            if ( allmatches[i].distance <= max ( 2*min_dist, 30.0 ) )
            {
                matches.push_back ( allmatches[i] );
            }
        }
    }

Point2d pixel2cam ( const Point2d& p, const Mat& K )
{
    return Point2d
           (
               ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),
               ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )
           );
}

void bundleAdjustment(
    const vector<Point3f> points_3d,
    const vector<Point2f> points_2d,
    const Mat &K,
    Mat &R, Mat &t){
        typedef g2o::BlockSolver< g2o::BlockSolverTraits<6, 3>> Block;
        // typedef g2o::BlockSolver_6_3 Block;

        Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();

        // Routine in pose estimation
        Block *solver_ptr = new Block(linearSolver);
        g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
        g2o::SparseOptimizer optimizer;
        optimizer.setAlgorithm (solver);
        optimizer.setVerbose(true);

        // Vertex SE(3)
        g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // Camera pose SE(3)
        Eigen::Matrix3d R_mat;

        R_mat <<
            R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ),
            R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ),
            R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 );

        pose->setId(0);
        pose->setEstimate(g2o::SE3Quat(
            R_mat,
            Eigen::Vector3d(t.at<double> ( 0,0 ), t.at<double> ( 1,0 ), t.at<double> ( 2,0 ))
        ));     // SE3Quat is 6 dimensional, first three rotation, last three translation.
                // Actually it's using Quaternion
        optimizer.addVertex(pose);

        int index = 1;
        // landmarks, use g2o::VertexSBAPointXYZ
        for (const Point3f p:points_3d){
            g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
            point->setId(index++);
            point->setEstimate(Eigen::Vector3d(p.x, p.y, p.z));   // easy to set up, only need Eigen::Vector3d
            point->setMarginalized(true);
            optimizer.addVertex(point);
        }

        // parameter: camera intrinsics. Routine setup
        g2o::CameraParameters* camera = new g2o::CameraParameters (
            K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0
        );
        camera->setId ( 0 );
        optimizer.addParameter ( camera );

        index = 1;
        // Error terms. use edge g2o::EdgeProjectXYZ2UV from 3d to 2d. Routine
        for(const Point2f p:points_2d){
            g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
            edge->setId(index);
            edge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(index++)));
            edge->setVertex(1, pose);    // One side to pose, another side to each landmark
            edge->setMeasurement(Eigen::Vector2d(p.x, p.y));
            edge->setParameterId(0, 0);
            edge->setInformation(Eigen::Matrix2d::Identity());  //  infomation matrix, covariance matrix
            optimizer.addEdge(edge);
        }

        chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
        // Setup and initial optimization. Routine
        optimizer.setVerbose ( true );
        optimizer.initializeOptimization();
        optimizer.optimize ( 100 );
        chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
        chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
        cout<<"optimization costs time: "<<time_used.count() <<" seconds."<<endl;

        cout<<endl<<"after optimization:"<<endl;
        cout<<"T="<<endl<<Eigen::Isometry3d ( pose->estimate() ).matrix() <<endl;

    }

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

     if ( argc != 5 )
    {
        cout<<"usage: pose_estimation_3d2d img1 img2 depth1 depth2"<<endl;
        return 1;
    }

    Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
    Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );

    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    feature_matching ( img_1, img_2, keypoints_1, keypoints_2, matches );
    cout<<"Total "<<matches.size() <<" Matching Points" <<endl;

    Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );
    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    vector<Point3f> pts_3d;
    vector<Point2f> pts_2d;
    for ( DMatch m:matches )
    {
        ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
        if ( d == 0 )
            continue;
        float dd = d/5000.0;
        Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
        pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );  // Find feature point's 3D coordinates from Depth image
        pts_2d.push_back ( keypoints_2[m.trainIdx].pt );
    }

    cout<<"3d-2d pairs: "<<pts_3d.size() <<endl;

    Mat r, t;
    solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false );
    Mat R;
    cv::Rodrigues ( r, R ); // Rodrigues formula: Rotation vector to rotation matrix

    cout<<"R="<<endl<<R<<endl;
    cout<<"t="<<endl<<t<<endl;

    cout<<"calling bundle adjustment"<<endl;

    bundleAdjustment ( pts_3d, pts_2d, K, R, t );
}


#include "../../def_submodule.hpp"

#include "../../../../../type/math/coord.hpp"

using namespace ::math::linear::vector;

#include <boost/python.hpp>

typedef GS_DDMRM::S_IceRay::S_type::GT_int               GTs_int;
typedef GS_DDMRM::S_IceRay::S_type::GT_size               GTs_size;
typedef GS_DDMRM::S_IceRay::S_type::GT_scalar             GTs_scalar;
//typedef GS_DDMRM::S_IceRay::S_type::S_color::GT_scalar    GTs_color;
typedef GS_DDMRM::S_IceRay::S_type::S_coord::GT_int2D    GTs_cell2D;
typedef GS_DDMRM::S_IceRay::S_type::S_coord::GT_int3D    GTs_cell3D;
typedef GS_DDMRM::S_IceRay::S_type::S_coord::GT_int4D    GTs_cell4D;

typedef GS_DDMRM::S_IceRay::S_type::S_coord::GT_size2D    GTs_size2D;
typedef GS_DDMRM::S_IceRay::S_type::S_coord::GT_size3D    GTs_size3D;
typedef GS_DDMRM::S_IceRay::S_type::S_coord::GT_size4D    GTs_size4D;

typedef GS_DDMRM::S_IceRay::S_type::S_coord::GT_scalar2D  GTs_coord2D;
typedef GS_DDMRM::S_IceRay::S_type::S_coord::GT_scalar3D  GTs_coord3D;
typedef GS_DDMRM::S_IceRay::S_type::S_coord::GT_scalar4D  GTs_coord4D;

namespace
 {

  template < typename N_number, unsigned N_dimension>
   N_number get( typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model<N_number, N_dimension>::T_typedef const&v, int P_index )
    {
     if( P_index >= 0 && P_index < v.size() )
      {
       return v.at( P_index );
      }

     PyErr_SetString( PyExc_IndexError, "index out of range" );
     return 0;
    }

   template < typename N_scalar, typename N_number, unsigned N_dimension>
    void set( typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model<N_scalar, N_dimension>::T_typedef & v, int P_index, N_number value )
     {
      if(P_index >= 0 && P_index < v.size())
       {
        v.at( P_index ) = value;
        return;
       }

       PyErr_SetString(PyExc_IndexError, "index out of range" );
       return;
     }

   template < typename N_number >
    typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model< N_number, 2 >::T_typedef &
    load2( typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model<N_number, 2 >::T_typedef & v, N_number x, N_number y )
     {
      v[0] = x;
      v[1] = y;
      return v;
     }
   template < typename N_number >
    typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model< N_number, 3 >::T_typedef &
    load3( typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model<N_number, 3 >::T_typedef & v, N_number x, N_number y, N_number z )
     {
      v[0] = x;
      v[1] = y;
      v[2] = z;
      return v;
     }

   template < typename N_number >
    typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model< N_number, 4 >::T_typedef &
    load4( typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model<N_number, 4 >::T_typedef & v, N_number x, N_number y, N_number z, N_number t )
     {
      v[0] = x;
      v[1] = y;
      v[2] = z;
      v[3] = t;
      return v;
     }

   template < typename N_number, unsigned dimension_number >
    typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model< N_number, dimension_number >::T_typedef &
    fill( typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model<N_number, dimension_number >::T_typedef & v, N_number x )
     {
      ::math::linear::vector::fill( v, x );
      return v;
     }

    template < typename N_number >
     typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model< N_number, 3 >::T_typedef
      GFs_cross
       (
         typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model<N_number, 3 >::T_typedef const& P_left
        ,typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model<N_number, 3 >::T_typedef const& P_right
       )
       {
        typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model<N_number, 3 >::T_typedef Ir_right;
        ::math::linear::vector::cross( Ir_right, P_left, P_right );
        return Ir_right;
       }

    template < typename N_number, unsigned N_dimension >
     N_number
     GFs_dot
      (
        typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model<N_number, N_dimension >::T_typedef const& P_left
       ,typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model<N_number, N_dimension >::T_typedef const& P_right
      )
      {
       return  ::math::linear::vector::dot( P_left, P_right );
      }

  }

void expose_math_type_coord_cell()
 {
//MAKE_SUBMODULE( IceRay );
  MAKE_SUBMODULE( library   );
  MAKE_SUBMODULE( math   );

  boost::python::class_<GTs_cell2D>( "MathTypeCell2D" )
    .def( boost::python::init<>() )
  //.def( boost::python::init<GTs_int>() )
  //.def( boost::python::init<GTs_scalar>() )
  //.def( boost::python::init<GTs_int,GTs_int>( TODO ) )
    .def( boost::python::init<GTs_cell2D>() )

    .def("__getitem__", &get<GTs_int, 2 > )
    .def("__setitem__", &set<GTs_int, GTs_int, 2 > )
    .def("__setitem__", &set<GTs_int, GTs_scalar, 2 > )

    .def( "load",       &load2<GTs_size   >, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "load",       &load2<GTs_scalar >, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "fill",       &fill<GTs_size,2>, boost::python::return_value_policy<boost::python::return_by_value>() )

    .def( boost::python::self + boost::python::self )
    .def( boost::python::self - boost::python::self )
    .def( boost::python::self * GTs_size() )
    .def( GTs_size() * boost::python::self  )
    .def( boost::python::self / GTs_int() )
    .def( boost::python::self /= GTs_int() )
    .def( boost::python::self *= GTs_int() )
    .def( boost::python::self += boost::python::self )
    .def( boost::python::self -= boost::python::self )
  ;

  boost::python::class_<GTs_cell3D>( "MathTypeCell3D" )
    .def( boost::python::init<>() )
  //.def( boost::python::init<GTs_size>() )
  //.def( boost::python::init<GTs_scalar>() )
  //.def( boost::python::init<GTs_size,GTs_size,GTs_size>( TODO ) )
   .def( boost::python::init<GTs_cell3D>() )

    .def("__getitem__", &get<GTs_int, 3 > )
    .def("__setitem__", &set<GTs_int, GTs_int, 3 > )
    .def("__setitem__", &set<GTs_int, GTs_scalar, 3 > )

    .def( "load",       &load3<GTs_int   >, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "load",       &load3<GTs_scalar >, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "fill",       &fill<GTs_int,3>, boost::python::return_value_policy<boost::python::return_by_value>() )

    .def( boost::python::self + boost::python::self )
    .def( boost::python::self - boost::python::self )
    .def( boost::python::self * GTs_scalar() )
    .def( GTs_scalar() * boost::python::self  )
    .def( boost::python::self / GTs_int() )
    .def( boost::python::self /= GTs_int() )
    .def( boost::python::self *= GTs_int() )
    .def( boost::python::self += boost::python::self )
    .def( boost::python::self -= boost::python::self )
  ;

  boost::python::class_<GTs_cell4D>( "MathTypeCell4D" )
    .def( boost::python::init<>() )
    //.def( boost::python::init<GTs_int>() )
    //.def( boost::python::init<GTs_scalar>() )
    //.def( boost::python::init<GTs_int,GTs_int,GTs_int,GTs_int>( TODO ) )
    .def( boost::python::init<GTs_cell4D>() )

    .def("__getitem__", &get<GTs_int, 4 > )
    .def("__setitem__", &set<GTs_int, GTs_int, 4 > )
    .def("__setitem__", &set<GTs_int, GTs_scalar, 4 > )

    .def( "load",       &load4<GTs_int    >, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "load",       &load4<GTs_scalar >, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "fill",       &fill<GTs_int,   4>, boost::python::return_value_policy<boost::python::return_by_value>() )

    .def( boost::python::self + boost::python::self )
    .def( boost::python::self - boost::python::self )
    .def( boost::python::self * GTs_scalar() )
    .def( GTs_scalar() * boost::python::self  )
    .def( boost::python::self / GTs_int() )
    .def( boost::python::self /= GTs_int() )
    .def( boost::python::self *= GTs_int() )
    .def( boost::python::self += boost::python::self )
    .def( boost::python::self -= boost::python::self )
    ;
 }

void expose_math_type_coord_size()
 {
//MAKE_SUBMODULE( IceRay );
  MAKE_SUBMODULE( library   );
  MAKE_SUBMODULE( math   );

  boost::python::class_<GTs_size2D>( "MathTypeSize2D" )
    .def( boost::python::init<>() )
  //.def( boost::python::init<GTs_size>() )
  //.def( boost::python::init<GTs_scalar>() )
  //.def( boost::python::init<GTs_size,GTs_size>( TODO ) )
    .def( boost::python::init<GTs_size2D>() )

    .def("__getitem__", &get<GTs_size, 2 > )
    .def("__setitem__", &set<GTs_size, GTs_size, 2 > )
    .def("__setitem__", &set<GTs_size, GTs_scalar, 2 > )

    .def( "load",       &load2<GTs_size   >, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "load",       &load2<GTs_scalar >, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "fill",       &fill<GTs_size,2>, boost::python::return_value_policy<boost::python::return_by_value>() )

    .def( boost::python::self + boost::python::self )
    .def( boost::python::self - boost::python::self )
    .def( boost::python::self * GTs_size() )
    .def( GTs_size() * boost::python::self  )
    .def( boost::python::self / GTs_size() )
    .def( boost::python::self /= GTs_size() )
    .def( boost::python::self *= GTs_size() )
    .def( boost::python::self += boost::python::self )
    .def( boost::python::self -= boost::python::self )
  ;

  boost::python::class_<GTs_size3D>( "MathTypeSize3D" )
    .def( boost::python::init<>() )
  //.def( boost::python::init<GTs_size>() )
  //.def( boost::python::init<GTs_scalar>() )
  //.def( boost::python::init<GTs_size,GTs_size,GTs_size>( TODO ) )
   .def( boost::python::init<GTs_size3D>() )

    .def("__getitem__", &get<GTs_size, 3 > )
    .def("__setitem__", &set<GTs_size, GTs_size, 3 > )
    .def("__setitem__", &set<GTs_size, GTs_scalar, 3 > )

    .def( "load",       &load3<GTs_size   >, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "load",       &load3<GTs_scalar >, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "fill",       &fill<GTs_size,3>, boost::python::return_value_policy<boost::python::return_by_value>() )

    .def( boost::python::self + boost::python::self )
    .def( boost::python::self - boost::python::self )
    .def( boost::python::self * GTs_scalar() )
    .def( GTs_scalar() * boost::python::self  )
    .def( boost::python::self / GTs_size() )
    .def( boost::python::self /= GTs_size() )
    .def( boost::python::self *= GTs_size() )
    .def( boost::python::self += boost::python::self )
    .def( boost::python::self -= boost::python::self )
  ;

  boost::python::class_<GTs_size4D>( "MathTypeSize4D" )
    .def( boost::python::init<>() )
    //.def( boost::python::init<GTs_size>() )
    //.def( boost::python::init<GTs_scalar>() )
    //.def( boost::python::init<GTs_size,GTs_size,GTs_size,GTs_size>( TODO ) )
    .def( boost::python::init<GTs_size4D>() )

    .def("__getitem__", &get<GTs_size, 4 > )
    .def("__setitem__", &set<GTs_size, GTs_size, 4 > )
    .def("__setitem__", &set<GTs_size, GTs_scalar, 4 > )

    .def( "load",       &load4<GTs_size   >, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "load",       &load4<GTs_scalar >, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "fill",       &fill<GTs_size,4>, boost::python::return_value_policy<boost::python::return_by_value>() )

    .def( boost::python::self + boost::python::self )
    .def( boost::python::self - boost::python::self )
    .def( boost::python::self * GTs_scalar() )
    .def( GTs_scalar() * boost::python::self  )
    .def( boost::python::self / GTs_size() )
    .def( boost::python::self /= GTs_size() )
    .def( boost::python::self *= GTs_size() )
    .def( boost::python::self += boost::python::self )
    .def( boost::python::self -= boost::python::self )
    ;
 }

// TODO: s * v, v * s, v / s, v+v, v-v
void expose_math_type_coord_scalar()
 {
//MAKE_SUBMODULE( IceRay );
  MAKE_SUBMODULE( library   );
  MAKE_SUBMODULE( math   );

  boost::python::class_<GTs_coord2D>( "MathTypeCoord2D" )
    .def( boost::python::init<>() )
  //.def( boost::python::init<GTs_scalar>() )
  //.def( boost::python::init<GTs_scalar,GTs_scalar>( TODO ) )
    .def( boost::python::init<GTs_coord2D>() )

    .def("__getitem__", &get<GTs_scalar, 2 > )
    .def("__setitem__", &set<GTs_scalar, GTs_scalar, 2 > )

    .def( "load",        &load2<GTs_scalar >, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "fill",        &fill<GTs_scalar,2>, boost::python::return_value_policy<boost::python::return_by_value>() )

    .def( boost::python::self + boost::python::self )
    .def( boost::python::self - boost::python::self )

    .def( boost::python::self * GTs_scalar() )
    .def( GTs_scalar() * boost::python::self  )

    .def( boost::python::self / GTs_scalar() )

    .def( boost::python::self /= GTs_scalar() )
    .def( boost::python::self *= GTs_scalar() )

    .def( boost::python::self += boost::python::self )
    .def( boost::python::self -= boost::python::self )
  ;

  boost::python::def("MathCoord2D_dot",  &GFs_dot<GTs_scalar, 2 > );

  boost::python::def("MathLinearVector2DDot",  &GFs_dot<GTs_scalar, 2 > );
  boost::python::def("MathLinearVector3DLength",  &::math::linear::vector::length<GTs_scalar,GTs_scalar, 4 > );

  boost::python::class_<GTs_coord3D>( "MathTypeCoord3D" )
    .def( boost::python::init<>() )
   //.def( boost::python::init<GTs_scalar>() )
   //.def( boost::python::init<GTs_scalar,GTs_scalar,GTs_scalar>( TODO ) )
   .def( boost::python::init<GTs_coord3D>() )

     .def( "__getitem__", &get<GTs_scalar, 3 > )
     .def( "__setitem__", &set<GTs_scalar, GTs_scalar, 3 > )
   //.def("__setitem__", &set<GTs_scalar, GTs_size, 3 > )

    .def( "load",        &load3<GTs_scalar >, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "fill",        &fill<GTs_scalar,3>, boost::python::return_value_policy<boost::python::return_by_value>() )

    .def( boost::python::self + boost::python::self )
    .def( boost::python::self - boost::python::self )

    .def( boost::python::self * GTs_scalar() )
    .def( GTs_scalar() * boost::python::self  )

    .def( boost::python::self / GTs_scalar() )

    .def( boost::python::self /= GTs_scalar() )
    .def( boost::python::self *= GTs_scalar() )

    .def( boost::python::self += boost::python::self )
    .def( boost::python::self -= boost::python::self )
   ;

  boost::python::def("MathCoord3D_dot",    &GFs_dot<GTs_scalar, 3 > );
  boost::python::def("MathCoord3D_cross",  &GFs_cross<GTs_scalar > );

  boost::python::def("MathLinearVector3DDot",  &GFs_dot<GTs_scalar, 2 > );
  boost::python::def("MathLinearVector3DLength",  &::math::linear::vector::length<GTs_scalar,GTs_scalar, 3 > );

  boost::python::class_<GTs_coord4D>( "MathTypeCoord4D" )
    .def( boost::python::init<>() )
  //.def( boost::python::init<GTs_scalar>() )
  //.def( boost::python::init<GTs_scalar,GTs_scalar,GTs_scalar,GTs_scalar>() )
   .def( boost::python::init<GTs_coord4D>() )

   .def("__getitem__", &get<GTs_scalar, 4 > )
   .def("__setitem__", &set<GTs_scalar, GTs_scalar, 4 > )

    .def( "load",       &load4<GTs_scalar>, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "load",       &load4<GTs_size>, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "fill",       &fill<GTs_scalar,4>, boost::python::return_value_policy<boost::python::return_by_value>() )

    .def( boost::python::self + boost::python::self )
    .def( boost::python::self - boost::python::self )
    .def( boost::python::self * GTs_scalar() )
    .def( GTs_scalar() * boost::python::self  )
    .def( boost::python::self / GTs_scalar() )
    .def( boost::python::self /= GTs_scalar() )
    .def( boost::python::self *= GTs_scalar() )
    .def( boost::python::self += boost::python::self )
    .def( boost::python::self -= boost::python::self )
   ;

  boost::python::def("MathCoord4D_dot",  &GFs_dot<GTs_scalar, 4 > );

  boost::python::def("MathLinearVector4DDot",  &GFs_dot<GTs_scalar, 2 > );
  boost::python::def("MathLinearVector4DLength",  &::math::linear::vector::length<GTs_scalar,GTs_scalar, 4 > );

 }

void expose_math_type_coord_coord()
 {
  expose_math_type_coord_cell();
  expose_math_type_coord_size();
  expose_math_type_coord_scalar();
 }