/*  $Id$
 *
 *  Copyright 2010 Anders Wallin (anders.e.e.wallin "at" gmail.com)
 *
 *  This file is part of OpenCAMlib.
 *
 *  OpenCAMlib is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  OpenCAMlib is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with OpenCAMlib.  If not, see <http://www.gnu.org/licenses/>.
*/

// this is mostly a translation to c++ of the earlier c# code
// http://code.google.com/p/monocam/source/browse/trunk/Project2/monocam_console/monocam_console/kdtree.cs

#include <boost/foreach.hpp>

#include "millingcutter.h"
#include "point.h"
#include "triangle.h"
#include "numeric.h"
#include "kdtree.h"

namespace ocl
{

//#define DEBUG_KD

//********   KDNode ********************** */

KDNode::KDNode(int d, double cv, KDNode *parent, KDNode *hi_c, KDNode *lo_c,
            const std::list<Triangle> *tlist, int lev)
{
    dim = d;
    cutval = cv;
    up = parent;
    hi = hi_c;
    lo = lo_c;
    tris = tlist;
    level = lev;
}

/// given a list of triangles, build and return the root node of a kd-tree with the triangles
KDNode* KDNode::build_kdtree(const std::list<Triangle> *tris,
                             unsigned int bucketSize,   // defaults to 1
                             int level,                 // defualts to 0 == root
                             KDNode *parent)            // defaults to NULL
{

    if (tris->size() == 0) { //this is a fatal error.
        std::cout << "kdtree.cpp ERROR: build_kdtree() called with tris->size()==0 ! \n";
        assert(0);
        return 0;
    }

    // calculate spread in order to know how to cut
    //static int spreadstat[4];
    Spread* spr = KDNode::spread(tris);
    //spreadstat[ spr->d ]++;
    // calculate cut value
    double cutvalue = spr->start + spr->val/2; // cut in the middle

    // if spr.val==0 (no need/possibility to subdivide anymore)
    // OR number of triangles is smaller than bucketSize
    // then return a bucket node
    if ( (tris->size() <= bucketSize) || (spr->val == 0.0)) {
        //std::cout << "Bucket with len(tris)=" << tris->size() << "\n";
        KDNode *bucket;
        if ( (spr->d == 0) || (spr->d == 2) )  {// maxx or maxy cut
            // does the node spread value make a difference here??
            bucket = new KDNode(spr->d, spr->start+spr->val, parent , NULL, NULL, tris, level);
            //bucket = new KDNode(spr->d, 0.0 , parent , NULL, NULL, tris, level);
        }
        else {// the min cut case:
            // does the node spread value make a difference here??
             bucket = new KDNode(spr->d, spr->start, parent , NULL, NULL, tris, level);
            //bucket = new KDNode(spr->d, 0.0 , parent , NULL, NULL, tris, level);
        }
        return bucket;
    }

    // build lists of triangles for hi and lo child nodes
    std::list<Triangle> *lolist = new std::list<Triangle>();
    std::list<Triangle> *hilist = new std::list<Triangle>();
    BOOST_FOREACH(Triangle t, *tris) { // loop through each triangle and put it in either lolist or hilist
        #ifdef DEBUG_KD
            std::cout << "adding tri=" << t;
            std::cout << " dim=" << spr->d << "\n";
        #endif
        if (spr->d == 0) { // cut along maxx
            if (t.maxx > cutvalue) {
                hilist->push_back(t);
            }
            else {
                lolist->push_back(t);
            }
        } else if (spr->d == 1) { // cut along minx
            if (t.minx > cutvalue) {
                hilist->push_back(t);
            }
            else {
                lolist->push_back(t);
            }
        } else if (spr->d == 2) { // cut along maxy
            if (t.maxy > cutvalue) {
                hilist->push_back(t);
            }
            else {
                lolist->push_back(t);
            }
        } else if (spr->d == 3) { // cut along miny
            if (t.miny > cutvalue) {
                hilist->push_back(t);
            }
            else {
                lolist->push_back(t);
            }
        }

    } // end loop through each triangle

    if (hilist->size() == 0) {// an error ??
        std::cout << "kdtree.cpp: hilist.size()==0!\n";
        assert(0);
    }
    if (lolist->size() == 0) {
        std::cout << "kdtree.cpp: lolist.size()==0!\n";
        assert(0);
    }

    /*
    std::cout << "hilist.size()=" << hilist->size() << "\n";
    std::cout << "lolist.size()=" << lolist->size() << "\n";
    std::cout << "sum: " << hilist->size()+lolist->size() << "== " << tris->size() << " ? \n";
    char c;
    std::cin >> c;
    */

    //                         dim     value    parent  hi   lo   trilist  level
    KDNode *node = new KDNode(spr->d, cutvalue, parent, NULL,NULL,NULL, level);

    // recursion:                   list    bucketsize   level   parent
    node->hi = KDNode::build_kdtree(hilist, bucketSize, level+1, node);
    node->lo = KDNode::build_kdtree(lolist, bucketSize, level+1, node);

    /*
    for (int m=0;m<4;m++)
        std::cout << m << " : " << spreadstat[ m ] << "\n";
    */
    // return a new node
    return node;
}

bool Spread::spread_compare(Spread *x, Spread *y) {
    if (x->val > y->val)
        return true;
    else
        return false;
}

int KDNode::cutcount=0;

/// find the maximum 'extent' of Triangle list tris along dimension d
Spread* KDNode::spread(const std::list<Triangle> *tris) {
    double max_xplus, min_xplus, max_xminus, min_xminus;
    double max_yplus, min_yplus, max_yminus, min_yminus;

    double spr_xplus, spr_xminus, spr_yplus, spr_yminus;

    if (tris->size() == 0) {
        std::cout << "kdtree.cpp ERROR, spread() called with tris->size()==0 ! \n";
        assert( 0 );
        return NULL;
    } else {
        bool first=true;
        BOOST_FOREACH(Triangle t, *tris) {
            // t.calcBB(); //ugly...
            if (first) { // initialize things on the first run
                max_xplus  = t.maxx;
                min_xplus  = t.maxx;
                max_xminus = t.minx;
                min_xminus = t.minx;

                max_yplus  = t.maxy;
                min_yplus  = t.maxy;
                max_yminus = t.miny;
                min_yminus = t.miny;
                first=false;
            } // end initialize
            else { // FIXME madness really, but for only 4 dimensions this works...

                // compute spread in xplus
                if (t.maxx > max_xplus)
                    max_xplus = t.maxx;
                if (t.maxx < min_xplus)
                    min_xplus = t.maxx;

                // compute spread in xminus
                if (t.minx > max_xminus)
                    max_xminus = t.minx;
                if (t.minx < min_xminus)
                    min_xminus = t.minx;

                // compute spread in yplus
                if (t.maxy > max_yplus)
                    max_yplus = t.maxy;
                if (t.maxy < min_yplus)
                    min_yplus = t.maxy;

                // compute spread in yminus
                if (t.miny > max_yminus)
                    max_yminus = t.miny;
                if (t.miny < min_yminus)
                    min_yminus = t.miny;
            }

        } // end FOREACH triangle in tris

        // calculate the spread along each dimension
        spr_xplus  =  max_xplus  - min_xplus;
        spr_xminus =  max_xminus - min_xminus;
        spr_yplus  =  max_yplus  - min_yplus;
        spr_yminus =  max_yminus - min_yminus;

        // spreads are distances, they should be zero or positive.
        assert(  spr_xplus  >= 0.0 );
        assert(  spr_xminus >= 0.0 );
        assert(  spr_yplus  >= 0.0 );
        assert(  spr_yminus >= 0.0 );

        // put the spreads in a list
        std::vector<Spread*> spreads;
        spreads.push_back( new Spread(0, spr_xplus , min_xplus)   );  // dim=0  is maxx
        spreads.push_back( new Spread(1, spr_xminus, min_xminus) );   // dim=1  is minx
        spreads.push_back( new Spread(2, spr_yplus , min_yplus)   );  // dim=2  is maxy
        spreads.push_back( new Spread(3, spr_yminus, min_yminus) );   // dim=3  is miny
        std::sort(spreads.begin(), spreads.end(), Spread::spread_compare); // sort the list
        // priority-queue could also be used ??

        /*
        std::cout << "\n";
        std::cout <<"spreads for " << tris->size() << " triangles:\n";
        std::cout <<"0: spread=" << spr_xplus << "\n";
        std::cout <<"1: spread=" << spr_xminus << "\n";
        std::cout <<"2: spread=" << spr_yplus << "\n";
        std::cout <<"3: spread=" << spr_yminus << "\n";
        std::cout << " selecting " << (spreads[0])->d << " with s="<< (spreads[0])->val << "\n";
        char c;
        std::cin >> c;
        */

        cutcount++;
        if (cutcount == 4)
            cutcount = 0;

        // select each dim in turn
        //return spreads[ cutcount ];

        // select the biggest spread and return
        return spreads[ 0 ];

    } // end tris->size != 0

} // end spread()

bool KDNode::overlap(const KDNode *node, const CLPoint &cl, const MillingCutter &cutter)
{
    switch(node->dim) {
        case 0: // cut along xplus
            if ( node->cutval <= cl.x - cutter.getRadius() )
                return false;
            else
                return true;
            break;
        case 1: // cut along xminus
            if ( node->cutval >= cl.x + cutter.getRadius() )
                return false;
            else
                return true;
            break;
        case 2: // cut along yplus
            if ( node->cutval <= cl.y - cutter.getRadius() )
                return false;
            else
                return true;
            break;
        case 3: // cut along yminus
            if ( node->cutval >= cl.y + cutter.getRadius() )
                return false;
            else
                return true;
            break;
        default:
            assert(0);
    } // end of switch(dim)

    return false;
}

/// returns all triangles under KDNode node in the tree
void KDNode::getTriangles( std::list<Triangle> *tris, KDNode *node)
{
    if (node->tris != NULL) {
        // found a bucket node, add all triangles
        BOOST_FOREACH(Triangle t, *(node->tris)) {
            tris->push_back(t);
        }
        return;
    }
    // not a bucket node, so search recursively high and low:
    KDNode::getTriangles(tris, node->hi);
    KDNode::getTriangles(tris, node->lo);
    return;
}

#define DEBUG_KD_SEARCH

/// search kd-tree starting at KDNode node for triangles.
/// find the ones which overlap (in the xy-plane)
/// with the MillingCutter cutter positioned at  Point cl
/// these triangles are added to the tris list.
void KDNode::search_kdtree( std::list<Triangle>* tris,      // found triangles added to tris
                            const CLPoint &cl,              // cutter positioned at cl
                            const MillingCutter &cutter,    // cutter
                            KDNode *node)                   // start search here and recurse into tree
{
    // we found a bucket node, so add all triangles and return.
    if (node->tris != NULL) {
        //std::cout << "bucket: cl=" << cl << "r=" << cutter.getRadius()
        //          << " len(tris)=" << node->tris->size() << " KDNode:" << *node << "\n";

        //if ( KDNode::overlap(node,cl,cutter) ) {  // check if node overlaps
            #ifdef DEBUG_KD_SEARCH
                 //std::cout << " returning bucket node with len(tris)="<< (*(node->tris)).size() << "\n";
                 //char c;
                 //std::cin >> c;
            #endif
            BOOST_FOREACH( Triangle t, *(node->tris) ) {
                //std::cout << t << "\n";
                //double r = cutter.getRadius();
                //std::cout << "T: " << t.minx << "\t" << t.maxx << "\t" << t.miny << "\t" << t.maxy << "\n";
                //std::cout << "C: " << cl.x-r << "\t" << cl.x+r << "\t" << cl.y-r << "\t" << cl.y+r << "\n";
                //std::cout << "overlap?:" << KDTree::overlap(node,cl,cutter) << "\n";

                // THIS IS PROBABLY WRONG, should not have to do an explicit overlap check here
                // if ( cutter.overlaps(cl,t) ) { // explicit cutter-overlap check
                    tris->push_back(t);
                //}

            } // loop through triangles

        //} // node overlap check

        return;
    } // end bucket-node

    // not a bucket node, so recursevily seach hi/lo branches of KDNode

    #ifdef DEBUG_KD_SEARCH
        //std::cout << "dim=" << node->dim << " cv=" << node->cutval << "\n";
    #endif
    //std::cout << "cl=" << cl << "r=" << cutter.getRadius()
    //              << " Internal KDNode:" << *node << "\n";
    switch(node->dim) { // ugly, solve with polymorphism?
        case 0: // cut along xplus
            // if one child node is not overlapping, search only the other
            if ( node->cutval < ( cl.x - cutter.getRadius() ) ) {
                #ifdef DEBUG_KD_SEARCH
                    //std::cout << " dim=" << node->dim << " branch hi\n";
                    //std::cout << "NO triangles with xmax < " << node-> cutval << "\n";
                #endif
                KDNode::search_kdtree(tris, cl, cutter, node->hi); //hi
            }
            else { // else we need to search both child-nodes
                #ifdef DEBUG_KD_SEARCH
                    //std::cout << " dim=" << node->dim << " branch hi AND lo\n";
                #endif
                KDNode::search_kdtree(tris, cl, cutter, node->hi);
                KDNode::search_kdtree(tris, cl, cutter, node->lo);
            }
            break;
        case 1: // cut along xminus
            if ( node->cutval > ( cl.x + cutter.getRadius() ) ) {
                #ifdef DEBUG_KD_SEARCH
                    //std::cout << " dim=" << node->dim << " branch lo\n";
                    //std::cout << "NO triangles with xmin < " << node-> cutval << "\n";
                #endif
                KDNode::search_kdtree(tris, cl, cutter, node->lo);
            }
            else {
                KDNode::search_kdtree(tris, cl, cutter, node->hi);
                KDNode::search_kdtree(tris, cl, cutter, node->lo);
            }
            break;
        case 2: // cut along yplus
            if ( node->cutval < ( cl.y - cutter.getRadius() ) ) {
                #ifdef DEBUG_KD_SEARCH
                    // std::cout << " dim=" << node->dim << " branch hi\n";
                    //std::cout << "NO triangles with ymax < " << node-> cutval << "\n";
                #endif
                KDNode::search_kdtree(tris, cl, cutter, node->hi);
            }
            else {
                #ifdef DEBUG_KD_SEARCH
                    // std::cout << " dim=" << node->dim << " branch hi AND lo\n";
                #endif
                KDNode::search_kdtree(tris, cl, cutter, node->hi);
                KDNode::search_kdtree(tris, cl, cutter, node->lo);
            }
            break;
        case 3: // cut along yminus
            if ( node->cutval > ( cl.y + cutter.getRadius() ) ) {
                #ifdef DEBUG_KD_SEARCH
                    //std::cout << " dim=" << node->dim << " branch lo\n";
                    //std::cout << "NO triangles with ymin > " << node-> cutval << "\n";
                #endif
                KDNode::search_kdtree(tris, cl, cutter, node->lo);
            }
            else {
                #ifdef DEBUG_KD_SEARCH
                    //std::cout << " dim=" << node->dim << " branch hi AND lo\n";
                #endif
                KDNode::search_kdtree(tris, cl, cutter, node->lo);
                KDNode::search_kdtree(tris, cl, cutter, node->hi);
            }
            break;
        default:
            std::cout << "kdtree.cpp ERROR!\n"; // error
            assert(0);
    } // end of switch(dim)

    // we get here after all the recursive calls above.
    return;

} // end search_kdtree()

//*********** Spread ****************

Spread::Spread(int dim, double v, double s)
{
    d = dim;
    val = v;
    start = s;
}

//********  string output ********************** */
std::string KDNode::str() const
{
    std::ostringstream o;
    o << *this;
    return o.str();
}

std::ostream& operator<<(std::ostream &stream, const KDNode root)
{
    stream << "KDNode d:" << root.dim << " cv:" << root.cutval;
    return stream;
}

} // end namespace
// end file kdtree.cpp


