/*
   Copyright 2016 Mitchell Young

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
*/

#pragma once

#include <memory>

#include <Eigen/Sparse>

#include "util/global_config.hpp"
#include "util/timers.hpp"
#include "coarse_data.hpp"
#include "eigen_interface.hpp"
#include "mesh.hpp"
#include "source_isotropic.hpp"
#include "xs_mesh_homogenized.hpp"

namespace mocc {
class CMFD : public HasOutput {
public:
    CMFD(const pugi::xml_node &input, const CoreMesh *mesh,
         SP_XSMeshHomogenized_t xsmesh);

    /**
     * \brief Solve the CMFD system
     *
     * \param [in,out] k the initial guess to use for the system eigenvalue.
     * Updated byt the solve.
     */
    void solve(real_t &k);

    /**
     * \brief Return a pointer to the coarse data.
     *
     * This is used to couple sweepers and other objects that need access to
     * the coarse data to the CMFD solver.
     */
    CoarseData *get_data()
    {
        return &coarse_data_;
    }

    /**
     * \brief Return a reference to the \ref CoarseData object.
     */
    CoarseData &coarse_data()
    {
        return coarse_data_;
    }

    /**
     * \brief Return a reference to the MG flux.
     */
    const ArrayB2 &flux() const
    {
        return coarse_data_.flux;
    }

    /**
     * \brief Return whether the CMFD object is meant to actually do a
     * solve.
     *
     * It is possible to need a CMFD object to be constructed to use its
     * \ref CoarseData and other functionality, but not want to perform
     * actual CMFD solves.
     */
    bool is_enabled() const
    {
        return is_enabled_;
    }

    /**
     * \brief Set the eigenvalue convergence tolerance
     */
    void set_k_tolerance(real_t tol)
    {
        assert(tol > REAL_FUZZ);

        k_tol_ = tol;
        return;
    }

    /**
     * \brief Set the fission source convergence tolerance
     */
    void set_psi_tolerance(real_t tol)
    {
        assert(tol > REAL_FUZZ);

        psi_tol_ = tol;
        return;
    }

    void output(H5Node &node) const;

private:
    // Private methods
    /**
     * \brief Compute and return the L-2 norm of the residual of the CMFD
     * system, scaled by the size of the system.
     *
     * This calculates the residual of the CMFD system, considering all groups.
     * Since this implementation of the method must calculate the source for all
     * groups and needs to do repeated copies of the flux, it is somewhat more
     * expensive than the single-group implementation, which assumes that the
     * source is already calculated. As a result this is useful for calculating
     * the initial residual at the beginning of the CMFD solve, while the other
     * is more ideal for calculating the residual for each iteration.
     *
     * \pre The group-independent fission source has been calculated and stored
     * in \c fs_.
     *
     * \sa CMFD::residual(int)
     */
    real_t residual();

    /**
     * \brief Compute and return the squared L-2 norm of the residual for a
     * single group of the CMFD system
     *
     * \param group the group for which to calculate residual
     *
     * This is similar to the all-group method, but it only computes the
     * contribution to the residual for the requested group. It assumes that the
     * source (right-hand side) has already been computed and is therefore
     * cheaper to use for assessing convergence.
     *
     * Unlike \ref CMFD::residual(), this returns an un-scaled, squared L-2
     * norm. This should lead to the following code producing the same result as
     * the group-independent \ref CMFD::residual() method:
     * \code
norm = 0.0;
for(int ig=0; ig<n_group_; ig++) {
    // Satisfy preconditions
    norm += this->residual(group);
}
norm = std::sqrt(norm)/(n_cell_*n_group_);
     * \endcode
     *
     * \pre The group-independent fission source has been calculated and stored
     * in \c fs_.
     *
     * \pre The source (fission and inscattering) must be calculated for the
     * current group.
     *
     * \pre The current-group flux is already stored in the \c x_ vector
     *
     * \sa CMFD::residual()
     */
    real_t residual(int group) const;
    real_t solve_1g(int group);
    void fission_source(real_t k);
    void print(int iter, real_t k, real_t k_err, real_t psi_err,
               real_t resid_ratio);

    /**
     * \brief Calculate CMFD-derived currents after a CMFD solve and store
     * on the \ref CoarseData.
     *
     * After performing a CMFD solve, it is useful to have access to the
     * state of the current, as predicted by the CMFD system given the
     * current state of the D-hats. This allows us to calculate, for
     * instance, transverse leakage for an MoC sweeper, which otherwise
     * wouldn't know what the current looks like in the axial dimension.
     */
    void store_currents();

    /**
     * Set up the linear systems for each group. This doesnt need to be done
     * for each iteration, nor in the case of non-zero currents/d-hat terms
     * can it, since the flux is allowed to change, which in turn will
     * affect the new D-hats. While this conusmes more memory to store the
     * systems for each group, it should be faster.
     */
    void setup_solve();
    real_t total_fission();

    // Private data
    Timer &timer_;
    Timer &timer_init_;
    Timer &timer_setup_;
    Timer &timer_solve_;

    Mesh mesh_;
    const Mesh *fine_mesh_;
    XSMeshHomogenized xsmesh_;
    int n_cell_;
    int n_surf_;
    int n_group_;
    CoarseData coarse_data_;
    // Scratch space for manipulating currents on the CMFD mesh
    ArrayB1 current_1g_;
    bool is_enabled_;

    // Single-group fission source
    ArrayB1 fs_;
    ArrayB1 fs_old_;

    // Single-group flux result from LS solve, also used for initial guess
    VectorX x_;

    SourceIsotropic source_;

    // Vector of one-group sparse matrix
    std::vector<Eigen::SparseMatrix<real_t>> m_;

    // Vector of BiCGSTAB objects.
    std::vector<Eigen::BiCGSTAB<Eigen::SparseMatrix<real_t>>> solvers_;

    // Surface quantities. We need to keep these around to do the current
    // update without having to recalculate. Based on profiling, might be
    // nice to still get these on the fly to save on memory, but this is
    // fine for now.
    ArrayB2 d_hat_;
    ArrayB2 d_tilde_;
    ArrayB2 s_hat_;
    ArrayB2 s_tilde_;

    // Number of times solve() has been called
    int n_solve_;

    // Convergence options
    real_t k_tol_;
    real_t psi_tol_;
    real_t resid_reduction_;
    int max_iter_;

    // Other options
    bool zero_fixup_;
    bool dump_current_;
};
typedef std::unique_ptr<CMFD> UP_CMFD_t;
}


#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <string>
#include <pcl/io/boost.h>
#include <pcl/io/pcd_io.h>
#include <Eigen/Dense>
#include <pcl/registration/icp.h>
#include <pcl/registration/ndt.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/search/organized.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/features/fpfh.h>
#include "DoNFilter.hpp"
#include "CubeAligner.hpp"
#include "mapgmuutils.hpp"

#ifndef TRANSFORMER_H
#define TRANSFORMER_H

using namespace pcl;
using namespace std;

typedef pcl::PointXYZI PointType;
typedef PointCloud<pcl::PointXYZI> Cloud;
typedef PointCloud<pcl::PointXYZI>::Ptr CloudPtr;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud<PointNormalT> PointCloudWithNormals;
typedef pcl::PointCloud<PointNormalT>::Ptr PointCloudWithNormalsPtr;
typedef pcl::KdTreeFLANN<PointType> TreeType;
typedef TreeType::Ptr TreePtr;

class MyPointRepresentation : public pcl::PointRepresentation <PointNormalT>
{
  using pcl::PointRepresentation<PointNormalT>::nr_dimensions_;

public:

  MyPointRepresentation ()
  {
    // Define the number of dimensions
    nr_dimensions_ = 4;
  }

  // Override the copyToFloatArray method to define our feature vector
  virtual inline void copyToFloatArray (const PointNormalT &p, float * out) const
  {
    // < x, y, z, curvature >
    out[0] = p.x;
    out[1] = p.y;
    out[2] = p.z;
    out[3] = p.curvature;
  }
};

class Transformer
{
public:

  inline double getFitness(CloudPtr base, CloudPtr toAlign, Eigen::Matrix4f mat)
  {
    pcl::IterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI> icp;
    pcl::PointCloud<pcl::PointXYZI> Final;

    icp.setMaxCorrespondenceDistance (0.5);
    icp.setMaximumIterations (0);

    icp.setInputSource(toAlign);
    icp.setInputTarget(base);
    icp.align(Final);
    return icp.getFitnessScore();
  }

  inline Eigen::Matrix4f getCubeTransform(CloudPtr base, CloudPtr toAlign,
					  double cSize, double *eVal)
  {
    Eigen::Matrix4f baseTrans = Eigen::Matrix4f::Identity();
    CloudPtr copyCloud(new Cloud);
    CubeAligner cb(base,cSize);
    *copyCloud += *toAlign;
    cb.filterCloudCubic(copyCloud);

    return cb.alignToMatrix(baseTrans,copyCloud,eVal);
  }

  inline Eigen::Matrix4f getCubeICPTransform(CloudPtr base, CloudPtr toAlign,
					     double cSize, double *eVal)
  {
    Eigen::Matrix4f tmpTrans;
    Eigen::Matrix4f baseTrans = Eigen::Matrix4f::Identity();

    pcl::IterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI> icp;
    pcl::PointCloud<pcl::PointXYZI> Final;
    CloudPtr copyCloud(new Cloud);
    int maxIter = 100;
    double eps = 1e-8;

    CubeAligner cb(base,cSize);

    *copyCloud += *toAlign;
    cb.filterCloudCubic(copyCloud);

    //binary cube alignment using cubic filtered toAlign
    tmpTrans =  cb.alignToMatrix(baseTrans,copyCloud,eVal);

    //scale added for tuning, not necessary
    icp.setMaxCorrespondenceDistance (cSize*2.0);
    icp.setMaximumIterations (maxIter);
    icp.setEuclideanFitnessEpsilon (eps);

    //icp using the filtered copyCloud
    icp.setInputSource(copyCloud);
    icp.setInputTarget(base);
    icp.align(Final,tmpTrans);

    return icp.getFinalTransformation();
  }

  //assumes gpsGuess already identity, sets the translation values
  inline void calcGpsGuess(Eigen::Matrix4f& lastTrans,mapgmu::PathFinder *pf,
			   double currTime, double dt, Eigen::Matrix4f& gpsGuess)
  {
    Vector3f rotOffset;
    //create gps estimate, store in 4x4 matrix
    rotOffset =  lastTrans.block(0,0,3,3).transpose()*pf->getDeltaForTime(currTime,dt);
    gpsGuess(0,3)=rotOffset(0);
    gpsGuess(1,3)=rotOffset(1);
    gpsGuess(2,3)=rotOffset(2);
  }

  inline Eigen::Matrix4f getGpsIcpNoCubeTransform(CloudPtr base, CloudPtr toAlign,
					  Eigen::Matrix4f trans,
					  mapgmu::PathFinder *pf, double *cSize,
					  double currTime, double dt)
  {
    pcl::IterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI> icp;
    pcl::IterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI> icp2;
    pcl::PointCloud<pcl::PointXYZI> Final;
    int maxIter = 50;
    double eps = 1e-8;
    double eVal = *cSize*2.0;

    Matrix4f gpsGuess = Matrix4f::Identity();
    Matrix4f transMat;

    calcGpsGuess(trans,pf,currTime,dt,gpsGuess);

    //icp pass 1
    icp.setMaxCorrespondenceDistance (eVal);
    icp.setMaximumIterations (maxIter);
    icp.setEuclideanFitnessEpsilon (eps);
    icp.setInputSource(toAlign);
    icp.setInputTarget(base);
    icp.align(Final,gpsGuess);

    transMat =  icp.getFinalTransformation();

    Final.clear();

    //icp pass 2, smaller corr, higher euclid fitness
    icp2.setMaxCorrespondenceDistance (*cSize/15.0);
    icp2.setMaximumIterations (maxIter);
    icp2.setEuclideanFitnessEpsilon (eps*10);
    icp2.setInputSource(toAlign);
    icp2.setInputTarget(base);
    icp2.align(Final,transMat);

    transMat = icp2.getFinalTransformation();

    return transMat;
  }

