#include <boost/math/distributions/chi_squared.hpp>
#include <Eigen/Eigen>
//#include <ros/ros.h>

//using boost::math::chi_squared;
//using boost::math::cdf;

namespace perception_oru
{

template <typename PointT>
void AdaptiveOctTree<PointT>::setParameters(bool _useDornikHansen,
        bool _useFlatness,
        double _RSS_THRESHOLD,
        double _DH_SIGNIFICANCE_LVL,
        double _MIN_CELL_SIZE,
        double _FLAT_FACTOR,
        double _BIG_CELL_SIZE,
        double _SMALL_CELL_SIZE
                                           )
{

    this->BIG_CELL_SIZE = _BIG_CELL_SIZE;
    this->SMALL_CELL_SIZE = _SMALL_CELL_SIZE;

    useDornikHansen      = _useDornikHansen;
    useFlatness          = _useFlatness;
    RSS_THRESHOLD        = _RSS_THRESHOLD;
    DH_SIGNIFICANCE_LVL  = _DH_SIGNIFICANCE_LVL;
    MIN_CELL_SIZE        = _MIN_CELL_SIZE;
    FLAT_FACTOR          = _FLAT_FACTOR;

    parametersSet = true;
}
/** empty! default constructor
  */
template <typename PointT>
AdaptiveOctTree<PointT>::AdaptiveOctTree() : OctTree<PointT>()
{
    if(!parametersSet)
    {
        setParameters();
    }
}

/** constructor, calls parent OctTree constructor
  */
template <typename PointT>
AdaptiveOctTree<PointT>::AdaptiveOctTree(pcl::PointXYZ center, double xsize, double ysize,
        double zsize, NDTCell<PointT>* type, OctTree<PointT> *_parent, unsigned int _depth) :
    OctTree<PointT>(center,xsize,ysize,zsize,type,_parent,_depth)
{
    if(!parametersSet)
    {
        setParameters();
    }
}

/** empty destructor, all data are deallocated by parent class
  */
template <typename PointT>
AdaptiveOctTree<PointT>::~AdaptiveOctTree()
{
}

/**
  finds all leafs of this tree and fills the vector of pointers to the leafs
  */
template <typename PointT>
void AdaptiveOctTree<PointT>::computeTreeLeafs()
{
    if(this->isLeaf())
    {
        myTreeLeafs.push_back(this);
        return;
    }

    myTreeLeafs.clear();
    std::vector<OctTree<PointT>*> next;
    next.push_back(this);

    while(next.size()>0)
    {
        OctTree<PointT> *cur = next.front();
        if(cur!=NULL)
        {
            if(cur->isLeaf())
            {
                myTreeLeafs.push_back(cur);
            }
            else
            {
                for(int i=0; i<8; i++)
                {
                    OctTree<PointT>* tmp = cur->getChild(i);
                    if(tmp!=NULL)
                    {
                        next.push_back(tmp);
                    }
                }
            }
        }
        next.erase(next.begin());
    }
}

/**
go through all leafs and split the ones with high residuals
  */
template <typename PointT>
void AdaptiveOctTree<PointT>::postProcessPoints()
{

    //compute leafs as OctTree*
    computeTreeLeafs();
    //cout<<"leafs :"<<myTreeLeafs.size()<<endl;

    for(unsigned int i=0; i<myTreeLeafs.size(); i++)
    {
        NDTCell<PointT> * nd = dynamic_cast<NDTCell<PointT>*>((myTreeLeafs[i])->myCell_);
        if(nd == NULL) continue;
        if(useDornikHansen)
        {
            double significance = computeDornikHansen(nd);
            if(significance < 0)
            {
                //there were not enough points, let's clear the cell!
                //(myTreeLeafs[i])->myCell->points.clear();
                continue;
            }
            if(significance < DH_SIGNIFICANCE_LVL)
            {
                //split the leafs
                std::vector<OctTree<PointT>*> newLeafs = splitTree(myTreeLeafs[i]);
                myTreeLeafs.insert(myTreeLeafs.end(),newLeafs.begin(),newLeafs.end());
            }
        }
        else if(useFlatness)
        {
            nd->computeGaussian();
            if(!nd->hasGaussian_)
            {
                continue;
            }

            Eigen::Vector3d evals = nd->getEvals();
            int idMin, idMax;
            double minEval = evals.minCoeff(&idMin);
            double maxEval = evals.maxCoeff(&idMax);
            int idMiddle = -1;
            for(int j=0; j<3; j++)
            {
                if(j!=idMin && j!=idMax)
                {
                    idMiddle =  j;
                }
            }
            if(idMiddle < 0) continue;

            if(minEval*FLAT_FACTOR > evals(idMiddle))
            {
                std::vector<OctTree<PointT>*> newLeafs = splitTree(myTreeLeafs[i]);
                myTreeLeafs.insert(myTreeLeafs.end(),newLeafs.begin(),newLeafs.end());
            }

        }
        else
        {
            double rss = computeResidualSquare(nd);
            if(rss > RSS_THRESHOLD)
            {
                //split the leafs
                std::vector<OctTree<PointT>*> newLeafs = splitTree(myTreeLeafs[i]);
                myTreeLeafs.insert(myTreeLeafs.end(),newLeafs.begin(),newLeafs.end());
            }
        }
    }

    this->leafsCached_ = false;
}

/**
  performs the Dornik-Hansen Omnibus normality test
*/
template <typename PointT>
double AdaptiveOctTree<PointT>::computeDornikHansen(NDTCell<PointT> *cell)
{
    double pval = 1;
    double Ep = 0;
    //test statistics breaks down for n<=7
    if(cell->points_.size() <= 7) return -1;
    cell->computeGaussian();

    //degree is 2*number_of_dimensions
    boost::math::chi_squared dist(6);

    Eigen::Vector3d mean = cell->getMean();
    Eigen::Matrix3d C = cell->getCov();

    Eigen::MatrixXd Xhat(cell->points_.size(),3); //, tmpMat;

    for(unsigned int i=0; i< cell->points_.size(); i++)
    {
        Xhat(i,0) = cell->points_[i].x - mean(0);
        Xhat(i,1) = cell->points_[i].y - mean(1);
        Xhat(i,2) = cell->points_[i].z - mean(2);
    }

    //Compute transform for observed points
    Eigen::Matrix3d V;
    Eigen::Matrix3d H;
    Eigen::Vector3d lambda;
    Eigen::Matrix3d L;
    Eigen::Matrix3d R1;

    Eigen::MatrixXd R;

    V(0,0) = 1/sqrt(C(0,0));
    V(1,1) = 1/sqrt(C(1,1));
    V(2,2) = 1/sqrt(C(2,2));

    C = V*C*V;
    Eigen::EigenSolver<Eigen::Matrix3d> eig(C);
    H = eig.eigenvectors().real();
    lambda = eig.eigenvalues().real();

    //covariance is not positive semidefinate
    if(lambda.minCoeff() <= 0) return -1;

    L(0,0) = 1/sqrt(lambda(0));
    L(1,1) = 1/sqrt(lambda(1));
    L(2,2) = 1/sqrt(lambda(2));

    //transform observations
    R1 = H*L*H.transpose()*V;
    R = R1*Xhat.transpose();

    //compute skewness and kurtois of new observations (in each dimension)
    //samples are zero mean, compute for each dimension standard deviation
    Ep = 0.0;
    double n = R.cols();
    for(unsigned int dim = 0; dim < 3; dim ++)
    {
        double m2 = 0, m3 = 0, m4 =0;
        double b1 = 0, b2;
        double beta, omega2, gamma, y;
        double gamma2, a,c, k, alpha, chi;
        double z1,z2;

        for(unsigned int i=0; i<R.cols(); i++)
        {
            m2 += pow(R(dim,i),2);
            m3 += pow(R(dim,i),3);
            m4 += pow(R(dim,i),4);
        }
        m2 /= R.cols();
        m3 /= R.cols();
        m4 /= R.cols();
//	cout <<"dim "<<dim<<" m2 "<<m2<<" m3 "<<m3<<" m4 "<<m4<<endl;
        b1 = m3/(pow(m2,1.5));
        b2 = m4/(pow(m2,2));

//	cout<<"b1 "<<b1<<" b2 "<<b2<<endl;

        //compute Z1 and Z2
        beta = 3*(n*n + 27*n -70)*(n+1)*(n+3)/((n-2)*(n+5)*(n+7)*(n+9));
        omega2 = -1+sqrt(2*(beta-1));
        gamma = 1/sqrt(log(sqrt(omega2)));
        y = b1*sqrt((omega2-1)*(n+1)*(n+3)/(12*(n-2)));
        z1 = gamma*log(y+sqrt(y*y+1));

        gamma2 = (n-3)*(n+1)*(n*n+15*n-4);
        a = (n-2)*(n+5)*(n+7)*(n*n+27*n-70)/(6*gamma2);
        c = (n-7)*(n+5)*(n+7)*(n*n+2*n-5)/(6*gamma2);
        k = (n+5)*(n+7)*(pow(n,3)+37*n*n+11*n-313)/(12*gamma2);
        alpha = a + b1*b1*c;
        chi = (b2-1-b1*b1)*2*k;
        z2 = (pow((chi/(2*alpha)),(1./3.))-1+(1./(9*alpha)))*sqrt(9*alpha);

//	cout<<"z1: "<<z1<<" z2: "<<z2<<endl;

        //compute Ep
        Ep += z1*z1 + z2*z2;
    }

    //compute probability from chi square cdf
    pval = 1-boost::math::cdf(dist,Ep);

//    cout<<"Ep "<<Ep<<endl;
//    cout<<"P: "<<pval<<endl;
    if(pval>DH_SIGNIFICANCE_LVL)
    {
        double dx,dy,dz;
        cell->getDimensions(dx,dy,dz);
        std::cout<<"final split was at ("<<dx<<","<<dy<<","<<dz<<"); pval is "<<pval<<std::endl;
    }
    return 0;

}
/**
fits a 3d gaussian in the cell and computes the residual squares sum
*/
template <typename PointT>
double AdaptiveOctTree<PointT>::computeResidualSquare(NDTCell<PointT> *cell)
{
    double rss = 0; //residual sum squared
    double meanResidual = 0;
    double residualVar = 0;
    if(cell->points_.size() <= 3) return 0;

    cell->computeGaussian();

    Eigen::Vector3d cur, curProd;
    Eigen::Matrix3d cov = cell->getCov();
    Eigen::Vector3d mean = cell->getMean();
    //Eigen::LLT<Eigen::Matrix3d> lltOfCov = cov.llt();

    for(unsigned int i=0; i< cell->points_.size(); i++)
    {

        cur(0) = cell->points_[i].x - mean(0);
        cur(1) = cell->points_[i].y - mean(1);
        cur(2) = cell->points_[i].z - mean(2);

        //lltOfCov.solve(cur,&curProd);
        //rss += cur.dot(curProd);
        rss += cur.dot(cur);
        meanResidual += cur.norm()/cell->points_.size();
    }

    for(unsigned int i=0; i< cell->points_.size(); i++)
    {
        cur(0) = cell->points_[i].x - mean(0);
        cur(1) = cell->points_[i].y - mean(1);
        cur(2) = cell->points_[i].z - mean(2);

        residualVar += pow(cur.norm()-meanResidual,2)/(cell->points_.size()-1);
    }
    double bic = rss/residualVar + log(cell->points_.size());
//    ROS_INFO("rss %lf mean %lf var %lf bic %lf",rss,meanResidual,residualVar,bic);
    return bic;
}

/**
  splits a cell and returns a vector of the newly created children
  iterates points downwards
  */
template <typename PointT>
std::vector<OctTree<PointT>*> AdaptiveOctTree<PointT>::splitTree(OctTree<PointT> *octLeaf)
{
    std::vector<OctTree<PointT>*> newLeafs;

    if(octLeaf->isLeaf())
    {
        double xs,ys,zs;
        octLeaf->myCell_->getDimensions(xs,ys,zs);

        double cellSize = (xs+ys+zs)/3.; //average for now

        if(octLeaf->depth_ > this->MAX_DEPTH || cellSize <= this->MIN_CELL_SIZE )
        {
            //just store point, we can't split any more
            return newLeafs;
        }

        pcl::PointXYZ myCenter = octLeaf->myCell_->getCenter();

        //branch leaf
        for(unsigned int it=0; it<8; it++)
        {

            pcl::PointXYZ newCenter;

            //computes the center of the it'th child
            newCenter.x = (myCenter.x + pow(-1.,it/4)*xs/4.);
            newCenter.y = (myCenter.y + pow(-1.,it/2)*ys/4.);
            newCenter.z = (myCenter.z + pow(-1.,(it+1)/2)*zs/4.);

            octLeaf->children_[it] = new OctTree<PointT>(newCenter,xs/2,ys/2,
                    zs/2, octLeaf->myCell_, this, this->depth_+1);
            newLeafs.push_back(octLeaf->children_[it]);
        }
        //add current points
        for(unsigned int jt=0; jt<octLeaf->myCell_->points_.size(); jt++)
        {
            size_t ind = octLeaf->getIndexForPoint(octLeaf->myCell_->points_[jt]);
            octLeaf->children_[ind]->addPoint(octLeaf->myCell_->points_[jt]);
        }
        octLeaf->leaf_=false;
        octLeaf->myCell_->points_.clear();
    }

    return newLeafs;
}

/**
  creates an oct tree with the same parameters.
  \note the points are not copied in teh returned instance
  */
template <typename PointT>
SpatialIndex<PointT>* AdaptiveOctTree<PointT>::clone()
{
    if(this->myCell_ == NULL)
    {
        return new AdaptiveOctTree();
    }
    double sx,sy,sz;
    this->myCell_->getDimensions(sx,sy,sz);
    AdaptiveOctTree<PointT> *tr = new AdaptiveOctTree<PointT>(this->myCell_->getCenter(),sx,sy,sz,this->myCell_);
    return tr;
}

}