#pragma once
#include <Eigen/Dense>
#include <Eigen/Sparse>

#include <cereal/archives/binary.hpp>
namespace cereal
{
	template<class Archive, class S, int Rows_, int Cols_, int Ops_,
		int MaxRows_, int MaxCols_>
	inline void save(Archive& ar,
		const Eigen::Matrix<S, Rows_, Cols_, Ops_, MaxRows_, MaxCols_>& g )
	{
		int rows = g.rows();
		int cols = g.cols();
		ar(rows, cols, cereal::binary_data(g.data(), sizeof(S)*rows * cols));
	}

	template<class Archive, class S, int Rows_,	int Cols_, int Ops_,
		int MaxRows_, int MaxCols_>
	inline void load(Archive & ar,
		Eigen::Matrix<S, Rows_, Cols_, Ops_, MaxRows_, MaxCols_>& g )
	{
		int rows, cols;
		ar(rows);
		ar(cols);

		if((Rows_ != Eigen::Dynamic) && (Rows_ != rows))
			throw cereal::Exception("Saved rows mismatches with the rows of the current matrix.");

		if((Cols_ != Eigen::Dynamic) && (Cols_ != cols))
			throw cereal::Exception("Saved rows mismatches with the rows of the current matrix.");

		g.resize(rows, cols);
		ar(cereal::binary_data(g.data(), sizeof(S)*rows * cols));
	}

	template<class Archive, class S>
	void save(Archive& ar, const Eigen::SparseMatrix<S, Eigen::ColMajor>& m)
	{
		Eigen::SparseMatrix<S, Eigen::ColMajor> m_s = m;
		m_s.makeCompressed();

		int rows = m_s.rows();
		int cols = m_s.cols();
		int nnz = m_s.nonZeros();

		ar(rows, cols, nnz);
		using StorageIndex = typename Eigen::SparseMatrix<S, Eigen::ColMajor>::StorageIndex;

		ar(cereal::binary_data(m_s.valuePtr(), sizeof(S)*nnz));
		ar(cereal::binary_data(m_s.innerIndexPtr(), sizeof(StorageIndex)*nnz));
		ar(cereal::binary_data(m_s.outerIndexPtr(), sizeof(StorageIndex)*(cols+1)));
	}
	template<class Archive, class S>
	void load(Archive& ar, Eigen::SparseMatrix<S, Eigen::ColMajor>& m)
	{
		int rows, cols, nnz;
		ar(rows, cols, nnz);
		using StorageIndex = typename Eigen::SparseMatrix<S, Eigen::ColMajor>::StorageIndex;

		m.resize(rows, cols);

		std::vector<S> values;
		values.resize(nnz);
		std::vector<StorageIndex> innerIndices;
		innerIndices.resize(nnz);
		std::vector<StorageIndex> outerIndices;
		outerIndices.resize(cols+1);

		ar(cereal::binary_data(values.data(), sizeof(S)*nnz));
		ar(cereal::binary_data(innerIndices.data(), sizeof(StorageIndex)*nnz));
		ar(cereal::binary_data(outerIndices.data(), sizeof(StorageIndex)*(cols+1)));

		m = Eigen::Map<Eigen::SparseMatrix<S, Eigen::ColMajor> >(rows, cols, nnz,
				outerIndices.data(), innerIndices.data(), values.data());
	}
} // namespace cereal


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

    @copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-reduction
     This function object returns the number of non @ref Zero elements of the
   argument.

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

    @par Note

     This function is peculiarly useful in horizontal branching idioms.

    @par Example:

      @snippet nbtrue.cpp nbtrue

    @par Possible output:

      @snippet nbtrue.txt nbtrue

  **/
  std::size_t nbtrue(Value const& x);
} }
#endif

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

#endif


//==============================================================================
//         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_DISPATCH_META_UPGRADE_HPP_INCLUDED
#define BOOST_DISPATCH_META_UPGRADE_HPP_INCLUDED

//////////////////////////////////////////////////////////////////////////////
// Returns the input type rebound with a type that is twice as big
// as its primitive, with optional sign.
// If the primitive type is floating-point, sign is ignored.
// See: http://nt2.metascale.org/sdk/meta/traits/upgrade.html
//////////////////////////////////////////////////////////////////////////////

#include <boost/dispatch/meta/sign_of.hpp>
#include <boost/dispatch/meta/factory_of.hpp>
#include <boost/dispatch/meta/primitive_of.hpp>
#include <boost/dispatch/meta/make_integer.hpp>
#include <boost/dispatch/meta/make_floating.hpp>
#include <boost/dispatch/meta/strip.hpp>
#include <boost/fusion/include/is_sequence.hpp>
#include <boost/fusion/include/transform.hpp>
#include <boost/fusion/include/as_vector.hpp>
#include <boost/mpl/eval_if.hpp>
#include <boost/type_traits/is_void.hpp>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/type_traits/is_integral.hpp>
#include <boost/utility/enable_if.hpp>

namespace boost { namespace dispatch { namespace meta
{
  template<class T,class Sign = void>
  struct upgrade;
} } }

namespace boost { namespace dispatch { namespace details
{
  template<class T, class Sign, class Lambda, class Enable = void>
  struct upgrade_impl;

  template<class T, class Sign, class Lambda>
  struct upgrade_impl< T, Sign, Lambda
                     , typename enable_if< boost::is_integral<T> >::type
                     >
  {
    BOOST_STATIC_CONSTANT ( std::size_t
                          , size = (sizeof(T) < 8) ? sizeof(T)*2 : 8
                          );

    typedef typename mpl::eval_if < is_void<Sign>
                                  , meta::sign_of<T>
                                  , mpl::identity<Sign>
                                  >::type         sign;

    typedef typename
            meta::make_integer<size,sign,Lambda>::type type;
  };

  template<class T, class Sign, class Lambda>
  struct upgrade_impl< T, Sign, Lambda
                     , typename enable_if< boost::is_floating_point<T> >::type
                     >
  {
    BOOST_STATIC_CONSTANT ( std::size_t
                          , size = (sizeof(T) < 8) ? sizeof(T)*2 : 8
                          );

    typedef typename meta::make_floating<size,Lambda>::type  type;
  };

  template<class Sign>
  struct upgrade_fusion
  {
    template<class Sig>
    struct result;

    template<class This, class X>
    struct result<This(X)>
         : meta::upgrade<typename meta::strip<X>::type, Sign>
    {
    };

    template<class X>
    typename meta::upgrade<X>::type operator()(X const&) const;
  };

  template<class T, class Sign, class Lambda>
  struct upgrade_impl< T, Sign, Lambda
                     , typename enable_if< boost::fusion::traits::is_sequence<T> >::type
                     >
       : Lambda::template apply< typename boost::fusion::result_of::
                                 as_vector< typename boost::fusion::result_of::
                                            transform<T, upgrade_fusion<Sign> >::type
                                          >::type
                               >
  {
  };
} } }

namespace boost { namespace dispatch { namespace meta
{
  //////////////////////////////////////////////////////////////////////////////
  // For any type, return the integer type of size equals to sizeof(T)*2
  // with an optional sign change
  //////////////////////////////////////////////////////////////////////////////
  template<class T,class Sign>
  struct upgrade
  {
    typedef typename meta::factory_of<T>::type  lambda;
    typedef typename meta::primitive_of<T>::type  base;
    typedef typename meta::strip<base>::type  stripped;
    typedef typename details::upgrade_impl<stripped, Sign, lambda>::type type;
  };

  template<class T, class Sign>
  struct upgrade<T&,Sign> : upgrade<T,Sign> {};

  template<class T, class Sign>
  struct upgrade<T const,Sign> : upgrade<T,Sign> {};
} } }

#endif


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, 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)

#include <algorithm>
#include <iterator>
#include <sstream>
#include <string>

#include <geometry_test_common.hpp>

#include <boost/geometry/iterators/closing_iterator.hpp>

#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/io/wkt/read.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

// The closing iterator should also work on normal std:: containers
void test_non_geometry()
{
    std::vector<int> v;
    v.push_back(1);
    v.push_back(2);
    v.push_back(3);

    typedef bg::closing_iterator
        <
            std::vector<int> const
        > closing_iterator;

    closing_iterator it(v);
    closing_iterator end(v, true);

    std::ostringstream out;
    for (; it != end; ++it)
    {
        out << *it;
    }
    BOOST_CHECK_EQUAL(out.str(), "1231");
}