  inline Eigen::Matrix4f getGpsIcpTransform(CloudPtr base, CloudPtr toAlign,
					  Eigen::Matrix4f trans,
					  mapgmu::PathFinder *pf, double *cSize,
					  double currTime, double dt)
  {
    pcl::IterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI> icp;
    pcl::IterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI> icp2;
    pcl::PointCloud<pcl::PointXYZI> Final;
    CloudPtr copyCloud(new Cloud);
    int maxIter = 50;
    double eps = 1e-8;
    double eVal = *cSize*2.0;

    Vector3f rotOffset;
    Matrix4f gpsGuess = Matrix4f::Identity();
    Matrix4f transMat;
    CubeAligner cb(base,*cSize);

    *copyCloud += *toAlign;
    cb.filterCloudCubic(copyCloud);

    calcGpsGuess(trans,pf,currTime,dt,gpsGuess);

    CVertex c1(gpsGuess);

    //first pass icp, filtered copyCloud used
    icp.setMaxCorrespondenceDistance (eVal);
    icp.setMaximumIterations (maxIter);
    icp.setEuclideanFitnessEpsilon (eps);
    icp.setInputSource(copyCloud);
    icp.setInputTarget(base);
    icp.align(Final,gpsGuess);

    transMat =  icp.getFinalTransformation();

    Final.clear();

    //second pass icp, full toAlign cloud used, smalled corr, higher euc
    icp2.setMaxCorrespondenceDistance (*cSize/15.0);
    icp2.setMaximumIterations (maxIter);
    icp2.setEuclideanFitnessEpsilon (eps*10);
    icp2.setInputSource(toAlign);
    icp2.setInputTarget(base);
    icp2.align(Final,transMat);

    transMat = icp2.getFinalTransformation();

    //uncomment for distance/metric calculations
    /*CVertex c2(transMat);
    CVertex c3 = c2-c1;

    std::cout << "Cube: " << *cSize << std::endl
		<< "Conv: " << eVal << std::endl
		<< "Converged: " << icp2.hasConverged () << std::endl
		<< "Fitness: " << icp2.getFitnessScore () << std::endl
		<< "Vexp: " << c1.getDist() << std::endl
		<< "Vfit: " << c2.getDist() << std::endl
		<< "PathDist: " << c3.getDist()  << std::endl;*/

    return transMat;
  }

  inline Eigen::Matrix4f getGpsIcpCubeTransform(CloudPtr base, CloudPtr toAlign,
					  Eigen::Matrix4f trans,
					  mapgmu::PathFinder *pf, double *cSize,
					  double *eVal, double currTime, double dt)
  {
    Eigen::Matrix4f tmpTrans;
    pcl::IterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI> icp;
    pcl::PointCloud<pcl::PointXYZI> Final;
    CloudPtr copyCloud(new Cloud);
    int maxIter = 50;
    double eps = 1e-6;

    Vector3f rotOffset;
    Matrix4f gpsGuess = Matrix4f::Identity();
    CubeAligner cb(base,*cSize);

    *copyCloud += *toAlign;
    cb.filterCloudCubic(copyCloud);

    calcGpsGuess(trans,pf,currTime,dt,gpsGuess);

    //icp pass
    icp.setMaxCorrespondenceDistance (*cSize);
    icp.setMaximumIterations (maxIter);
    icp.setEuclideanFitnessEpsilon (eps);
    icp.setInputSource(copyCloud);
    icp.setInputTarget(base);
    icp.align(Final,gpsGuess);

    std::cout << "Converged: " << icp.hasConverged () << std::endl
		<< "Fitness: " << icp.getFitnessScore () << std::endl;

    tmpTrans = icp.getFinalTransformation();

    //cubic alignment
    return cb.alignToMatrix(tmpTrans,toAlign,eVal);
  }

  inline Eigen::Matrix4f getCubeGpsIcpTransform(CloudPtr base, CloudPtr toAlign,
					  Eigen::Matrix4f trans,
					  mapgmu::PathFinder *pf, double *cSize,
					  double *eVal, double currTime, double dt)
  {
    Eigen::Matrix4f tmpTrans;

    pcl::IterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI> icp;
    pcl::IterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI> icp2;
    pcl::PointCloud<pcl::PointXYZI> Final;
    CloudPtr copyCloud(new Cloud);
    int maxIter = 100;
    double eps = 1e-8;

    Vector3f rotOffset;
    Matrix4f gpsGuess = Matrix4f::Identity();
    Matrix4f transMat;
    CubeAligner cb(base,*cSize);

    *copyCloud += *toAlign;
    cb.filterCloudCubic(copyCloud);

    calcGpsGuess(trans,pf,currTime,dt,gpsGuess);

    tmpTrans =  cb.alignToMatrix(gpsGuess,copyCloud,eVal);

    //icp pass one, filtered copyCloud
    icp.setMaxCorrespondenceDistance (*cSize*2.0);
    icp.setMaximumIterations (maxIter);
    icp.setEuclideanFitnessEpsilon (eps);
    icp.setInputSource(copyCloud);
    icp.setInputTarget(base);
    icp.align(Final,tmpTrans);

    transMat = icp.getFinalTransformation();

    Final.clear();

    //icp pass two full toAlign cloud
    icp2.setMaxCorrespondenceDistance (*cSize/15.0);
    icp2.setMaximumIterations (maxIter);
    icp2.setEuclideanFitnessEpsilon (eps*100);
    icp2.setInputSource(toAlign);
    icp2.setInputTarget(base);
    icp2.align(Final,transMat);

    transMat = icp2.getFinalTransformation();

    //attempts at dynamically updating eVal
    /* *eVal = (*eVal + cb.getNextEval(&gpsGuess,&tmpTrans))/2.0;
    if (*eVal < 0.05) *eVal = 0.05;

    std::cout << "New EVAL " << *eVal << std::endl;

    std::cout << "Converged: " << icp.hasConverged () << std::endl
		<< "Fitness: " << icp.getFitnessScore () << std::endl;*/

    return transMat;
  }

  inline Eigen::Matrix4f getCubeGpsIcpTransformAdapt(CloudPtr base, CloudPtr toAlign,
					  Eigen::Matrix4f trans,
					  mapgmu::PathFinder *pf, CVertex lastVel,
					  double currTime, double dt)
  {
    Eigen::Matrix4f cubeTrans;
    Eigen::Vector3f vel = pf->getDeltaForTime(currTime,dt);

    pcl::IterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI> icp;
    pcl::IterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI> icp2;
    pcl::PointCloud<pcl::PointXYZI> Final;
    CloudPtr copyCloud(new Cloud);
    int maxIter = 100;
    double eps = 1e-8;

    Vector3f rotOffset;

    Matrix4f transMat;
    double cSize;
    double eVal;

    Matrix4f gpsGuess = Matrix4f::Identity();
    calcGpsGuess(trans,pf,currTime,dt,gpsGuess);

    cSize = vel.norm();
    if (lastVel.getDist()>0)
    {
      cSize += lastVel.getDist();
      cSize /= 2.0;
    }

    if (cSize > 0.45)cSize = 0.45;
    if (cSize < 0.05)cSize = 0.05;

    eVal = cSize / 2.0;

    CubeAligner cb(base,cSize);

    *copyCloud += *toAlign;
    cb.filterCloudCubic(copyCloud);

    //cube alignment with gps guess seed
    cubeTrans =  cb.alignToMatrix(gpsGuess,copyCloud,&eVal);

    //icp pass one with cube alignment seed, filtered copyCloud
    icp.setMaxCorrespondenceDistance (cSize*1.33);
    icp.setMaximumIterations (maxIter);
    icp.setEuclideanFitnessEpsilon (eps);
    icp.setInputSource(copyCloud);
    icp.setInputTarget(base);
    icp.align(Final,cubeTrans);

    transMat = icp.getFinalTransformation();

    Final.clear();

    //icp pass two full toAlign cloud
    icp2.setMaxCorrespondenceDistance (cSize/10.0);
    icp2.setMaximumIterations (maxIter/5.0);
    icp2.setEuclideanFitnessEpsilon (eps*100);
    icp2.setInputSource(toAlign);
    icp2.setInputTarget(base);
    icp2.align(Final,transMat);

    transMat = icp2.getFinalTransformation();

    return transMat;
  }

  inline Eigen::Matrix4f getCubeGpsTransform(CloudPtr base, CloudPtr toAlign,
					  Eigen::Matrix4f trans,
					  mapgmu::PathFinder *pf, double *cSize,
					  double *eVal, double currTime, double dt)
  {
    CubeAligner cb(base,*cSize);

    Matrix4f gpsGuess = Matrix4f::Identity();
    calcGpsGuess(trans,pf,currTime,dt,gpsGuess);

    CVertex rv(gpsGuess);
    *eVal = rv.getDist();

    return cb.alignToMatrix(gpsGuess,toAlign,eVal);
  }

  //Tranlation only GPS velocity vector transforms
  inline Eigen::Matrix4f getGpsTransform(Eigen::Matrix4f trans,
					  mapgmu::PathFinder *pf,
					  double currTime, double dt)
  {
    Matrix4f gpsGuess = Matrix4f::Identity();
    calcGpsGuess(trans,pf,currTime,dt,gpsGuess);

    return gpsGuess;
  }

  //simple ICP method
  inline Eigen::Matrix4f getICPTransform(CloudPtr base, CloudPtr toAlign)
  {
    pcl::IterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI> icp;
    pcl::PointCloud<pcl::PointXYZI> Final;
    int maxIter = 50;
    double eps = 1e-6;
    double eVal = 1.0;

    icp.setMaxCorrespondenceDistance (eVal);
    icp.setMaximumIterations (maxIter);
    icp.setEuclideanFitnessEpsilon (eps);
    icp.setInputSource(toAlign);
    icp.setInputTarget(base);
    icp.align(Final);

    return icp.getFinalTransformation();
  }

  //Intensity filtered two pass ICP trans
  inline Eigen::Matrix4f getICPVoxTransform(CloudPtr base, CloudPtr toAlign)
  {
    pcl::IterativeClosestPoint<PointType, PointType> icp;
    pcl::VoxelGrid<PointType> voxGrid;
    CloudPtr voxBase (new Cloud);
    CloudPtr voxAlign (new Cloud);
    Cloud Final;
    double maxCorrDist = 0.2;
    int maxIter = 50;
    double eps = 1e-8;
    double leafSize = 0.1;

    char *corr,*epsC, *leafStr;

    corr = getenv("CORR_DIST");
    epsC = getenv("CONV_EPS");
    leafStr = getenv("LEAF_SIZE");
    if (corr)
    {
      maxCorrDist = atof(corr);
    }
    if (epsC)
    {
      eps = atof(epsC);
    }
    if (leafStr)
    {
      leafSize = atof(leafStr);
    }

    voxGrid.setLeafSize(leafSize,leafSize,leafSize);
    voxGrid.setInputCloud(base);
    voxGrid.filter(*voxBase);
    voxGrid.setInputCloud(toAlign);
    voxGrid.filter(*voxAlign);

    //icp pass, both source and dest voxel filtered
    icp.setMaxCorrespondenceDistance (maxCorrDist);
    icp.setMaximumIterations (maxIter);
    icp.setEuclideanFitnessEpsilon (eps);
    icp.setInputSource(voxAlign);
    icp.setInputTarget(voxBase);
    icp.align(Final);

    std::cout << maxCorrDist << "," << eps << "," << icp.getFitnessScore () << "," << icp.hasConverged () << ",";

    return icp.getFinalTransformation();
  }