////////////////////////////////////////////////////////////////////////////////
// Materials.hh
////////////////////////////////////////////////////////////////////////////////
/*! @file
//      Parametrized materials that can be used with MaterialField for purposes
//      of material optimization. Each material provides getETensorDerivative,
//      which gives the derivative of the elasticity tensor with respect to one
//      material parameter.
//
//      The exception is ConstantMaterial, which is intended to be read from a
//      file and which doesn't support material optimization.
*/
//  Author:  Julian Panetta (jpanetta), julian.panetta@gmail.com
//  Company:  New York University
//  Created:  07/11/2014 15:48:34
////////////////////////////////////////////////////////////////////////////////
#ifndef MATERIAL_HH
#define MATERIAL_HH

#include <MeshFEM/Types.hh>
#include <MeshFEM/Flattening.hh>
#include <MeshFEM/ElasticityTensor.hh>
#include <nlohmann/json.hpp>
#include <fstream>
#include <iostream>
#include <stdexcept>
#include <string>
#include <vector>

#define BOOST_PARSER 1
#if BOOST_PARSER
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#endif

#include <MeshFEM_export.h>

namespace Materials {

// Material parameter bounds
struct Bounds {
    struct Bound {
        Bound(size_t _var, Real _val) : var(_var), value(_val) { }
        size_t var; Real value;
    };

    Bounds() { }
    Bounds(const std::vector<Bound> &l, const std::vector<Bound> &u)
        : m_lower(l), m_upper(u) { }

    void MESHFEM_EXPORT setFromJson(const nlohmann::json &entry);

    const std::vector<Bound> &lower() const { return m_lower; }
    const std::vector<Bound> &upper() const { return m_upper; }

    void setLower(const std::vector<Bound> &l) { m_lower = l; }
    void setUpper(const std::vector<Bound> &u) { m_upper = u; }
private:
    std::vector<Bound> m_lower, m_upper;
};

// Base class for variable materials
template<size_t _N, template<size_t> class _Mat, size_t _NVars>
struct VariableMaterial {
    typedef ElasticityTensor<Real, _N> ETensor;
    static constexpr size_t numVars = _NVars;

    virtual void getETensorDerivative(size_t p, ETensor &d) const = 0;
    virtual void getTensor(ETensor &tensor) const = 0;

    static const std::vector<Bounds::Bound> &upperBounds() { return _Mat<_N>::g_bounds.upper(); }
    static const std::vector<Bounds::Bound> &lowerBounds() { return _Mat<_N>::g_bounds.lower(); }

    static void setUpperBounds(const std::vector<Bounds::Bound> &u) { _Mat<_N>::g_bounds.setUpper(u); }
    static void setLowerBounds(const std::vector<Bounds::Bound> &l) { _Mat<_N>::g_bounds.setLower(l); }

    static void setBoundsFromFile(const std::string &path) {
        std::ifstream is(path);
        if (!is.is_open()) {
            throw std::runtime_error("Couldn't open bounds " + path);
        }
        nlohmann::json config;
        is >> config;
        setBoundsFromJson(config);
    }

    static void setBoundsFromJson(const nlohmann::json &config) {
        _Mat<_N>::g_bounds.setFromJson(config);
        // Validate bounds.
        std::runtime_error indexError("Bounds variable index out-of-bounds.");
        for (const auto &b : _Mat<_N>::g_bounds.lower()) if (b.var >= numVars) throw indexError;
        for (const auto &b : _Mat<_N>::g_bounds.upper()) if (b.var >= numVars) throw indexError;
    }

    virtual ~VariableMaterial() { }

    Real vars[numVars];
};

// Var 0: Young's modulus, var 1: Poisson ratio
template<size_t _N>
struct Isotropic : public VariableMaterial<_N, Isotropic, 2> {
    static constexpr size_t N = _N;
    typedef ElasticityTensor<Real, _N> ETensor;
    typedef Eigen::Matrix<Real, flatLen(_N), 1> FlattenedSymmetricMatrix;
    typedef VariableMaterial<N, Materials::Isotropic, 2> Base;
    using Base::vars;

    // WARNING: bounds are shared by all isotropic materials! (static)
    struct IsotropicBounds : Bounds {
        IsotropicBounds() {
            // Default Bounds
            // Upper: Upper bounds should be based on base material's moduli.
            //        Poisson ratio can't be greater than or equal 0.5
            //        (at 0.5, 3D lambda becomes Inf)
            // Lower: Young's modulus must be positive and is hard to make
            //        small--this minimum should be set based on homogenization results
            //        Poisson ratio can't be less than -1, and for robustness we
            //        limit it to -0.75
            Bounds::setUpper({ Bounds::Bound(0, 292), Bounds::Bound(1, 0.6) });
            Bounds::setLower({ Bounds::Bound(0, 25),  Bounds::Bound(1, 0.1) });
        }
    };

    Isotropic() {
        // Default Parameters: midway between bounds (if they exist)
        const auto &lb = this->lowerBounds();
        const auto &ub = this->upperBounds();
        if ((lb.size() == 2) && (ub.size() == 2)) {
            for (auto &bd : lb) vars[bd.var]  = 0.5 * bd.value;
            for (auto &bd : ub) vars[bd.var] += 0.5 * bd.value;
        }
        else {
            vars[0] = 50.0;
            vars[1] = 0.3;
        }
    }

    static const std::string &variableName(size_t i) {
        static const std::vector<std::string> names = { "E", "nu" };
        return names.at(i);
    }

    // Used for adjoint method gradient-based optimization
    void getETensorDerivative(size_t p, ETensor &d) const {
        assert(p == 0 || p == 1);
        d.clear();
        Real E = vars[0], nu = vars[1];
        Real dL, dmu;
        if (_N == 2) {
            // 2D Lambda = (nu * E) / (1.0 - nu * nu);
            //    mu = E / (2.0 + 2.0 * nu);
            dL = (p == 0) ? nu / (1 - nu * nu)
                          : E * (1 + nu * nu) / ((1 - nu * nu) * (1 - nu * nu));
        }
        if (_N == 3) {
            // 3D Lambda = (nu * E) / ((1.0 + nu) * (1.0 - 2.0 * nu));
            Real denSqrt = 1 - nu - 2 * nu * nu;
            dL = (p == 0) ? nu / ((1.0 + nu) * (1.0 - 2 * nu)) : E * (1 + 2 * nu * nu) / (denSqrt * denSqrt);
        }

        // 2D and 3D mu: E / (2 (1 + nu))
        dmu = (p == 0) ? 1 / (2 * (1 + nu))
                       : -E / (2 * (1 + nu) * (1 + nu));
        for (size_t i = 0; i < flatLen(_N); ++i) {
            for (size_t j = i; j < _N; ++j)
                d.D(i, j) = dL;
            d.D(i, i) += (i < _N) ? 2 * dmu : dmu;
        }
    }

    void getTensor(ETensor &tensor) const {
        tensor.setIsotropic(vars[0], vars[1]);
    }

    // Ceres-compatible cost function to fit Young's modulus, Y, and Poisson
    // ratio, nu, to best achieve:
    //      s ~= E(Y, nu) : e
    // Where Y and nu are Young's modulus and Poisson ratio. In 2D, the
    // condition s = E(Y, nu) : e can be written in a linear form:
    //  [s_00]   [e_00,  s_11][Y ]
    //  [s_11] = [e_11,  s_00][nu]
    //  [s_01]   [e_01, -s_01]
    // We solve this in a least squares sense to get ``optimal'' Y and nu. This
    // is a slightly strange formulation in which the residual is harder to
    // interpret, but it's nice because the optimization variables are Y and nu
    // directly.
    //
    // Other options are to use Lame coefficients, which appear linearly in the
    // stress-strain relationship, or variables 1/Y and nu/Y (which also appear
    // linearly), but then E and nu are nonlinear functions of the variables
    // (and bounds/penalties/etc. on them must be transformed accordingly).
    // In 3D the corresponding condition is:
    //  [s_00]   [e_00,  s_11 + s_22]
    //  [s_11]   [e_11,  s_00 + s_22]
    //  [s_22] = [e_22,  s_00 + s_11][Y ]
    //  [s_12]   [e_12,        -s_12][nu]
    //  [s_02]   [e_02,        -s_02]
    //  [s_01]   [e_01,        -s_01]
    template<class SMatrix>
    struct StressStrainFitCostFunction {
        StressStrainFitCostFunction(const SMatrix &e, const SMatrix &s, Real vol)
            : strain(e), stress(s) {
            if (vol <= 0) throw std::runtime_error("Volume must be positive");
            volSqrt = sqrt(vol);
        }

        template<typename T>
        bool operator()(const T *x, T *e) const {
            // Nonlinear version
            if (_N == 3) {
                e[0] = T(stress[0]) - x[1] * T(stress[1] + stress[2]);
                e[1] = T(stress[1]) - x[1] * T(stress[0] + stress[2]);
                e[2] = T(stress[2]) - x[1] * T(stress[0] + stress[1]);
                e[3] = (T(1) + x[1]) * T(stress[3]);
                e[4] = (T(1) + x[1]) * T(stress[4]);
                e[5] = (T(1) + x[1]) * T(stress[5]);
            }
            else {
                e[0] = T(stress[0]) - x[1] * T(stress[1]);
                e[1] = T(stress[1]) - x[1] * T(stress[0]);
                e[2] = (T(1) + x[1]) * T(stress[2]);
            }
            for (size_t i = 0; i < flatLen(_N); ++i) {
                e[i] /= x[0];
                e[i] -= T(strain[i]);
                if (i >= _N) e[i] *= T(sqrt(2.0));
                e[i] *= T(volSqrt);
            }

            // // Linear version
            // if (_N == 3) {
            //     e[0] = T(strain[0]) * x[0] + T(stress[1] + stress[2]) * x[1];
            //     e[1] = T(strain[1]) * x[0] + T(stress[0] + stress[2]) * x[1];
            //     e[2] = T(strain[2]) * x[0] + T(stress[0] + stress[1]) * x[1];
            //     e[3] = T(strain[3]) * x[0] - T(stress[3]) * x[1];
            //     e[4] = T(strain[4]) * x[0] - T(stress[4]) * x[1];
            //     e[5] = T(strain[5]) * x[0] - T(stress[5]) * x[1];
            // }
            // else {
            //     e[0] = T(strain[0]) * x[0] + T(stress[1]) * x[1];
            //     e[1] = T(strain[1]) * x[0] + T(stress[0]) * x[1];
            //     e[2] = T(strain[2]) * x[0] - T(stress[2]) * x[1];
            // }
            // for (size_t i = 0; i < flatLen(_N); ++i) {
            //     e[i] -= T(stress[i]);
            // }

            return true;
        }

        SMatrix strain, stress;
        Real volSqrt;
    };
private:
    friend Base;
    static IsotropicBounds g_bounds;
};

// Static variable needs to be explicitly defined...
template<size_t _N>
typename Isotropic<_N>::IsotropicBounds Isotropic<_N>::g_bounds;

// Axis-aligned orthotropic material.
// 2D: 4 variables
// Vars 0..1: Young's moduli,
// Var     2: Poisson ratio (YX)
// Var     3: Shear modulus
// 3D: 9 variables
// Vars 0..2: Young's moduli,
// Vars 3..5: Poisson ratios (YX, ZX, ZY)
// Vars 6..8: Shear ratios   (YZ, ZX, XY)
size_t constexpr nOrthotropicVars(size_t n) { return (n == 3) ? 9 : 4; }
template<size_t _N>
struct Orthotropic : public VariableMaterial<_N, Orthotropic, nOrthotropicVars(_N)> {
    static constexpr size_t N = _N;
    typedef ElasticityTensor<Real, _N> ETensor;
    typedef Eigen::Matrix<Real, flatLen(_N), 1> FlattenedSymmetricMatrix;

    typedef VariableMaterial<N, Materials::Orthotropic, nOrthotropicVars(N)> Base;
    using Base::vars;