template <typename Geometry>
void test_geometry(std::string const& wkt)
{
    Geometry geometry;
    bg::read_wkt(wkt, geometry);
    typedef bg::closing_iterator<Geometry const> closing_iterator;

    // 1. Test normal behaviour
    {
        closing_iterator it(geometry);
        closing_iterator end(geometry, true);

        std::ostringstream out;
        for (; it != end; ++it)
        {
            out << " " << bg::get<0>(*it) << bg::get<1>(*it);
        }
        BOOST_CHECK_EQUAL(out.str(), " 11 14 44 41 11");

        // Check compilations, it is (since Oct 2010) random access
        it--;
        --it;
        it += 2;
        it -= 2;
    }

    // 2: check copy behaviour
    {
        typedef typename boost::range_iterator<Geometry const>::type normal_iterator;
        Geometry copy;

        std::copy<closing_iterator>(
            closing_iterator(geometry),
            closing_iterator(geometry, true),
            std::back_inserter(copy));

        std::ostringstream out;
        for (normal_iterator cit = boost::begin(copy); cit != boost::end(copy); ++cit)
        {
                out << " " << bg::get<0>(*cit) << bg::get<1>(*cit);
        }
        BOOST_CHECK_EQUAL(out.str(), " 11 14 44 41 11");
    }
}

template <typename P>
void test_all()
{
    test_non_geometry();
    test_geometry<bg::model::ring<P> >("POLYGON((1 1,1 4,4 4,4 1))");
}

int test_main(int, char* [])
{
    test_all<bg::model::d2::point_xy<double> >();

    return 0;
}


#include "ftpoint.h"

#include <boost/python.hpp>

#include <FTGL/ftgl.h>

class FTPointDefVisitor : public boost::python::def_visitor<FTPointDefVisitor>
{
    friend class def_visitor_access;
public:
    template <class T>
    void visit(T &class_) const
    {
        class_
            .def("__eq__", &__eq__)
            ;
    }

    static bool __eq__(FTPoint &self, const FTPoint &rhs)
    {
        return self == rhs;
    }

};