  // used to determine next single step ICP where [last] is the last alignment
  inline Eigen::Matrix4f getNextICPStep(CloudPtr base, CloudPtr toAlign,
					Eigen::Matrix4f last)
  {
    pcl::IterativeClosestPoint<PointType, PointType> icp;
    pcl::VoxelGrid<PointType> voxGrid;
    Cloud Final;
    double maxCorrDist = 0.3;
    double eps = 1e-8;

    char *corr,*epsC;

    corr = getenv("CORR_DIST");
    epsC = getenv("CONV_EPS");
    if (corr)
    {
      maxCorrDist = atof(corr);
    }
    if (epsC)
    {
      eps = atof(epsC);
    }

    icp.setMaxCorrespondenceDistance (maxCorrDist);
    icp.setMaximumIterations (1);
    icp.setEuclideanFitnessEpsilon (eps);
    icp.setInputSource(toAlign);
    icp.setInputTarget(base);
    icp.align(Final,last);

    std::cout << "ICP Converged:" << icp.hasConverged () << std::endl
	<< "Fitness: " << icp.getFitnessScore () << std::endl
	<< "EucFit Eps: " << icp.getEuclideanFitnessEpsilon () << std::endl;

    return icp.getFinalTransformation();
  }

  //regular icp transform using "guess" to seed the transform
  inline Eigen::Matrix4f getICPTransformGuess(CloudPtr base, CloudPtr toAlign,
					      Eigen::Matrix4f guess,
					      double maxCorrDist)
  {
    pcl::IterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI> icp;
    pcl::PointCloud<pcl::PointXYZI> Final;
    int maxIter = 50;
    double eps = 1e-6;

    icp.setMaxCorrespondenceDistance (maxCorrDist);
    icp.setMaximumIterations (maxIter);
    icp.setEuclideanFitnessEpsilon (eps);
    icp.setInputSource(toAlign);
    icp.setInputTarget(base);
    icp.align(Final, guess);

    std::cout << "Converged: " << icp.hasConverged () << std::endl
		<< "Fitness: " << icp.getFitnessScore () << std::endl;

    return icp.getFinalTransformation();
  }

  inline Eigen::Matrix4f getICPIntTransform(CloudPtr base, CloudPtr toAlign)
  {
    pcl::IterativeClosestPoint<PointType, PointType> icp;
    CloudPtr filter_base(new Cloud);
    CloudPtr filter_toAlign(new Cloud);
    Cloud Final;
    Eigen::Matrix4f intensityMat = Eigen::Matrix4f::Identity();
    double maxCorrDist = 2.0;
    double eps = 1e-8;
    DoNFilter<PointType> donFilter;

    char *corr,*epsC;

    corr = getenv("CORR_DIST");
    epsC = getenv("CONV_EPS");
    if (corr)
    {
      maxCorrDist = atof(corr);
    }
    if (epsC)
    {
      eps = atof(epsC);
    }

    //filter by intensity, see DoNFilter.hpp
    donFilter.intensityFilter(1.2,base,filter_base);
    donFilter.intensityFilter(1.2,toAlign,filter_toAlign);

    //icp pass 1, use filtered clouds
    icp.setMaxCorrespondenceDistance (maxCorrDist);
    icp.setMaximumIterations (50);
    icp.setEuclideanFitnessEpsilon (eps);
    icp.setInputSource(filter_toAlign);
    icp.setInputTarget(filter_base);
    icp.align(Final);

    intensityMat = icp.getFinalTransformation();

    //icp pass 2, use full clouds
    icp.setMaxCorrespondenceDistance (0.1);
    icp.setInputSource(toAlign);
    icp.setInputTarget(base);
    icp.align(Final,intensityMat);

    std::cout << maxCorrDist << "," << eps << "," << icp.getFitnessScore () << "," << icp.hasConverged () << ",";

    return icp.getFinalTransformation();
  }

  //this was from the PCL site, may work if tweaked, never bother to fix it
  /*inline Eigen::Matrix4f getICPFPFHTransform(CloudPtr base, CloudPtr toAlign,
						      Eigen::Matrix4f guess)
  {
    pcl::IterativeClosestPoint<PointNormalT, PointNormalT> icp;
    PointCloudWithNormalsPtr filter_base(new PointCloudWithNormals);
    PointCloudWithNormalsPtr filter_toAlign(new PointCloudWithNormals);
    pcl::PointCloud<PointNormalT> Final;

    pcl::NormalEstimation<PointType, PointNormalT> norm_est;
    pcl::search::KdTree<PointType>::Ptr tree (new pcl::search::KdTree<PointType> ());
    norm_est.setSearchMethod (tree);
    norm_est.setRadiusSearch (0.1);

    norm_est.setInputCloud (base);
    norm_est.compute (*filter_base);
    pcl::copyPointCloud (*base, *filter_base);

    norm_est.setInputCloud (toAlign);
    norm_est.compute (*filter_toAlign);
    pcl::copyPointCloud (*toAlign, *filter_toAlign);

    pcl::FPFHEstimation<PointType, pcl::Normal, pcl::FPFHSignature33> fpfh;
    // alternatively, if cloud is of tpe PointNormal, do fpfh.setInputNormals (cloud);

    fpfh.setSearchMethod (tree);

    // Output datasets
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_base (new pcl::PointCloud<pcl::FPFHSignature33> ());
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_toAlign (new pcl::PointCloud<pcl::FPFHSignature33> ());

    // Use all neighbors in a sphere of radius 5cm
    // IMPORTANT: the radius used here has to be larger than the radius used to estimate the surface normals!!!
    fpfh.setRadiusSearch (0.2);

    // Compute the features
    //fpfh.setInputCloud (base);
    fpfh.setInputNormals (filter_base);
    fpfh.compute (*fpfhs_base);
    //fpfh.setInputCloud (toAlign);
    fpfh.setInputNormals (filter_toAlign);
    fpfh.compute (*fpfhs_toAlign);

    icp.setMaxCorrespondenceDistance (maxCorrDist);
    // Set the maximum number of iterations (criterion 1)
    icp.setMaximumIterations (maxIter);
    // Set the transformation epsilon (criterion 2)
    icp.setEuclideanFitnessEpsilon (eps);

    icp.setInputSource(filter_toAlign);
    icp.setInputTarget(filter_base);
    icp.align(Final,guess);

    std::cout << "Converged: " << icp.hasConverged () << std::endl
		<< "Fitness: " << icp.getFitnessScore () << std::endl;

    return icp.getFinalTransformation();
    return Eigen::Matrix4f::Identity();
  }*/

  inline Eigen::Matrix4f getICPDoNTransform(CloudPtr base, CloudPtr toAlign)
  {
    pcl::IterativeClosestPoint<PointNormalT, PointNormalT> icp;
    PointCloudWithNormalsPtr filter_base(new PointCloudWithNormals);
    PointCloudWithNormalsPtr filter_toAlign(new PointCloudWithNormals);
    pcl::PointCloud<PointNormalT> Final;
    double maxCorrDist = 2.0;
    double eps = 1e-8;
    DoNFilter<PointType> donFilter;

    char *corr,*epsC;

    corr = getenv("CORR_DIST");
    epsC = getenv("CONV_EPS");
    if (corr)
    {
      maxCorrDist = atof(corr);
    }
    if (epsC)
    {
      eps = atof(epsC);
    }

    donFilter.filter(base,filter_base);
    donFilter.filter(toAlign,filter_toAlign);

    //icp with both source and dest don filtered
    icp.setMaxCorrespondenceDistance (maxCorrDist);
    icp.setMaximumIterations (150);
    icp.setEuclideanFitnessEpsilon (eps);
    icp.setInputSource(filter_toAlign);
    icp.setInputTarget(filter_base);
    icp.align(Final);

    std::cout << maxCorrDist << "," << eps << "," << icp.getFitnessScore () << "," << icp.hasConverged () << ",";

    return icp.getFinalTransformation();
  }

  //one step DoN, not efficient since DoN filtering is done on each step
  inline Eigen::Matrix4f getNextDoNStep(CloudPtr base, CloudPtr toAlign,
					Eigen::Matrix4f last)
  {
    pcl::IterativeClosestPoint<PointNormalT, PointNormalT> icp;
    PointCloudWithNormalsPtr filter_base(new PointCloudWithNormals);
    PointCloudWithNormalsPtr filter_toAlign(new PointCloudWithNormals);
    pcl::PointCloud<PointNormalT> Final;
    double maxCorrDist = 0.3;
    int maxIter = 1;
    double eps = 1e-8;
    DoNFilter<PointType> donFilter;

    char *corr,*epsC;

    corr = getenv("CORR_DIST");
    epsC = getenv("CONV_EPS");
    if (corr)
    {
      maxCorrDist = atof(corr);
    }
    if (epsC)
    {
      eps = atof(epsC);
    }

    donFilter.filter(base,filter_base);
    donFilter.filter(toAlign,filter_toAlign);

    icp.setMaxCorrespondenceDistance (maxCorrDist);
    icp.setMaximumIterations (maxIter);
    icp.setTransformationEpsilon (eps);
    icp.setInputSource(filter_toAlign);
    icp.setInputTarget(filter_base);
    icp.align(Final,last);

    std::cout << "ICP DoN Converged:" << icp.hasConverged () << std::endl
	<< "Fitness: " << icp.getFitnessScore () << std::endl
	<< "EucFit Eps: " << icp.getEuclideanFitnessEpsilon () << std::endl;

    return icp.getFinalTransformation();
  }

  inline Eigen::Matrix4f getNDTDoNTransform(CloudPtr base, CloudPtr toAlign)
  {
    PointCloudWithNormalsPtr filter_base(new PointCloudWithNormals);
    PointCloudWithNormalsPtr filter_toAlign(new PointCloudWithNormals);
    pcl::PointCloud<PointNormalT> Final;

    DoNFilter<PointType> donFilter;

    char *stepSzC,*iter,*epsC,*resC;
    double stepSize = 0.1;
    int maxIter = 35;
    double eps = 0.01;
    double resolution = 1.0;

    stepSzC = getenv("STEP_SIZE");
    iter = getenv("MAX_ITER");
    epsC = getenv("CONV_EPS");
    resC = getenv("NDT_RES");
    if (resC)
    {
      resolution = atof(resC);
    }
    if (stepSzC)
    {
      stepSize = atof(stepSzC);
    }
    if (iter)
    {
      maxIter =atoi(iter);
    }
    if (epsC)
    {
      eps = atof(epsC);
    }

    donFilter.filter(base,filter_base);
    donFilter.filter(toAlign,filter_toAlign);

     // Initializing Normal Distributions Transform (NDT).
    pcl::NormalDistributionsTransform<PointNormalT, PointNormalT> ndt;

    // Setting scale dependent NDT parameters
    // Setting minimum transformation difference for termination condition.
    ndt.setEuclideanFitnessEpsilon (eps);
    // Setting maximum step size for More-Thuente line search.
    ndt.setStepSize (stepSize);
    //Setting Resolution of NDT grid structure (VoxelGridCovariance).
    ndt.setResolution (resolution);

    // Setting max number of registration iterations.
    ndt.setMaximumIterations (maxIter);

    // Setting point cloud to be aligned.
    ndt.setInputSource (filter_toAlign);
    // Setting point cloud to be aligned to.
    ndt.setInputTarget (filter_base);

    ndt.align (Final);

    std::cout << "0" << "," << eps << "," << ndt.getFitnessScore () << "," << ndt.hasConverged () << ",";

    return ndt.getFinalTransformation();
  }