    // WARNING: bounds are shared by all orthotropic materials! (static)
    struct OrthotropicBounds : Bounds {
        OrthotropicBounds() {
            // Default Bounds
            // Upper: Upper bounds should be based on base material's moduli.
            //        Poisson ratios can't be greater than 0.5
            //        (at 0.5, 3D isotropic lambda becomes Inf, so we avoid it
            //        here too)
            // Lower: Young's and shear moduli must be positive and are hard to make
            //        small--this minimum should be set based on homogenization results
            //        Poisson ratios can't be less than -1, and for robustness we
            //        limit them to -0.75
            if (_N == 3) Base::setUpperBounds({ Bound(3, 0.45), Bound(4, 0.45), Bound(5, 0.45) });
            else         Base::setUpperBounds({ Bound(0,  384), Bound(1,  384), Bound(2, 0.45), Bound(3, 102) });
            if (_N == 3) {
                Base::setLowerBounds({ Bound(0,  0.01), Bound(1,  0.01), Bound(2,  0.01),
                                       Bound(3, -0.75), Bound(4, -0.75), Bound(5, -0.75),
                                       Bound(6,  0.01), Bound(7,  0.01), Bound(8,  0.01) });
            }
            else Base::setLowerBounds({ Bound(0,  18), Bound(1,  18),
                                        Bound(2, 0.0), Bound(3,  2) });
        }
    };

    Orthotropic() {
        // Default Parameters
        if (_N == 3) {
            vars[0] = vars[1] = vars[2] = 1.0;
            vars[3] = vars[4] = vars[5] = 0.3;
            vars[6] = vars[7] = vars[8] = 1 / (2.0 * (1 + 0.3));
        }
        else {
            vars[0] = vars[1] = 200.0;
            vars[2] = 0.3;
            vars[3] = 200.0 / (2.0 * (1 + 0.3));
        }
    }

    static const std::string &variableName(size_t i) {
        if (_N == 3) {
            static const std::vector<std::string> names3D = {
                "E_x", "E_y", "E_z",
                "nu_yx", "nu_zx", "nu_zy",
                "mu_yz", "mu_zx", "mu_xy" };
            return names3D.at(i);
        }
        else {
            static const std::vector<std::string> names2D = {
                "E_x", "E_y", "nu_yx", "mu" };
            return names2D.at(i);
        }
    }

    // Used for adjoint method gradient-based optimization
    void getETensorDerivative(size_t p, ETensor &d) const;

    void getTensor(ETensor &tensor) const {
        if (_N == 3) {
            tensor.setOrthotropic3D(vars[0], vars[1], vars[2],
                                    vars[3], vars[4], vars[5],
                                    vars[6], vars[7], vars[8]);
        }
        else {
            tensor.setOrthotropic2D(vars[0], vars[1], vars[2], vars[3]);
        }
    }

    // Ceres-compatible cost function to fit orthotropic material parameters to
    // best achieve:
    //      e ~= E^(-1)(Y_x, Y_y, ...) : s
    template<class SMatrix>
    struct StressStrainFitCostFunction {
        StressStrainFitCostFunction(const SMatrix &e, const SMatrix &s, Real vol)
            : strain(e), stress(s) {
            if (vol <= 0) throw std::runtime_error("Volume must be positive");
            volSqrt = sqrt(vol);
        }

        template<typename T>
        bool operator()(const T *x, T *e) const {
            if (_N == 3) {
                T D01 =  -x[3] / x[1], // -nu_yx / E_y
                  D02 =  -x[4] / x[2], // -nu_zx / E_z
                  D12 =  -x[5] / x[2]; // -nu_zy / E_z
                e[0] = T(stress[0]) / x[0] + T(stress[1]) *  D01 + T(stress[2]) *  D02;
                e[1] = T(stress[0]) *  D01 + T(stress[1]) / x[1] + T(stress[2]) *  D12;
                e[2] = T(stress[0]) *  D02 + T(stress[1]) *  D12 + T(stress[2]) / x[2];
                e[3] = T(0.5 * stress[3]) / x[6];
                e[4] = T(0.5 * stress[4]) / x[7];
                e[5] = T(0.5 * stress[5]) / x[8];
            }
            else {
                T D01 = -x[2] / x[1]; // -nu_yx / E_y
                e[0] = T(stress[0]) / x[0] + T(stress[1]) *  D01;
                e[1] = T(stress[0]) *  D01 + T(stress[1]) / x[1];
                e[2] = T(0.5 * stress[2]) / x[3];
            }

            for (size_t i = 0; i < flatLen(_N); ++i) {
                e[i] -= T(strain[i]);
                if (i >= _N) e[i] *= T(sqrt(2.0));
                e[i] *= T(volSqrt);
            }

            return true;
        }

        Real volSqrt;
        SMatrix strain, stress;
    };
private:
    friend Base;
    static OrthotropicBounds g_bounds;
};

// Static variable needs to be explicitly defined...
template<size_t _N>
typename Orthotropic<_N>::OrthotropicBounds Orthotropic<_N>::g_bounds;

template<size_t _N>
struct Constant {
    static constexpr size_t N = _N;
    static constexpr size_t numVars = 0;
    typedef ElasticityTensor<Real, _N> ETensor;

    Constant() { m_E.setIsotropic(1.0, 0.3); }
    Constant(const std::string &materialFile) { setFromFile(materialFile); }

    void setFromFile(const std::string &materialFile);
    void setFromJson(const nlohmann::json &config);
#if BOOST_PARSER
    void setFromPTree(const boost::property_tree::ptree &pt);
#endif

    // Used for adjoint method gradient-based optimization
    void getETensorDerivative(size_t /* p */, ETensor &/* d */) const {
        throw std::runtime_error("Constant material can't be optimized\n");
    }

    const ETensor &getTensor()      const { return m_E; }
    void getTensor(ETensor &tensor) const { tensor = m_E; }
    void setTensor(const ETensor &tensor) { m_E = tensor; }

    void setIsotropic(Real E, Real nu) { m_E.setIsotropic(E, nu); }

    nlohmann::json getJson() const;

    // "type": "anisotropic",
    // "dim": 3,
    // "material_matrix": [[C_00, C_01, C02, C03, C04, C05],
    //                     [C_10, C_11, C12, C13, C14, C15],
    //                     [C_20, C_21, C22, C23, C24, C25],
    //                     [C_30, C_31, C32, C33, C34, C35],
    //                     [C_40, C_41, C42, C43, C44, C45],
    //                     [C_50, C_51, C52, C53, C54, C55]]
    friend std::ostream &operator<<(std::ostream &os, const Constant &cmat) {
        os << cmat.getJson().dump(4);
        return os;
        // os << "{ \"type\": \"anisotropic\"," << std::endl;
        // os << "\"material_matrix\": [";
        // for (size_t i = 0; i < flatLen(N); ++i) {
        //     for (size_t j = 0; j < flatLen(N); ++j) {
        //         os << (j ? ", " : "[") << cmat.m_E.D(i, j);
        //     }
        //     os << (i == flatLen(N) - 1 ? "]]" : "],") << std::endl;
        // }
        // os << "}";
        // return os;
    }

private:
    ETensor m_E;
};

} // Materials

#endif /* end of include guard: MATERIAL_HH */


#include "../include/franka_emulator/model.h"
#include <Eigen/Geometry>
#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/crba.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#ifndef _GNU_SOURCE
    #define _GNU_SOURCE
#endif
#include <link.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>

FRANKA_EMULATOR::Frame FRANKA_EMULATOR::operator++(Frame& frame, int /* dummy */) noexcept
{
    Frame result = frame;
    frame = static_cast<Frame>(static_cast<unsigned int>(result) + 1);
    return result;
}

FRANKA_EMULATOR::Model::Model(FRANKA_EMULATOR::Network&)
{
    //Searching for urdf file
    struct LibrarySearchCallbackData
    {
        bool found              = false;
        std::string directory   = "";
    } library_search_callback_data;
    auto library_search_callback = [](struct dl_phdr_info *info, size_t size, void *data) -> int
    {
        const char *library_name = "libfranka_emulator_model.so";
        if (strstr(info->dlpi_name, library_name) != nullptr)
        {
            LibrarySearchCallbackData *library_search_callback_data = (LibrarySearchCallbackData*)data;
            library_search_callback_data->found = true;
            library_search_callback_data->directory.assign(info->dlpi_name);
            library_search_callback_data->directory.erase(library_search_callback_data->directory.find(library_name), strlen(library_name));
        }
        return 0;
    };
    dl_iterate_phdr(library_search_callback, &library_search_callback_data);
    if (!library_search_callback_data.found) throw std::runtime_error("franka_emulator::Model::Model: Could not find library path");
    struct stat model_stat;
    if (stat((library_search_callback_data.directory + "model/franka_emulator.urdf").c_str(), &model_stat) == 0)
        pinocchio::urdf::buildModel(library_search_callback_data.directory + "model/franka_emulator.urdf", _model);
    if (stat((library_search_callback_data.directory + "../model/franka_emulator.urdf").c_str(), &model_stat) == 0)
        pinocchio::urdf::buildModel(library_search_callback_data.directory + "../model/franka_emulator.urdf", _model);
    else if (stat((library_search_callback_data.directory + "../share/franka_emulator/model/franka_emulator.urdf").c_str(), &model_stat) == 0)
        pinocchio::urdf::buildModel(library_search_callback_data.directory + "../share/franka_emulator/model/franka_emulator.urdf", _model);
    else throw std::runtime_error("franka_emulator::Model::Model: Could not find model file");

    //Other initialization
    _data = pinocchio::Data(_model);
    for (size_t joint = 0; joint < 8; joint++)
    {
        std::string name = "panda_joint" + std::to_string(joint + 1);
        if (!_model.existFrame(name)) throw std::runtime_error("Joint frame not found");
        _joint_frame_id[joint] = _model.getFrameId(name);
    }
    for (size_t link = 0; link < 8; link++)
    {
        std::string name = "panda_link" + std::to_string(link + 1);
        if (!_model.existFrame(name)) throw std::runtime_error("Link frame not found");
        _link_frame_id[link] = _model.getFrameId(name);
    }
}

FRANKA_EMULATOR::Model::Model(Model &&other) noexcept
{
    _model = other._model;
    _data = other._data;
    for (size_t i = 0; i < 8; i++) _joint_frame_id[i] = other._joint_frame_id[i];
    for (size_t i = 0; i < 8; i++) _link_frame_id[i] = other._link_frame_id[i];
}

FRANKA_EMULATOR::Model& FRANKA_EMULATOR::Model::operator=(Model&&) noexcept
{
    return *this;
}

FRANKA_EMULATOR::Model::~Model() noexcept
{}

std::array<double, 16> FRANKA_EMULATOR::Model::pose(Frame frame, const FRANKA_EMULATOR::RobotState& robot_state) const
{
    return pose(frame, robot_state.q, robot_state.F_T_EE, robot_state.EE_T_K);
}

std::array<double, 16> FRANKA_EMULATOR::Model::pose(
    Frame frame,
    const std::array<double, 7>& q,
    const std::array<double, 16>& F_T_EE,
    const std::array<double, 16>& EE_T_K) const
{

    pinocchio::forwardKinematics(_model, _data, Eigen::Matrix<double, 7, 1>::Map(&q[0]));
    pinocchio::SE3 placement;
    if (static_cast<size_t>(frame) >= static_cast<size_t>(Frame::kJoint1) && static_cast<size_t>(frame) <= static_cast<size_t>(Frame::kJoint7))
    {
        placement = _data.oMi[static_cast<size_t>(frame) + 1];
    }
    else if (frame == Frame::kFlange || frame == Frame::kEndEffector || frame == Frame::kStiffness)
    {
        placement = pinocchio::updateFramePlacement(_model, _data, _joint_frame_id[7]);
        if (frame == Frame::kEndEffector || frame == Frame::kStiffness) placement = placement.act_impl(pinocchio::SE3(Eigen::Matrix4d::Map(&F_T_EE[0])));
        if (frame == Frame::kStiffness) placement = placement.act_impl(pinocchio::SE3(Eigen::Matrix4d::Map(&EE_T_K[0])));
    }
    else throw std::runtime_error("franka_emulator::Model::pose: Invalid frame");

    Eigen::Affine3d transform;
    transform.linear() = placement.rotation_impl();
    transform.translation() = placement.translation_impl();
    std::array<double, 16> result;
    Eigen::Matrix4d::Map(&result[0]) = transform.matrix();
    return result;
}

std::array<double, 42> FRANKA_EMULATOR::Model::bodyJacobian(Frame frame, const FRANKA_EMULATOR::RobotState& robot_state) const
{
    return bodyJacobian(frame, robot_state.q, robot_state.F_T_EE, robot_state.EE_T_K);
}