void FTPoint_init()
{
    boost::python::class_<FTPoint>("FTPoint")
        .def(boost::python::init<double, double, boost::python::optional<double>>())
        .def("Normalise", &FTPoint::Normalise)
        .def("__add__", &FTPoint::operator +)
        .def("__sub__", &FTPoint::operator -)
        .def("__mul__", static_cast<FTPoint(FTPoint::*)(double)const>(&FTPoint::operator *))
        .def("__pow__", &FTPoint::operator ^)
        .add_property("X",
            static_cast<FTGL_DOUBLE(FTPoint::*)()const>(&FTPoint::X),
            static_cast<void(FTPoint::*)(FTGL_DOUBLE)>(&FTPoint::X))
        .add_property("Y",
            static_cast<FTGL_DOUBLE(FTPoint::*)()const>(&FTPoint::Y),
            static_cast<void(FTPoint::*)(FTGL_DOUBLE)>(&FTPoint::Y))
        .add_property("Z",
            static_cast<FTGL_DOUBLE(FTPoint::*)()const>(&FTPoint::Z),
            static_cast<void(FTPoint::*)(FTGL_DOUBLE)>(&FTPoint::Z))
        ;
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

// this hack is needed to make this file compiles with -pedantic (gcc)
#ifdef __GNUC__
#define throw(X)
#endif

#ifdef __INTEL_COMPILER
  // disable "warning #76: argument to macro is empty" produced by the above hack
  #pragma warning disable 76
#endif

// discard stack allocation as that too bypasses malloc
#define EIGEN_STACK_ALLOCATION_LIMIT 0
// any heap allocation will raise an assert
#define EIGEN_NO_MALLOC

#include "main.h"
#include <Eigen/Cholesky>
#include <Eigen/Eigenvalues>
#include <Eigen/LU>
#include <Eigen/QR>
#include <Eigen/SVD>

template<typename MatrixType> void nomalloc(const MatrixType& m)
{
  /* this test check no dynamic memory allocation are issued with fixed-size matrices
  */
  typedef typename MatrixType::Index Index;
  typedef typename MatrixType::Scalar Scalar;

  Index rows = m.rows();
  Index cols = m.cols();

  MatrixType m1 = MatrixType::Random(rows, cols),
             m2 = MatrixType::Random(rows, cols),
             m3(rows, cols);

  Scalar s1 = internal::random<Scalar>();

  Index r = internal::random<Index>(0, rows-1),
        c = internal::random<Index>(0, cols-1);

  VERIFY_IS_APPROX((m1+m2)*s1,              s1*m1+s1*m2);
  VERIFY_IS_APPROX((m1+m2)(r,c), (m1(r,c))+(m2(r,c)));
  VERIFY_IS_APPROX(m1.cwiseProduct(m1.block(0,0,rows,cols)), (m1.array()*m1.array()).matrix());
  VERIFY_IS_APPROX((m1*m1.transpose())*m2,  m1*(m1.transpose()*m2));

  m2.col(0).noalias() = m1 * m1.col(0);
  m2.col(0).noalias() -= m1.adjoint() * m1.col(0);
  m2.col(0).noalias() -= m1 * m1.row(0).adjoint();
  m2.col(0).noalias() -= m1.adjoint() * m1.row(0).adjoint();

  m2.row(0).noalias() = m1.row(0) * m1;
  m2.row(0).noalias() -= m1.row(0) * m1.adjoint();
  m2.row(0).noalias() -= m1.col(0).adjoint() * m1;
  m2.row(0).noalias() -= m1.col(0).adjoint() * m1.adjoint();
  VERIFY_IS_APPROX(m2,m2);

  m2.col(0).noalias() = m1.template triangularView<Upper>() * m1.col(0);
  m2.col(0).noalias() -= m1.adjoint().template triangularView<Upper>() * m1.col(0);
  m2.col(0).noalias() -= m1.template triangularView<Upper>() * m1.row(0).adjoint();
  m2.col(0).noalias() -= m1.adjoint().template triangularView<Upper>() * m1.row(0).adjoint();

  m2.row(0).noalias() = m1.row(0) * m1.template triangularView<Upper>();
  m2.row(0).noalias() -= m1.row(0) * m1.adjoint().template triangularView<Upper>();
  m2.row(0).noalias() -= m1.col(0).adjoint() * m1.template triangularView<Upper>();
  m2.row(0).noalias() -= m1.col(0).adjoint() * m1.adjoint().template triangularView<Upper>();
  VERIFY_IS_APPROX(m2,m2);

  m2.col(0).noalias() = m1.template selfadjointView<Upper>() * m1.col(0);
  m2.col(0).noalias() -= m1.adjoint().template selfadjointView<Upper>() * m1.col(0);
  m2.col(0).noalias() -= m1.template selfadjointView<Upper>() * m1.row(0).adjoint();
  m2.col(0).noalias() -= m1.adjoint().template selfadjointView<Upper>() * m1.row(0).adjoint();

  m2.row(0).noalias() = m1.row(0) * m1.template selfadjointView<Upper>();
  m2.row(0).noalias() -= m1.row(0) * m1.adjoint().template selfadjointView<Upper>();
  m2.row(0).noalias() -= m1.col(0).adjoint() * m1.template selfadjointView<Upper>();
  m2.row(0).noalias() -= m1.col(0).adjoint() * m1.adjoint().template selfadjointView<Upper>();
  VERIFY_IS_APPROX(m2,m2);

  m2.template selfadjointView<Lower>().rankUpdate(m1.col(0),-1);
  m2.template selfadjointView<Lower>().rankUpdate(m1.row(0),-1);

  // The following fancy matrix-matrix products are not safe yet regarding static allocation
//   m1 += m1.template triangularView<Upper>() * m2.col(;
//   m1.template selfadjointView<Lower>().rankUpdate(m2);
//   m1 += m1.template triangularView<Upper>() * m2;
//   m1 += m1.template selfadjointView<Lower>() * m2;
//   VERIFY_IS_APPROX(m1,m1);
}

template<typename Scalar>
void ctms_decompositions()
{
  const int maxSize = 16;
  const int size    = 12;

  typedef Eigen::Matrix<Scalar,
                        Eigen::Dynamic, Eigen::Dynamic,
                        0,
                        maxSize, maxSize> Matrix;

  typedef Eigen::Matrix<Scalar,
                        Eigen::Dynamic, 1,
                        0,
                        maxSize, 1> Vector;

  typedef Eigen::Matrix<std::complex<Scalar>,
                        Eigen::Dynamic, Eigen::Dynamic,
                        0,
                        maxSize, maxSize> ComplexMatrix;

  const Matrix A(Matrix::Random(size, size)), B(Matrix::Random(size, size));
  Matrix X(size,size);
  const ComplexMatrix complexA(ComplexMatrix::Random(size, size));
  const Matrix saA = A.adjoint() * A;
  const Vector b(Vector::Random(size));
  Vector x(size);

  // Cholesky module
  Eigen::LLT<Matrix>  LLT;  LLT.compute(A);
  X = LLT.solve(B);
  x = LLT.solve(b);
  Eigen::LDLT<Matrix> LDLT; LDLT.compute(A);
  X = LDLT.solve(B);
  x = LDLT.solve(b);

  // Eigenvalues module
  Eigen::HessenbergDecomposition<ComplexMatrix> hessDecomp;        hessDecomp.compute(complexA);
  Eigen::ComplexSchur<ComplexMatrix>            cSchur(size);      cSchur.compute(complexA);
  Eigen::ComplexEigenSolver<ComplexMatrix>      cEigSolver;        cEigSolver.compute(complexA);
  Eigen::EigenSolver<Matrix>                    eigSolver;         eigSolver.compute(A);
  Eigen::SelfAdjointEigenSolver<Matrix>         saEigSolver(size); saEigSolver.compute(saA);
  Eigen::Tridiagonalization<Matrix>             tridiag;           tridiag.compute(saA);

  // LU module
  Eigen::PartialPivLU<Matrix> ppLU; ppLU.compute(A);
  X = ppLU.solve(B);
  x = ppLU.solve(b);
  Eigen::FullPivLU<Matrix>    fpLU; fpLU.compute(A);
  X = fpLU.solve(B);
  x = fpLU.solve(b);

  // QR module
  Eigen::HouseholderQR<Matrix>        hQR;  hQR.compute(A);
  X = hQR.solve(B);
  x = hQR.solve(b);
  Eigen::ColPivHouseholderQR<Matrix>  cpQR; cpQR.compute(A);
  X = cpQR.solve(B);
  x = cpQR.solve(b);
  Eigen::FullPivHouseholderQR<Matrix> fpQR; fpQR.compute(A);
  // FIXME X = fpQR.solve(B);
  x = fpQR.solve(b);

  // SVD module
  Eigen::JacobiSVD<Matrix> jSVD; jSVD.compute(A, ComputeFullU | ComputeFullV);
}

void test_zerosized() {
  // default constructors:
  Eigen::MatrixXd A;
  Eigen::VectorXd v;
  // explicit zero-sized:
  Eigen::ArrayXXd A0(0,0);
  Eigen::ArrayXd v0(std::ptrdiff_t(0)); // FIXME ArrayXd(0) is ambiguous

  // assigning empty objects to each other:
  A=A0;
  v=v0;
}

template<typename MatrixType> void test_reference(const MatrixType& m) {
  typedef typename MatrixType::Scalar Scalar;
  enum { Flag          =  MatrixType::IsRowMajor ? Eigen::RowMajor : Eigen::ColMajor};
  enum { TransposeFlag = !MatrixType::IsRowMajor ? Eigen::RowMajor : Eigen::ColMajor};
  typename MatrixType::Index rows = m.rows(), cols=m.cols();
  // Dynamic reference:
  typedef Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Flag         > > Ref;
  typedef Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, TransposeFlag> > RefT;

  Ref r1(m);
  Ref r2(m.block(rows/3, cols/4, rows/2, cols/2));
  RefT r3(m.transpose());
  RefT r4(m.topLeftCorner(rows/2, cols/2).transpose());

  VERIFY_RAISES_ASSERT(RefT r5(m));
  VERIFY_RAISES_ASSERT(Ref r6(m.transpose()));
  VERIFY_RAISES_ASSERT(Ref r7(Scalar(2) * m));
}

void test_nomalloc()
{
  // check that our operator new is indeed called:
  VERIFY_RAISES_ASSERT(MatrixXd dummy(MatrixXd::Random(3,3)));
  CALL_SUBTEST_1(nomalloc(Matrix<float, 1, 1>()) );
  CALL_SUBTEST_2(nomalloc(Matrix4d()) );
  CALL_SUBTEST_3(nomalloc(Matrix<float,32,32>()) );

  // Check decomposition modules with dynamic matrices that have a known compile-time max size (ctms)
  CALL_SUBTEST_4(ctms_decompositions<float>());
  CALL_SUBTEST_5(test_zerosized());
  CALL_SUBTEST_6(test_reference(Matrix<float,32,32>()));
}


// Copyright (c) 2012 Christopher Lux <christopherlux@gmail.com>
// Distributed under the Modified BSD License, see license.txt.

#include <iostream>
#include <sstream>
#include <vector>

#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/io/ios_state.hpp>

#ifdef _WIN32
#include <windows.h>
#endif

#include <scm/core.h>
#include <scm/log.h>
#include <scm/core/pointer_types.h>
#include <scm/core/io/tools.h>
#include <scm/core/time/accum_timer.h>
#include <scm/core/time/high_res_timer.h>

#include <scm/gl_util/data/analysis/transfer_function/piecewise_function_1d.h>
#include <scm/gl_util/data/analysis/transfer_function/build_lookup_table.h>
#include <scm/gl_util/data/volume/volume_reader_raw.h>
#include <scm/gl_util/data/volume/volume_reader_segy.h>
#include <scm/gl_util/data/volume/volume_reader_vgeo.h>

#include <scm/gl_core.h>
#include <scm/gl_core/buffer_objects/uniform_buffer_adaptor.h>

#include <scm/gl_util/data/imaging/texture_loader.h>
#include <scm/gl_util/data/volume/volume_loader.h>

#include <scm/gl_util/manipulators/trackball_manipulator.h>
#include <scm/gl_util/primitives/box.h>
#include <scm/gl_util/primitives/quad.h>
#include <scm/gl_util/primitives/wavefront_obj.h>

//#include <GL/gl.h>
#include <GL/freeglut.h>
//#include <GL/gl3.h>

static bool fraps_bug = true;

static int winx = 600;
static int winy = 600;

const scm::math::vec3f diffuse(0.7f, 0.7f, 0.7f);
const scm::math::vec3f specular(0.2f, 0.7f, 0.9f);
const scm::math::vec3f ambient(0.1f, 0.1f, 0.1f);
const scm::math::vec3f position(1, 1, 1);

class demo_app
{
    struct transform_matrices
    {
        transform_matrices()
          : _mv_matrix(scm::math::mat4f::identity()),
            _mv_matrix_inverse(scm::math::mat4f::identity()),
            _mv_matrix_inverse_transpose(scm::math::mat4f::identity()),
            _p_matrix(scm::math::mat4f::identity()),
            _p_matrix_inverse(scm::math::mat4f::identity()),
            _mvp_matrix(scm::math::mat4f::identity()),
            _mvp_matrix_inverse(scm::math::mat4f::identity())
        {}

        scm::math::mat4f _mv_matrix;
        scm::math::mat4f _mv_matrix_inverse;
        scm::math::mat4f _mv_matrix_inverse_transpose;

        scm::math::mat4f _p_matrix;
        scm::math::mat4f _p_matrix_inverse;

        scm::math::mat4f _mvp_matrix;
        scm::math::mat4f _mvp_matrix_inverse;
    };

public:
    demo_app() {
        _initx = 0;
        _inity = 0;

        _lb_down = false;
        _mb_down = false;
        _rb_down = false;

        _dolly_sens = 10.0f;
        _sampling_distance = 1.0f;
    }
    virtual ~demo_app();

    bool initialize();
    void display();
    void resize(int w, int h);
    void mousefunc(int button, int state, int x, int y);
    void mousemotion(int x, int y);
    void keyboard(unsigned char key, int x, int y);

protected:
    scm::gl::texture_1d_ptr create_color_map(scm::gl::render_device& in_device,
                                             unsigned in_size,
                                             const scm::data::piecewise_function_1d<float, float>& in_alpha,
                                             const scm::data::piecewise_function_1d<float, scm::math::vec3f>& in_color) const;

    void            update_transforms(const scm::gl::render_context& in_context);

private:
    scm::gl::trackball_manipulator _trackball_manip;
    float _initx;
    float _inity;

    bool _lb_down;
    bool _mb_down;
    bool _rb_down;

    float _dolly_sens;

    float               _sampling_distance;
    scm::math::vec3f    _max_volume_bounds;

    scm::data::piecewise_function_1d<float, float>                  _alpha_transfer;
    scm::data::piecewise_function_1d<float, scm::math::vec3f>       _color_transfer;

    scm::shared_ptr<scm::gl::render_device>     _device;
    scm::shared_ptr<scm::gl::render_context>    _context;

    scm::gl::program_ptr                _shader_program;

    scm::shared_ptr<scm::gl::box_geometry>  _box;

    scm::gl::texture_3d_ptr             _volume_texture;
    scm::gl::texture_1d_ptr             _colormap_texture;

    scm::gl::depth_stencil_state_ptr    _depth_less;

    scm::gl::blend_state_ptr            _no_blend;
    scm::gl::blend_state_ptr            _blend_omsa;

    scm::gl::sampler_state_ptr          _filter_linear;
    scm::gl::sampler_state_ptr          _filter_nearest;

    scm::gl::texture_2d_ptr             _color_buffer;
    scm::gl::texture_2d_ptr             _depth_buffer;
    scm::gl::frame_buffer_ptr           _framebuffer;
    scm::shared_ptr<scm::gl::quad_geometry>  _quad;
    scm::gl::program_ptr                _pass_through_shader;
    scm::gl::depth_stencil_state_ptr    _depth_no_z;
    scm::gl::rasterizer_state_ptr       _no_back_face_cull;

    scm::gl::rasterizer_state_ptr       _cull_back;

    scm::gl::program_ptr                _zfill_shader;
    scm::gl::frame_buffer_ptr           _zfill_framebuffer;
    scm::gl::rasterizer_state_ptr       _zfill_cull_front;

    scm::gl::timer_query_ptr            _timer_zfill;
    scm::gl::timer_query_ptr            _timer_volume;

    scm::gl::uniform_block<transform_matrices> _current_transforms;
    //scm::shared_ptr<transform_matrices> _current_transforms;
    //scm::gl::buffer_ptr                 _current_transforms_buffer;

}; // class demo_app

namespace  {

scm::scoped_ptr<demo_app> _application;

} // namespace

demo_app::~demo_app()
{
    _shader_program.reset();

    _box.reset();

    _volume_texture.reset();
    _colormap_texture.reset();

    _no_blend.reset();
    _blend_omsa.reset();

    _depth_less.reset();

    _filter_linear.reset();
    _filter_nearest.reset();

    _color_buffer.reset();
    _depth_buffer.reset();
    _framebuffer.reset();
    _quad.reset();
    _pass_through_shader.reset();
    _depth_no_z.reset();
    _no_back_face_cull.reset();

    _cull_back.reset();

    _zfill_shader.reset();
    _zfill_framebuffer.reset();
    _zfill_cull_front.reset();

    _timer_zfill.reset();
    _timer_volume.reset();

    _current_transforms.reset();

    _context.reset();
    _device.reset();
}

scm::gl::texture_1d_ptr
demo_app::create_color_map(scm::gl::render_device& in_device,
                           unsigned in_size,
                           const scm::data::piecewise_function_1d<float, float>& in_alpha,
                           const scm::data::piecewise_function_1d<float, scm::math::vec3f>& in_color) const
{
    using namespace scm::gl;
    using namespace scm::math;

    scm::scoped_array<scm::math::vec3f>  color_lut;
    scm::scoped_array<float>             alpha_lut;

    color_lut.reset(new vec3f[in_size]);
    alpha_lut.reset(new float[in_size]);

    if (   !scm::data::build_lookup_table(color_lut, in_color, in_size)
        || !scm::data::build_lookup_table(alpha_lut, in_alpha, in_size)) {
        std::cout << "demo_app::create_color_map(): error during lookuptable generation" << std::endl;
        return (texture_1d_ptr());
    }
    scm::scoped_array<float> combined_lut;

    combined_lut.reset(new float[in_size * 4]);

    for (unsigned i = 0; i < in_size; ++i) {
        combined_lut[i*4   ] = color_lut[i].x;
        combined_lut[i*4 +1] = color_lut[i].y;
        combined_lut[i*4 +2] = color_lut[i].z;
        combined_lut[i*4 +3] = alpha_lut[i];
    }

    std::vector<void*> in_data;
    in_data.push_back(combined_lut.get());

    texture_1d_ptr new_tex = in_device.create_texture_1d(in_size, FORMAT_RGBA_8, 1, 1, FORMAT_RGBA_32F, in_data);

    if (!new_tex) {
        std::cout << "demo_app::create_color_map(): error during color map texture generation." << std::endl;
        return (texture_1d_ptr());
    }

    return (new_tex);
}

bool
demo_app::initialize()
{
    using namespace scm;
    using namespace scm::gl;
    using namespace scm::math;
    using boost::assign::list_of;

    // initialize transfer functions //////////////////////////////////////////////////////////////
    _alpha_transfer.clear();
    _color_transfer.clear();

#if 0
    _alpha_transfer.add_stop(0,       1.0f);
    _alpha_transfer.add_stop(0.33f,   0.5f);
    _alpha_transfer.add_stop(0.40f,   0.0f);
    _alpha_transfer.add_stop(0.60f,   0.0f);
    _alpha_transfer.add_stop(0.66f,   0.5f);
    _alpha_transfer.add_stop(1.0f,    1.0f);
#else
    _alpha_transfer.add_stop(0,       0.0f);
    _alpha_transfer.add_stop(1.0f,    1.0f);
#endif

#if 0
    // blue-grey-orange
    _color_transfer.add_stop(0,       vec3f(0.0f, 1.0f, 1.0f));
    _color_transfer.add_stop(0.25f,   vec3f(0.0f, 0.0f, 1.0f));
    _color_transfer.add_stop(0.375f,  vec3f(0.256637f, 0.243243f, 0.245614f));
    _color_transfer.add_stop(0.50f,   vec3f(0.765487f, 0.738739f, 0.72807f));
    _color_transfer.add_stop(0.625f,  vec3f(0.530973f, 0.27027f, 0.0f));
    _color_transfer.add_stop(0.75f,   vec3f(1.0f, 0.333333f, 0.0f));
    _color_transfer.add_stop(1.0f,    vec3f(1.0f, 1.0f, 0.0f));
#else
    // blue-white-red
    _color_transfer.add_stop(0.0f, vec3f(0.0f, 0.0f, 1.0f));
    _color_transfer.add_stop(0.5f, vec3f(1.0f, 1.0f, 1.0f));
    _color_transfer.add_stop(1.0f, vec3f(1.0f, 0.0f, 0.0f));
#endif
    // initialize shaders /////////////////////////////////////////////////////////////////////////
    std::string vs_source;
    std::string fs_source;

    if (   !io::read_text_file("../../../src/shaders/volume_ray_cast.glslv", vs_source)
        || !io::read_text_file("../../../src/shaders/volume_ray_cast.glslf", fs_source)) {
        scm::err() << "error reading shader files" << log::end;
        return (false);
    }

    _device.reset(new scm::gl::render_device());
    _context = _device->main_context();
    scm::out() << *_device << scm::log::end;

    _shader_program = _device->create_program(list_of(_device->create_shader(STAGE_VERTEX_SHADER, vs_source))
                                                     (_device->create_shader(STAGE_FRAGMENT_SHADER, fs_source)));

    if (!_shader_program) {
        scm::err() << "error creating shader program" << log::end;
        return (false);
    }

    // initialize state objects ///////////////////////////////////////////////////////////////////
    _depth_less = _device->create_depth_stencil_state(true, true, COMPARISON_LESS);

    _no_blend   = _device->create_blend_state(false, FUNC_ONE, FUNC_ZERO, FUNC_ONE, FUNC_ZERO);
    _blend_omsa = _device->create_blend_state(true, FUNC_SRC_ALPHA, FUNC_ONE_MINUS_SRC_ALPHA, FUNC_ONE, FUNC_ZERO);

    // initialize textures ////////////////////////////////////////////////////////////////////////
    _filter_linear  = _device->create_sampler_state(FILTER_MIN_MAG_LINEAR, WRAP_CLAMP_TO_EDGE);
    _filter_nearest = _device->create_sampler_state(FILTER_MIN_MAG_NEAREST, WRAP_CLAMP_TO_EDGE);

    volume_loader loader;

    _volume_texture = loader.load_texture_3d(*_device, "../../../res/volume/Engine_w256_h256_d256_c1_b8.raw", false);

    //_volume_texture = load_volume(*_device, "f:/data/src/Lux_Christopher_20999_4_1_mm_WT_w512_h512_d202_c1_b16.raw");
    //_volume_texture = load_volume(*_device, "../../../res/volume/Engine_w256_h256_d256_c1_b8.raw");
    //_volume_texture = load_volume(*_device, "e:/data/volume/vrgeo/new_zealand/pari_full_rm_8float_bri_TRIMMED.vol");
    //_volume_texture = load_volume(*_device, "d:/data/src/Engine_w256_h256_d256_c1_b8.raw");
    //_volume_texture = load_volume(*_device, "f:/data/src/wfarm_w512_h439_d512_c1_b8.raw");
    //_volume_texture = load_volume(*_device, "f:/data/src/mig_dep_0_2200.bri.vol");
    //_volume_texture = load_volume(*_device, "e:/data/gulfaks/gfaks.vol");
    //_volume_texture = load_volume(*_device, "e:/data/volume/general volumes/foot_w256_h256_d256_c1_b8.raw");
    _colormap_texture = create_color_map(*_device, 512, _alpha_transfer, _color_transfer);

    // initialize geometry ////////////////////////////////////////////////////////////////////////
    unsigned max_dim = max(max(_volume_texture->descriptor()._size.x,
                               _volume_texture->descriptor()._size.y),
                               _volume_texture->descriptor()._size.z);
    _max_volume_bounds = vec3f(_volume_texture->descriptor()._size) / max_dim;
    _sampling_distance = .5f / max_dim;

    _box = make_shared<box_geometry>(_device, vec3f(0.0f), _max_volume_bounds);//vec3f(-0.5f), vec3f(0.5f)));

    // initialize framebuffer /////////////////////////////////////////////////////////////////////
    texture_loader tex_loader;
    //_color_buffer =  tex_loader.load_texture_2d(*_device, "e:/working_copies/schism_x64/resources/textures/DH213SN.hdr", true, true);
    _color_buffer = _device->create_texture_2d(vec2ui(winx, winy), FORMAT_RGBA_16F);
    _depth_buffer = _device->create_texture_2d(vec2ui(winx, winy), FORMAT_D24);
    _framebuffer  = _device->create_frame_buffer();

    _framebuffer->attach_color_buffer(0, _color_buffer);
    _framebuffer->attach_depth_stencil_buffer(_depth_buffer);

    _quad.reset(new quad_geometry(_device, vec2f(0.0f, 0.0f), vec2f(1.0f, 1.0f)));
    _depth_no_z = _device->create_depth_stencil_state(false, false);
    _no_back_face_cull = _device->create_rasterizer_state(FILL_SOLID, CULL_NONE);

    std::string v_pass = "\
        #version 330\n\
        \
        uniform mat4 mvp;\
        out vec2 tex_coord;\
        layout(location = 0) in vec3 in_position;\
        layout(location = 2) in vec2 in_texture_coord;\
        void main()\
        {\
            gl_Position = mvp * vec4(in_position, 1.0);\
            tex_coord = in_texture_coord;\
        }\
        ";
    std::string f_pass = "\
        #version 330\n\
        \
        in vec2 tex_coord;\
        uniform sampler2D in_texture;\
        layout(location = 0) out vec4 out_color;\
        void main()\
        {\
            out_color = texelFetch(in_texture, ivec2(gl_FragCoord.xy), 0).rgba;\
        }\
        ";
    ////texture(in_texture, tex_coord);
    _pass_through_shader = _device->create_program(list_of(_device->create_shader(STAGE_VERTEX_SHADER, v_pass))
                                                          (_device->create_shader(STAGE_FRAGMENT_SHADER, f_pass)));

    std::string v_zfill = "\
        #version 330\n\
        layout(std140, column_major) uniform;\
        \
        uniform transform_matrices\
        {\
            mat4 mv_matrix;\
            mat4 mv_matrix_inverse;\
            mat4 mv_matrix_inverse_transpose;\
            \
            mat4 p_matrix;\
            mat4 p_matrix_inverse;\
            \
            mat4 mvp_matrix;\
            mat4 mvp_matrix_inverse;\
        } current_transform;\
        out vec3 obj_position;\
        layout(location = 0) in vec3 in_position;\
        void main()\
        {\
            obj_position = in_position;\
            gl_Position = current_transform.mvp_matrix * vec4(in_position, 1.0);\
        }\
        ";
    std::string f_zfill = "\
        #version 330\n\
        \
        in vec3 obj_position;\
        layout(location = 0) out vec4 out_color;\
        void main()\
        {\
            out_color = vec4(obj_position, 1.0);\
        }\
        ";
    _zfill_shader = _device->create_program(list_of(_device->create_shader(STAGE_VERTEX_SHADER, v_zfill))
                                                   (_device->create_shader(STAGE_FRAGMENT_SHADER, f_zfill)));
    _zfill_framebuffer = _device->create_frame_buffer();
    _zfill_framebuffer->attach_depth_stencil_buffer(_depth_buffer);
    _zfill_cull_front = _device->create_rasterizer_state(FILL_SOLID, CULL_FRONT);

    _cull_back = _device->create_rasterizer_state(FILL_SOLID, CULL_BACK);

    _timer_zfill    = _device->create_timer_query();
    _timer_volume   = _device->create_timer_query();

    _current_transforms        = make_uniform_block<transform_matrices>(_device);
    //_current_transforms        = scm::make_shared<transform_matrices>();
    //_current_transforms_buffer = _device->create_buffer(BIND_UNIFORM_BUFFER, USAGE_DYNAMIC_DRAW, sizeof(transform_matrices));

    // set clear color, which is used to fill the background on glClear
    //glClearColor(0.2f,0.2f,0.2f,1);

    // setup depth testing

    // set polygonmode to fill front and back faces
    //glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
    //glShadeModel(GL_SMOOTH);

    _trackball_manip.dolly(2.5f);

    return (true);
}

unsigned plah             = 0;
double  time_zfill_accum  = 0.0;
double  time_volume_accum = 0.0;
unsigned accum_count      = 0;

void
demo_app::display()
{
    using namespace scm::gl;
    using namespace scm::math;

    // clear the color and depth buffer
    //glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);

    mat4f    view_matrix         = _trackball_manip.transform_matrix();
    mat4f    model_matrix        = mat4f::identity();
    translate(model_matrix, -(_max_volume_bounds * 0.5f));

    _current_transforms.begin_manipulation(_context);
    _current_transforms->_mv_matrix                    = view_matrix * model_matrix;
    _current_transforms->_mv_matrix_inverse            = inverse(_current_transforms->_mv_matrix);
    _current_transforms->_mv_matrix_inverse_transpose  = inverse(_current_transforms->_mv_matrix_inverse);
    _current_transforms->_mvp_matrix                   = _current_transforms->_p_matrix * _current_transforms->_mv_matrix;
    _current_transforms->_mvp_matrix_inverse           = inverse(_current_transforms->_mvp_matrix);
    _current_transforms.end_manipulation();

    vec3f   camera_location =  vec3f(_current_transforms->_mv_matrix_inverse.column(3))
                             / _current_transforms->_mv_matrix_inverse.column(3).w;

    //update_transforms(*_context);

    _context->clear_default_color_buffer(FRAMEBUFFER_BACK, vec4f(0.2f, .2f, .2f, 1.0f));
    _context->clear_default_depth_stencil_buffer();

    { // volume pass
        context_uniform_buffer_guard ubg(_context);

        _context->bind_uniform_buffer(_current_transforms.block_buffer(), 0);

        _context->begin_query(_timer_zfill);
        { // fill z of back faces
            context_state_objects_guard csg(_context);
            context_texture_units_guard tug(_context);

            //_context->clear_depth_stencil_buffer(_zfill_framebuffer);
            //_context->set_frame_buffer(_zfill_framebuffer);

            _context->clear_color_buffer(_framebuffer, 0, vec4f( .2f, .2f, .2f, 1.0f));
            _context->clear_depth_stencil_buffer(_framebuffer);
            _context->set_frame_buffer(_framebuffer);

            //_zfill_shader->uniform("mvp", _current_transforms->_mvp_matrix);
            _zfill_shader->uniform_buffer("transform_matrices", 0);

            _context->set_depth_stencil_state(_depth_less);
            _context->set_rasterizer_state(_zfill_cull_front);
            _context->set_blend_state(_no_blend);

            _context->reset_texture_units();

            _context->bind_program(_zfill_shader);

            _box->draw(_context, geometry::MODE_SOLID);
        }
        _context->end_query(_timer_zfill);

        //if (0)
        _context->begin_query(_timer_volume);
        {
            context_state_objects_guard     csg(_context);
            context_texture_units_guard     tug(_context);
            context_uniform_buffer_guard    ubg(_context);

            _shader_program->uniform("camera_location",             camera_location);
            _shader_program->uniform("sampling_distance",           _sampling_distance);
            _shader_program->uniform("max_bounds",                  _max_volume_bounds);
            //_shader_program->uniform("projection_matrix",           _current_transforms->_p_matrix);
            //_shader_program->uniform("model_view_matrix",           _current_transforms->_mv_matrix);
            //_shader_program->uniform("model_view_matrix_inverse",   _current_transforms->_mv_matrix_inverse);
            //_shader_program->uniform("mvp_inverse",                 _current_transforms->_mvp_matrix_inverse);
            //_shader_program->uniform("model_view_matrix_inverse_transpose", mv_inv_transpose);
            _shader_program->uniform_sampler("volume_texture",    0);
            _shader_program->uniform_sampler("color_map_texture", 1);
            _shader_program->uniform_sampler("depth_texture",     2);

            _shader_program->uniform_buffer("transform_matrices", 0);

            _context->reset_state_objects();

            _context->set_default_frame_buffer();
            //_context->set_depth_stencil_state(_depth_less);
            _context->set_blend_state(_blend_omsa);
            //_context->set_rasterizer_state(_cull_back);

            _context->bind_program(_shader_program);

            _context->reset_texture_units();
            _context->bind_texture(_volume_texture, _filter_linear, 0);
            _context->bind_texture(_colormap_texture, _filter_linear, 1);
            //_context->bind_texture(_depth_buffer, _filter_nearest, 2);
            _context->bind_texture(_color_buffer, _filter_nearest, 2);

            _box->draw(_context, geometry::MODE_SOLID);
        }
        _context->end_query(_timer_volume);

        _context->reset_uniform_buffers();
    }

    //_context->collect_query_results(_timer_zfill);
    //_context->collect_query_results(_timer_volume);

    time_zfill_accum  += scm::time::to_milliseconds(scm::time::nanosec(_timer_zfill->result()));
    time_volume_accum += scm::time::to_milliseconds(scm::time::nanosec(_timer_volume->result()));
    accum_count       += 1;

    if ((time_zfill_accum + time_volume_accum) > 1000.0) {
        boost::io::ios_all_saver ias(std::cout);
        std::cout << "zfill time:  " << std::fixed << time_zfill_accum / accum_count  << "msec, "
                  << "volume time: " << std::fixed << time_volume_accum / accum_count << "msec" << std::endl;
        time_zfill_accum  = 0.0;
        time_volume_accum = 0.0;
        accum_count       = 0;
    }

    if (0)
    { // fullscreen pass
        mat4f   pass_mvp = mat4f::identity();
        ortho_matrix(pass_mvp, 0.0f, 1.0f, 0.0f, 1.0f, -1.0f, 1.0f);

        _pass_through_shader->uniform_sampler("in_texture", 0);
        _pass_through_shader->uniform("mvp", pass_mvp);

        _context->set_default_frame_buffer();
        _context->set_depth_stencil_state(_depth_no_z);
        _context->set_blend_state(_no_blend);
        _context->set_rasterizer_state(_cull_back);
        _context->bind_texture(_color_buffer, _filter_nearest, 0);
        _context->bind_program(_pass_through_shader);

        _quad->draw(_context);
    }

    // swap the back and front buffer, so that the drawn stuff can be seen
    glutSwapBuffers();

    if (fraps_bug) {
        if (glGetError() != GL_NO_ERROR) {
            std::cout << "fraps bug after swap handled" << std::endl;
        }
        else {
            //std::cout << "nothing after swap" << std::endl;
        }
        fraps_bug = false;
    }
}

void
demo_app::resize(int w, int h)
{
    // safe the new dimensions
    winx = w;
    winy = h;

    // set the new viewport into which now will be rendered

    using namespace scm::gl;
    using namespace scm::math;

    _context->set_viewport(viewport(vec2ui(0, 0), vec2ui(w, h)));
    //glViewport(0, 0, w, h);

    scm::math::perspective_matrix(_current_transforms->_p_matrix, 60.f, float(w)/float(h), 0.1f, 100.0f);
    _current_transforms->_p_matrix_inverse = inverse(_current_transforms->_p_matrix);
}

void
demo_app::mousefunc(int button, int state, int x, int y)
{
    switch (button) {
        case GLUT_LEFT_BUTTON:
            {
                _lb_down = (state == GLUT_DOWN) ? true : false;
            }break;
        case GLUT_MIDDLE_BUTTON:
            {
                _mb_down = (state == GLUT_DOWN) ? true : false;
            }break;
        case GLUT_RIGHT_BUTTON:
            {
                _rb_down = (state == GLUT_DOWN) ? true : false;
            }break;
    }

    _initx = 2.f * float(x - (winx/2))/float(winx);
    _inity = 2.f * float(winy - y - (winy/2))/float(winy);
}

void
demo_app::mousemotion(int x, int y)
{
    float nx = 2.f * float(x - (winx/2))/float(winx);
    float ny = 2.f * float(winy - y - (winy/2))/float(winy);

    //std::cout << "nx " << nx << " ny " << ny << std::endl;

    if (_lb_down) {
        _trackball_manip.rotation(_initx, _inity, nx, ny);
    }
    if (_rb_down) {
        _trackball_manip.dolly(_dolly_sens * (ny - _inity));
    }
    if (_mb_down) {
        _trackball_manip.translation(nx - _initx, ny - _inity);
    }

    _inity = ny;
    _initx = nx;
}

void
demo_app::keyboard(unsigned char key, int x, int y)
{
}

void
glut_display()
{
    if (_application)
        _application->display();

}

void
glut_resize(int w, int h)
{
    if (_application)
        _application->resize(w, h);
}

void
glut_mousefunc(int button, int state, int x, int y)
{
    if (_application)
        _application->mousefunc(button, state, x, y);
}

void
glut_mousemotion(int x, int y)
{
    if (_application)
        _application->mousemotion(x, y);
}

void
glut_idle()
{
    glutPostRedisplay();
}

void
glut_keyboard(unsigned char key, int x, int y)
{
    switch (key) {
        // ESC key
        case 27: {
            _application.reset();
            std::cout << "reset application" << std::endl;
            exit (0);
                 }
            break;
        case 'f':
            glutFullScreenToggle();
            break;
        default:
            _application->keyboard(key, x, y);
    }
}

int main(int argc, char **argv)
{
    scm::shared_ptr<scm::core>      scm_core(new scm::core(argc, argv));
    _application.reset(new demo_app());

    // the stuff that has to be done
    glutInit(&argc, argv);
    glutInitContextVersion(4, 3);
    glutInitContextProfile(GLUT_CORE_PROFILE);
    //glutInitContextProfile(GLUT_COMPATIBILITY_PROFILE);

    // init a double buffered framebuffer with depth buffer and 4 channels
    glutInitDisplayMode(GLUT_DOUBLE | GLUT_DEPTH | GLUT_RGBA | GLUT_ALPHA);
    // create window with initial dimensions
    glutInitWindowSize(winx, winy);
    glutCreateWindow("simple_glut");

    // init the GL context
    if (!_application->initialize()) {
        std::cout << "error initializing gl context" << std::endl;
        return (-1);
    }

    // set the callbacks for resize, draw and idle actions
    glutReshapeFunc(glut_resize);
    glutDisplayFunc(glut_display);
    glutKeyboardFunc(glut_keyboard);
    glutMouseFunc(glut_mousefunc);
    glutMotionFunc(glut_mousemotion);
    glutIdleFunc(glut_idle);

    // and finally start the event loop
    glutMainLoop();

    return (0);
}


/*! Google Analytics Domain Hash Calculator
 *
 * When analyzing Google Analytics cookies, you will see an Urchin Tracking Module A (UTMA)
 * value (utma=) in the URL. The part for comparison is only the number before the first dot
 * ("."). For instance, in "utma=173272373.nnnnn...", the domain hash is 173272373
 * ("google.com", where n is any arbitrary number). Enter the domain (without the protocol,
 * i.e., don't include "http://", just use "google.com") and the domain hash will be calculated.
 *
 * MIT License
 *
 * Copyright (c) 2017 Dan O'Day <d@4n68r.com>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 * Concerning the use of Boost in this code:
 * Distributed under the Boost Software License, Version 1.0
 * (See accompanying file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <boost/program_options.hpp>
#include <boost/iostreams/device/mapped_file.hpp>
#include <iostream>

/*!
 * Calculate Google Analytics domain hash
 *
 * @param [in] domain Domain to calculate hash for
 * @return int representing Google Analytics domain hash or 0 if error
 */
int hash(const std::string& domain)
{
    int a = 1;
    int c = 0;
    int h, o;
    if (!domain.empty()) {
        a = 0;

        // domain.length() returns an ulong which could overflow h (int)
        // check for unlikely event of int overflow here and return 0 to prevent
        if (domain.length() > INT_MAX) {
            return 0;
        }

        for (h = domain.length() - 1; h >= 0; h--) {
            o = domain[h];
            a = (a << 6 & 268435455) + o + (o << 14);
            c = a & 266338304;
            a = c != 0 ? a ^ c >> 21 : a;
        }
    }

    return a;
}  // hash

/*!
 * Iterate over lines in istream input and stream parsed results to output
 *
 * @param [in] input input stream containing list of domains to parse
 * @param [in] delimiter delimiter to use for results output
 * @param [out] output output stringstream containing final parsed data for all domains
 */
void iterateLines(std::istream& input, const std::string& delimiter, std::ostringstream& output) {
    // iterate over file contents and stream parsed delimited results to output
    for (std::string line; std::getline(input, line); ) {
        output << line << delimiter << std::to_string(hash(line)) << "\n";
    }
}  // iterateLines

/*!
 * Read from stdin
 *
 * @param [in] delimiter delimiter to use for results output
 */
void readStdIn(const std::string& delimiter) {
    // allegedly improves performance, see https://stackoverflow.com/a/9371717/
    std::ios_base::sync_with_stdio(false);

    // instantiate output stringstream and stream header row
    std::ostringstream output;
    output << "domain" << delimiter << "hash\n";

    // iterate over lines and generate output
    iterateLines(std::cin, delimiter, output);

    // stream final output to stdout
    std::cout << output.str();
} // readStdIn

/*!
 * Print version
 */
void printVersion() {
    std::cout << "Google Analytics Domain Hash Calculator v0.2" << "\n";
    std::cout << "Copyright (C) 2017 Dan O'Day <d@4n68r.com>\n\n";

    std::cout << "This work is free. You can redistribute it and/or modify it under\n" \
                 "the terms of the MIT License. See the LICENSE file in the gadhash\n" \
                 "distribution for more details.\n\n";
} // printVersion

/*!
 * Print usage
 */
void printUsage() {
    std::cout << "When analyzing Google Analytics cookies, you will see an Urchin Tracking\n"  \
                 "Module A (UTMA) value (utma=) in the URL. The part for comparison is only\n" \
                 "the number before the first dot (\".\"). For instance, in \"utma=173272373.nnnnn...\",\n" \
                 "the domain hash is 173272373 (\"google.com\", where n is any arbitrary number).\n" \
                 "Enter the domain (without the protocol, i.e., don't include \"http://\", just use\n" \
                 "\"google.com\") and the domain hash will be calculated. A hash of 0 indicates an error.\n";
}  // printUsage

/*!
 * main
 */
int main(int argc, const char* argv[]) {
    try {
        std::string delimiter = ",";
        std::vector<std::string> files;
        namespace po = boost::program_options;

        po::options_description desc("Options");
        desc.add_options()
                ("help,h", "get usage help")
                ("version,v", "display version")
                ("delimiter,d", po::value<std::string>(&delimiter)->default_value(","),
                 "specify output delimiter string (comma/CSV by default; can be multiple characters)")
                ("file,f", po::value<std::vector<std::string>>(&files),
                 "file(s) containing list of domains (one per line); gadhash reads stdin if no file(s) " \
                         "specified or a hyphen ('-') is given");

        po::positional_options_description posDesc;
        posDesc.add("file", -1);

        po::variables_map vm;

        try {
            po::store(po::command_line_parser(argc, argv).options(desc).positional(posDesc).run(), vm);

            // version info
            if (vm.count("version")) {
                printVersion();
                if (!vm.count("help")) {
                    // only exit if help not also requested
                    return EXIT_SUCCESS;
                }
            }

            // help option
            if (vm.count("help")) {
                printUsage();
                std::cout << '\n' << desc << '\n';
                return EXIT_SUCCESS;
            }

            // call notify after displaying version and/or help in case it finds error and throws exception
            po::notify(vm);

        } catch(po::error& e) {
            std::cerr << "ERROR: " << e.what() << '\n';
            std::cerr << desc << '\n';
            return EXIT_FAILURE;
        }

        // boost literal-escapes all user input (which is secure, but in this case I want a tab char)
        if (delimiter == "\\t") {
            delimiter = "\t";
        } else if (delimiter == "\\n") {  // I suppose someone could want a newline delimiter for some reason (consider)
            delimiter = "\n";
        }

        // read domain(s) from file(s)
        if (!files.empty()) {
            // instantiate output stringstream and stream header row
            std::ostringstream output;
            output << "domain" << delimiter << "hash\n";

            for (std::string& fn : files) {
                // handle if we encounter hyphen as file parameter
                if (fn == "-") {
                    if (files.size() > 1) {
                        std::cerr << "ERROR: Mixture of file name(s) and stdin specified. " \
                                     "Specify file name(s) OR stdin ('-').\n";
                        return EXIT_FAILURE;

                    } else {  // only one element in vector: '-' (read from stdin)
                        readStdIn(delimiter);
                        return EXIT_SUCCESS;
                    }
                }

                // memory map file for speed, files should be small enough to fit in RAM
                boost::iostreams::mapped_file_source f(fn);
                std::string fileContents(f.data(), f.size());
                f.close();

                // iterate over lines and generate output
                std::istringstream input(fileContents);
                iterateLines(input, delimiter, output);
            }

            // stream final output to stdout
            std::cout << output.str();

            return EXIT_SUCCESS;
        }

        // read domain(s) from stdin (default behavior)
        readStdIn(delimiter);

    } catch(std::exception& e) {
        std::cerr << "Unexpected error:\n" << e.what() << '\n'
                  << "If you're feeling helpful, email this error to Dan at d@4n68r.com and tell him what you tried.\n";
        return EXIT_FAILURE;
    }

//    std::cout << std::endl;  // final buffer flush
    return EXIT_SUCCESS;
}  // main


/*
    This file is part of WARG's computer-vision

    Copyright (c) 2015, Waterloo Aerial Robotics Group (WARG)
    All rights reserved.

    Redistribution and use in source and binary forms, with or without
    modification, are permitted provided that the following conditions are met:
    1. Redistributions of source code must retain the above copyright
       notice, this list of conditions and the following disclaimer.
    2. 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.
    3. Usage of this code MUST be explicitly referenced to WARG and this code
       cannot be used in any competition against WARG.
    4. Neither the name of the WARG 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 WARG ''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 WARG 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 "k_means_filter.h"
#include <boost/log/trivial.hpp>
#include <opencv2/imgproc/imgproc.hpp>

using namespace cv;

cv::Mat * KMeansFilter::filter(const Mat & src) {
    int ratio = 3;
    int kernel_size = 3;
    int reductionFactor = 2; //beyond 2 seems to decrease effectiveness of the diff'd image
    BOOST_LOG_TRIVIAL(info) << "Calculating kmeans...";

    Mat tmp;
    resize(src, tmp, Size( src.cols/reductionFactor, src.rows/reductionFactor ));

    /// Analyze colour clusters in image
    Mat samples(tmp.rows * tmp.cols, 3, CV_32F);
    for( int y = 0; y < tmp.rows; y++ )
        for( int x = 0; x < tmp.cols; x++ )
            for( int z = 0; z < 3; z++)
                samples.at<float>(y + x*tmp.rows, z) = tmp.at<Vec3b>(y,x)[z];

    int clusterCount = 10;
    Mat labels;
    int attempts = 1;
    Mat centers;
    kmeans(samples, clusterCount, labels, TermCriteria(CV_TERMCRIT_ITER, 1, 1), attempts, KMEANS_PP_CENTERS, centers );

    BOOST_LOG_TRIVIAL(info) << "Generating new image...";
    Mat * new_image = new Mat( src.size(), src.type() );
    for( int y = 0; y < src.rows; y++ ) {
        for( int x = 0; x < src.cols; x++ ) {
            int cluster_idx = labels.at<int>((y/reductionFactor) + (x/reductionFactor)*(src.rows/reductionFactor),0);
            new_image->at<Vec3b>(y,x)[0] = abs(src.at<Vec3b>(y,x)[0] - centers.at<float>(cluster_idx, 0));
            new_image->at<Vec3b>(y,x)[1] = abs(src.at<Vec3b>(y,x)[1] - centers.at<float>(cluster_idx, 1));
            new_image->at<Vec3b>(y,x)[2] = abs(src.at<Vec3b>(y,x)[2] - centers.at<float>(cluster_idx, 2));
        }
    }
    erode(*new_image, *new_image, 5);
    dilate(*new_image, *new_image, 5);

    /// Reduce noise with a kernel
    blur(*new_image, *new_image, Size(kernel_size,kernel_size));
    return new_image;
}


#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <tf/transform_listener.h>
#include <boost/foreach.hpp>
#include <string>
#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/LaserScan.h>
#include <algorithm>
#include <iostream>
#include <cmath>
#include <cfloat>
#define foreach BOOST_FOREACH

double getDistance(const geometry_msgs::PoseStamped::ConstPtr& pose, double x, double y)
{
	return sqrt( (pose->pose.position.x - x) * (pose->pose.position.x -x) + (pose->pose.position.y - y)*(pose->pose.position.y - y) );
}

int main(int argc, char** argv)
{
	std::string bag_file;
	ros::init(argc, argv, "metrics");
	ros::NodeHandle n;
	ros::NodeHandle pn("~");

	double start_x, start_y;
	double goal_x, goal_y;
	double last_x,last_y;
	double distance_threshold;
	tf::TransformListener tf;

	pn.param<double>("start_x",start_x,3.46);
	pn.param<double>("start_y",start_y,4.62);
	pn.param<double>("distance_threshold",distance_threshold,0.25);

	pn.param<double>("goal_x",goal_x,6.0);
	pn.param<double>("goal_y",goal_y,9.0);

	pn.param<std::string>("bag_file",bag_file,"");

	rosbag::Bag bag;
	ROS_INFO("Opening bag %s...",bag_file.c_str());
	bag.open(bag_file,rosbag::bagmode::Read);
	ROS_INFO("Processing bag %s...",bag_file.c_str());

	std::vector<std::string> topics;

	topics.push_back("/map_pose");
	topics.push_back("/scan");
	bool first_pose=true;
	double time=0,distance=0;
	rosbag::View view(bag, rosbag::TopicQuery(topics));
	ros::Time initial_time;
	std::vector<double> laser_mins;
	foreach(rosbag::MessageInstance const m, view) {

		if (m.getTopic().compare("/map_pose")==0) {
			geometry_msgs::PoseStamped::ConstPtr pose = m.instantiate<geometry_msgs::PoseStamped>();
			if (first_pose && getDistance(pose,start_x,start_y)>distance_threshold) {
				ROS_ERROR("Invalid initial position: (%f,%f) Distance: %f ",pose->pose.position.x,pose->pose.position.y,  getDistance(pose,start_x,start_y));
				break;
			} else if (getDistance(pose,goal_x,goal_y)<distance_threshold) {
				time = (pose->header.stamp - initial_time).toSec();
				ROS_INFO("Trajectory finished");
				break;
			}
			if (first_pose) {
				initial_time = pose->header.stamp;
				first_pose=false;
			} else {
				distance+=getDistance(pose,last_x,last_y);
			}
			last_x = pose->pose.position.x;
			last_y = pose->pose.position.y;

		} else if (m.getTopic().compare("/scan")==0) {

			sensor_msgs::LaserScan::ConstPtr laser = m.instantiate<sensor_msgs::LaserScan>();
			double min = 999999;
			unsigned counter=0;
			for (unsigned i = 0; i< laser->ranges.size(); i++) {
				if (!std::isnan(laser->ranges[i]) && !std::isinf(laser->ranges[i])) {
					if (laser->ranges[i]<min) {
						min = laser->ranges[i];
					}
					counter++;
				}
			}
			if (counter>0) {
				laser_mins.push_back(min);
			}

		}

	}
	std::cout<<"TRAJECTORY TIME: "<<time<<" seconds"<<std::endl;
	std::cout<<"TRAJECTORY DISTANCE: "<<distance<<" meters"<<std::endl;
	if (laser_mins.size()>0) {
		double average_value=0;
		for (unsigned i=0;i<laser_mins.size();i++) {
			average_value += laser_mins[i];
		}
		average_value/=(double)laser_mins.size();
		double sd=0;
		for (unsigned i=0;i<laser_mins.size();i++) {
			sd+= (laser_mins[i] - average_value) * (laser_mins[i] - average_value);
		}
		sd /=(double)laser_mins.size();
		sd = sqrt(sd);
		std::cout<<"AVERAGE MIN DISTANCE TO OBSTACLES: "<<average_value<<" meters. Standard Deviation = "<<sd<<std::endl;
	}

}


#include "CloudPoints_Tools.h"
#include <qdebug.h>
//
//#include <iostream>
//#include <string>
//
//#include <pcl/io/ply_io.h>
//#include <pcl/point_types.h>
//#include <pcl/registration/icp.h>
//#include <pcl/visualization/pcl_visualizer.h>
//#include <pcl/console/time.h>   // TicToc

#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/time.h>
#include <pcl/console/print.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/statistical_outlier_removal.h>

using pcl::visualization::PointCloudColorHandlerGenericField;
using pcl::visualization::PointCloudColorHandlerCustom;
//typedef pcl::PointXYZRGB PointT;
//typedef pcl::PointCloud<PointT> PointCloudT;
//
//bool next_iteration = false;

//typedef pcl::PointNormal PointNT;
//typedef pcl::PointCloud<PointNT> PointCloudT;
//typedef pcl::FPFHSignature33 FeatureT;
//typedef pcl::FPFHEstimationOMP<PointNT, PointNT, FeatureT> FeatureEstimationT;
//typedef pcl::PointCloud<FeatureT> FeatureCloudT;
//typedef pcl::visualization::PointCloudColorHandlerCustom<PointXYZRGB> ColorHandlerT;

//void
//keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event,
//	void* nothing)
//{
//	if (event.getKeySym() == "space" && event.keyDown())
//		next_iteration = true;
//}

std::vector<PointIndices> CloudPoints_Tools::CloudSegmentation(PointCloud<PointXYZRGB>::Ptr nowLayerCloud
	, int sliderValue,float nowCloud_avg_distance) {
	std::vector<PointCloud<PointXYZRGB>::Ptr> cluster_clouds_;
	std::vector<PointIndices> cluster_indice;
	if (nowLayerCloud->size() > 700000)
	{
		return cluster_indice;
	}
	PointCloud<PointXYZRGB>::Ptr nowLayrCloudClone(new PointCloud<PointXYZRGB>);
	copyPointCloud(*nowLayerCloud, *nowLayrCloudClone);

	search::KdTree<PointXYZRGB>::Ptr tree(new search::KdTree<PointXYZRGB>);
	tree->setInputCloud(nowLayrCloudClone);

	if (nowCloud_avg_distance == 0)
	{
		return cluster_indice;
		qDebug() << "NO DATA";
	}

	qDebug() << "22222222222INSEGMENTATION2222";
	EuclideanClusterExtraction<PointXYZRGB> ec;
	ec.setClusterTolerance(double(sliderValue *nowCloud_avg_distance*0.01));
	ec.setMinClusterSize(300);
	ec.setSearchMethod(tree);
	ec.setInputCloud(nowLayrCloudClone);
	ec.extract(cluster_indice);

	return cluster_indice;
}

std::vector<complax_cloudInformation> CloudPoints_Tools::cloudSegComplax(PointCloud<PointXYZRGB>::Ptr nowLayerCloud, int sliderValue) {
	std::vector<complax_cloudInformation> cluster_clouds_;
	if (nowLayerCloud->size() > 100000)
	{
		qDebug() << "Too Many points to be deal.";
		return cluster_clouds_;
	}
	qDebug() << "IM IN!!!!!";
	qDebug() << "IM IN!!!!!";
	qDebug() << "IM IN!!!!!";
	PointCloud<PointXYZRGB>::Ptr nowLayrCloudClone(new PointCloud<PointXYZRGB>);
	nowLayrCloudClone = nowLayerCloud;

	search::KdTree<PointXYZRGB>::Ptr tree(new search::KdTree<PointXYZRGB>);
	tree->setInputCloud(nowLayrCloudClone);
	//EuclideanClusterExtraction<PointXYZRGB>::KdTreePtr tree(new EuclideanClusterExtraction<PointXYZRGB>);
	//tree->setInputCloud(nowLayrCloudClone);

	EuclideanClusterExtraction<PointXYZRGB> ec;
	ec.setClusterTolerance(float(sliderValue *0.001));
	qDebug() << "11111111111111";
	ec.setMinClusterSize(300);
	qDebug() << "2222222222222";
	ec.setSearchMethod(tree);
	ec.setInputCloud(nowLayrCloudClone);
	std::vector<PointIndices> cluster_indice;
	qDebug() << "333333333333";
	ec.extract(cluster_indice);
	qDebug() << "IM TREE!!!!!";
	qDebug() << "IM TREE!!!!!";
	qDebug() << "IM TREE!!!!!";
	qDebug() << "IM TREE!!!!!";
	for (int i = 0; i < nowLayerCloud->size(); ++i)
	{
		nowLayerCloud->points[i].r = 255;
		nowLayerCloud->points[i].g = 255;
		nowLayerCloud->points[i].b = 255;
	}
	qDebug() << "IM CLUSTING!!!!!";
	qDebug() << "IM CLUSTING!!!!!";
	qDebug() << "IM CLUSTING!!!!!";
	qDebug() << "IM CLUSTING!!!!!";
	int irrr = 0;
	for (std::vector<PointIndices>::const_iterator i = cluster_indice.begin(); i != cluster_indice.end(); ++i)
	{
		++irrr;
		qDebug() << "--------" << irrr;
		int color_R = rand() % 250;
		int color_G = rand() % 250;
		int color_B = rand() % 250;
		//--------------
		complax_cloudInformation cluster;
		cluster.cloud_data.reset(new pcl::PointCloud<PointXYZRGB>);
		for (std::vector<int>::const_iterator point = i->indices.begin(); point != i->indices.end(); ++point) {
			nowLayrCloudClone->points[*point].r = color_R;
			nowLayrCloudClone->points[*point].g = color_G;
			nowLayrCloudClone->points[*point].b = color_B;
			cluster.cloud_data->points.push_back(nowLayrCloudClone->points[*point]);
			cluster.points_id.push_back(point - i->indices.begin());
			nowLayerCloud->points[*point].r = color_R;
			nowLayerCloud->points[*point].g = color_G;
			nowLayerCloud->points[*point].b = color_B;
		}
		cluster.cloud_data->width = cluster.cloud_data->points.size();
		cluster.cloud_data->height = 1;
		cluster.cloud_data->is_dense = true;

		if (cluster.cloud_data->points.size() <= 0)
			continue;

		cluster_clouds_.push_back(cluster);
	}
	return cluster_clouds_;
}

void CloudPoints_Tools::CloudColor(PointCloud<PointXYZRGB>::Ptr nowLayerCloud) {

}

PointCloud<PointXYZRGB>::Ptr CloudPoints_Tools::CloudSmooth(PointCloud<PointXYZRGB>::Ptr nowLayerCloud, float smooth_strength) {
	PointCloud<PointXYZRGB>::Ptr smoothCloud(new PointCloud<PointXYZRGB>);
	MovingLeastSquares<PointXYZRGB, PointXYZRGB>::Ptr mls(new MovingLeastSquares<PointXYZRGB, PointXYZRGB>);
	search::KdTree<PointXYZRGB>::Ptr tree(new search::KdTree<PointXYZRGB>);

	tree->setInputCloud(nowLayerCloud);

	mls->setComputeNormals(true);
	mls->setInputCloud(nowLayerCloud);
	mls->setSearchMethod(tree);
	//mls->setPolynomialOrder(2);
	//mls->setNumberOfThreads(1);
	//mls->setPointDensity(nowLayerCloud->size());
	mls->setSearchRadius(smooth_strength);
	//mls->setPolynomialFit(true);
	mls->process(*smoothCloud);

	return smoothCloud;
}
//
//void print4x4Matrix(const Eigen::Matrix4d & matrix)    //打印旋转矩阵和平移矩阵
//{
//	printf("Rotation matrix :\n");
//	printf("    | %6.3f %6.3f %6.3f | \n", matrix(0, 0), matrix(0, 1), matrix(0, 2));
//	printf("R = | %6.3f %6.3f %6.3f | \n", matrix(1, 0), matrix(1, 1), matrix(1, 2));
//	printf("    | %6.3f %6.3f %6.3f | \n", matrix(2, 0), matrix(2, 1), matrix(2, 2));
//	printf("Translation vector :\n");
//	printf("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix(0, 3), matrix(1, 3), matrix(2, 3));
//}
//pcl::visualization::PCLVisualizer *p;
//int vp_1, vp_2;
//void showCloudsRight(const PointCloud<PointNormal>::Ptr cloud_target, const PointCloud<PointNormal>::Ptr cloud_source)
//{
//	p->removePointCloud("source");
//	p->removePointCloud("target");
//
//
//	PointCloudColorHandlerGenericField<PointNormal> tgt_color_handler(cloud_target, "curvature");
//	if (!tgt_color_handler.isCapable())
//		PCL_WARN("Cannot create curvature color handler!");
//
//	PointCloudColorHandlerGenericField<PointNormal> src_color_handler(cloud_source, "curvature");
//	if (!src_color_handler.isCapable())
//		PCL_WARN("Cannot create curvature color handler!");
//
//
//	p->addPointCloud(cloud_target, tgt_color_handler, "target", vp_2);
//	p->addPointCloud(cloud_source, src_color_handler, "source", vp_2);
//
//	p->spinOnce();
//}
//void showCloudsLeft(const PointCloud<PointXYZRGB>::Ptr cloud_target, const PointCloud<PointXYZRGB>::Ptr cloud_source)
//{
//	p->removePointCloud("vp1_target");
//	p->removePointCloud("vp1_source");
//
//	PointCloudColorHandlerCustom<PointXYZRGB> tgt_h(cloud_target, 0, 255, 0);
//	PointCloudColorHandlerCustom<PointXYZRGB> src_h(cloud_source, 255, 0, 0);
//	p->addPointCloud(cloud_target, tgt_h, "vp1_target", vp_1);
//	qDebug() << cloud_target->size();
//	p->addPointCloud(cloud_source, src_h, "vp1_source", vp_1);
//	qDebug() << cloud_source->size();
//	PCL_INFO("Press q to begin the registration.\n");
//	p->spin();
//}
//void pairAlign(const PointCloud<PointXYZRGB>::Ptr sourceCloud,
//	const PointCloud<PointXYZRGB>::Ptr targetCloud,
//	PointCloud<PointXYZRGB>::Ptr output,
//	Eigen::Matrix4f &final_transform, bool downsample = false) {
//
//	PointCloud<PointXYZRGB>::Ptr src(new PointCloud<PointXYZRGB>);
//	PointCloud<PointXYZRGB>::Ptr tgt(new PointCloud<PointXYZRGB>);
//
//	//=============================
//	//減少點雲
//	qDebug() << "VoxelGrid";
//	VoxelGrid<PointXYZRGB> grid;
//	const float leaf = 0.002f;
//	grid.setLeafSize(leaf, leaf, leaf);
//	grid.setInputCloud(targetCloud);
//	grid.filter(*tgt);
//	grid.setInputCloud(sourceCloud);
//	grid.filter(*src);
//
//	//估計法線
//	qDebug() << "Normal";
//	NormalEstimation<PointXYZRGB, pcl::PointNormal> n;
//	PointCloud<pcl::PointNormal>::Ptr target_normal(new PointCloud<pcl::PointNormal>);
//	search::KdTree<PointXYZRGB>::Ptr tree(new search::KdTree<PointXYZRGB>);
//	tree->setInputCloud(targetCloud);
//	n.setInputCloud(tgt);
//	n.setSearchMethod(tree);
//	//n.setRadiusSearch(0.05);
//	n.setKSearch(30);
//	n.compute(*target_normal);
//	pcl::copyPointCloud(*tgt, *target_normal);
//	qDebug() << target_normal->size();
//
//	NormalEstimation<PointXYZRGB, pcl::PointNormal> n2;
//	PointCloud<pcl::PointNormal>::Ptr src_normal(new PointCloud<pcl::PointNormal>);
//	search::KdTree<PointXYZRGB>::Ptr tree2(new search::KdTree<PointXYZRGB>);
//	tree2->setInputCloud(sourceCloud);
//	n2.setInputCloud(src);
//	n2.setSearchMethod(tree2);
//	//n2.setRadiusSearch(0.05);
//	n2.setKSearch(30);
//	n2.compute(*src_normal);
//	pcl::copyPointCloud(*src, *src_normal);
//	qDebug() << src_normal->size();
//
//
//	//-----------
//	MyPointRepresentation point_representation;
//	float alpha[4] = { 1.0,1.0,1.0,1.0 };
//	point_representation.setRescaleValues(alpha);
//	//--------------------
//	pcl::IterativeClosestPointNonLinear<PointNormal, PointNormal> reg;
//	reg.setTransformationEpsilon(1e-10);
//	reg.setMaxCorrespondenceDistance(0.01);
//	/*reg.setEuclideanFitnessEpsilon(0.005);*/
//	reg.setMaximumIterations(1);
//	// Set the point representation
//	reg.setPointRepresentation(boost::make_shared<const MyPointRepresentation>(point_representation));
//	reg.setInputSource(src_normal);
//	reg.setInputTarget(target_normal);
//
//	Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity(), prev, targetToSource;
//	PointCloud<PointNormal>::Ptr reg_result(new PointCloud<PointNormal>);
//	reg_result = src_normal;
//	for (int i = 0; i < 30; ++i)
//	{
//		PCL_INFO("Iteration Nr. %d.\n", i);
//
//		// save cloud for visualization purpose
//		src_normal = reg_result;
//		qDebug() << "src_normal" << src_normal->size();
//
//		// Estimate
//		reg.setInputSource(src_normal);
//		qDebug() << "align...";
//		reg.align(*reg_result);
//		qDebug() << "align_normal" << reg_result->size();
//
//		//accumulate transformation between each Iteration
//		Ti = reg.getFinalTransformation() * Ti;
//
//		//if the difference between this transformation and the previous one
//		//is smaller than the threshold, refine the process by reducing
//		//the maximal correspondence distance
//		if (std::abs((reg.getLastIncrementalTransformation() - prev).sum()) < reg.getTransformationEpsilon())
//			reg.setMaxCorrespondenceDistance(reg.getMaxCorrespondenceDistance() - 0.005);
//
//		prev = reg.getLastIncrementalTransformation();
//
//		// visualize current state
//		showCloudsRight(target_normal, src_normal);
//	}
//
//	//
//  // Get the transformation from target to source
//	targetToSource = Ti.inverse();
//
//	//
//	// Transform target back in source frame
//	pcl::transformPointCloud(*targetCloud, *output, targetToSource);
//
//	p->removePointCloud("source");
//	p->removePointCloud("target");
//
//	PointCloudColorHandlerCustom<PointXYZRGB> cloud_tgt_h(output, 0, 255, 0);
//	PointCloudColorHandlerCustom<PointXYZRGB> cloud_src_h(sourceCloud, 255, 0, 0);
//	p->addPointCloud(output, cloud_tgt_h, "target", vp_2);
//	p->addPointCloud(sourceCloud, cloud_src_h, "source", vp_2);
//
//	PCL_INFO("Press q to continue the registration.\n");
//	p->spin();
//
//	p->removePointCloud("source");
//	p->removePointCloud("target");
//
//	//add the source to the transformed target
//	*output += *sourceCloud;
//
//	final_transform = targetToSource;
//}
//
//void CloudPoints_Tools::registrationClouds(vector<PointCloud<PointXYZRGB>::Ptr> clouds) {
//	if (clouds.size() < 2)
//		return;
//	////==============
//	//Target為移動目標
//	PointCloud<PointXYZRGB>::Ptr targetCloud(new PointCloud<PointXYZRGB>);
//	targetCloud = clouds[1]->makeShared();
//	PointCloud<PointXYZRGB>::Ptr sourceCloud(new PointCloud<PointXYZRGB>);
//	sourceCloud = clouds[0]->makeShared();
//	////----------濾除離群點---------
//	//VoxelGrid<PointXYZRGB> grid;
//	//const float leaf = 0.0008f;
//	//grid.setLeafSize(leaf, leaf, leaf);
//	//grid.setInputCloud(targetCloud);
//	//grid.filter(*targetCloud);
//
//	//grid.setLeafSize(leaf, leaf, leaf);
//	//grid.setInputCloud(sourceCloud);
//	//grid.filter(*sourceCloud);
//	//PointCloud<PointXYZRGB>::Ptr outFile_source(new PointCloud<PointXYZRGB>);
//	//PointCloud<PointXYZRGB>::Ptr outFile_target(new PointCloud<PointXYZRGB>);
//
//	//pcl::StatisticalOutlierRemoval<PointXYZRGB> sor;
//	//sor.setInputCloud(sourceCloud);
//	//sor.setMeanK(500);
//	//sor.setStddevMulThresh(0.01);
//	//sor.filter(*outFile_source);
//
//	//sor.setInputCloud(targetCloud);
//	//sor.filter(*outFile_target);
//
//	//============================
//	p = new pcl::visualization::PCLVisualizer();
//	p->createViewPort(0.0, 0, 0.5, 1.0, vp_1);
//	p->createViewPort(0.5, 0, 1.0, 1.0, vp_2);
//
//	Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity(), pairTransform;
//	showCloudsLeft(sourceCloud, targetCloud);
//	//---------------------------
//	PointCloud<PointXYZRGB>::Ptr temp(new PointCloud<PointXYZRGB>);
//	//可以做到極度貼近，但ALIGN會出錯
//	pairAlign(sourceCloud, targetCloud, temp, pairTransform, true);
//	PointCloud<PointXYZRGB>::Ptr result(new PointCloud<PointXYZRGB>);
//	pcl::transformPointCloud(*targetCloud, *result, pairTransform);
//	GlobalTransform *= pairTransform;
//	//result->clear();
//	//pcl::transformPointCloud(*sourceCloud, *result, GlobalTransform);
//	pcl::io::savePCDFileASCII("D:/transformed.pcd", *result);
//
//	qDebug() << result->size();
//	//--------------
//}