  //adaptive correlation distances, not fully implemented to match:
  //Registration of point clouds using sample-sphere and adaptive distance restriction
  //DOI	 10.1007/s00371-011-0580-0
  inline Eigen::Matrix4f getICPAdaptTransform(CloudPtr base, CloudPtr toAlign)
  {
    pcl::IterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI> icp;
    pcl::PointCloud<pcl::PointXYZI> Final;
    Eigen::Matrix4f trans = Eigen::Matrix4f::Identity();
    double r_start = 1.0;
    double r_stop = 0.1;
    double r_scale = 0.75;
    double r;

    cout << "Warning ADAPT method not fully implemented\n";

    r = r_start;
    while (r > r_stop)
    {
      icp.setMaxCorrespondenceDistance (r);
      icp.setMaximumIterations (2);
      icp.setTransformationEpsilon (1e-10);
      icp.setInputSource(toAlign);
      icp.setInputTarget(base);
      icp.align(Final,trans);

      trans = icp.getFinalTransformation()*trans;
      r = r*r_scale;
      cout << "Step @ r=" << r <<"\n";
    }

    std::cout << "Converged: " << icp.hasConverged () << std::endl
		<< "Fitness: " << icp.getFitnessScore () << std::endl;

    return trans;
  }

  inline Eigen::Matrix4f getNDTTransform(CloudPtr base, CloudPtr toAlign)
  {
    // Filtering input scan to roughly 10% of original size to increase speed of registration.
    pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZI>);

    char *stepSzC,*iter,*epsC,*resC;
    double stepSize = 0.1;
    int maxIter = 35;
    double eps = 0.01;
    double resolution = 1.0;

    stepSzC = getenv("STEP_SIZE");
    iter = getenv("MAX_ITER");
    epsC = getenv("CONV_EPS");
    resC = getenv("NDT_RES");
    if (resC)
    {
      resolution = atof(resC);
    }
    if (stepSzC)
    {
      stepSize = atof(stepSzC);
    }
    if (iter)
    {
      maxIter =atoi(iter);
    }
    if (epsC)
    {
      eps = atof(epsC);
    }

    // Initializing Normal Distributions Transform (NDT).
    pcl::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI> ndt;

    // Setting scale dependent NDT parameters
    // Setting minimum transformation difference for termination condition.
    ndt.setEuclideanFitnessEpsilon (eps);
    // Setting maximum step size for More-Thuente line search.
    ndt.setStepSize (stepSize);
    //Setting Resolution of NDT grid structure (VoxelGridCovariance).
    ndt.setResolution (resolution);

    // Setting max number of registration iterations.
    ndt.setMaximumIterations (maxIter);

    // Setting point cloud to be aligned.
    ndt.setInputSource (toAlign);
    // Setting point cloud to be aligned to.
    ndt.setInputTarget (base);

    // Calculating required rigid transform to align the input cloud to the target cloud.
    pcl::PointCloud<pcl::PointXYZI>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZI>);
    ndt.align (*output_cloud);

    std::cout << "0" << "," << eps << "," << ndt.getFitnessScore () << "," << ndt.hasConverged () << ",";

    return ndt.getFinalTransformation();
  }

  //this is ICP_NL_NORM??
  inline Eigen::Matrix4f getPairTransform(CloudPtr base, CloudPtr toAlign)
  {
    CloudPtr src (new Cloud);
    CloudPtr tgt (new Cloud);
    pcl::ApproximateVoxelGrid<PointType> grid;

    double leafSize = 0.05;
    int kNorms = 10;
    char *leafStr,*kNStr,*corr,*iter,*epsC;
    double maxCorrDist = 0.5;
    int maxIter = 40;
    double eps = 1e-8;

    corr = getenv("CORR_DIST");
    iter = getenv("MAX_ITER");
    epsC = getenv("CONV_EPS");
    leafStr = getenv("LEAF_SIZE");
    kNStr = getenv("K_NORMS");
    if (corr)
    {
      maxCorrDist = atof(corr);
    }
    if (iter)
    {
      maxIter =atoi(iter);
    }
    if (epsC)
    {
      eps = atof(epsC);
    }
    if (leafStr)
    {
      leafSize = atof(leafStr);
    }
    if (kNStr)
    {
      kNorms = atoi(kNStr);
    }

    grid.setLeafSize (leafSize,leafSize,leafSize);
    grid.setInputCloud (base);
    grid.filter (*src);

    grid.setInputCloud (toAlign);
    grid.filter (*tgt);

    // Compute surface normals and curvature
    PointCloudWithNormals::Ptr points_with_normals_src (new PointCloudWithNormals);
    PointCloudWithNormals::Ptr points_with_normals_tgt (new PointCloudWithNormals);
    PointCloudWithNormals::Ptr reg_result (new PointCloudWithNormals);

    pcl::NormalEstimation<PointType, PointNormalT> norm_est;
    pcl::search::KdTree<PointType>::Ptr tree (new pcl::search::KdTree<PointType> ());
    norm_est.setSearchMethod (tree);
    norm_est.setKSearch (kNorms);

    norm_est.setInputCloud (src);
    norm_est.compute (*points_with_normals_src);
    pcl::copyPointCloud (*src, *points_with_normals_src);

    norm_est.setInputCloud (tgt);
    norm_est.compute (*points_with_normals_tgt);
    pcl::copyPointCloud (*tgt, *points_with_normals_tgt);

    //
    // Instantiate our custom point representation (defined above) ...
    MyPointRepresentation point_representation;
    // ... and weight the 'curvature' dimension so that it is balanced against x, y, and z
    float alpha[4] = {1.0, 1.0, 1.0, 1.0};
    point_representation.setRescaleValues (alpha);

    //
    // Align
    pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg;
    reg.setEuclideanFitnessEpsilon (eps);
    // Set the maximum distance between two correspondences (src<->tgt) to 10cm
    // Note: adjust this based on the size of your datasets
    reg.setMaxCorrespondenceDistance (maxCorrDist);
    // Set the point representation
    reg.setPointRepresentation (boost::make_shared<const MyPointRepresentation> (point_representation));

    reg.setInputSource (points_with_normals_src);
    reg.setInputTarget (points_with_normals_tgt);

    // Run the same optimization in a loop and visualize the results
    reg.setMaximumIterations (maxIter);

    // Estimate
    reg.align (*reg_result);

    std::cout << maxCorrDist << "," << eps << "," << reg.getFitnessScore () << "," << reg.hasConverged () << ",";

    // Get the transformation from target to source
    return reg.getFinalTransformation();
  }

};

#endif


#include "cliquesolver/algorithms/algorithms.hpp"

#include <boost/program_options.hpp>

using namespace cliquesolver;
namespace po = boost::program_options;

Output cliquesolver::run(
        std::string algorithm,
        const Instance& instance,
        std::mt19937_64& generator,
        optimizationtools::Info info)
{
    (void)generator;

    std::vector<std::string> algorithm_args = po::split_unix(algorithm);
    std::vector<char*> algorithm_argv;
    for (Counter i = 0; i < (Counter)algorithm_args.size(); ++i)
        algorithm_argv.push_back(const_cast<char*>(algorithm_args[i].c_str()));

    if (algorithm.empty() || algorithm_args[0].empty()) {
        std::cerr << "\033[32m" << "ERROR, missing algorithm." << "\033[0m" << std::endl;
        return Output(instance, info);

    } else if (algorithm_args[0] == "greedy_gwmin") {
        return greedy_gwmin(instance, info);
    } else if (algorithm_args[0] == "greedy_strong") {
        return greedy_strong(instance, info);

#if CPLEX_FOUND
    } else if (algorithm_args[0] == "milp_cplex") {
        MilpCplexOptionalParameters parameters;
        parameters.info = info;
        return milp_cplex(instance, parameters);
#endif

    } else {
        std::cerr << "\033[31m" << "ERROR, unknown algorithm: " << algorithm_argv[0] << "\033[0m" << std::endl;
        assert(false);
        return Output(instance, info);
    }
}


#include <stdio.h>

#include <iostream>
#include <string>

#define  BOOST_VARIANT_USE_RELAXED_GET_BY_DEFAULT

#include <boost/variant/recursive_variant.hpp>
#include <boost/variant/apply_visitor.hpp>
#include <boost/variant/get.hpp>

#include <QString>
#include <QVariant>
#include <QMessageBox>
#include <QDebug>

const int cmd_invalid     = 0;
const int cmd_assign      = 1;  // var = 12
const int cmd_assign_plus = 2;  // var = 12 + 44
const int cmd_assign_new  = 3;  // var = new ...

enum SymbolType_ {
    NoError,
    Undefined,
    NumberError,
    EndOfText
};
SymbolType_ SymbolType;

// ------------------------------------------------------
// namespace to avoid conflicts with other frameworks ...
// ------------------------------------------------------
namespace kallup
{
    int line_no  = 1;
    int file_eof = 0;
    int last_op  = 0;

    QString number, ident;

    int    m_pos = 0; // buffer position
    char * m_TextBuffer;    // the source code buffer
    double value = 0.00;    // math

    struct add_ { };
    struct sub_ { };
    struct mul_ { };
    struct div_ { };

    struct expr   { };
    struct sconst { };
    struct mconst { };

    template <typename OpTag1> struct binary_op;

    class expression_wrapper { public:
          expression_wrapper() { }

        typedef boost::variant<
          double
        , std::string

        , boost::recursive_wrapper< binary_op<add_> >
        , boost::recursive_wrapper< binary_op<sub_> >
        , boost::recursive_wrapper< binary_op<mul_> >
        , boost::recursive_wrapper< binary_op<div_> >

        > expression;

        expression_wrapper::expression operator = (TypeDouble<int> const &val) {
            std::cout << "assiggnn" << std::endl;
        }
    };

    template <typename OpTag1>
    struct binary_op
    {
        expression_wrapper::expression * left ;
        expression_wrapper::expression * right;

        binary_op() { }
        binary_op(double val1, double val2) {
            double res = val1 * val2;
            value = res;
        }

        binary_op(expression_wrapper::expression &exp, double val) {
            auto *left  = new expression_wrapper::expression;
            auto *right = new expression_wrapper::expression;

            left  = value;
            right = val;

            std::cout << "L:   " << left  << std::endl;
            std::cout << "R:   " << right << std::endl;
            std::cout << "val: " << val << std::endl;
            exp = val;
        }
        binary_op(double val, expression_wrapper::expression &exp) {
            auto *left  = new expression_wrapper::expression;
            auto *right = new expression_wrapper::expression;

            //left  = val;
            //right = value;

            std::cout << "L2:   " << left  << std::endl;
            std::cout << "R2:   " << right << std::endl;
            std::cout << "val2: " << val << std::endl;
            //value = boost::get<double>(left);
            exp   = value;
        }

        void get(binary_op<mul_> &obj) {
            auto *left  = new expression_wrapper::expression;
            auto *right = new expression_wrapper::expression;

            std::cout << "pp: " << std::endl;
            left  = obj.left;
            right = obj.right;
        }