std::array<double, 42> FRANKA_EMULATOR::Model::bodyJacobian(
    Frame frame,
    const std::array<double, 7>& q,
    const std::array<double, 16>& F_T_EE,
    const std::array<double, 16>& EE_T_K) const
{
    Eigen::Matrix<double, 6, 7> result_matrix = Eigen::Matrix<double, 6, 7>::Zero();
    if (static_cast<size_t>(frame) >= static_cast<size_t>(Frame::kJoint1) && static_cast<size_t>(frame) <= static_cast<size_t>(Frame::kJoint7))
    {
        pinocchio::computeJointJacobian(_model, _data, Eigen::Matrix<double, 7, 1>::Map(&q[0]), static_cast<size_t>(frame) + 1, result_matrix);
    }
    else if (frame == Frame::kFlange)
    {
        pinocchio::computeFrameJacobian(_model, _data, Eigen::Matrix<double, 7, 1>::Map(&q[0]), _joint_frame_id[7], result_matrix);
    }
    else if (frame == Frame::kEndEffector || frame == Frame::kStiffness)
    {
        pinocchio::SE3 placement = pinocchio::updateFramePlacement(_model, _data, _joint_frame_id[7]).act_impl(pinocchio::SE3(Eigen::Matrix4d::Map(&F_T_EE[0])));
        if (frame == Frame::kStiffness) placement = placement.act_impl(pinocchio::SE3(Eigen::Matrix4d::Map(&EE_T_K[0])));
        for (size_t i = 0; i < 7; i++)
        {
            result_matrix.block<3, 1>(0, i) = placement.rotation_impl().transpose() * (_data.oMi[i+1].rotation_impl().col(2).cross(placement.translation_impl() - _data.oMi[1+i].translation_impl()));
            result_matrix.block<3, 1>(3, i) = placement.rotation_impl().transpose() * _data.oMi[i+1].rotation_impl().col(2);
        }
    }
    else throw std::runtime_error("franka_emulator::Model::bodyJacobian: Invalid frame");

    std::array<double, 42> result;
    Eigen::Matrix<double, 6, 7>::Map(&result[0]) = result_matrix;
    return result;
}

std::array<double, 42> FRANKA_EMULATOR::Model::zeroJacobian(Frame frame, const FRANKA_EMULATOR::RobotState& robot_state) const
{
    return zeroJacobian(frame, robot_state.q, robot_state.F_T_EE, robot_state.EE_T_K);
}

std::array<double, 42> FRANKA_EMULATOR::Model::zeroJacobian(
    Frame frame,
    const std::array<double, 7>& q,
    const std::array<double, 16>& F_T_EE,
    const std::array<double, 16>& EE_T_K) const
{
    Eigen::Matrix<double, 6, 7> result_matrix = Eigen::Matrix<double, 6, 7>::Zero();
    if (static_cast<size_t>(frame) >= static_cast<size_t>(Frame::kJoint1) && static_cast<size_t>(frame) <= static_cast<size_t>(Frame::kJoint7))
    {
        pinocchio::forwardKinematics(_model, _data, Eigen::Matrix<double, 7, 1>::Map(&q[0]));
        pinocchio::computeJointJacobian(_model, _data, Eigen::Matrix<double, 7, 1>::Map(&q[0]), static_cast<size_t>(frame) + 1, result_matrix);
        result_matrix.block<3, 7>(0,0) = _data.oMi[static_cast<size_t>(frame) + 1].rotation_impl() * result_matrix.block<3, 7>(0,0);
        result_matrix.block<3, 7>(3,0) = _data.oMi[static_cast<size_t>(frame) + 1].rotation_impl() * result_matrix.block<3, 7>(3,0);
    }
    else if (frame == Frame::kFlange)
    {
        pinocchio::SE3 placement = pinocchio::updateFramePlacement(_model, _data, _joint_frame_id[7]);
        pinocchio::computeFrameJacobian(_model, _data, Eigen::Matrix<double, 7, 1>::Map(&q[0]), _joint_frame_id[7], result_matrix);
        result_matrix.block<3, 7>(0,0) = placement.rotation_impl() * result_matrix.block<3, 7>(0,0);
        result_matrix.block<3, 7>(3,0) = placement.rotation_impl() * result_matrix.block<3, 7>(3,0);
    }
    else if (frame == Frame::kEndEffector || frame == Frame::kStiffness)
    {
        pinocchio::SE3 placement = pinocchio::updateFramePlacement(_model, _data, _joint_frame_id[7]).act_impl(pinocchio::SE3(Eigen::Matrix4d::Map(&F_T_EE[0])));
        if (frame == Frame::kStiffness) placement = placement.act_impl(pinocchio::SE3(Eigen::Matrix4d::Map(&EE_T_K[0])));
        for (size_t i = 0; i < 7; i++)
        {
            result_matrix.block<3, 1>(0, i) = _data.oMi[i+1].rotation_impl().col(2).cross(placement.translation_impl() - _data.oMi[i+1].translation_impl());
            result_matrix.block<3, 1>(3, i) = _data.oMi[i+1].rotation_impl().col(2);
        }
    }
    else throw std::runtime_error("franka_emulator::Model::zeroJacobian: Invalid frame");

    std::array<double, 42> result;
    Eigen::Matrix<double, 6, 7>::Map(&result[0]) = result_matrix;
    return result;
}

std::array<double, 49> FRANKA_EMULATOR::Model::mass(const FRANKA_EMULATOR::RobotState& robot_state) const noexcept
{
    return mass(robot_state.q, robot_state.I_total, robot_state.m_total, robot_state.F_x_Ctotal);
}

std::array<double, 49> FRANKA_EMULATOR::Model::mass(
    const std::array<double, 7>& q,
    const std::array<double, 9>& I_total,
    double m_total,
    const std::array<double, 3>& F_x_Ctotal) const noexcept
{
    pinocchio::Inertia initial_end_inertia = _model.inertias[7];
    _model.inertias[7].mass() += m_total;
    _model.inertias[7].inertia().data()[0] += I_total[0];                    //xx
    _model.inertias[7].inertia().data()[1] += (I_total[1] + I_total[3]) / 2; //xy
    _model.inertias[7].inertia().data()[2] += I_total[4];                    //yy
    _model.inertias[7].inertia().data()[3] += (I_total[2] + I_total[6]) / 2; //xz
    _model.inertias[7].inertia().data()[4] += (I_total[5] + I_total[7]) / 2; //yz
    _model.inertias[7].inertia().data()[5] += I_total[8];                    //zz
    Eigen::Matrix<double, 7, 7> result_matrix = pinocchio::crba(_model, _data, Eigen::Matrix<double, 7, 1>::Map(&q[0]));
    _model.inertias[7] = initial_end_inertia;
    result_matrix.triangularView<Eigen::StrictlyLower>() = result_matrix.transpose().triangularView<Eigen::StrictlyLower>();
    std::array<double, 49> result;
    Eigen::Matrix<double, 7, 7>::Map(&result[0]) = result_matrix;
    return result;
}

std::array<double, 7> FRANKA_EMULATOR::Model::coriolis(const FRANKA_EMULATOR::RobotState& robot_state) const noexcept
{
    return coriolis(robot_state.q, robot_state.dq, robot_state.I_total, robot_state.m_total, robot_state.F_x_Ctotal);
}

std::array<double, 7> FRANKA_EMULATOR::Model::coriolis(
    const std::array<double, 7>& q,
    const std::array<double, 7>& dq,
    const std::array<double, 9>& I_total,
    double m_total,
    const std::array<double, 3>& F_x_Ctotal) const noexcept
{
    pinocchio::Inertia initial_end_inertia = _model.inertias[7];
    _model.inertias[7].mass() += m_total;
    _model.inertias[7].inertia().data()[0] += I_total[0];                    //xx
    _model.inertias[7].inertia().data()[1] += (I_total[1] + I_total[3]) / 2; //xy
    _model.inertias[7].inertia().data()[2] += I_total[4];                    //yy
    _model.inertias[7].inertia().data()[3] += (I_total[2] + I_total[6]) / 2; //xz
    _model.inertias[7].inertia().data()[4] += (I_total[5] + I_total[7]) / 2; //yz
    _model.inertias[7].inertia().data()[5] += I_total[8];                    //zz
    Eigen::Matrix<double, 7, 7> result_matrix = pinocchio::computeCoriolisMatrix(_model, _data, Eigen::Matrix<double, 7, 1>::Map(&q[0]), Eigen::Matrix<double, 7, 1>::Map(&dq[0]));
    _model.inertias[7] = initial_end_inertia;
    std::array<double, 7> result;
    Eigen::Matrix<double, 7, 1>::Map(&result[0]) = result_matrix * Eigen::Matrix<double, 7, 1>::Map(&dq[0]);
    return result;
}

std::array<double, 7> FRANKA_EMULATOR::Model::gravity(
    const std::array<double, 7>& q,
    double m_total,
    const std::array<double, 3>& F_x_Ctotal,
    const std::array<double, 3>& gravity_earth) const noexcept
{
    _model.gravity.linear_impl() = Eigen::Matrix<double, 3, 1>::Map(&gravity_earth[0]);
    double initial_end_mass = _model.inertias[7].mass();
    _model.inertias[7].mass() += m_total;
    std::array<double, 7> result;
    Eigen::Matrix<double, 7, 1>::Map(&result[0]) = pinocchio::computeGeneralizedGravity(_model, _data, Eigen::Matrix<double, 7, 1>::Map(&q[0]));
    _model.inertias[7].mass() = initial_end_mass;
    return result;
}

std::array<double, 7> FRANKA_EMULATOR::Model::gravity(
    const FRANKA_EMULATOR::RobotState& robot_state,
    const std::array<double, 3>& gravity_earth) const noexcept
{
    return gravity(robot_state.q, robot_state.m_total, robot_state.F_x_Ctotal, gravity_earth);
}


#include <MeshFEM/MeshIO.hh>
#include <MeshFEM/MSHFieldWriter.hh>
#include <MeshFEM/LinearElasticity.hh>
#include <MeshFEM/Materials.hh>
#include <MeshFEM/PeriodicHomogenization.hh>
#include <MeshFEM/OrthotropicHomogenization.hh>
#include <MeshFEM/GlobalBenchmark.hh>
#include <MeshFEM/TensorProjection.hh>
#include <vector>

#include <queue>
#include <iostream>
#include <iomanip>
#include <memory>
#include <cmath>

#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>

namespace po = boost::program_options;
using namespace std;
using namespace PeriodicHomogenization;

[[ noreturn ]] void usage(int exitVal, const po::options_description &visible_opts) {
    cout << "Usage: OpenLinkage output_name [options] mesh" << endl;
    cout << visible_opts << endl;
    exit(exitVal);
}

po::variables_map parseCmdLine(int argc, const char *argv[])
{
    po::options_description hidden_opts("Hidden Arguments");
    hidden_opts.add_options()
        ("name",       po::value<string>(),                     "name of experiment")
        ("mesh",       po::value<string>(),                     "input mesh")
        ;
    po::positional_options_description p;
    p.add("name",                1);
    p.add("mesh",                1);

    po::options_description visible_opts;
    visible_opts.add_options()("help", "Produce this help message")
        ("material,m", po::value<string>(),                 "base material")
        ("degree,d",   po::value<int>()->default_value(1), "degree of finite elements")
        ("ignorePeriodicMismatch",                         "Ignore mismatched nodes on the periodic faces (useful for voxel grids)")
        ("manualPeriodicVertices", po::value<string>(),    "Manually specify identified periodic vertices using a hacky file format (see PeriodicCondition constructor)")
        ("orthotropicCell,O",                              "Analyze the orthotropic symmetry base cell only")
        ("openingSpeed,s", po::value<Real>()->default_value(0.01), "Opening step length")
        ("numSteps,n", po::value<size_t>()->default_value(20), "Number of opening iterations to run")
        ("outputFreq", po::value<size_t>()->default_value(100), "How many iterations between output frames")
        ;

    po::options_description cli_opts;
    cli_opts.add(visible_opts).add(hidden_opts);

    po::variables_map vm;
    try {
        po::store(po::command_line_parser(argc, argv).
                  options(cli_opts).positional(p).run(), vm);
        po::notify(vm);
    }
    catch (std::exception &e) {
        cout << "Error: " << e.what() << endl << endl;
        usage(1, visible_opts);
    }

    bool fail = false;
    if (vm.count("mesh") == 0) {
        cout << "Error: must specify input mesh" << endl;
        fail = true;
    }

    int d = vm["degree"].as<int>();
    if (d < 1 || d > 2) {
        cout << "Error: FEM Degree must be 1 or 2" << endl;
        fail = true;
    }

    if (fail || vm.count("help"))
        usage(fail, visible_opts);

    return vm;
}

// Sum the values appearing on periodically-identified vertices.
// First sum onto the reduced DoFs, then redistribute.
template<class Sim, class VField>
VField sumIdentifiedValues(const Sim &sim, VField v) {
    const auto &mesh = sim.mesh();
    if (v.domainSize() != mesh.numNodes())
        throw std::runtime_error("Expected per-node vector field");

    VField dofField(sim.numDoFs());
    dofField.clear();
    for (size_t i = 0; i < mesh.numNodes(); ++i)
        dofField(sim.DoF(i)) +=  v(i);
    for (size_t i = 0; i < mesh.numNodes(); ++i)
        v(i) = dofField(sim.DoF(i));

    return v;
}

template<size_t _N>
using HMG = LinearElasticity::HomogenousMaterialGetter<Materials::Constant>::template Getter<_N>;