        binary_op(expression_wrapper::expression &lhs) {
            auto *left  = new expression_wrapper::expression;
            auto *right = new expression_wrapper::expression;

            left = lhs;
            std::cout << "popser: " << /*boost::get<double>(left) <<*/ std::endl;
        }

        binary_op(expression_wrapper::expression *&obj) {

        }

        binary_op(
              expression_wrapper::expression & lhs,
              expression_wrapper::expression & rhs)
            : left(lhs), right(rhs)
        {
        }

        binary_op(binary_op<mul_> *mul, double val)
        {
            auto *left  = new expression_wrapper::expression;
            auto *right = new expression_wrapper::expression;

            std::cout << "mexter" << std::endl;
            left  = mul;
            right = val;
        }

        binary_op(const binary_op<sub_> &bo)   {
            std::cout << "subser: " << bo.left ->type().name() << std::endl;
            std::cout << "subser: " << bo.right->type().name() << std::endl;

            double res2 = 1.2; //boost::get<double>(bo.right);
            {
                //auto *left  = new expression_wrapper::expression;
                //auto *right = new expression_wrapper::expression;

                value = value - res2;
                //left  = value ;
                std::cout << "ein lefter: " << value << std::endl;
            }
        }
        binary_op(const binary_op<add_> &bo)   {

        }
        binary_op(const binary_op<mul_> &bo)   {
            std::cout << "mopser" << std::endl;
            //auto *left  = new expression_wrapper::expression;
            //auto *right = new expression_wrapper::expression;

            double l = 1; //boost::get<double>(bo.left );
            double r = 2; //boost::get<double>(bo.right);
            double s = r * l;

            //left = value = s;

            std::cout <<  "multerL " << l << std::endl;
            std::cout <<  "multerR " << r << std::endl;
            std::cout <<  "multerS " << s << std::endl;
        }
        binary_op(const binary_op<div_> &bo)   {
        }
    };

    expression_wrapper::expression operator + (
        expression_wrapper::expression &exp1,
        expression_wrapper::expression &exp2)
    {
        std::cout << "plus" << std::endl;

        return exp1;
    }
    expression_wrapper::expression& operator += (expression_wrapper::expression &exp, double val) {
        expression_wrapper::expression tmp = val;
        exp = exp + tmp;
        return exp;
    }

    expression_wrapper::expression& operator - (expression_wrapper::expression& exp,
                            const binary_op<sub_> &v) {
        std::cout << "minus" << std::endl;
        exp = boost::get<boost::recursive_wrapper<double>>(v.left)
            - boost::get<boost::recursive_wrapper<double>>(v.right);
        return exp;
    }
    expression_wrapper::expression& operator +=
       (expression_wrapper::expression &exp1,
        expression_wrapper::expression &exp2) {
        exp1 = exp2;
        return exp1;
    }
    expression_wrapper::expression& operator += (
        expression_wrapper::expression &exp,
        cmd_varset<TypeDouble<int> > &v) {
        std::cout << "molobster1" << std::endl;
        cmd_varset<TypeDouble<int> > tdp = v;
        TypeDouble<int> td = tdp.value; //.value.value = v.value.value;
        td.value = tdp.value.value;
        double r = td.value;
        std::cout << "molobster2" << std::endl;
        std::cout << r  << std::endl;
        value = r;
        exp  += r;
        return  exp;
    }

    expression_wrapper::expression& operator += (
    expression_wrapper::expression& exp, const cmd_print<TypeDoublePrint<int> > &v) {
        exp += v.getVal().value;
        return exp;
    }

    class ast_struct: public boost::static_visitor<double> {
    public:
        double operator()(std::string txt) const {
            std::cout << txt << std::endl;
            return 123.42;
        }
        double operator()(const binary_op<add_> &obj) {

        }
        double operator()(double val) const {
            std::cout << "getter value01: " << val << std::endl;
            value = val;
            //return boost::apply_visitor( ast_struct(), tdp.expr );
            return  value;
        }

        double operator()(binary_op<add_> & binary) {
            //return boost::apply_visitor( ast_struct(), binary.left )
            //     + boost::apply_visitor( ast_struct(), binary.right);
        }
        double operator()(const binary_op<sub_> & binary) const     {
            std::cout << "double subserle" << std::endl;
            std::cout << boost::get<boost::recursive_wrapper<double>>(binary.left)  << std::endl;
            std::cout << boost::get<boost::recursive_wrapper<double>>(binary.right) << std::endl;

            return 2.2; //boost::apply_visitor( ast_struct(), binary.left );
               //- boost::apply_visitor( ast_struct(), binary.right );
        }
        /*
        double operator()(binary_op<mul_> &d0) {
            std::cout << "double:" << std::endl;

            expression_wrapper::expression d1; d1 = binary_op<mul_>(d0.left );
            expression_wrapper::expression d2; d2 = binary_op<mul_>(d0.right);

            expression_wrapper::expression d4; d4 = d1;

            return boost::apply_visitor( ast_struct(), d1)
                 * boost::apply_visitor( ast_struct(), d2);
        }*/
        /*
        double operator()(binary_op<div_> & binary) {
            //boost::recursive_wrapper<binary_op<div_>> tmp1; tmp1 = binary;
            //auto d2 = binary_op<div_>(tmp1);

            expression_wrapper::expression d1; d1 = binary_op<div_>(binary.left);
            expression_wrapper::expression d2; d2 = binary_op<div_>(binary.right);

            return boost::apply_visitor( ast_struct(), d1)
                 / boost::apply_visitor( ast_struct(), d2);
        }*/
    };

    std::ostream& operator << (std::ostream& os, const binary_op<add_> &val) {
        os << "adder" << std::endl;
        return os;
    }
    std::ostream& operator<< (std::ostream& os, const binary_op<sub_> &val) {
        std::cout << "subser" << std::endl;
        return os;
    }
    std::ostream& operator<< (std::ostream& os, binary_op<mul_> val_) {
        os << "muller" << std::endl;
        return os;
    }
    std::ostream& operator<< (std::ostream& os, const binary_op<div_> &val) {
        os << "diver" << std::endl;
        return os;
    }

    // ----------------------------------
    // inline's, to reduce code size, and
    // make code better in reading ...
    // ----------------------------------
    inline int get_ch() {
        return m_TextBuffer[m_pos++];
    }
    inline void get_endl() {
        int c;
        while (1) {
            c = get_ch();
            if (c == '\n') {
                line_no++;
                break;
            }
        }
    }

    bool TokenSkipper = false;  // ws each token?
    // ------------------------------------
    // cut spaces, and return next char ...
    // ------------------------------------
    void SkipWhiteSpaces()
    {
        int c = 0;
        while (1) {
            c = get_ch();
            if (c == '&') {
                c = get_ch();
                if (c == '&') {
                    get_endl();
                    continue;
                }   else break;
            }
            else if (c == '*') {
                c = get_ch();
                if (c == '*') {
                    get_endl();
                    continue;
                }   else break;
            }
            else if (c == '/') {
                c = get_ch();
                if (c == '/') {
                    get_endl();
                    continue;
                }
                else if (c == '*') {
                    while (1) {
                        c = get_ch();
                        if (c == '\n')
                        line_no++;
                        else if (c == '*') {
                            c = get_ch();
                            if (c == '/')
                            break;
                        }
                    }
                    continue;
                }
                else {
                    --m_pos;
                    break;
                }
            }
            else if (isspace(c)) {
                if (c == '\n')
                line_no++;
                continue;
            }
            break;
        }
        --m_pos;
        return;
    }

    QString GetIdent()
    {
        SkipWhiteSpaces();
        ident.clear();

        int c = 0;
        while (1) {
            c = get_ch();
            if (isalnum(c)) {
                ident += c;
            }
            else if (c == '/') {
                c = get_ch();
                if (c == '/') {
                    get_endl();
                    continue;
                }
                else if (c == '*') {
                    while (1) {
                        c = get_ch();
                        if (c == '\n')
                        line_no++;
                        else if (c == '*') {
                            c = get_ch();
                            if (c == '/')
                            break;
                        }
                    }
                }
                break;
            }   else {
                --m_pos;
                break;
            }
        }

        if (ident.length() < 1)
        throw QString("Syntax Error - No data?");

        SkipWhiteSpaces();
        return ident;
    }

    bool Match(char expected)
    {
        SkipWhiteSpaces();
        if (m_TextBuffer[m_pos++] == expected) {
            qDebug() << expected;
            return true;
        }
        throw QString("Expected token '%1' at position: %2")
            .arg(expected)
            .arg(m_pos);
        return false;
    }

    QString GetNumber(void)
    {
        SkipWhiteSpaces();
        number.clear();

        while (1) {
            int c = get_ch();
            if (isdigit(c)) {
                number += c;
                continue;
            }
            else if (c == '.') {
                number += '.';
                continue;
            }
            else if (c == '/') {
                c = get_ch();
                if (c == '/') {
                    get_endl();
                    continue;
                }
                else if (c == '*') {
                    while (1) {
                        c = get_ch();
                        if (c == '\n')
                        line_no++;
                        else if (c == '*') {
                            c = get_ch();
                            if (c == '/')
                            break;
                        }
                    }
                }
            }
            else if (c == EOF || c == 0) {
                if (number.size() > 0)
                break ; else
                throw QString("wrong token as excpected.");
            }
            else if (isspace(c)) {
                if (number.size() > 0)
                break;
                continue;
            }
            else {
                throw QString("number expected, wrong type: >>%1<<").arg(c);
                break;
            }
        }
        SkipWhiteSpaces();
        return number;
    }

    template <typename T>
    struct Token {
        Token() {
            qDebug() << "new token";
        }
    };

    template < typename T >
    struct Token<T*> {
        Token(){ std::cout << "A< T* >" << std::endl; }
    };

    struct TokenTypeAssign { };
    struct TokenTypeSpace  { };

    struct bool_   { };
    struct int_    { };
    struct char_   { };
    struct space_  { };
    struct assign_ { };

    template <> struct Token<TokenTypeAssign> {
    public:
        Token() {
            name = QString("TokenTypeAssign");
            value = 0;
        }
        bool parse() {
            if (!Match('='))
            throw QString("ASSIGN SIGN '=' expected.");
            return true;
        }
        QVariant value;
        QString  name;
    };
    template <> struct Token<TokenTypeSpace> {
    public:
        Token() {
            name  = QString("TokenSpace");
            value = 0;
        }
        bool parse() {
            qDebug() << "parse space";
            SkipWhiteSpaces();
            return true;
        }
        QVariant value;
        QString  name;
    };

    template <> struct Token<bool> {
    public:
        Token() {
            name  = QString("bool");
            value = static_cast<bool>(false);
        }
        QVariant value;
        QString  name;
    };

    template <> struct Token<int> {
    public:
        Token() {
            name  = QString("int");
            value = static_cast<int>(0);
        }
        bool parse() {
            qDebug() << "parse int";
            qDebug() << GetNumber();
            return true;
        }

        QVariant value;
        QString  name;
    };

    template <> struct Token<char> {
    public:
        Token() {
            name  = QString("char");
            value = QString("");
        }
        bool parse() {
            qDebug() << "parse char";
            qDebug() << GetIdent();
            return true;
        }

        QVariant value;
        QString  name;
    };

    Token<bool> bool_;
    Token<int>  int_;
    Token<char> char_;

    Token<TokenTypeSpace> space_;
    Token<TokenTypeAssign> assign_;

    class ParserCommon {
    public:
        ParserCommon(QString src) {
            m_TextBuffer = new char[src.size()+1];
            strcpy(m_TextBuffer,src.toLatin1().data());
        }
        ~ParserCommon() {
            delete m_TextBuffer;
        }
    };

    // ------------------------------------
    // wrapper class for token scanning ...
    // ------------------------------------
    class grammar {
    public:
        grammar() { }
        template <typename T> grammar & operator << (T &t) { t.parse();return *this; }
        template <typename T> grammar & operator +  (T &t) { t.parse();return *this; }
    };

    // -----------------------------------
    // parser oode for an dBase parser ...
    // -----------------------------------
    class dBase {
    public:
        dBase() {
            qDebug() << "handle dbase grammar...";
        }
        void start(QString src)  {
            ParserCommon pc(src);
            grammar go;

            // grammar:
            go +char_ << assign_ << int_;

            // ---------------------------------
            // sanity check, if all token read ?
            // ---------------------------------
            if (m_pos < strlen(m_TextBuffer))
            throw QString("not all input are proceed.");
        }
    };

    template   <typename T>
    class Parser: public T {
    public:
        Parser() {
            qDebug() << "parser init.";
            line_no = 1;
            m_pos   = 0;

            ident .clear();
            number.clear();
        }
    };

    void test()
    {
        expression_wrapper::expression res1 = binary_op<mul_>(2.0 ,5.0);
        //expression_wrapper::expression res2 = binary_op<sub_>(3.0,res1);
        expression_wrapper::expression result = ( res1 );

        expression_wrapper::expression res3 = binary_op<add_>(21.13,0.00);

        std::cout << std::endl
                  << "start visitor..."
                  << std::endl;
        std::cout << boost::apply_visitor(ast_struct(),res3  ) << std::endl; std::cout << "next...\n";
        std::cout << boost::apply_visitor(ast_struct(),result) << std::endl;

    }
}

// int main() {
bool parseText(QString src)
{
    using namespace kallup;
    kallup::test();
    return true;
    try {
        Parser<dBase> p;
        p.start(src);

        QMessageBox::information(0,"Info","SUCCESS");
        return true;
    }
    catch (QString &e) {
        QMessageBox::critical(0,"Error",
        QString("Error in line: %1\n%2")
        .arg(line_no)
        .arg(e));
    }
    return false;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_ALGORITHMS_UNION_HPP
#define BOOST_GEOMETRY_ALGORITHMS_UNION_HPP

#include <boost/mpl/if.hpp>

#include <boost/range/metafunctions.hpp>

#include <boost/geometry/core/is_areal.hpp>
#include <boost/geometry/core/point_order.hpp>
#include <boost/geometry/core/reverse_dispatch.hpp>
#include <boost/geometry/geometries/concepts/check.hpp>
#include <boost/geometry/algorithms/detail/overlay/overlay.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    // tag dispatching:
    typename TagIn1, typename TagIn2, typename TagOut,
    // metafunction finetuning helpers:
    bool Areal1, bool Areal2, bool ArealOut,
    // real types
    typename Geometry1, typename Geometry2,
    bool Reverse1, bool Reverse2, bool ReverseOut,
    typename OutputIterator,
    typename GeometryOut,
    typename Strategy
>
struct union_insert
{
    BOOST_MPL_ASSERT_MSG
        (
            false, NOT_OR_NOT_YET_IMPLEMENTED_FOR_THIS_GEOMETRY_TYPES
            , (types<Geometry1, Geometry2, GeometryOut>)
        );
};

template
<
    typename TagIn1, typename TagIn2, typename TagOut,
    typename Geometry1, typename Geometry2,
    bool Reverse1, bool Reverse2, bool ReverseOut,
    typename OutputIterator,
    typename GeometryOut,
    typename Strategy
>
struct union_insert
    <
        TagIn1, TagIn2, TagOut,
        true, true, true,
        Geometry1, Geometry2,
        Reverse1, Reverse2, ReverseOut,
        OutputIterator, GeometryOut,
        Strategy
    > : detail::overlay::overlay
        <Geometry1, Geometry2, Reverse1, Reverse2, ReverseOut, OutputIterator, GeometryOut, overlay_union, Strategy>
{};

template
<
    typename GeometryTag1, typename GeometryTag2, typename GeometryTag3,
    bool Areal1, bool Areal2, bool ArealOut,
    typename Geometry1, typename Geometry2,
    bool Reverse1, bool Reverse2, bool ReverseOut,
    typename OutputIterator, typename GeometryOut,
    typename Strategy
>
struct union_insert_reversed
{
    static inline OutputIterator apply(Geometry1 const& g1,
            Geometry2 const& g2, OutputIterator out,
            Strategy const& strategy)
    {
        return union_insert
            <
                GeometryTag2, GeometryTag1, GeometryTag3,
                Areal2, Areal1, ArealOut,
                Geometry2, Geometry1,
                Reverse2, Reverse1, ReverseOut,
                OutputIterator, GeometryOut,
                Strategy
            >::apply(g2, g1, out, strategy);
    }
};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace union_
{

template
<
    typename GeometryOut,
    typename Geometry1, typename Geometry2,
    typename OutputIterator,
    typename Strategy
>
inline OutputIterator insert(Geometry1 const& geometry1,
            Geometry2 const& geometry2,
            OutputIterator out,
            Strategy const& strategy)
{
    return boost::mpl::if_c
        <
            geometry::reverse_dispatch<Geometry1, Geometry2>::type::value,
            dispatch::union_insert_reversed
            <
                typename tag<Geometry1>::type,
                typename tag<Geometry2>::type,
                typename tag<GeometryOut>::type,
                geometry::is_areal<Geometry1>::value,
                geometry::is_areal<Geometry2>::value,
                geometry::is_areal<GeometryOut>::value,
                Geometry1, Geometry2,
                overlay::do_reverse<geometry::point_order<Geometry1>::value>::value,
                overlay::do_reverse<geometry::point_order<Geometry2>::value>::value,
                overlay::do_reverse<geometry::point_order<GeometryOut>::value>::value,
                OutputIterator, GeometryOut,
                Strategy
            >,
            dispatch::union_insert
            <
                typename tag<Geometry1>::type,
                typename tag<Geometry2>::type,
                typename tag<GeometryOut>::type,
                geometry::is_areal<Geometry1>::value,
                geometry::is_areal<Geometry2>::value,
                geometry::is_areal<GeometryOut>::value,
                Geometry1, Geometry2,
                overlay::do_reverse<geometry::point_order<Geometry1>::value>::value,
                overlay::do_reverse<geometry::point_order<Geometry2>::value>::value,
                overlay::do_reverse<geometry::point_order<GeometryOut>::value>::value,
                OutputIterator, GeometryOut,
                Strategy
            >
        >::type::apply(geometry1, geometry2, out, strategy);
}

/*!
\brief_calc2{union} \brief_strategy
\ingroup union
\details \details_calc2{union_insert, spatial set theoretic union}
    \brief_strategy. details_insert{union}
\tparam GeometryOut output geometry type, must be specified
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\tparam OutputIterator output iterator
\tparam Strategy \tparam_strategy_overlay
\param geometry1 \param_geometry
\param geometry2 \param_geometry
\param out \param_out{union}
\param strategy \param_strategy{union}
\return \return_out

\qbk{distinguish,with strategy}
*/
template
<
    typename GeometryOut,
    typename Geometry1,
    typename Geometry2,
    typename OutputIterator,
    typename Strategy
>
inline OutputIterator union_insert(Geometry1 const& geometry1,
            Geometry2 const& geometry2,
            OutputIterator out,
            Strategy const& strategy)
{
    concept::check<Geometry1 const>();
    concept::check<Geometry2 const>();
    concept::check<GeometryOut>();

    return detail::union_::insert<GeometryOut>(geometry1, geometry2, out, strategy);
}

/*!
\brief_calc2{union}
\ingroup union
\details \details_calc2{union_insert, spatial set theoretic union}.
    \details_insert{union}
\tparam GeometryOut output geometry type, must be specified
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\tparam OutputIterator output iterator
\param geometry1 \param_geometry
\param geometry2 \param_geometry
\param out \param_out{union}
\return \return_out
*/
template
<
    typename GeometryOut,
    typename Geometry1,
    typename Geometry2,
    typename OutputIterator
>
inline OutputIterator union_insert(Geometry1 const& geometry1,
            Geometry2 const& geometry2,
            OutputIterator out)
{
    concept::check<Geometry1 const>();
    concept::check<Geometry2 const>();
    concept::check<GeometryOut>();

    typedef strategy_intersection
        <
            typename cs_tag<GeometryOut>::type,
            Geometry1,
            Geometry2,
            typename geometry::point_type<GeometryOut>::type
        > strategy;

    return union_insert<GeometryOut>(geometry1, geometry2, out, strategy());
}

}} // namespace detail::union_
#endif // DOXYGEN_NO_DETAIL

/*!
\brief Combines two geometries which each other
\ingroup union
\details \details_calc2{union, spatial set theoretic union}.
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\tparam Collection output collection, either a multi-geometry,
    or a std::vector<Geometry> / std::deque<Geometry> etc
\param geometry1 \param_geometry
\param geometry2 \param_geometry
\param output_collection the output collection
\note Called union_ because union is a reserved word.

\qbk{[include reference/algorithms/union.qbk]}
*/
template
<
    typename Geometry1,
    typename Geometry2,
    typename Collection