template<size_t _N, size_t _FEMDegree>
void execute(const po::variables_map &args,
             const vector<MeshIO::IOVertex> &inVertices,
             const vector<MeshIO::IOElement> &inElements) {
    auto &mat = HMG<_N>::material;
    if (args.count("material")) mat.setFromFile(args["material"].as<string>());

    typedef LinearElasticity::Mesh<_N, _FEMDegree, HMG> Mesh;
    typedef LinearElasticity::Simulator<Mesh> Simulator;
    Simulator sim(inElements, inVertices);
    typedef typename Simulator::ETensor ETensor;
    typedef typename Simulator::VField  VField;

    BENCHMARK_START_TIMER_SECTION("Cell Problems");
    std::vector<VField> w_ij;
    std::unique_ptr<PeriodicCondition<_N>> pc;
    ETensor Eh;

    auto &mesh = sim.mesh();

    std::vector<Real> origLengths;
    for (auto he : mesh.halfEdges())
        origLengths.push_back((he.tip().node()->p - he.tail().node()->p).norm());

    std::cout.precision(19);

    auto name = args["name"].as<string>();
    ofstream eigenvalueFile(name + "_minEigenvalue.txt");
    ofstream ellipseFile(name + "_openingStrain_ellipse.txt");

    Real maxRelDiff = 0;
    for (size_t it = 0; it < args["numSteps"].as<size_t>(); ++it) {
        if (args.count("manualPeriodicVertices"))
            pc = Future::make_unique<PeriodicCondition<_N>>(mesh, args["manualPeriodicVertices"].as<string>());
        if (args.count("orthotropicCell") == 0) {
            solveCellProblems(w_ij, sim, 1e-7, args.count("ignorePeriodicMismatch"), std::move(pc));
        }
        else {
            auto systems = PeriodicHomogenization::Orthotropic::solveCellProblems(w_ij, sim, 1e-7);
            cout << systems.size() << endl;
        }

        BENCHMARK_STOP_TIMER_SECTION("Cell Problems");

        BENCHMARK_START_TIMER_SECTION("Compute Tensor");
        // ETensor Eh = homogenizedElasticityTensor(w_ij, sim);
        if (args.count("orthotropicCell") == 0)   Eh = homogenizedElasticityTensorDisplacementForm(w_ij, sim);
        else Eh = PeriodicHomogenization::Orthotropic::homogenizedElasticityTensorDisplacementForm(w_ij, sim);
        BENCHMARK_STOP_TIMER_SECTION("Compute Tensor");

        // cout << setprecision(16);
        // cout << "Homogenized elasticity tensor:" << endl;
        // cout << Eh << endl << endl;

        auto eigs = Eh.computeEigenstrains();
        // Make all eigenstrains positive
        if (eigs.strains(0, 0) < 0) eigs.strains.col(0) *= -1;
        if (eigs.strains(0, 1) < 0) eigs.strains.col(1) *= -1;
        if (eigs.strains(0, 2) < 0) eigs.strains.col(2) *= -1;

        // cout << "Minimum Eh eigenvalue " << eigs.lambdas[0] << " for eigenstrain:\t"
        //      << eigs.strains.col(0).transpose() << endl;

        SymmetricMatrixValue<Real, _N> minEigenstrain(eigs.strains.col(0));
        SymmetricMatrixValue<Real, _N> midEigenstrain(eigs.strains.col(1));
        SymmetricMatrixValue<Real, _N> maxEigenstrain(eigs.strains.col(2));
        auto openingStrain = minEigenstrain;

        eigenvalueFile << eigs.lambdas[0] << std::endl;

        auto bbox = mesh.boundingBox();
        VectorND<_N> center = bbox.center();
        VField cstrainDisp(mesh.numNodes());
        for (auto n : mesh.nodes())
            cstrainDisp(n.index()) = openingStrain.contract(n->p - center);

        // Remove rigid translation of fluctuation displacements relative to the
        // base cell (i.e. try to keep the fluctuation-displaced microstructure
        // "within" the base cell):
        // The no-rigid-motion constraint on fluctuation displacements
        // ensures the microstructure's center of mass doesn't move, but
        // this is not what we need. Instead, we need to ensure vertices on
        // periodic boundary do not move off the boundary. We enforce this in an
        // average sense for each cell face by translating so that the
        // corresponding displacement component's average over all vertices on
        // the face is zero.
        for (auto &w : w_ij) {
            VectorND<_N> translation(VectorND<_N>::Zero());
            vector<int> numAveraged(_N);

            for (size_t bni = 0; bni < mesh.numBoundaryNodes(); ++bni) {
                auto n = mesh.boundaryNode(bni).volumeNode();
                for (size_t d = 0; d < _N; ++d) {
                    if (std::abs(n->p[d] - bbox.minCorner[d]) < 1e-9) {
                        translation[d] += w(n.index())[d];
                        ++numAveraged[d];
                    }
                }
            }
            for (size_t d = 0; d < _N; ++d)
                translation[d] /= numAveraged[d];
            for (size_t n = 0; n < w.domainSize(); ++n)
                w(n) -= translation;
        }

        for (size_t i = 0; i < w_ij.size(); ++i) {
            VField tmp(w_ij[i]);
            tmp *= (((i < _N) ? 1.0 : 2.0) * openingStrain[i]);
            cstrainDisp += tmp;
        }

        VField descentStep = cstrainDisp;
        descentStep.maxColumnNormalize();
        descentStep *= args["openingSpeed"].as<Real>();
        std::vector<MeshIO::IOVertex> pts(mesh.numVertices());
        for (auto v : mesh.vertices()) {
            pts[v.index()].point  = padTo3D(v.node()->p);
            pts[v.index()].point += padTo3D(PointND<_N>(descentStep(v.index())));
        }

        sim.updateMeshNodePositions(pts);

        if ((it % args["outputFreq"].as<size_t>()) == 0) {
            MSHFieldWriter writer(name + "open_it_" + std::to_string(it) + ".msh", sim.mesh());
            writer.addField("opening direction", descentStep, DomainType::PER_NODE);

            auto principalStrains = openingStrain.eigenvalueScaledEigenvectors();
            Real theta = -atan2(principalStrains(1, 0), principalStrains(0, 0));
            Real w = 100 * principalStrains.col(0).norm();
            Real h = 100 * principalStrains.col(1).norm();
            ellipseFile << "push graphic-context translate 100,100 rotate " << 180 * theta / M_PI << " fill purple stroke black "
                        << "ellipse 0,0 " << w << ',' << h << " 0,360 pop graphic-context" << std::endl;
        }

        for (auto he : mesh.halfEdges()) {
            Real len = (he.tip() .node()->p -
                        he.tail().node()->p).norm();
            Real ol = origLengths.at(he.index());
            maxRelDiff = std::max(maxRelDiff, std::abs(len - ol) / ol);
        }

    }
    std::cout << "Maximum relative edge length change: " << maxRelDiff << std::endl;
    MSHFieldWriter writer("opened.msh", sim.mesh());

    BENCHMARK_REPORT();
}

////////////////////////////////////////////////////////////////////////////////
/*! Program entry point
//  @param[in]  argc    Number of arguments
//  @param[in]  argv    Argument strings
//  @return     status  (0 on success)
*///////////////////////////////////////////////////////////////////////////////
int main(int argc, const char *argv[])
{
    po::variables_map args = parseCmdLine(argc, argv);

    vector<MeshIO::IOVertex>  inVertices;
    vector<MeshIO::IOElement> inElements;
    string meshPath = args["mesh"].as<string>();
    auto type = load(meshPath, inVertices, inElements, MeshIO::FMT_GUESS,
                     MeshIO::MESH_GUESS);

    // Infer dimension from mesh type.
    if (type != MeshIO::MESH_TRI) throw std::runtime_error("Only support triangle meshes");

    // Look up and run appropriate homogenizer instantiation.
    int deg = args["degree"].as<int>();
    auto exec = ((deg == 2) ? execute<2, 2> : execute<2, 1>);

    exec(args, inVertices, inElements);

    return 0;
}


////////////////////////////////////////////
/////A* Algorithm (最優先探索アルゴリズム)///////
////////////////////////////////////////////
//#define debug//★

#ifdef _DEBUG
#include <boost/timer/timer.hpp>
#endif

#include "slide_algorithm/algorithm_2.hpp"
#include <iostream>

//コンストラクタ
heap::heap(){
}
//デストラクタ
heap::~heap(){
}

void heap::setup(const int *in_y, const int *in_x){
	y = *in_y;
	x = *in_x;
	yx = y*x;
	//配列のリサイズ☆おおよそ
	cost.resize(1000000);
	history.resize(1000000);
	history_limit.resize(1000000);
	table.resize(1000000, std::vector<int>(yx));
	heaptable.resize(1000000);
	LIST_OC.resize(1000000);
}
void heap::pop(int *in_cost, std::vector<int> &in_table, std::vector<int> &in_history, int *in_history_limit){

	int i;
	int me;
	int top = pos;

	if (size > 1000000 * sizemaxcount - 1){
		sizemaxcount++;
		cost.resize(1000000 * sizemaxcount);
		history.resize(1000000 * sizemaxcount);
		history_limit.resize(1000000 * sizemaxcount);
		table.resize(1000000 * sizemaxcount, std::vector<int>(yx));
		heaptable.resize(1000000 * sizemaxcount);
		LIST_OC.resize(1000000 * sizemaxcount);
		std::cout << ">< heap.cpp vector pass1,000,000" << std::endl;
	}
	//重複確認2
	bool ok = true;
	int sizepos;
	auto match_pos = NODE_.find(NODE{in_table,0});//TEの第二引数は関係ない
	if (match_pos != NODE_.end()){ //テーブルが一致した
		sizepos = match_pos->pos;
		ok = false;
		if (*in_cost < cost[sizepos]){
			//このノードがiのテーブルよりコストが低い
			//ラベルつけてあとで消したほうがいいかも？←その必要はなさそう
			if (LIST_OC[sizepos] == true){
				//このノードと同じテーブルがOpenリストに含まれている
				//◆書き換え
				cost[sizepos] = *in_cost;
				history_limit[sizepos] = *in_history_limit;
				history[sizepos].resize(history_limit[sizepos]);
				for (int m = 0; m < history_limit[sizepos]; m++){
					history[sizepos][m] = in_history[m];
				}
			}
			else{
				//このノードと同じテーブルがCloseリストに含まれている
				//◆書き換え
				LIST_OC[sizepos] = true;
				cost[sizepos] = *in_cost;
				history_limit[sizepos] = *in_history_limit;
				history[sizepos].resize(history_limit[sizepos]);
				for (int m = 0; m < history_limit[sizepos]; m++){
					history[sizepos][m] = in_history[m];
				}
			}
		}
	}
	if (ok == true){
		//コスト挿入
		cost[size] = *in_cost;

		//履歴挿入
		history[size].resize(*in_history_limit);
		for (i = 0; i < *in_history_limit; i++){
			history[size][i] = in_history[i];
		}
		history_limit[size] = *in_history_limit;

		//テーブル挿入
		for (i = 0; i < yx; i++){
			table[size][i] = in_table[i];
		}

		//NODEに挿入
		NODE_.insert(NODE(table[size], size));

		//OPEN_LIST挿入
		LIST_OC[size] = true;

		//ヒープ配列挿入
		while (1){
			me = top;
			top = (top - 1) / 2;
			if (me == top){
				break;
			}
			else if (cost[heaptable[top]] > *in_cost){
				heaptable[me] = heaptable[top];
			}
			else{
				break;
			}
		}
		heaptable[me] = size;

		pos++;
		size++;
	}
}

void heap::push(int *out_cost, std::vector<int> &out_table, std::vector<int> &out_history, int *out_history_limit){
	const int out = heaptable[0];
	int i = 0;
	int me = 0;
	int bottom = 0;

	pos--;

	//OPENからCLOSE_LISTに移動
	LIST_OC[out] = false;

	//コスト挿入
	*out_cost = cost[out];

	//履歴挿入
	i = 0;
	*out_history_limit = history_limit[out];
	while (i < *out_history_limit){
		out_history[i] = history[out][i];
		i++;
	}

	//テーブル挿入
	for (i = 0; i < yx; i++){
		out_table[i] = table[out][i];
	}

	//ヒープ配列挿入
	while (1){
		me = bottom;
		bottom = (bottom * 2);
		if (pos>bottom + 2){
			if (cost[heaptable[bottom + 1]] < cost[heaptable[bottom + 2]]){
				if (cost[heaptable[pos]] > cost[heaptable[bottom + 1]]){
					bottom += 1;
					heaptable[me] = heaptable[bottom];
				}
				else{
					break;
				}
			}
			else{
				if (cost[heaptable[pos]] > cost[heaptable[bottom + 2]]){
					bottom += 2;
					heaptable[me] = heaptable[bottom];
				}
				else{
					break;
				}
			}
		}
		else if (pos>bottom + 1){
			if (cost[heaptable[pos]] > cost[heaptable[bottom + 1]]){
				bottom += 1;
				heaptable[me] = heaptable[bottom];
			}
			else{
				break;
			}
		}
		else{
			break;
		}
	}

	heaptable[me] = heaptable[pos];
	heaptable.erase(heaptable.end() - 1);
}

void heap::end(){
	std::vector<int>().swap(cost);
	std::vector<int>().swap(heaptable);
	std::vector<bool>().swap(LIST_OC);
	std::vector<int>().swap(cost);
	std::vector<std::vector<int>>().swap(table);
	std::vector<std::vector<int>>().swap(history);
	std::vector<int>().swap(history_limit);
}
//コンストラクタ
algorithm_2::algorithm_2()
{
}

//デストラクタ
algorithm_2::~algorithm_2()
{
}

//初期化
void algorithm_2::reset(question_data const& data)
{
	// データのクローン
	data_ = data.clone();

	// 幅と高さ
	size_y = data_->size.second;
	size_x = data_->size.first;
	size = size_y * size_x;

	// コストとレート
	cost_s = data_->cost_select;
	cost_c = data_->cost_change;
	cost_slimit = data_->selectable;

	// テーブル
	//tableによそから貰ってきたマトリクスを整理して挿入
	table.resize(size);
	for (int y = 0; size_y > y; y++){
		for (int x = 0; size_x > x; x++){
			table[y * size_x + x] = data_->block[y][x].y * size_x + data_->block[y][x].x;
		}
	}
	//初期化
	harray.setup(&size_y, &size_x);

	history.resize(10000);
	keiro.resize(1024);
	sub_history.resize(10000);
	root1.resize(1024);
	root2.resize(1024);
#ifdef _DEBUG_incost
	std::cout << "\ngoal=";
	std::cin >> goal;
	std::cout << "sentaku=";
	std::cin >> sentaku;
	std::cout << "coukan=";
	std::cin >> coukan;
#endif
}
bool algorithm_2::overlimitcheck(){
	if (OVER_LIMIT == true){
		return false;
	}
	else{
		return true;
	}
}

auto algorithm_2::get() -> boost::optional<return_type>
{
	int i, count;
	std::cout << "algorythm_2 start" << std::endl;
	std::cout << "size_y=" << size_y << ",size_x=" << size_x << std::endl;
#ifdef _DEBUG
	for (int y = 0; size_y > y; y++){
		for (int x = 0; size_x > x; x++){
			std::cout << std::setw(3) << table[y * size_x + x] << ",";
		}
		std::cout << std::endl;
	}
	//パズルスタート
	std::cout << "start" << std::endl;
	boost::timer::cpu_timer timer; // 時間計測を開始
#endif

	while (1){
		//探索
		algorithm_2::prescanning();
		//取得
		harray.push(&cost, table, history, &history_limit);
		//完成判定
		count = 0;
		for (i = 0; i<size; i++){
			if (table[i] == i) count++;
		}
		if (count == size){
			std::cout << "algorithm_2 finish" << std::endl;
			int ANSWER_S = 0, ANSWER_C = 0;
			for (int i = 0; i < history_limit; i++){
				if (history[i] < 16){
					ANSWER_S += 1;
				}
				else{
					ANSWER_C += 1;
				}
			}
			ANSWER_S /= 2;
			if (ANSWER_S > cost_slimit){
				//最大選択回数を超えた
				OVER_LIMIT = true;
			}
			else{
				for (int i = 0; i < history_limit; i++){
					switch (history[i]){
					case 16:
					case 20:
						std::cout << ",U";
						break;
					case 17:
					case 21:
						std::cout << ",R";
						break;
					case 18:
					case 22:
						std::cout << ",D";
						break;
					case 19:
					case 23:
						std::cout << ",L";
						break;
					default:
						std::cout << "," << history[i];
						break;
					}

				}
			}
			std::cout << std::endl;
            std::cout << "選択コスト=" << cost_s << ",交換コスト=" << cost_c << std::endl;
			std::cout << "cost=" << ANSWER_S * cost_s + ANSWER_C * cost_c << " S: " << ANSWER_S << " C: " << ANSWER_C << std::endl;
#ifdef _DEBUG
			//完成☆後で消したりなんたり
			std::string result = timer.format();
			std::cout << "end" << std::endl;
			std::cout << "処理時間:" << result << std::endl;
			for (int i = 0; i < history_limit; i++){
				switch (history[i]){
				case 16:
				case 20:
					std::cout << ",U";
					break;
				case 17:
				case 21:
					std::cout << ",R";
					break;
				case 18:
				case 22:
					std::cout << ",D";
					break;
				case 19:
				case 23:
					std::cout << ",L";
					break;
				default:
					std::cout << "," << history[i];
					break;
				}
			}
			std::cout << std::endl;
			int _DEBUG_S = 0, _DEBUG_C = 0;
			for (int i = 0; i < history_limit; i++){
				if (history[i] < 16){
					_DEBUG_S += 1;
				}
				else{
					_DEBUG_C += 1;
				}
			}
			_DEBUG_S /= 2;
			std::cout << "選択コスト=" << cost_s << ",交換コスト=" << cost_c << std::endl;
			std::cout << "cost=" << _DEBUG_S * cost_s + _DEBUG_C * cost_c << " S: " << _DEBUG_S << " C: " << _DEBUG_C << std::endl;
#endif
			harray.end();
			//解をanswer_typeにして返す
			answer_type answerlist;
			point_type position;
			std::ostringstream stream;
			int pos = 0;
			for (int count = 0; pos < history_limit; count++){
				position.y = history[pos];
				pos++;
				position.x = history[pos];
				pos++;
				while (pos < history_limit && 16 <= history[pos]){
					switch (history[pos]){
					case 16:
					case 20:
						stream << "U";
						break;
					case 17:
					case 21:
						stream << "R";
						break;
					case 18:
					case 22:
						stream << "D";
						break;
					case 19:
					case 23:
						stream << "L";
						break;
					}
					pos++;
				}
				answerlist.list.push_back({ position, stream.str() });
				stream.str("");
			}
			return answerlist;
		}
	}
}

//走査準備(全てのマスを一つづつscaningで調べる)
void algorithm_2::prescanning(){
	for (int y = 0; y < size_y; y++){
		for (int x = 0; x < size_x; x++){
			keiro[0] = y;
			keiro[1] = x;
			keiro_count = 2;
			scanning(y, x, y, x, -1);
		}
	}
}

//走査
void algorithm_2::scanning(int y, int x, int y_before, int x_before, int URDL){
	int i, buff;
	bool feel, feeled;

	//最初だけ上下左右を通りあとは再帰的に来て保存する（ちょっと無駄だけど直す時間がなかった）

	if ((y == y_before && x == x_before)){
		//進む(上右下左の順)
		//上
		if (y > 0 && URDL != 2){
			if (table[y * size_x + x] / size_x < y){
				feel = true;
			}
			else{
				feel = false;
			}
			if (table[(y - 1) * size_x + x] / size_x > y - 1)
			{
				feeled = true;
			}
			else{
				feeled = false;
			}
			if (feel == true/* || feeled==true*/){
				if (feel == true && feeled == true){
					keiro[keiro_count] = 16;
				}
				else {
					keiro[keiro_count] = 20;
				}
				keiro_count += 1;
				//交換
				buff = table[y * size_x + x];
				table[y * size_x + x] = table[(y - 1) * size_x + x];
				table[(y - 1) * size_x + x] = buff;
				scanning(y - 1, x, y, x, 0);
			}
		}
		//右
		if (x < size_x - 1 && URDL != 3){
			if (table[y * size_x + x] % size_x > x){
				feel = true;
			}
			else{
				feel = false;
			}
			if (table[y * size_x + (x + 1)] % size_x < x + 1)
			{
				feeled = true;
			}
			else{
				feeled = false;
			}
			if (feel == true/* || feeled==true*/){
				if (feel == true && feeled == true){
					keiro[keiro_count] = 17;
				}
				else {
					keiro[keiro_count] = 21;
				}
				keiro_count += 1;
				//交換
				buff = table[y * size_x + x];
				table[y * size_x + x] = table[y * size_x + (x + 1)];
				table[y * size_x + (x + 1)] = buff;
				scanning(y, x + 1, y, x, 1);
			}
		}
		//下
		if (y < size_y - 1 && URDL != 0){
			if (table[y * size_x + x] / size_x > y){
				feel = true;
			}
			else{
				feel = false;
			}
			if (table[(y + 1) * size_x + x] / size_x < y + 1)
			{
				feeled = true;
			}
			else{
				feeled = false;
			}
			if (feel == true/* || bbuff2==true*/){
				if (feel == true && feeled == true){
					keiro[keiro_count] = 18;
				}
				else {
					keiro[keiro_count] = 22;
				}
				keiro_count += 1;
				//交換
				buff = table[y * size_x + x];
				table[y * size_x + x] = table[(y + 1) * size_x + x];
				table[(y + 1) * size_x + x] = buff;
				scanning(y + 1, x, y, x, 2);
			}
		}
		//左
		if (x > 0 && URDL != 1){
			if (table[y * size_x + x] % size_x < x){
				feel = true;
			}
			else{
				feel = false;
			}
			if (table[y * size_x + (x - 1)] % size_x > x - 1)
			{
				feeled = true;
			}
			else{
				feeled = false;
			}
			if (feel == true/* || bbuff2==true*/){
				if (feel == true && feeled == true){
					keiro[keiro_count] = 19;
				}
				else {
					keiro[keiro_count] = 23;
				}
				keiro_count += 1;
				//交換
				buff = table[y * size_x + x];
				table[y * size_x + x] = table[y * size_x + (x - 1)];
				table[y * size_x + (x - 1)] = buff;
				scanning(y, x - 1, y, x, 3);
			}
		}
	}
	if ((y != y_before || x != x_before)) //一番最初以外
	{
		if (history_limit > 10000 * sizemaxhistory - 2){
			sizemaxhistory++;
			history.resize(10000 * sizemaxhistory);
			sub_history.resize(10000 * sizemaxhistory);
#ifdef _DEBUG
			std::cout << ">< search.cpp history vector pass10,000" << std::endl;
#endif
		}
		//パターン配列(経路)に保存
		for (i = 0; i < keiro_count; i++){
			history[history_limit + i] = keiro[i];
		}
		history_limit += keiro_count;
		//■■■■shorting■■■■
		sub_history_limit = history_limit;
		for (i = 0; i < sub_history_limit; i++){
			sub_history[i] = history[i];
		}
		shorting();
		//■■■■■■■■■■■■
		//cost = 選択コスト+交換コスト+ゴールまでの距離;
		G = 0;
		S = 0;
		C = 0;
		//ゴールまでの距離
		for (i = 0; i < size; i++){
			G += abs(table[i] / size_x - i / size_x) + abs(table[i] % size_x - i % size_x);
		}
		//交換コストと選択コストも入れる
		for (i = 0; i < sub_history_limit; i++){
			if (sub_history[i] < 16){
				S++;
			}
			else{
				C++;
			}
		}
		G *= goal;
		S *= sentaku;
		C *= coukan;
		cost = G + S + C;

#ifdef test2
		//@@@重要：：ここやらなきゃ←やらなくてもいいかも
		//最大選択回数より大きいならやめる
		if (S / 15 < 3){
			harray.pop(&cost, table, subhistory, &subhistory_limit);
		}
#else
		//登録
		harray.pop(&cost, table, sub_history, &sub_history_limit);
#endif
		history_limit -= keiro_count;
	}
	//全部見終わったらtableを元通りにして返す
	buff = table[y * size_x + x];
	table[y * size_x + x] = table[y_before * size_x + x_before];
	table[y_before * size_x + x_before] = buff;

	//うち経路消すん
	keiro[keiro_count] = 0;
	if (keiro_count>2) keiro_count -= 1;

	y = y_before;
	x = x_before;
	//最初の階層に上がる
}
void algorithm_2::shorting(){
	//経路をまとめて短縮する
	int i;
	int x1_head, y1_head, root1_count, y1_tail, x1_tail;
	int x2_head, y2_head, root2_count, y2_tail, x2_tail;
	int start, count = 0, end;

	while (count + 5<sub_history_limit){

		root1_count = 0;
		root2_count = 0;
		start = count;
		y1_head = sub_history[count];
		count++;
		x1_head = sub_history[count];
		count++;
		while (1){
			if (!(count<sub_history_limit && sub_history[count] >= 16 && sub_history[count] <= 23)) break;
			if (root1_count > 1024 * sizemaxroot1 - 1){
				sizemaxroot1++;
				root1.resize(1024 * sizemaxroot1);
#ifdef _DEBUG
				std::cout << ">< search.cpp root1 vector pass1,024" << std::endl;
#endif
			}
			root1[root1_count] = sub_history[count];
			root1_count++;
			count++;
		}
		if (!(count<sub_history_limit)){
			break;
		}
		end = count;
		y2_head = sub_history[end];
		end++;
		x2_head = sub_history[end];
		end++;
		while (1){
			if (!(end < sub_history_limit && sub_history[end] >= 16 && sub_history[end] <= 23)) break;
			if (root2_count > 1024 * sizemaxroot2 - 1){
				sizemaxroot2++;
				root2.resize(1024 * sizemaxroot2);
#ifdef _DEBUG
				std::cout << ">< search.cpp root2 vector pass10,000" << std::endl;
#endif
			}
			root2[root2_count] = sub_history[end];
			root2_count++;
			end++;
		}
		//get tail
		y1_tail = y1_head;
		x1_tail = x1_head;
		for (i = 0; i < root1_count; i++){
			switch (root1[i]){
			case 16:
				y1_tail -= 1;
				break;
			case 17:
				x1_tail += 1;
				break;
			case 18:
				y1_tail += 1;
				break;
			case 19:
				x1_tail -= 1;
				break;
			case 20:
				y1_tail -= 1;
				break;
			case 21:
				x1_tail += 1;
				break;
			case 22:
				y1_tail += 1;
				break;
			case 23:
				x1_tail -= 1;
				break;
			}
		}
		y2_tail = y2_head;
		x2_tail = x2_head;
		for (i = 0; i < root2_count; i++){
			switch (root2[i]){
			case 16:
				y2_tail -= 1;
				break;
			case 17:
				x2_tail += 1;
				break;
			case 18:
				y2_tail += 1;
				break;
			case 19:
				x2_tail -= 1;
				break;
			case 20:
				y2_tail -= 1;
				break;
			case 21:
				x2_tail += 1;
				break;
			case 22:
				y2_tail += 1;
				break;
			case 23:
				x2_tail -= 1;
				break;
			}
		}
		//Rule1
		if (y1_tail == y2_head && x1_tail == x2_head){
			for (i = 0; i < root2_count; i++){
				sub_history[count] = root2[i];
				count++;
			}
			for (i = 0; i + end < sub_history_limit; i++){
				sub_history[count + i] = sub_history[end + i];
			}
			sub_history_limit = count + i;
			count = 0;
		}
		//Rule2
		else if (y1_tail == y2_tail && x1_tail == x2_tail && root2_count == 1){
			//root2の各方向を反転
			reverse(&root2[0]);
			sub_history[count] = root2[0];
			count++;
			for (i = 0; i + end < sub_history_limit; i++){
				sub_history[count + i] = sub_history[end + i];
			}
			sub_history_limit = count + i;
			count = 0;
		}
		//Rule3
		else if (y1_head == y2_head && x1_head == x2_head && root1_count == 1){
			//root1の各方向を反転
			reverse(&root1[0]);
			sub_history[start] = y1_tail;
			start++;
			sub_history[start] = x1_tail;
			start++;
			sub_history[start] = root1[0];
			start++;
			for (i = 0; i < root2_count; i++){
				sub_history[start] = root2[i];
				start++;
			}
			for (i = 0; i + end < sub_history_limit; i++){
				sub_history[start + i] = sub_history[end + i];
			}
			sub_history_limit = start + i;
			count = 0;
		}
		//Rule4
		else if (y1_head == y2_tail && x1_head == x2_tail && root1_count == 1 && root2_count == 1){
			//root1の各方向を反転
			reverse(&root1[0]);
			//root2の各方向を反転
			reverse(&root2[0]);
			sub_history[start] = y1_tail;
			sub_history[start + 1] = x1_tail;
			sub_history[start + 2] = root1[0];
			sub_history[start + 3] = root2[0];
			for (i = 0; i + end < sub_history_limit; i++){
				sub_history[start + 4 + i] = sub_history[end + i];
			}
			sub_history_limit = start + 4 + i;
			count = 0;
		}
	}
}
void algorithm_2::reverse(int *x){
	switch (*x){
	case 16:
		*x = 18;
		break;
	case 17:
		*x = 19;
		break;
	case 18:
		*x = 16;
		break;
	case 19:
		*x = 17;
		break;
	case 20:
		*x = 22;
		break;
	case 21:
		*x = 23;
		break;
	case 22:
		*x = 20;
		break;
	case 23:
		*x = 21;
		break;
	}
	return;
}


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