>
inline void union_(Geometry1 const& geometry1,
            Geometry2 const& geometry2,
            Collection& output_collection)
{
    concept::check<Geometry1 const>();
    concept::check<Geometry2 const>();

    typedef typename boost::range_value<Collection>::type geometry_out;
    concept::check<geometry_out>();

    detail::union_::union_insert<geometry_out>(geometry1, geometry2,
                std::back_inserter(output_collection));
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_UNION_HPP


/**************************************************************
 *
 * Licensed to the Apache Software Foundation (ASF) under one
 * or more contributor license agreements.  See the NOTICE file
 * distributed with this work for additional information
 * regarding copyright ownership.  The ASF licenses this file
 * to you under the Apache License, Version 2.0 (the
 * "License"); you may not use this file except in compliance
 * with the License.  You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing,
 * software distributed under the License is distributed on an
 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
 * KIND, either express or implied.  See the License for the
 * specific language governing permissions and limitations
 * under the License.
 *
 *************************************************************/

#ifndef OOX_DRAWINGML_TYPES_HXX
#define OOX_DRAWINGML_TYPES_HXX

#include <boost/shared_ptr.hpp>
#include <com/sun/star/style/TabAlign.hpp>
#include <com/sun/star/drawing/TextVerticalAdjust.hpp>
#include <com/sun/star/geometry/IntegerRectangle2D.hpp>
#include <com/sun/star/awt/Point.hpp>
#include <com/sun/star/awt/Size.hpp>
#include <com/sun/star/xml/sax/XFastAttributeList.hpp>
#include "oox/helper/helper.hxx"

namespace oox {
namespace drawingml {

// ============================================================================

const sal_Int32 PER_PERCENT     = 1000;
const sal_Int32 MAX_PERCENT     = 100 * PER_PERCENT;

const sal_Int32 PER_DEGREE      = 60000;
const sal_Int32 MAX_DEGREE      = 360 * PER_DEGREE;

// ============================================================================

struct LineProperties;
typedef ::boost::shared_ptr< LineProperties > LinePropertiesPtr;

struct FillProperties;
typedef ::boost::shared_ptr< FillProperties > FillPropertiesPtr;

struct GraphicProperties;
typedef ::boost::shared_ptr< GraphicProperties > GraphicPropertiesPtr;

struct TextCharacterProperties;
typedef ::boost::shared_ptr< TextCharacterProperties > TextCharacterPropertiesPtr;

struct TextBodyProperties;
typedef ::boost::shared_ptr< TextBodyProperties > TextBodyPropertiesPtr;

class TextBody;
typedef ::boost::shared_ptr< TextBody > TextBodyPtr;

class Shape;
typedef ::boost::shared_ptr< Shape > ShapePtr;

class Theme;
typedef ::boost::shared_ptr< Theme > ThemePtr;

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

namespace table {

class TableProperties;
typedef ::boost::shared_ptr< TableProperties > TablePropertiesPtr;

} // namespace table

// ============================================================================

/** converts the attributes from an CT_Point2D into an awt Point with 1/100th mm */
com::sun::star::awt::Point GetPoint2D( const ::com::sun::star::uno::Reference< ::com::sun::star::xml::sax::XFastAttributeList >& xAttributes );

/** converts the attributes from an CT_TLPoint into an awt Point with 1/1000% */
com::sun::star::awt::Point GetPointPercent( const ::com::sun::star::uno::Reference< ::com::sun::star::xml::sax::XFastAttributeList >& xAttribs );

/** converts the attributes from an CT_Size2D into an awt Size with 1/100th mm */
com::sun::star::awt::Size GetSize2D( const ::com::sun::star::uno::Reference< ::com::sun::star::xml::sax::XFastAttributeList >& xAttributes );

/** converts the attributes from a CT_RelativeRect to an IntegerRectangle2D */
com::sun::star::geometry::IntegerRectangle2D GetRelativeRect( const ::com::sun::star::uno::Reference< ::com::sun::star::xml::sax::XFastAttributeList >& xAttributes );

/** converts an emu string into 1/100th mmm but constrain as per ST_TextMargin
 * see 5.1.12.73
 */
sal_Int32 GetTextMargin( const ::rtl::OUString& sValue );

/** converts EMUs into 1/100th mmm */
sal_Int32 GetCoordinate( sal_Int32 nValue );

/** converts an emu string into 1/100th mmm */
sal_Int32 GetCoordinate( const ::rtl::OUString& sValue );

/** converts a ST_Percentage % string into 1/1000th of % */
sal_Int32 GetPercent( const ::rtl::OUString& sValue );

/** Converts a ST_PositiveFixedPercentage to a float. 1.0 == 100% */
double GetPositiveFixedPercentage( const ::rtl::OUString& sValue );

/** converts the ST_TextFontSize to point */
float GetTextSize( const ::rtl::OUString& rValue );

/** converts the ST_TextSpacingPoint to 1/100mm */
sal_Int32 GetTextSpacingPoint(  const ::rtl::OUString& sValue );
sal_Int32 GetTextSpacingPoint(  const sal_Int32 nValue );

::com::sun::star::drawing::TextVerticalAdjust GetTextVerticalAdjust( sal_Int32 nToken );

/** */
::com::sun::star::style::TabAlign GetTabAlign( ::sal_Int32 aToken );

float GetFontHeight( sal_Int32 nHeight );

sal_Int16 GetFontUnderline( sal_Int32 nToken );

sal_Int16 GetFontStrikeout( sal_Int32 nToken );

sal_Int16 GetCaseMap( sal_Int32 nToken );

/** converts a paragraph align to a ParaAdjust */
sal_Int16 GetParaAdjust( sal_Int32 nAlign );

// ============================================================================

// CT_IndexRange
struct IndexRange {
	sal_Int32 start;
	sal_Int32 end;
};

/** retrieve the content of CT_IndexRange */
IndexRange GetIndexRange( const ::com::sun::star::uno::Reference< ::com::sun::star::xml::sax::XFastAttributeList >& xAttributes );

// ============================================================================

const sal_Int32 EMU_PER_HMM = 360;      /// 360 EMUs per 1/100 mm.

/** Converts the passed 32-bit integer value from 1/100 mm to EMUs. */
inline sal_Int64 convertHmmToEmu( sal_Int32 nValue )
{
    return static_cast< sal_Int64 >( nValue ) * EMU_PER_HMM;
}

/** Converts the passed 64-bit integer value from EMUs to 1/100 mm. */
inline sal_Int32 convertEmuToHmm( sal_Int64 nValue )
{
    return getLimitedValue< sal_Int32, sal_Int64 >( (nValue + EMU_PER_HMM / 2) / EMU_PER_HMM, 0, SAL_MAX_INT32 );
}

// ============================================================================

/** A structure for a point with 64-bit integer components. */
struct EmuPoint
{
    sal_Int64           X;
    sal_Int64           Y;

    inline explicit     EmuPoint() : X( 0 ), Y( 0 ) {}
    inline explicit     EmuPoint( sal_Int64 nX, sal_Int64 nY ) : X( nX ), Y( nY ) {}
};

// ============================================================================

/** A structure for a size with 64-bit integer components. */
struct EmuSize
{
    sal_Int64           Width;
    sal_Int64           Height;

    inline explicit     EmuSize() : Width( 0 ), Height( 0 ) {}
    inline explicit     EmuSize( sal_Int64 nWidth, sal_Int64 nHeight ) : Width( nWidth ), Height( nHeight ) {}
};

// ============================================================================

/** A structure for a rectangle with 64-bit integer components. */
struct EmuRectangle : public EmuPoint, public EmuSize
{
    inline explicit     EmuRectangle() {}
    inline explicit     EmuRectangle( const EmuPoint& rPos, const EmuSize& rSize ) : EmuPoint( rPos ), EmuSize( rSize ) {}
    inline explicit     EmuRectangle( sal_Int64 nX, sal_Int64 nY, sal_Int64 nWidth, sal_Int64 nHeight ) : EmuPoint( nX, nY ), EmuSize( nWidth, nHeight ) {}

    inline void         setPos( const EmuPoint& rPos ) { static_cast< EmuPoint& >( *this ) = rPos; }
    inline void         setSize( const EmuSize& rSize ) { static_cast< EmuSize& >( *this ) = rSize; }
};

// ============================================================================

} // namespace drawingml
} // namespace oox

#endif


#ifndef BOOST_MPL_AUX_SINGLE_ELEMENT_ITER_HPP_INCLUDED
#define BOOST_MPL_AUX_SINGLE_ELEMENT_ITER_HPP_INCLUDED

// Copyright Aleksey Gurtovoy 2000-2004
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/mpl for documentation.

// $Id$
// $Date$
// $Revision$

#include <boost/mpl/iterator_tags.hpp>
#include <boost/mpl/advance_fwd.hpp>
#include <boost/mpl/distance_fwd.hpp>
#include <boost/mpl/next_prior.hpp>
#include <boost/mpl/deref.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/aux_/value_wknd.hpp>
#include <boost/mpl/aux_/config/ctps.hpp>

namespace boost { namespace mpl {

namespace aux {

template< typename T, int is_last_ >
struct sel_iter;

template< typename T >
struct sel_iter<T,0>
{
    typedef random_access_iterator_tag category;
    typedef sel_iter<T,1> next;
    typedef T type;
};

template< typename T >
struct sel_iter<T,1>
{
    typedef random_access_iterator_tag category;
    typedef sel_iter<T,0> prior;
};

} // namespace aux

template< typename T, int is_last_, typename Distance >
struct advance< aux::sel_iter<T,is_last_>,Distance>
{
    typedef aux::sel_iter<
          T
        , ( is_last_ + BOOST_MPL_AUX_NESTED_VALUE_WKND(int, Distance) )
        > type;
};

template<
      typename T
    , int l1
    , int l2
    >
struct distance< aux::sel_iter<T,l1>, aux::sel_iter<T,l2> >
    : int_<( l2 - l1 )>
{
};

}}

#endif // BOOST_MPL_AUX_SINGLE_ELEMENT_ITER_HPP_INCLUDED


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_BITWISE_FUNCTIONS_SIMD_SSE_SSE2_SHRAI_HPP_INCLUDED
#define BOOST_SIMD_BITWISE_FUNCTIONS_SIMD_SSE_SSE2_SHRAI_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT
#include <boost/simd/bitwise/functions/shrai.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/include/functions/simd/is_gtz.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/group.hpp>
#include <boost/simd/include/functions/simd/split.hpp>
#include <boost/simd/include/functions/simd/make.hpp>
#include <boost/simd/bitwise/functions/simd/common/shrai.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::shrai_, boost::simd::tag::sse2_
                            , (A0)(A1)
                            , ((simd_<int32_<A0>,boost::simd::tag::sse_>))
                              (scalar_< integer_<A1> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      typedef typename dispatch::meta::as_integer<A0,signed>::type sint;
      sint const that = _mm_srai_epi32(bitwise_cast<sint>(a0), a1);
      return bitwise_cast<A0>(that);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::shrai_, boost::simd::tag::sse2_
                            , (A0)(A1)
                            , ((simd_<int16_<A0>,boost::simd::tag::sse_>))
                              (scalar_< integer_<A1> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      typedef typename dispatch::meta::as_integer<A0,signed>::type sint;
      sint const that = _mm_srai_epi16(bitwise_cast<sint>(a0), a1);
      return bitwise_cast<A0>(that);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::shrai_, boost::simd::tag::sse2_
                            , (A0)(A1)
                            , ((simd_<int8_<A0>,boost::simd::tag::sse_>))
                              (scalar_< integer_<A1> >)
                            )
  {
    typedef A0                                              result_type;
    typedef typename meta::make_dependent<int16_t,A0>::type sub_t;
    typedef native<sub_t, boost::simd::tag::sse_>           gen_t;
    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      gen_t a0h, a0l;
      boost::fusion::tie(a0l, a0h) = split(a0);
      return bitwise_cast<A0>(group(shrai(a0l, a1),shrai(a0h, a1)));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::shrai_, boost::simd::tag::sse2_
                            , (A0)(A1)
                            , ((simd_<int64_<A0>,boost::simd::tag::sse_>))
                              (scalar_< integer_<A1> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      return make<A0>(shrai(a0[0], a1), shrai(a0[1], a1));
    }
  };
} } }

#endif
#endif


#include <ros/ros.h>
#include <cstdio>
#include <Eigen/Eigen>
#include "stdio.h"
#include "iostream"
#include "message_filters/subscriber.h"
#include "visualization_msgs/MarkerArray.h"
#include "std_srvs/Empty.h"
#include "semanticscreator.h"

#define PALLET_THETA 0.01564559473350094
/*
BIG Pick items
      <x>-4.252623558044434</x>
      <y>0.051172733306884766</y>
      <theta>-0.005226923516850496</theta>*/
#define P1 -3.45,-0.75,0
#define P2 -4.95,-0.75,0
#define P3 -4.95, 0.75,0
#define P4 -3.45, 0.75,0

/*small item picking site 2
    <Pose name="toPose">
      <x>-21.755008697509766</x>
      <y>2.7174549102783203</y>
      <theta>-1.5736400093450944</theta>*/
#define P11 -22.5,2.0  ,0
#define P12 -21  ,2.0  ,0
#define P13 -21  ,3.5  ,0
#define P14 -22.5,3.5  ,0

/*      <x>-22.966995239257812</x>
      <y>-5.024758815765381</y>
      <theta>0.01564559473350094</theta>*/
using namespace std;