#ifndef TCG_HH
#define TCG_HH

#include <numeric>

#include <boost/circular_buffer.hpp>

#include "dune/common/timer.hh"
#include "dune/istl/istlexception.hh"
#include "dune/istl/operators.hh"
#include "dune/istl/preconditioners.hh"
#include "dune/istl/scalarproducts.hh"
#include "dune/istl/solvers.hh"

#include "utilities/geometric_sequence.hh"
#include "utilities/scalar.hh"
#include "utilities/duneInterface.hh"

#include "linalg/apcg.hh"

namespace Kaskade
{
  struct NoRegularization
  {
    template <class X, class Xstar>
    void operator()(const X&, Xstar&){}
  };

  template <class Linearization, class VariableSet>
  struct DoRegularization
  {
    DoRegularization(Linearization const& lin_, typename VariableSet::Descriptions const& description) : lin(lin_), v(description)
    {}

    template <class X, class Xstar>
    void operator()(const X& x, Xstar& r)
    {
      v = x; //
      Bridge::Vector<VariableSet> y(v), z(v);
      y *= 0;
      lin.ddxpy(y,z,0,2,2,3);
      y *= -1;
      v = r; //

      boost::fusion::at_c<0>(v.data) += boost::fusion::at_c<0>(y.get().data);
      boost::fusion::at_c<1>(v.data) += boost::fusion::at_c<1>(y.get().data);

      r = v; //
    }

    private:
      Linearization const& lin;
      VariableSet v;
  };

  /**
   * \ingroup linalgsolution
   * \brief preconditioned conjugate gradient method
   *
   * This implements a preconditioned IterateType::CG iteration for an operator \f$ A: X\to x^* \f$, preconditioned by a
   * preconditioner \f$ B^{-1}: X^* \to X \f$. The termination is based on an estimate of the absolute energy error.
   *
   * The implementation follows Deuflhard/Weiser, Section 5.3.3.
   *
   */
  template<class X, class Xstar, class Regularization=NoRegularization>
  class TCG : public Dune::InverseOperator<X,Xstar> {
  public:
    enum class Result { Converged, Failed, EncounteredNonConvexity };
    /**
     * \brief the real field type corresponding to X::field_type
     */
    typedef typename ScalarTraits<typename GetScalar<X>::type >::Real Real;

    /**
     * \brief Set up conjugate gradient solver with absolute energy error termination criterion.
     *
     * \param verbose
     */
    template <class Int=int, class enable = typename std::enable_if<!std::is_same<Regularization,NoRegularization>::value && std::is_same<Int,int>::value>::type>
    TCG(Dune::LinearOperator<X,Xstar>& op_, Dune::Preconditioner<X,Xstar>& prec_, DualPairing<X,Xstar> const& dp_,
        Regularization& regularization_, double relTol_=1e-3, size_t maxSteps_=100, Int verbose_=0) :
          op(op_), prec(prec_), dp(dp_), regularization(regularization_), relTol(relTol_), absTol(1e-9), maxSteps(maxSteps_), verbose(verbose_)
    {
      // Do we need this in Kaskade7?
      //         dune_static_assert( static_cast<int>(L::category) == static_cast<int>(P::category),
      //                             "L and P must have the same category!");
      //         dune_static_assert( static_cast<int>(L::category) == static_cast<int>(SolverCategory::sequential),
      //                             "L must be sequential!");
    }

    /**
     * \brief Set up conjugate gradient solver with absolute energy error termination criterion.
     *
     * \param verbose
     */
    template <class Int=int, class enable = typename std::enable_if<std::is_same<Regularization,NoRegularization>::value && std::is_same<Int,int>::value>::type>
    TCG(Dune::LinearOperator<X,Xstar>& op_, Dune::Preconditioner<X,Xstar>& prec_, DualPairing<X,Xstar> const& dp_,
        double relTol_=1e-3, size_t maxSteps_=100, Int verbose_=0) :
          op(op_), prec(prec_), dp(dp_), relTol(relTol_), absTol(1e-9), maxSteps(maxSteps_), verbose(verbose_)
    {
      // Do we need this in Kaskade7?
      //         dune_static_assert( static_cast<int>(L::category) == static_cast<int>(P::category),
      //                             "L and P must have the same category!");
      //         dune_static_assert( static_cast<int>(L::category) == static_cast<int>(SolverCategory::sequential),
      //                             "L must be sequential!");
    }

    /**
     * \brief Apply inverse operator.
     *
     * \param u the initial value (starting iterate)
     * \param b the right hand side
     */
    virtual int apply (X& u, X& q, Xstar& b, Dune::InverseOperatorResult& res)
    {
      result = Result::Failed;
      int numberOfResults = 1;
      res.clear();                // clear solver statistics
      //terminate.clear();          // clear termination criterion
      Dune::Timer watch;          // start a timer

      prec.pre(u,b);

      Xstar r(b);
      op.applyscaleadd(-1.0,u,r);  // r = b-Au
      regularization(u,r);

      X rq(u), du(u); rq = 0;

      prec.apply(rq,r); // rq = B^{-1} r

      q = rq;

      X Aq(b);

      // some local variables
      Real alpha,beta,sigma,gamma;
      sigma = dp(rq,r); // preconditioned residual norm squared

      //terminate.residual(sigma);
      double const sigma0 = std::abs(sigma);

      if (verbose>0) {            // printing
        std::cout << "=== Kaskade7 TCG" << std::endl;
        if (verbose>1) {
          this->printHeader(std::cout);
          this->printOutput(std::cout,0,sigma0);
        }
      }

      // the loop
      int i=0;
      while(true)
      {
        ++i;
        // minimize in given search direction p
        op.apply(q,Aq);             // h = Aq
        Real qAq = dp(q,Aq);

        if(qAq <=0)
        {
          result = Result::EncounteredNonConvexity;
          if(verbose > 0) std::cout << "Nonconvexity at iteration " << i << ", qAq=" << qAq << ", ||q||=" << sqrt(q*q) << std::endl;
          if(i==1)
          {
            u.axpy(1.0,q);
            break;
          }
          numberOfResults = 2;
          break;      // Abbruch wg. fehlender Konvexität
        }

        alpha = sigma/qAq;
        u.axpy(alpha,q);
        du = q;
        du *= 1./alpha;
        if(dp(du,du) < absTol)
        {
          result = Result::Converged;
          break;
        }

        gamma = sigma*alpha;
        //terminate.step(ScalarTraits<Real>::real(gamma));
        resDebug.push_back(std::sqrt(sigma));

        // convergence test
        //if (terminate)
          //break;

        r.axpy(-alpha,Aq); // r = r - alpha*A*q
        regularization(u,r);
        rq = 0;
        prec.apply(rq,r); // rq = B^{-1}r

        // determine new search direction
        double sigmaNew = dp(rq,r);
        //terminate.residual(ScalarTraits<Real>::real(sigmaNew));
        if (verbose>1)             // print
          this->printOutput(std::cout,i,std::abs(sigmaNew),std::abs(sigma));

        // convergence check to prevent division by zero if we obtained an exact solution
        // (which may happen for low dimensional systems)
        if (std::abs(sigmaNew) < relTol*sigma0)
        {
          result = Result::Converged;
          break;
        }
        beta = sigmaNew/sigma;
        if(verbose > 0) std::cout << "step reduction: " << (sigmaNew/sigma) << ", overall reduction: " << (sigmaNew/sigma0) << std::endl;
        sigma = sigmaNew;
        q *= beta;                  // scale old search direction
        q += rq;                     // orthogonalization with correction

        if(i > maxSteps) break;
      }

      res.iterations = i;               // fill statistics
      res.reduction = std::sqrt(std::abs(sigma)/sigma0);
      res.conv_rate  = pow(res.reduction,1.0/i);
      res.elapsed = watch.elapsed();

      if (verbose>0)                 // final print
      {
        std::cout << "=== rate=" << res.conv_rate
            << ", time=" << res.elapsed
            << ", iterations=" << i << std::endl;
      }

      prec.post(u);

      return numberOfResults;
    }

    /**
     * \brief Apply tcg and possibly forget second descent direction
     */
    virtual void apply (X& u, Xstar& b, Dune::InverseOperatorResult& res) {
      X q(u);
      apply(u,q,b,res);
    }

    void apply (X& u, Xstar& b)
    {
      Dune::InverseOperatorResult res;
      apply(u,b,res);
    }

    /**
     * \brief Apply inverse operator with given absolute tolerance.
     */
    virtual void apply (X& x, X& b, double relTol, Dune::InverseOperatorResult& res) {
      //terminate.relTol(relTol);
      (*this).apply(x,b,res);
    }

    void setRelativeAccuracy(double relTol_) { relTol = relTol_; }

    void setMaxSteps(size_t maxSteps_) { maxSteps = maxSteps_; }

    bool localConvergenceLikely() const { return result == Result::Converged; }

    bool encounteredNonConvexity() const { return result == Result::EncounteredNonConvexity; }

  private:
    Dune::LinearOperator<X,Xstar>& op;
    Dune::Preconditioner<X,Xstar>& prec;
    DualPairing<X,Xstar> const& dp;
    typename std::conditional<std::is_same<Regularization,NoRegularization>::value,NoRegularization,Regularization&>::type regularization;
    double relTol, absTol;
    size_t maxSteps;
    int verbose;
    std::vector<double> resDebug;
    Result result;
  };

  template <class X, class Xstar, class Regularization = NoRegularization> using TPCG = TCG<X,Xstar,Regularization>;
} // namespace Kaskade
#endif


/*******************************************************************************
#      ____               __          __  _      _____ _       _               #
#     / __ \              \ \        / / | |    / ____| |     | |              #
#    | |  | |_ __   ___ _ __ \  /\  / /__| |__ | |  __| | ___ | |__   ___      #
#    | |  | | '_ \ / _ \ '_ \ \/  \/ / _ \ '_ \| | |_ | |/ _ \| '_ \ / _ \     #
#    | |__| | |_) |  __/ | | \  /\  /  __/ |_) | |__| | | (_) | |_) |  __/     #
#     \____/| .__/ \___|_| |_|\/  \/ \___|_.__/ \_____|_|\___/|_.__/ \___|     #
#           | |                                                                #
#           |_|                                                                #
#                                                                              #
#                                (c) 2011 by                                   #
#           University of Applied Sciences Northwestern Switzerland            #
#                     Institute of Geomatics Engineering                       #
#                           martin.christen@fhnw.ch                            #
********************************************************************************
*     Licensed under MIT License. Read the file LICENSE for more information   *
*******************************************************************************/

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