using namespace Eigen;
class semantic_map_publisher {

public:
  // Constructor
  semantic_map_publisher(ros::NodeHandle param_nh)
  {

    param_nh.param<std::string>("topic_prefix",topic_prefix_,"");

    marker_publisher_=nh_.advertise<visualization_msgs::MarkerArray>(topic_prefix_+"semantics_output",100);
    // marker_publisher_text_=nh_.advertise<visualization_msgs::MarkerArray>(topic_prefix_+"semantics_output_text",100);
    publish_service_ = param_nh.advertiseService("publish_semantics", &semantic_map_publisher::PublishSemantics, this);
    semantics_creator_=new  semantics::SemanticsCreator();

  }
  bool PublishSemantics(std_srvs::Empty::Request  &req,
                        std_srvs::Empty::Response &res ) {
    unsigned id=0;
    cout<<"Plotting semantics "<<endl;
    Eigen::Affine3d pose_pallet=Eigen::Affine3d::Identity();
    pose_pallet.translation()<<-22.966995239257812,-5.024758815765381,0.075;

    Eigen::AngleAxisd rollAngle(0, Eigen::Vector3d::UnitZ());
    Eigen::AngleAxisd yawAngle(PALLET_THETA, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd pitchAngle(0, Eigen::Vector3d::UnitX());
    Eigen::Quaternion<double> q = rollAngle * yawAngle * pitchAngle;
       pose_pallet.linear()=q.matrix();
 visualization_msgs::MarkerArray pallet_marker;
    semantics_creator_->GetEuroPallet(pose_pallet,pallet_marker,1,"pallet1");
    publishSemanticsWithText(pallet_marker);
    {
      visualization_msgs::MarkerArray arr_picking_site1;
      std_vector3d points;
      Eigen::Vector3d point1(P1);
      points.push_back(point1);
      Eigen::Vector3d point2(P2);
      points.push_back(point2);
      Eigen::Vector3d point3(P3);
      points.push_back(point3);
      Eigen::Vector3d point4(P4);
      points.push_back(point4);
      points.push_back(point1);
      semantics_creator_->GetPickingArea(arr_picking_site1,points,2,"Picking site: small items");
      publishSemanticAreaWithText(arr_picking_site1);
    }
    {
      visualization_msgs::MarkerArray arr_picking_site2;
      std_vector3d points;
      Eigen::Vector3d point1(P11);
      points.push_back(point1);
      Eigen::Vector3d point2(P12);
      points.push_back(point2);
      Eigen::Vector3d point3(P13);
      points.push_back(point3);
      Eigen::Vector3d point4(P14);
      points.push_back(point4);
      points.push_back(point1);
      semantics_creator_->GetPickingArea(arr_picking_site2,points,3,"Picking site: large items");
      publishSemanticAreaWithText(arr_picking_site2);
    }
    return true;

  }

  void publishSemanticAreaWithText( visualization_msgs::MarkerArray &arr){
    visualization_msgs::MarkerArray markers=arr;
    marker_publisher_.publish(markers);

    Eigen::Vector3d sum(0,0,0);
    for(int i=0;i<markers.markers[0].points.size();i++){
      sum(0)=sum(0)+markers.markers[0].points[i].x;
      sum(1)=sum(1)+markers.markers[0].points[i].y;
      sum(2)=sum(2)+markers.markers[0].points[i].z;
    }
    sum/=markers.markers[0].points.size();

    markers.markers[0].type=visualization_msgs::Marker::TEXT_VIEW_FACING;
    markers.markers[0].ns=markers.markers[0].ns+"_text";
    markers.markers[0].pose.position.x=sum(0);
    markers.markers[0].pose.position.y=sum(1);
    markers.markers[0].pose.position.z=sum(2)+markers.markers[0].scale.z/2.0+0.5;
    markers.markers[0].scale.z=0.4;
    std::cout<<"ns="<<markers.markers[0].ns<<", text="<<markers.markers[0].text<<endl;
    marker_publisher_.publish(markers);
  }

  void publishSemanticsWithText( visualization_msgs::MarkerArray &arr){
    visualization_msgs::MarkerArray markers=arr;
    marker_publisher_.publish(markers);
    markers.markers[0].type=visualization_msgs::Marker::TEXT_VIEW_FACING;
    markers.markers[0].ns=markers.markers[0].ns+"_text";
    markers.markers[0].pose.position.z=markers.markers[0].pose.position.z+markers.markers[0].scale.z/2.0+0.5;
    markers.markers[0].scale.z=0.4;
    marker_publisher_.publish(markers);
  }

private:
  ros::NodeHandle nh_;
  ros::Publisher marker_publisher_,marker_publisher_text_;
  std::string topic_prefix_;
  semantics::SemanticsCreator *semantics_creator_;
  ros::ServiceServer publish_service_;

};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "ndt_fuser_node");

  ros::NodeHandle param("~");
  semantic_map_publisher t(param);
  ros::spin();

  return 0;
}


/* By downloading, copying, installing or using the software you agree to this license.
 *
 *                           License Agreement
 *                      For heuristic_search library
 *
 * Copyright (c) 2016,
 * Maciej Przybylski <maciej.przybylski@mchtr.pw.edu.pl>,
 * Warsaw University of Technology.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the copyright holders nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include <cstdlib>
#include <iostream>

#include <boost/program_options.hpp>

#include <heuristic_search/AStar.h>
#include <heuristic_search/IncrementalSearch.h>
#include <heuristic_search/DStarExtraLite.h>
#include <heuristic_search/DStarMain.h>
#include <heuristic_search/loggers/DStarLogger.h>

#include <search_domains/movingai_benchmark/Domain.h>
#include <search_domains/movingai_benchmark/loader/MapLoader.h>
#include "search_domains/movingai_benchmark/loader/ScenarioLoader.h"
#include "search_domains/movingai_benchmark/benchmark/DynamicExperiment.h"
#include "search_domains/movingai_benchmark/ProgramOptions.h"
#include "search_domains/movingai_benchmark/debug_cv/DStarMain.h"
#include <search_domains/movingai_benchmark/debug_cv/debug_cv.h>
#include <search_domains/movingai_benchmark/debug_cv/Search.h>
#include <search_domains/movingai_benchmark/debug_cv/DynamicAlgorithms.h>

typedef heuristic_search::SearchAlgorithmBegin<
            heuristic_search::loggers::DStarLogger<
            heuristic_search::DStarMain<
            heuristic_search::SearchLoop<
            heuristic_search::DStarExtraLite<
            heuristic_search::IncrementalSearch_SearchStep<
            heuristic_search::AStar<
            heuristic_search::IncrementalSearch_Initialize<
            heuristic_search::HeuristicSearch<
            heuristic_search::StdOpenList<
            heuristic_search::StdSearchSpace<
        heuristic_search::SearchAlgorithmEnd<movingai_benchmark::Domain> > > > > > > > > > > >::Algorithm_t Algorithm_t;

typedef heuristic_search::SearchAlgorithmBegin<
            heuristic_search::loggers::DStarLogger<
            movingai_benchmark::debug_cv::DStarMain<
            heuristic_search::DStarMain<
            movingai_benchmark::debug_cv::Search<
            heuristic_search::SearchLoop<
            heuristic_search::DStarExtraLite<
            heuristic_search::IncrementalSearch_SearchStep<
            heuristic_search::AStar<
            heuristic_search::IncrementalSearch_Initialize<
            heuristic_search::HeuristicSearch<
            heuristic_search::StdOpenList<
            heuristic_search::StdSearchSpace<
        heuristic_search::SearchAlgorithmEnd<movingai_benchmark::Domain> > > > > > > > > > > > > >::Algorithm_t Algorithm_CV_t;

int main(int argc, char** argv)
{
    typedef movingai_benchmark::Domain::State State_t;

    namespace po = boost::program_options;

    po::options_description visible_ops;
    visible_ops.add(movingai_benchmark::generalOptions())
               .add(movingai_benchmark::singleProblemOptions())
               .add(movingai_benchmark::dynamicExperimentOptions())
               .add(movingai_benchmark::visualizationOptions())
               .add(movingai_benchmark::configOptions());

    po::options_description basic_ops;
    basic_ops.add(movingai_benchmark::generalOptions())
                .add(movingai_benchmark::configOptions());

    try
    {
        po::variables_map basic_vm;
        po::store(
            po::basic_command_line_parser<char>(argc,argv).options(basic_ops).allow_unregistered().run(),
            basic_vm);
        po::notify(basic_vm);

        if(basic_vm.count("help"))
        {
            std::cout << visible_ops << "\n";
            return 1;
        }

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

        if(basic_vm.count("config-file"))
        {
            std::string config_filename = basic_vm["config-file"].as<std::string>();
            std::cout << "Loading params from file: " << config_filename << "\n";
            po::store(po::parse_config_file<char>(config_filename.c_str(), visible_ops), vm);
        }

        po::notify(vm);

        movingai_benchmark::loader::Experiment problem;
        problem.SetMap(vm["map-file"].as<std::string>());
        problem.SetStartX(vm["start.x"].as<int>());
        problem.SetStartY(vm["start.y"].as<int>());
        problem.SetGoalX(vm["goal.x"].as<int>());
        problem.SetGoalY(vm["goal.y"].as<int>());

        movingai_benchmark::benchmark::DynamicExperimentParams dynamic_experiment_params;
        dynamic_experiment_params.observation_range = vm["observation-range"].as<int>();
        dynamic_experiment_params.preinitialized = vm["preinitialized"].as<bool>();
        dynamic_experiment_params.undirected = vm["undirected"].as<bool>();

        bool visualization = vm["visualization"].as<bool>();
        movingai_benchmark::debug_cv::VisualizationConfig vis_config;
        vis_config.scale = vm["vis-scale"].as<double>();
        vis_config.delay = vm["vis-delay"].as<int>();
        vis_config.hide_text = vm["vis-hide-text"].as<bool>();

        int W;
        int H;
        unsigned char *known_map;
        unsigned char *unknown_map;

        if(!movingai_benchmark::loader::loadMap(problem.GetMapName(),W,H,unknown_map))
        {
            std::cerr << "Couldn't open file " << problem.GetMapName() << std::endl;
            return -1;
        }

        State_t start_state(problem.GetStartX(),problem.GetStartY());
        State_t goal_state(problem.GetGoalX(),problem.GetGoalY());

        known_map = new unsigned char[W*H];
        std::memset(known_map, movingai_benchmark::UNKNOWN_TERRAIN, W*H);

        movingai_benchmark::Domain domain(known_map, unknown_map, W, H, dynamic_experiment_params.observation_range);

        if(visualization)
        {
            Algorithm_CV_t dstar_extra_lite(domain, heuristic_search::SearchingDirection::Backward,
                dynamic_experiment_params.preinitialized, dynamic_experiment_params.undirected);

            dstar_extra_lite.cv_window_name = "D* Extra Lite - demo";
            dstar_extra_lite.cv_scale = vis_config.scale;
            dstar_extra_lite.cv_window_delay = vis_config.delay;
            dstar_extra_lite.cv_hide_text = vis_config.hide_text;

            dstar_extra_lite.main(start_state, goal_state);

            heuristic_search::loggers::Log::writeHeader(std::cout);
            std::cout << std::endl;
            std::cout << dstar_extra_lite.log << std::endl;

            cv::waitKey(0);
            cv::destroyAllWindows();
        }
        else
        {
            Algorithm_t dstar_extra_lite(domain, heuristic_search::SearchingDirection::Backward,
                dynamic_experiment_params.preinitialized, dynamic_experiment_params.undirected);

            dstar_extra_lite.main(start_state, goal_state);

            heuristic_search::loggers::Log::writeHeader(std::cout);
            std::cout << std::endl;
            std::cout << dstar_extra_lite.log << std::endl;
        }

        delete [] known_map;
        delete [] unknown_map;

    }
    catch(boost::program_options::error const&e)
    {
        std::cout << e.what() << "\nCheck --help\n";
        return 1;
    }

    return 0;
}