#include "og.h"
#include "ogprocess.h"
#include "geo/MercatorQuadtree.h"
#include "geo/CoordinateTransformation.h"
#include "geo/PointCloudReader.h"
#include "math/Octocode.h"
#include "string/FilenameUtils.h"
#include "string/StringUtils.h"
#include "io/FileSystem.h"
#include <float.h>
#include <iostream>
#include <ctime>
#include <boost/program_options.hpp>

int _frominput(const std::vector<std::string>& vecFiles, const std::string& srs, bool bVerbose, bool bPointCloud);
void _calcfromwgs84(int, double, double, double, double);
//------------------------------------------------------------------------------

namespace po = boost::program_options;

int main(int argc, char *argv[])
{
   po::options_description desc("Program-Options");
   desc.add_options()
       ("maxlod", po::value<int>(), "desired level of detail (integer)")
       ("wgs84", po::value< std::vector<double> >()->multitoken(), "wgs84 coordinates lng0 lat0 lng1 lat1")
       ("srs", po::value<std::string>(), "spatial reference system for input files")
       ("input", po::value< std::vector<std::string> >()->multitoken(), "list input files")
       ("verbose", "display additional information")
       ("inputdir", po::value<std::string>(), "input directory")
       ("filetype",  po::value<std::string>(), "file type")
       ("point", "for point cloud")
       ;

   po::variables_map vm;

   try
   {
      po::store(po::parse_command_line(argc, argv, desc), vm);
      po::notify(vm);
   }
   catch (std::exception&)
   {
      std::cout << desc;
      return 4;
   }

   bool bVerbose = false;
   bool bPointCloud = false;

   if (vm.count("verbose"))
   {
      bVerbose = true;
   }

   if (vm.count("point"))
   {
      bPointCloud = true;
   }

   if ((vm.count("wgs84") && !vm.count("maxlod")) || (!vm.count("wgs84") && vm.count("maxlod")))
   {
      std::cout << "ERROR: option --wgs84 and --maxlod must be used together\n";
      return 1;
   }

   if (vm.count("wgs84") && vm.count("maxlod"))
   {
      int lod = vm["maxlod"].as<int>();
      if (lod < 0 || lod>23)
      {
         std::cout << "ERROR: lod value in wrong range\n";
         return 1;
      }

      double lng0, lat0, lng1, lat1;

      std::vector<double> vecCoords= vm["wgs84"].as< std::vector<double> >();
      if (vecCoords.size() != 4)
      {
         std::cout << "ERROR: wrong number of wgs84 coordinates\n";
         return 1;
      }

      lng0 = vecCoords[0];
      lat0 = vecCoords[1];
      lng1 = vecCoords[2];
      lat1 = vecCoords[3];

      if (lng0 >= lng1) { std::cout << "error: lng1 must be greater than lng0\n"; return 0; }
      if (lat0 >= lat1) { std::cout << "error: lat1 must be greater than lat0\n"; return 0; }
      if (lod < 4) { std::cout << "error: level of detail must be atleast 4\n"; return 0; }

      _calcfromwgs84(lod, lng0, lat0, lng1, lat1);

      return 0;
   }
   else if (vm.count("srs") && vm.count("input"))
   {
      std::vector<std::string> vecFiles = vm["input"].as< std::vector<std::string> >();
      std::string srs = vm["srs"].as<std::string>();

      return _frominput(vecFiles, srs, bVerbose, bPointCloud);
   }
   else if (vm.count("srs") && vm.count("inputdir") && vm.count("filetype"))
   {
      std::string srs = vm["srs"].as<std::string>();
      std::string inputdir = vm["inputdir"].as<std::string>();
      std::string filetype = vm["filetype"].as<std::string>();

      std::vector<std::string> vecFiles = FileSystem::GetFilesInDirectory(inputdir, filetype);

      if (vecFiles.size() == 0)
      {
         std::cout << "No files found!\n";
         return 1;
      }

      return _frominput(vecFiles, srs, bVerbose, bPointCloud);
   }
   else
   {
      std::cout << desc << "\n";
      std::cout << "From input files: use --srs and --input together\n";
      std::cout << "From wgs84 coord: use --wgs84 and --maxlod together\n";
      return 1;
   }

   return 0;
}

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

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

int _frominput(const std::vector<std::string>& vecFiles, const std::string& srs, bool bVerbose, bool bPointCloud)
{

   if (StringUtils::Left(srs, 5) != "EPSG:")
   {
      std::cout << "Error: only srs starting with EPSG: are currently supported";
      return 1;
   }

   int epsg = atoi(srs.c_str()+5);
   std::cout << "SRS epsg-code: " << epsg << "\n";

   if (!ProcessingUtils::init_gdal())
   {
      std::cout << "Warning: gdal-data directory not found. Ouput may be wrong!\n";
   }

   boost::shared_ptr<CoordinateTransformation> qCT;

   if (bPointCloud)
   {
      qCT = boost::shared_ptr<CoordinateTransformation>(new CoordinateTransformation(epsg, 4326));
      clock_t t0,t1;
      t0 = clock();

      // vecfiles contains xyz (or xyzi or xyzirgb) ASCII files.
      // a) find the center of the dataset
      // b) find extent (max,min of x,y and z component)

      double xmin, ymin, zmin;
      double xmax, ymax, zmax;
      double xcenter, ycenter, zcenter;

      xmin=ymin=zmin=1e20;
      xmax=ymax=zmax=-1e20;

      size_t numpts = 0;

      CloudPoint pt;
      for (size_t i = 0; i< vecFiles.size();++i)
      {
         PointCloudReader pr;

         if (pr.Open(vecFiles[i]))
         {
            while (pr.ReadPoint(pt))
            {
               qCT->Transform(&pt.x, &pt.y);
               xmin = math::Min<double>(xmin, pt.x);
               ymin = math::Min<double>(ymin, pt.y);
               zmin = math::Min<double>(zmin, pt.elevation);
               xmax = math::Max<double>(xmax, pt.x);
               ymax = math::Max<double>(ymax, pt.y);
               zmax = math::Max<double>(zmax, pt.elevation);

               numpts++;
            }
         }
      }

      xcenter = xmin+ fabs(0.5*(xmax-xmin));
      ycenter = ymin+fabs(0.5*(ymax-ymin));
      zcenter = zmin+fabs(0.5*(zmax-zmin));

      std::cout.precision(15);

      for (int i=1;i<23;i++)
      {
         std::cout << "LEVEL OF DETAIL " << i << ": ";
         _calcfromwgs84(i, xmin, ymin, xmax, ymax);
      }

      t1 = clock();
      std::cout << "calculated in: " << double(t1-t0)/double(CLOCKS_PER_SEC) << " s \n";

      std::cout << "There are " << numpts << " points...\n";
      //std::cout << "Point Cloud Center (WGS84): (" << xcenter << ", " << ycenter << ", " << zcenter <<")\n";
      std::cout << "Required for ogCreateLayer:\n";
      std::cout << "-------------------------------------------------------------------------------\n";
      std::cout << "Extent (WGS84): (" << xmin << ", " << ymin << ", " << zmin << ")-(" << xmax << ", " << ymax << ", " << zmax << ")\n";
      std::cout << "-------------------------------------------------------------------------------\n";

      return 0;
   }
   else
   {

      qCT = boost::shared_ptr<CoordinateTransformation>(new CoordinateTransformation(epsg, 3785));

      // create an array of Dataset info for parallel access.
      DataSetInfo* pDataset = new DataSetInfo[vecFiles.size()];

      clock_t t0,t1;
      t0 = clock();

      for (int i=0;i<(int)vecFiles.size();i++)
      {
         ProcessingUtils::RetrieveDatasetInfo(vecFiles[i], qCT.get(), &pDataset[i], bVerbose);
      }

      // at this point we finished calculating all the boundaries of all datasets, now
      // calculate the min/max

      double total_dest_ulx = 1e20;
      double total_dest_lry = 1e20;
      double total_dest_lrx = -1e20;
      double total_dest_uly = -1e20;
      double pixelsize = 1e20;

      for (size_t i=0;i<vecFiles.size();i++)
      {
         if (pDataset[i].bGood)
         {
            total_dest_ulx = math::Min<double>(pDataset[i].dest_ulx, total_dest_ulx);
            total_dest_lry = math::Min<double>(pDataset[i].dest_lry, total_dest_lry);
            total_dest_lrx = math::Max<double>(pDataset[i].dest_lrx, total_dest_lrx);
            total_dest_uly = math::Max<double>(pDataset[i].dest_uly, total_dest_uly);
            pixelsize      = math::Min<double>(pDataset[i].pixelsize, pixelsize);
         }
      }

      double pixelsize_m = pixelsize * 6378137.0;

      t1 = clock();

      std::cout << "GATHERED BOUNDARY (Mercator):\n";
      std::cout.precision(16);
      std::cout << "        ulx: " << total_dest_ulx << "\n";
      std::cout << "        lry: " << total_dest_lry << "\n";
      std::cout << "        lrx: " << total_dest_lrx << "\n";
      std::cout << "        uly: " << total_dest_uly << "\n";
      std::cout << "BOUNDARY in WGS84:\n";

      MercatorQuadtree* pQuadtree = new MercatorQuadtree();
      double x,y;

      double lng0, lat0, lng1, lat1;
      x = total_dest_ulx; y = total_dest_lry;
      pQuadtree->MercatorToWGS84(x, y);
      lng0 = x; lat0 = y;
      std::cout << "       lng0: " << x << "\n";
      std::cout << "       lat0: " << y << "\n";
      x = total_dest_lrx; y = total_dest_uly;
      pQuadtree->MercatorToWGS84(x, y);
      lng1 = x; lat1 = y;
      std::cout << "       lng1: " << x << "\n";
      std::cout << "       lat1: " << y << "\n";
      std::cout << " pixelsize: " << pixelsize_m << " m\n";

      double lat_avg = 0.5*(lat1 - lat0);

      double resolution_elevation[21];
      double resolution_image[21];

      for (int lod = 0; lod<21;lod++)
      {
         double v = cos(lat_avg*3.14159265358979/180)*2*3.14159265358979*6378137.0/pow(2.0,lod);
         resolution_elevation[lod] = v / 22.0; // ~500 points per tile
         resolution_image[lod] = v / 256;
      }

      int recommended_lod_elv = 0;
      int recommended_lod_img = 0;
      for (int lod = 0; lod<20;lod++)
      {
         if (pixelsize_m < resolution_elevation[lod] && pixelsize_m > resolution_elevation[lod+1])
         {
            recommended_lod_elv = lod;
         }
         if (pixelsize_m < resolution_image[lod] && pixelsize_m > resolution_image[lod+1])
         {
            recommended_lod_img = lod;
         }
      }

      for (int i=1;i<23;i++)
      {
         std::cout << "LEVEL OF DETAIL " << i << ": ";
         _calcfromwgs84(i, lng0, lat0, lng1, lat1);
      }

      std::cout << "****************************************\n";
      std::cout << "RECOMMENDATION (MINIMUM LOD):\n";
      if (recommended_lod_elv != 0)
      {
         std::cout << "IF THIS IS ELEVATION: LOD=" << recommended_lod_elv << ": ";
         _calcfromwgs84(recommended_lod_elv, lng0, lat0, lng1, lat1);
      }
      if (recommended_lod_img != 0)
      {
         std::cout << "\nIF THIS IS IMAGE: LOD=" << recommended_lod_img << ": ";
         _calcfromwgs84(recommended_lod_img, lng0, lat0, lng1, lat1);
      }
      if (recommended_lod_img == 0 && recommended_lod_elv==0)
      {
         std::cout << "recommendation is not possible.\n";
      }
      std::cout << "****************************************\n";

      delete pQuadtree;

      //std::cout << "calculated in: " << double(t1-t0)/double(CLOCKS_PER_SEC) << " s \n";

      delete[] pDataset;

      ProcessingUtils::exit_gdal();

      return 0;
   }
}

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

void _calcfromwgs84(int lod, double lng0, double lat0, double lng1, double lat1)
{
   MercatorQuadtree* pQuadtree = new MercatorQuadtree();

   int64 px0, py0, px1, py1, tx0, ty0, tx1, ty1;
   pQuadtree->WGS84ToPixel(lng0, lat0, lod, px0, py1);
   pQuadtree->WGS84ToPixel(lng1, lat1, lod, px1, py0);

   //std::cout << "Number of pixels in specified range: " << (px1-px0+1)*(py1-py0+1) << "\n";

   pQuadtree->PixelToTileCoord(px0, py0, tx0, ty0);
   pQuadtree->PixelToTileCoord(px1, py1, tx1, ty1);

   std::cout << "Tile Coords: (" << tx0 << ", " << ty0 << ")-(" << tx1 << ", " << ty1 << ")\n";

   delete pQuadtree;

}