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

// MARKER(update_precomp.py): autogen include statement, do not remove
#include "precompiled_basegfx.hxx"
#include <osl/diagnose.h>

#include <basegfx/curve/b2dcubicbezier.hxx>

#include <basegfx/tools/debugplotter.hxx>
#include <boost/bind.hpp>

namespace basegfx
{
    namespace
    {
        void outputHeader( const ::rtl::OString& rTitle,
                           ::std::ostream* 		 pStm )
        {
            // output gnuplot setup
            if( pStm )
            {
                *pStm << "#!/usr/bin/gnuplot -persist" << ::std::endl <<
                    "#" << ::std::endl <<
                    "# automatically generated by basegfx, don't change!" << ::std::endl <<
                    "#" << ::std::endl <<
                    "#   --- " << rTitle.getStr() << " ---" << ::std::endl <<
                    "#" << ::std::endl <<
                    "set parametric" << ::std::endl <<
                    "# set terminal postscript eps enhanced color " << ::std::endl <<
                    "# set output \"plot.eps\"" << ::std::endl <<
                    // This function plots a cubic bezier curve. P,q,r,s
                    // are the control point elements of the corresponding
                    // output coordinate component (i.e. x components for
                    // the x plot, and y components for the y plot)
                    "cubicBezier(p,q,r,s,t) = p*(1-t)**3+q*3*(1-t)**2*t+r*3*(1-t)*t**2+s*t**3" << ::std::endl <<
                    // This function plots the derivative of a cubic
                    // bezier curve. P,q,r,s are the control point
                    // components of the _original_ curve
                    "cubicBezDerivative(p,q,r,s,t) = 3*(q-p)*(1-t)**2+6*(r-q)*(1-t)*t+3*(s-r)*t**2" << ::std::endl <<
                    // Plot a line's component of a line between a and b
                    // (where a and b should be the corresponding
                    // components of the line's start and end point,
                    // respectively)
                    "line(p,q,r) = p*(1-t)+q*t" << ::std::endl <<
                    // Plot a line's x component of a line in implicit
                    // form ax + by + c = 0
                    "implicitLineX(a,b,c,t) = a*-c + t*-b" << ::std::endl <<
                    // Plot a line's y component of a line in implicit
                    // form ax + by + c = 0
                    "implicitLineY(a,b,c,t) = b*-c + t*a" << ::std::endl <<
                    "pointmarkx(c,t) = c-0.03*t" << ::std::endl <<										 // hack for displaying single points in parametric form
                    "pointmarky(c,t) = c+0.03*t" << ::std::endl <<										 // hack for displaying single points in parametric form
                    "# end of setup" << ::std::endl;
            }
            else
            {
                OSL_TRACE( "#!/usr/bin/gnuplot -persist\n",
                           "#\n",
                           "# automatically generated by basegfx, don't change!\n",
                           "#\n",
                           "#   --- %s ---\n",
                           "#\n",
                           "set parametric\n",
                           // This function plots a cubic bezier curve. P,q,r,s
                           // are the control point elements of the corresponding
                           // output coordinate component (i.e. x components for
                           // the x plot, and y components for the y plot)
                           "cubicBezier(p,q,r,s,t) = p*(1-t)**3+q*3*(1-t)**2*t+r*3*(1-t)*t**2+s*t**3\n",
                           // This function plots the derivative of a cubic
                           // bezier curve. P,q,r,s are the control point
                           // components of the _original_ curve
                           "cubicBezDerivative(p,q,r,s,t) = 3*(q-p)*(1-t)**2+6*(r-q)*(1-t)*t+3*(s-r)*t**2\n",
                           // Plot a line's component of a line between a and b
                           // (where a and b should be the corresponding
                           // components of the line's start and end point,
                           // respectively)
                           "line(p,q,r) = p*(1-t)+q*t\n",
                           // Plot a line's x component of a line in implicit
                           // form ax + by + c = 0
                           "implicitLineX(a,b,c,t) = a*-c + t*-b\n",
                           // Plot a line's y component of a line in implicit
                           // form ax + by + c = 0
                           "implicitLineY(a,b,c,t) = b*-c + t*a\n",
                           "pointmarkx(c,t) = c-0.03*t\n",										 // hack for displaying single points in parametric form
                           "pointmarky(c,t) = c+0.03*t\n",										 // hack for displaying single points in parametric form
                           "# end of setup\n",
                           rTitle.getStr() );
            }
        }

        class Writer
        {
        public:
            Writer( ::std::ostream* pStm ) :
                mpStream( pStm )
            {
            }

            void outputPoint( const ::std::pair< B2DPoint, ::rtl::OString >& rElem )
            {
                if( mpStream )
                    *mpStream << " " << rElem.first.getX() << "\t" << rElem.first.getY() << ::std::endl;
                else
                    OSL_TRACE( " %f\t%f\n", rElem.first.getX(), rElem.first.getY() );
            }

            void outputVector( const ::std::pair< B2DVector, ::rtl::OString >& rElem )
            {
                if( mpStream )
                    *mpStream << " " << rElem.first.getX() << "\t" << rElem.first.getY() << ::std::endl << ::std::endl;
                else
                    OSL_TRACE( " %f\t%f\n\n", rElem.first.getX(), rElem.first.getY() );
            }

            void outputRect( const ::std::pair< B2DRange, ::rtl::OString >& rElem )
            {
                const double nX0( rElem.first.getMinX() );
                const double nY0( rElem.first.getMinY() );
                const double nX1( rElem.first.getMaxX() );
                const double nY1( rElem.first.getMaxY() );

                if( mpStream )
                    *mpStream << " "
                              << nX0 << "\t" << nY0 << "\t"
                              << nX1 << "\t" << nY0 << "\t"
                              << nX1 << "\t" << nY1 << "\t"
                              << nX0 << "\t" << nY1 << "\t"
                              << nX0 << "\t" << nY0 << ::std::endl << ::std::endl;

                else
                    OSL_TRACE( " %f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n\n",
                               nX0, nY0,
                               nX1, nY0,
                               nX1, nY1,
                               nX0, nY1,
                               nX0, nY0 );
            }

        private:
            ::std::ostream* 	mpStream;
        };
    }

    DebugPlotter::DebugPlotter( const sal_Char* pTitle ) :
        maTitle( pTitle ),
        maPoints(),
        maVectors(),
        maRanges(),
        maPolygons(),
        mpOutputStream(NULL)
    {
    }

    DebugPlotter::DebugPlotter( const sal_Char* pTitle,
                                ::std::ostream& rOutputStream ) :
        maTitle( pTitle ),
        maPoints(),
        maVectors(),
        maRanges(),
        maPolygons(),
        mpOutputStream(&rOutputStream)
    {
    }

    DebugPlotter::~DebugPlotter()
    {
        const bool bHavePoints( !maPoints.empty() );
        const bool bHaveVectors( !maVectors.empty() );
        const bool bHaveRanges( !maRanges.empty() );
        const bool bHavePolygons( !maPolygons.empty() );

        if( bHavePoints ||
            bHaveVectors ||
            bHaveRanges ||
            bHavePolygons )
        {
            outputHeader( maTitle, mpOutputStream );

            print( "\n\n# parametric primitive output\n"
                   "plot [t=0:1] \\\n" );

            // output plot declarations for used entities
            bool bNeedColon( false );
            if( bHavePoints )
            {
                print( " '-' using ($1):($2) title \"Points\" with points" );
                bNeedColon = true;
            }
            if( bHaveVectors )
            {
                if( bNeedColon )
                    print( ", \\\n" );

                print( " '-' using ($1):($2) title \"Vectors\" with lp" );
                bNeedColon = true;
            }
            if( bHaveRanges )
            {
                if( bNeedColon )
                    print( ", \\\n" );

                print( " '-' using ($1):($2) title \"Ranges\" with lines" );
                bNeedColon = true;
            }
            if( bHavePolygons )
            {
                const ::std::size_t nSize( maPolygons.size() );
                for( ::std::size_t i=0; i<nSize; ++i )
                {
                    if( maPolygons.at(i).first.areControlPointsUsed() )
                    {
                        const B2DPolygon& rCurrPoly( maPolygons.at(i).first );

                        const sal_uInt32 nCount( rCurrPoly.count() );
                        for( sal_uInt32 k=0; k<nCount; ++k )
                        {
                            if( bNeedColon )
                                print( ", \\\n" );

                            const B2DPoint& rP0( rCurrPoly.getB2DPoint(k) );
                            const B2DPoint& rP1( rCurrPoly.getNextControlPoint(k) );
                            const B2DPoint& rP2( rCurrPoly.getPrevControlPoint((k + 1) % nCount) );
                            const B2DPoint& rP3( k+1<nCount ? rCurrPoly.getB2DPoint(k+1) : rCurrPoly.getB2DPoint(k) );

                            if( mpOutputStream )
                                *mpOutputStream << "  cubicBezier("
                                                << rP0.getX() << ","
                                    << rP1.getX() << ","
                                    << rP2.getX() << ","
                                    << rP3.getX() << ",t), \\\n   cubicBezier("
                                    << rP0.getY() << ","
                                    << rP1.getY() << ","
                                    << rP2.getY() << ","
                                    << rP3.getY() << ",t)";
                            else
                                OSL_TRACE( "  cubicBezier(%f,%f,%f,%f,t), \\\n"
                                           "   cubicBezier(%f,%f,%f,%f,t)",
                                           rP0.getX(),
                                           rP1.getX(),
                                           rP2.getX(),
                                           rP3.getX(),
                                           rP0.getY(),
                                           rP1.getY(),
                                           rP2.getY(),
                                           rP3.getY() );

                            bNeedColon = true;
                        }
                    }
                    else
                    {
                        if( bNeedColon )
                            print( ", \\\n" );

                        if( mpOutputStream )
                            *mpOutputStream << " '-' using ($1):($2) title \"Polygon "
                                            << maPolygons.at(i).second.getStr() << "\" with lp";
                        else
                            OSL_TRACE( " '-' using ($1):($2) title \"Polygon %s\" with lp",
                                       maPolygons.at(i).second.getStr() );

                        bNeedColon = true;
                    }
                }
            }

            if( bHavePoints )
            {
                Writer aWriter( mpOutputStream );

                ::std::for_each( maPoints.begin(),
                                 maPoints.end(),
                                 ::boost::bind( &Writer::outputPoint,
                                                ::boost::ref( aWriter ),
                                                _1 ) );
                print( "e\n" );
            }

            if( bHaveVectors )
            {
                Writer aWriter( mpOutputStream );

                ::std::for_each( maVectors.begin(),
                                 maVectors.end(),
                                 ::boost::bind( &Writer::outputVector,
                                                ::boost::ref( aWriter ),
                                                _1 ) );
                print( "e\n" );
            }

            if( bHaveRanges )
            {
                Writer aWriter( mpOutputStream );

                ::std::for_each( maRanges.begin(),
                                 maRanges.end(),
                                 ::boost::bind( &Writer::outputRect,
                                                ::boost::ref( aWriter ),
                                                _1 ) );
                print( "e\n" );
            }

            if( bHavePolygons )
            {
                const ::std::size_t nSize( maPolygons.size() );
                for( ::std::size_t i=0; i<nSize; ++i )
                {
                    if( !maPolygons.at(i).first.areControlPointsUsed() )
                    {
                        const B2DPolygon& rCurrPoly( maPolygons.at(i).first );

                        const sal_uInt32 nCount( rCurrPoly.count() );
                        for( sal_uInt32 k=0; k<nCount; ++k )
                        {
                            const B2DPoint& rP( rCurrPoly.getB2DPoint(k) );

                            if( mpOutputStream )
                                *mpOutputStream << " " << rP.getX() << "," << rP.getY();
                            else
                                OSL_TRACE( " %f,%f",
                                           rP.getX(),
                                           rP.getX() );
                        }

                        print( "\ne\n" );
                    }
                }
            }
        }
    }

    void DebugPlotter::plot( const B2DPoint& rPoint,
                             const sal_Char* pTitle )
    {
        maPoints.push_back( ::std::make_pair( rPoint,
                                              ::rtl::OString( pTitle ) ) );
    }

    void DebugPlotter::plot( const B2DVector&	rVec,
                             const sal_Char* 	pTitle )
    {
        maVectors.push_back( ::std::make_pair( rVec,
                                               ::rtl::OString( pTitle ) ) );
    }

    void DebugPlotter::plot( const B2DCubicBezier&	rBezier,
                             const sal_Char* 		pTitle )
    {
        B2DPolygon aPoly;
        aPoly.append(rBezier.getStartPoint());
		aPoly.appendBezierSegment(rBezier.getControlPointA(), rBezier.getControlPointB(), rBezier.getEndPoint());
        maPolygons.push_back( ::std::make_pair( aPoly,
                                                ::rtl::OString( pTitle ) ) );
    }

    void DebugPlotter::plot( const B2DRange& rRange,
                             const sal_Char* pTitle )
    {
        maRanges.push_back( ::std::make_pair( rRange,
                                              ::rtl::OString( pTitle ) ) );
    }

    void DebugPlotter::plot( const B2DPolygon&	rPoly,
                             const sal_Char* 	pTitle )
    {
        maPolygons.push_back( ::std::make_pair( rPoly,
                                                ::rtl::OString( pTitle ) ) );
    }

    void DebugPlotter::plot( const B2DPolyPolygon&	rPoly,
                             const sal_Char* 		pTitle )
    {
        const ::rtl::OString aTitle( pTitle );
        const sal_uInt32 nCount( rPoly.count() );
        for( sal_uInt32 i=0; i<nCount; ++i )
            maPolygons.push_back( ::std::make_pair( rPoly.getB2DPolygon( i ),
                                                    aTitle ) );
    }

    void DebugPlotter::print( const sal_Char* pStr )
    {
        if( mpOutputStream )
            *mpOutputStream << pStr;
        else
            OSL_TRACE( pStr );
    }
}


#ifndef SPECIAL_HPP
#define SPECIAL_HPP

#include <boost/shared_ptr.hpp>
#include "../workspace.hpp"
#include <cmath>
#include <memory>

namespace dqmc {
    namespace la {

#ifdef USE_DD
	inline void copy_to_dd(pmat_t& in, dd_mat& out) {
	    for (int c = 0; c < in.cols(); ++c) {
		for (int r = 0; r < in.rows(); ++r) {
		    out(r, c) = ddouble(in(r, c));
		}
	    }
	}

	inline void copy_to_dd(pvec_t& in, dd_vec& out) {
	    for (int c = 0; c < in.size(); ++c) {
		out(c) = ddouble(in(c));
	    }
	}

	inline void copy_from_dd(dd_mat& in, pmat_t& out) {
	    for (int c = 0; c < in.cols(); ++c) {
		for (int r = 0; r < in.rows(); ++r) {
		    out(r, c) = to_double(in(r, c));
		}
	    }
	}

	inline void copy_from_dd(dd_vec& in, pvec_t& out) {
	    for (int c = 0; c < in.rows(); ++c) {
		out(c) = to_double(in(c));
	    }
	}

	inline void dd_normalize(dd_vec&__restrict__ v) {
	    dd_real norm = 0;

	    for (int i = 0; i < v.rows() * v.cols(); ++i) {
		if ((v(i) * v(i)).isnan()) {
		    std::cout << v(i) << " squared is nan" << std::endl;
		    throw std::runtime_error("error");
		}
		norm += v(i) * v(i);
	    }

	    v /= sqrt(norm);
	}

	inline void dd_dot(dd_vec&__restrict__ v1, dd_vec&__restrict__ v2,
			   dd_real&__restrict__ result) {
	    result = dd_real(0, 0);

	    for (int i = 0; i < v1.rows() * v1.cols(); ++i) {
		if ((v1(i) * v2(i)).isnan()) {
		    std::cout << v1(i) << " times " << v2(i) << " is nan" << std::endl;
		    throw std::runtime_error("error");
		}
		result += v1(i) * v2(i);
	    }
	}

	inline void dd_from_col(int c, const dd_mat& in, dd_vec& out) {
	    for (int r = 0; r < in.rows(); ++r) {
		out(r) = in(r, c);
	    }
	}

	inline void dd_to_col(int c, const dd_vec& in, dd_mat& out) {
	    for (int r = 0; r < in.rows(); ++r) {
		out(r, c) = in(r);
	    }
	}

	inline void dd_from_row(int r, const dd_mat& in, dd_vec& out) {
	    for (int c = 0; c < in.cols(); ++c) {
		out(c) = in(r, c);
	    }
	}

	inline void dd_to_row(int r, const dd_vec& in, dd_mat& out) {
	    for (int c = 0; c < out.cols(); ++c) {
		out(r, c) = in(c);
	    }
	}

	inline void dd_thin_col_to_invertible(dd_mat& in, dd_mat& temp, dd_mat& out,
					      dd_vec& t_vec_1, dd_vec& t_vec_2) {
	    using namespace std;

	    for (int c = 0; c < in.cols(); ++c) {
		for (int r = 0; r < in.rows(); ++r) {
		    temp(r, c) = in(r,c);
		    out(r, c) = in(r, c);
		}
	    }

	    for (int c = in.cols(); c < out.cols(); ++c) {
		for (int r = 0; r < in.rows(); ++r) {
		    temp(r, c) = ddrand();// / dd_real::_max;
		}
	    }

	    // cout << "Randomized full col" << endl;
	    // dqmc::la::print_matrix(temp.rows(), temp.cols(), temp);
	    // Gram-Schmidt orthogonalization
	    dd_real projection;

	    for (int iter = 0; iter < 5; ++iter) {
		// cout << "First iteration" << endl;

		for (int c = in.cols(); c < out.cols(); ++c) {
		    // cout << "from_col" << endl;
		    dd_from_col(c, temp, t_vec_1);

		    for (int c2 = 0; c2 < c; ++c2) {
			// cout << "c2" << endl;
			dd_from_col(c2, out, t_vec_2);
			// cout << "dd_dot" << endl;
			dd_dot(t_vec_1, t_vec_2, projection);
			// cout << projection << endl;
			t_vec_1 -= projection * t_vec_2;
		    }

		    dd_normalize(t_vec_1);
		    dd_to_col(c, t_vec_1, out);
		}
		temp = out;
	    }
	    // dqmc::la::print_matrix(out.rows(), out.cols(), out);
	}

	inline void dd_thin_row_to_invertible(dd_mat& in, dd_mat& temp, dd_mat& out,
					      dd_vec& t_vec_1, dd_vec& t_vec_2) {
	    using namespace std;

	    for (int c = 0; c < in.cols(); ++c) {
		for (int r = 0; r < in.rows(); ++r) {
		    temp(r, c) = in(r,c);
		    out(r, c) = in(r, c);
		}
	    }

	    for (int c = in.rows(); c < out.rows(); ++c) {
		for (int r = 0; r < in.cols(); ++r) {
		    temp(r, c) = ddrand();// / dd_real::_max;
		    if (isnan(temp(r, c))) {
			cout << "Random nan???" << endl;
			throw std::runtime_error("no");
		    }
		}
	    }

	    dd_real projection;

	    for (int iter = 0; iter < 5; ++iter) {
		for (int r = in.rows(); r < out.rows(); ++r) {
		    dd_from_row(r, temp, t_vec_1);

		    for (int r2 = 0; r2 < r; ++r2) {
			dd_from_row(r2, out, t_vec_2);
			dd_dot(t_vec_1, t_vec_2, projection);
			t_vec_1 -= projection * t_vec_2;
		    }

		    dd_normalize(t_vec_1);
		    dd_to_row(r, t_vec_1, out);
		}
		temp = out;
	    }
	}
#endif

	inline void randomize(pmat_t& M) {
	    for (int r = 0; r < M.rows(); r++) {
		for ( int c = 0; c < M.cols(); c++) {
		    M(r, c) = (std::rand() % RAND_MAX)/double(RAND_MAX);
		}
	    }
	    std::srand (std::rand());
	}

	inline void normalize(pvec_t&__restrict__ v) {
	    pdouble_t norm = 0;

	    for (int i = 0; i < v.size(); ++i) {
		norm += v(i) * v(i);
	    }

	    for (int i = 0; i < v.size(); ++i) {
		v(i) /= sqrt(norm);
	    }
	}

	inline void random_with_inverse(pmat_t&__restrict__ in,
					pmat_t&__restrict__ in_inv) {
	    throw std::runtime_error("Broken");
	    // for (int c = 0; c < in.cols(); ++c) {
	    // 	for (int r = 0; r < in.rows(); ++r) {
	    // 	    in(r, c) = (std::rand() % RAND_MAX)/double(RAND_MAX);
	    // 	}
	    // }

	    // pmat_t U = in;
	    // pmat_t T = in;
	    // pvec_t D(in.rows());
	    // pmat_t U_inv = in;
	    // pmat_t T_inv = in;

	    // decompose_dgejsv_nt(in, U, D, T);
	    // matrix_transpose(U, T_inv);
	    // matrix_transpose(T, U_inv);
	    // thin_inv_sandwich(U_inv, D, T_inv, in_inv);
	}

	inline void random_orthogonal(pmat_t&__restrict__ in, boost::shared_ptr<dqmc::workspace> ws) {
	    for (int c = 0; c < in.cols(); ++c) {
		for (int r = 0; r < in.rows(); ++r) {
		    in(r, c) = (std::rand() % RAND_MAX)/double(RAND_MAX);
		}
	    }
	    pdouble_t projection, inner_prod;

	    pvec_t vec_1(in.rows());
	    pvec_t vec_2(in.rows());
	    pvec_t vec_3(in.rows());

	    for(int c = 0; c < in.cols(); ++c) {
		col(c, in, vec_1);
		normalize(vec_1);
		to_col(c, in, vec_1);
		dot(vec_1, vec_1, projection);
		// std::cout << "Projection of " << c << " - " << projection << std::endl;
	    }
	    // gram-Schmidt orthogonalization

	    for (int iter = 0; iter < 5; ++iter) {
		pmat_t mat_1 = in;
		for(int c = 0; c < in.cols(); ++c) {
		    col(c, in, vec_1);
		    dot(vec_1, vec_1, projection);
		    // std::cout << "Before Projection of " << c << " - " << projection << std::endl;
		}

		for (int i = 1; i < in.rows(); ++i) {
		    col(i, mat_1, vec_2);
		    col(i, mat_1, vec_3);

		    for (int j = 0; j < i; ++j) {
			col(j, mat_1, vec_1);

			dot(vec_1, vec_3, projection);
			dot(vec_1, vec_1, inner_prod);
			vec_2 -= projection/inner_prod * vec_1;
		    }
		    normalize(vec_2);
		    to_col(i, mat_1, vec_2);
		}
		in = mat_1;
		// std::cout << "Done with iter " << iter << std::endl;
		// for(int c = 0; c < in.cols(); ++c) {
		//     col(c, in, vec_1);
		//     dot(vec_1, vec_1, projection);
		//     std::cout << "Projection of " << c << " - " << projection << std::endl;
		// }

	    }
	    // for(int c = 0; c < in.cols(); ++c) {
	    // 	col(c, in, vec_1);
	    // 	dot(vec_1, vec_1, projection);
	    // 	std::cout << "Projection of " << c << " - " << projection << std::endl;
	    // }
	}

	inline void tiny_to_invertible(const pmat_t&__restrict__ in, const pmat_t& trans, pmat_t&__restrict__ out) {
	    // out.zeros();
	    out.block(0, 0, in.rows(), in.cols()) = in;
	    out.block(in.rows(), in.cols(), trans.rows(), trans.cols()) = trans;
	}

	inline void tiny_plus_random(const pmat_t&__restrict__ in, pmat_t& out) {
	    // out.zeros();
	    out.block(0, 0, in.rows(), in.cols()) = in;
	    out.block(in.rows(), in.cols(), out.rows() - in.rows(), out.cols() - in.cols()).setRandom();
	}

	inline void thin_col_plus_random(const pmat_t&__restrict__ in, pmat_t&__restrict__ out) {
	    out.block(0, 0, in.rows(), in.cols()) = in;
	    out.block(0, in.cols(), out.rows(), out.cols() - in.cols()).setRandom();
	}

	inline void thin_col_to_invertible(const pmat_t&__restrict__ in, pmat_t&__restrict__ out) {
	    using namespace std;
	    out.block(0, 0, in.rows(), in.cols()) = in;
	    out.block(0, in.cols(), out.rows(), out.cols() - in.cols()).setRandom();

	    // Gram-Schmidt orthogonalization
	    pdouble_t projection;

	    for (int iter = 0; iter < 2; ++iter) {
		for (int c = in.cols(); c < out.cols(); ++c) {
		    for (int c2 = 0; c2 < c; ++c2) {
			projection = out.col(c).transpose() * out.col(c2);
			out.col(c) -= projection * out.col(c2);
		    }
		    out.col(c).normalize();
		}
	    }
	}

	inline void thin_col_to_invertible_old(const pmat_t&__restrict__ in, pmat_t&__restrict__ out,
					       dqmc::workspace& ws) {
	    using namespace std;
	    // out.zeros();
	    for (int c = 0; c < in.cols(); ++c) {
		for (int r = 0; r < in.rows(); ++r) {
		    ws.la_mat_1(r, c) = in(r,c);
		    out(r, c) = in(r, c);
		}
	    }
	    // return;

	    for (int c = in.cols(); c < out.cols(); ++c) {
		for (int r = 0; r < in.rows(); ++r) {
		    ws.la_mat_1(r, c) = (std::rand() % RAND_MAX)/double(RAND_MAX);
		    // for (int o = 0; o < in.rows()*in.cols(); o++) {
		    // 	if (!(fabs(in((r * in.rows() + c * in.cols() + o) % in.rows(),
		    // 		      (r * in.cols() + c * in.rows() + o) % in.cols())) == 0)) {
		    // 	    ws.la_mat_1(r, c) = fabs(in((r * in.rows() + c * in.cols() + o) % in.rows(),
		    // 					 (r * in.cols() + c * in.rows() + o) % in.cols()));
		    // 	    break;
		    // 	}
		    // }
		    // ws.la_mat_1(r, c) = 0.;

		    if (r == c) {
			if (ws.la_mat_1(r, c) == 0) { ws.la_mat_1(r, c) = 1.;	}
			ws.la_mat_1(r, c) *= -1;
		    }
		}
	    }
	    // Gram-Schmidt orthogonalization
	    pdouble_t projection;

	    for (int iter = 0; iter < 5; ++iter) {
		for (int c = in.cols(); c < out.cols(); ++c) {
		    //col(c, ws.la_mat_1, ws.la_vec_1);
		    ws.la_vec_1 = ws.la_mat_1.col(c);

		    for (int c2 = 0; c2 < c; ++c2) {
			// col(c2, out, ws.la_vec_2);
			ws.la_vec_2 = out.col(c2);
			// dot(ws.la_vec_1, ws.la_vec_2, projection);
			projection = ws.la_vec_1.transpose() * ws.la_vec_2;
			ws.la_vec_1 -= projection * ws.la_vec_2;
		    }
		    // normalize(ws.la_vec_1);
		    ws.la_vec_1.normalize();
		    // to_col(c, out, ws.la_vec_1);
		    out.col(c) = ws.la_vec_1;
		}
		ws.la_mat_1 = out;
	    }
	}

	inline void thin_row_plus_random(const pmat_t&__restrict__ in,
					 pmat_t&__restrict__ out) {
	    out.block(0, 0, in.rows(), in.cols()) = in;
	    out.block(in.rows(), 0, out.rows() - in.rows(), in.cols()).setRandom();
	}

	inline void thin_row_to_invertible(const pmat_t&__restrict__ in,
					   pmat_t&__restrict__ out) {
	    out.block(0, 0, in.rows(), in.cols()) = in;
	    out.block(in.rows(), 0, out.rows() - in.rows(), in.cols()).setRandom();

	    // Gram-Schmidt orthogonalization
	    pdouble_t projection;

	    for (int iter = 0; iter < 2; ++iter) {
		for (int r = in.rows(); r < out.rows(); ++r) {
		    for (int r2 = 0; r2 < r; ++r2) {
			projection = out.row(r) * out.row(r2).transpose();
			out.row(r) -= projection * out.row(r2);
		    }
		    out.row(r).normalize();
		}
	    }
	}

	inline void thin_row_to_invertible_old(const pmat_t&__restrict__ in,
					       pmat_t&__restrict__ out,
					       dqmc::workspace& ws) {
	    // out.zeros();
	    for (int c = 0; c < in.cols(); ++c) {
		for (int r = 0; r < in.rows(); ++r) {
		    ws.la_mat_1(r, c) = in(r,c);
		    out(r, c) = in(r, c);
		}
	    }
	    // return;

	    for (int c = 0; c < out.cols(); ++c) {
		for (int r = in.rows(); r < out.rows(); ++r) {
		    ws.la_mat_1(r, c) = (std::rand() % RAND_MAX)/double(RAND_MAX);
		    if (r == c) {
			if (ws.la_mat_1(r, c) == 0) { ws.la_mat_1(r, c) = 1.;	}
			ws.la_mat_1(r, c) *= -1;
		    }
		}
	    }

	    // Gram-Schmidt orthogonalization
	    pdouble_t projection;
	    vec temp = ws.la_mat_1.row(0);

	    for (int iter = 0; iter < 5; ++iter) {
		for (int r = in.rows(); r < out.rows(); ++r) {
		    row(r, ws.la_mat_1, ws.la_vec_1);
		    // temp = ws.la_mat_1.row(r);
		    for (int r2 = 0; r2 < r; ++r2) {
			// projection = ws.la_mat_1.row(r) * out.row(r2).transpose();
			row(r2, out, ws.la_vec_2);
			dot(ws.la_vec_1, ws.la_vec_2, projection);
			ws.la_vec_1 -= projection * ws.la_vec_2;
			// ws.la_mat_1.row(r) -= projection * out.row(r2);
		    }

		    // ws.la_mat_1.row(r) /= sqrt(ws.la_mat_1.row(r) * ws.la_mat_1.row(r).transpose());
		    // ws.la_mat_1.row(r).normalize();
		    normalize(ws.la_vec_1);
		    to_row(r, out, ws.la_vec_1);
		}
		ws.la_mat_1 = out;
	    }
	}

	inline pdouble_t trace(pmat_t& M) {
	    pdouble_t tr = 0.;
	    for (int i = 0; i < M.rows(); i++) {
		tr += M(i, i);
	    }
	    return tr;
	}

	inline void split_vec_sqrt(pvec_t&__restrict__ in, pvec_t&__restrict__ out) {
	    for (int i = 0; i < in.size(); ++i) {
		out(i) = sqrt(in(i));
	    }
	}

	inline bool all_below_eps(const pmat_t&__restrict M) {
	    for (int c = 0; c < M.cols(); ++c) {
		for (int r = 0; r < M.rows(); ++r) {
		    if (fabs(M(r, c)) > 1e-14) { return false; }
		}
	    }
	    return true;
	}

	inline void sum(const pvec_t&__restrict__ v, double& sum) {
	    sum = 0;
	    for (int i = 0; i < v.size(); ++i) {
		sum += v(i);
	    }}

    }
}
#endif


//---------------------------------------------------------------------------//
//!
//! \file   DataGen_FreeGasElasticScatteringKernelFactor.cpp
//! \author Alex Robinson
//! \brief  Free gas elastic scattering gkq_set factor def.
//!
//---------------------------------------------------------------------------//

// Std Lib Includes
#include <iostream>
#include <limits>

// Boost Includes
#include <boost/math/special_functions/bessel.hpp>

// FRENSIE Includes
#include "DataGen_FreeGasElasticScatteringKernelFactor.hpp"
#include "Utility_PhysicalConstants.hpp"
#include "MonteCarlo_KinematicHelpers.hpp"
#include "Utility_ComparisonPolicy.hpp"
#include "Utility_DesignByContract.hpp"

namespace DataGen{

// Initialize static member data
double FreeGasElasticScatteringKernelFactor::neutron_kinetic_energy_multiplier=
  0.5*Utility::PhysicalConstants::neutron_rest_mass_energy/
  (Utility::PhysicalConstants::speed_of_light*
   Utility::PhysicalConstants::speed_of_light);

double FreeGasElasticScatteringKernelFactor::min_exp_arg =
  log(std::numeric_limits<double>::min());

// Constructor
FreeGasElasticScatteringKernelFactor::FreeGasElasticScatteringKernelFactor(
	  const std::shared_ptr<Utility::UnivariateDistribution>&
	  zero_temp_elastic_cross_section,
          const std::shared_ptr<MonteCarlo::NuclearScatteringAngularDistribution>&
	  cm_scattering_distribution,
	  const double A,
	  const double kT,
	  const double alpha,
	  const double beta,
	  const double E )
  : d_gkq_set( 1e-6 ),
    d_zero_temp_elastic_cross_section( zero_temp_elastic_cross_section ),
    d_cm_scattering_distribution( cm_scattering_distribution ),
    d_A( A ),
    d_kT( kT ),
    d_alpha( alpha ),
    d_beta( beta ),
    d_E( E )
{
  // Make sure the distributions are valid
  testPrecondition( zero_temp_elastic_cross_section.use_count() > 0 );
  testPrecondition( cm_scattering_distribution.use_count() > 0 );
  // Make sure the values are valid
  testPrecondition( A > 0.0 );
  testPrecondition( kT > 0.0 );
  testPrecondition( E > 0.0 );
  testPrecondition( beta >= MonteCarlo::calculateBetaMin( E, d_kT ) );
  remember( double alpha_min = MonteCarlo::calculateAlphaMin(E,beta,A,kT) );
  testPrecondition( alpha >= alpha_min );
  remember( double alpha_max = MonteCarlo::calculateAlphaMax(E,beta,d_A,d_kT) );
  testPrecondition( alpha <= alpha_max );

  calculateCachedValues();
}

// Set the alpha, beta, and energy values
void FreeGasElasticScatteringKernelFactor::setIndependentVariables(
							    const double alpha,
							    const double beta,
							    const double E )
{
  // Make sure the values are valid
  testPrecondition( E > 0.0 );
  testPrecondition( beta >= -E/d_kT );
  remember( double alpha_min_arg = sqrt(E)-sqrt(E+beta*d_kT) );
  remember( double alpha_min = alpha_min_arg*alpha_min_arg/(d_A*d_kT) );
  testPrecondition( alpha >= alpha_min );
  remember( double alpha_max_arg = sqrt(E)+sqrt(E+beta*d_kT) );
  remember( double alpha_max = alpha_max_arg*alpha_max_arg/(d_A*d_kT) );
  testPrecondition( alpha <= alpha_max );

  d_alpha = alpha;
  d_beta = beta;
  d_E = E;

  calculateCachedValues();
}

// Evaluate the factor at a desired value of the center-of-mass angle cosine
double FreeGasElasticScatteringKernelFactor::operator()(
						     const double mu_cm ) const
{
  // Make sure the cm angle is valid
  testPrecondition( mu_cm >= -1.0 );
  testPrecondition( mu_cm <= 1.0 );

  // The function goes to zero at mu_cm = 1.0
  if( mu_cm == 1.0 )
    return 0.0;
  else
  {
    double relative_velocity = d_relative_velocity_mult/sqrt(1.0-mu_cm);
    double relative_energy =
      neutron_kinetic_energy_multiplier*relative_velocity*
      relative_velocity;

    double term_1 =
      d_zero_temp_elastic_cross_section->evaluate( relative_energy )*
      d_cm_scattering_distribution->evaluatePDF( d_E, mu_cm )/
      ((1.0-mu_cm)*(1.0-mu_cm));

    double exp_arg = d_exponential_arg_mult/(1-mu_cm) +
      d_exponential_arg_const;

    double bessel_arg = d_bessel_arg_mult*
      sqrt((1.0+mu_cm)/(1.0-mu_cm));

    double term_2;

    try{
      if( exp_arg > min_exp_arg )
      {
	term_2 = boost::math::cyl_bessel_i( 0, bessel_arg, Policy() )*
	  exp( exp_arg );
      }
      else // Use extended precision
      {
	BoostLongDouble exp_arg_long = exp_arg;
	BoostLongDouble term_2_long =
	  boost::math::cyl_bessel_i( 0, bessel_arg, Policy() )*
	  exp( exp_arg_long );

	term_2 = term_2_long.convert_to<double>();
      }
    }
    // If the bessel argument is large use the asymptotic form of the function
    catch( std::exception& exception )
    {
      term_2 = exp( bessel_arg + exp_arg )/
	sqrt( 2*Utility::PhysicalConstants::pi*bessel_arg );
    }

    // Make sure the return value is valid
    testPostcondition(!Utility::QuantityTraits<double>::isnaninf(term_1*term_2));

    return term_1*term_2;
  }
}

// Get the integrated value
double FreeGasElasticScatteringKernelFactor::getIntegratedValue(
						 double& error_estimate ) const
{
  double integrated_value, lower_limit, upper_limit;

  this->findLimits( lower_limit, upper_limit );

  d_gkq_set.integrateAdaptively<15>( *this,
				    lower_limit,
				    upper_limit,
				    integrated_value,
				    error_estimate );

  // Make sure the value is valid
  testPostcondition( !Utility::QuantityTraits<double>::isnaninf( integrated_value ) );

  return integrated_value;
}

// Calculate the cached values
void FreeGasElasticScatteringKernelFactor::calculateCachedValues()
{
  d_energy_ratio = d_E/d_kT;

  d_exponential_arg_mult = -(d_A+1)*(d_A+1)*d_alpha/2.0;

  d_exponential_arg_const = -d_A*d_energy_ratio;

  double bessel_arg_mult_arg =
    4.0*d_A*d_alpha*d_energy_ratio -
    (d_beta - d_A*d_alpha)*(d_beta - d_A*d_alpha);

  // When alpha ~ alpha_min, alpha ~ alpha_max or beta ~ beta_min,
  // a very small negative argument is possible due to roundoff - set it to 0
  if( bessel_arg_mult_arg < 0.0 )
    bessel_arg_mult_arg = 0.0;

  d_bessel_arg_mult = (d_A+1)/2.0*sqrt(bessel_arg_mult_arg);

  d_relative_velocity_mult = (d_A+1)/d_A*
    sqrt(d_A*d_kT*d_alpha*Utility::PhysicalConstants::speed_of_light*
	 Utility::PhysicalConstants::speed_of_light/
	 Utility::PhysicalConstants::neutron_rest_mass_energy);
}

// Find limits to integrate over
void FreeGasElasticScatteringKernelFactor::findLimits(
						    double& lower_limit,
						    double& upper_limit ) const
{
  // find an independent value where the function is non-zero
  std::list<double> search_grid;

  double arg1 = 4*d_A*d_alpha*d_E/d_kT -
    (d_beta - d_A*d_alpha)*(d_beta - d_A*d_alpha);
  double arg2 = (d_A + 1)*(d_A + 1)*d_alpha*d_alpha;

  double estimated_peak_mu_cm = (arg1 - arg2)/(arg1 + arg2);

  double estimated_peak_exp_arg = d_exponential_arg_const +
    d_exponential_arg_mult/(1-estimated_peak_mu_cm) +
    d_bessel_arg_mult*
    sqrt((1.0+estimated_peak_mu_cm)/(1.0-estimated_peak_mu_cm));

  // Check if the integrand can be expected to return non-zero values
  if( estimated_peak_exp_arg > min_exp_arg )
  {
    search_grid.push_back( std::max( estimated_peak_mu_cm - 1e-6,
				     -1.0 ) );
    search_grid.push_back( std::min( estimated_peak_mu_cm + 1e-6,
				     1.0 ) );

    double center_value =
      this->findCMScatteringAngleCosineWithNonZeroFunctionValue( search_grid );

    // binary search to find closer lower and upper limits
    double tol = 1e-15;

    double lower_bound = -1.0;
    double upper_bound = center_value;
    double new_bound;

    while( Utility::RelativeErrorComparisonPolicy::calculateRelativeError(upper_bound, lower_bound ) > tol  )
    {
      new_bound = (upper_bound + lower_bound)/2;

      if( (*this)( new_bound ) == 0.0 )
	lower_bound = new_bound;
      else
	upper_bound = new_bound;
    }

    // set the lower integration limit
    lower_limit = lower_bound;

    lower_bound = center_value;
    upper_bound = 1.0;

    while( Utility::RelativeErrorComparisonPolicy::calculateRelativeError(upper_bound, lower_bound) > tol )
    {
      new_bound = (upper_bound + lower_bound)/2;

      if( (*this)( new_bound ) == 0.0 )
	upper_bound = new_bound;
      else
	lower_bound = new_bound;
    }

    // set the upper integration limit
    upper_limit = upper_bound;
  }
  else // integrand will always return zero
  {
    upper_limit = 0.0;
    lower_limit = 0.0;
  }
}

// Find a CM scattering angle cosine where the function is non-zero
double FreeGasElasticScatteringKernelFactor::findCMScatteringAngleCosineWithNonZeroFunctionValue(
					 std::list<double>& grid_points ) const
{
  std::list<double>::iterator first_grid_point = grid_points.begin();
  std::list<double>::iterator second_grid_point = first_grid_point;
  ++second_grid_point;

  double cm_center_value;

  // Find if any center points have non-zero function values
  while( second_grid_point != grid_points.end() )
  {
    cm_center_value = (*first_grid_point + *second_grid_point)/2;

    if( (*this)( cm_center_value ) > 0.0 )
      return cm_center_value;

    first_grid_point = grid_points.insert( second_grid_point,
					   cm_center_value );
    ++first_grid_point;
    ++second_grid_point;
  }

  return findCMScatteringAngleCosineWithNonZeroFunctionValue( grid_points );
}

} // end DataGen namespace

//---------------------------------------------------------------------------//
// end DataGen_FreeGasElasticScatteringKernelFactor.cpp
//---------------------------------------------------------------------------//


#include "multi_body_icp.h"

#include "vox_grid.h"
#include "frames.h"
#include "model.h"
#include "focal_grid.h"

#include <boost/thread/thread.hpp>
#include <boost/make_shared.hpp>
#include <string>
#include <stdint.h>
#include <iostream>
#include <iomanip>
#include <math.h>
#include <chrono>
#include <vector>
#include <thread>
#include <algorithm>
#include <dirent.h>
#include <armadillo>
#include <nlopt.h>
#include <pcl/console/parse.h>
#include <pcl/common/transforms.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/registration/icp.h>
#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.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/registration/icp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/transforms.h>

using namespace std;

// Types
typedef pcl::PointXYZ PointT;
typedef pcl::PointNormal PointNT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::PointCloud<PointNT> PointCloudT;
typedef pcl::FPFHSignature33 FeatureT;
typedef pcl::FPFHEstimationOMP<PointNT,PointNT,FeatureT> FeatureEstimationT;
typedef pcl::PointCloud<FeatureT> FeatureCloudT;

// Define a new point representation for < x, y, z, curvature >
class MyPointRepresentation : public pcl::PointRepresentation <PointNT>
{
  using pcl::PointRepresentation<PointNT>::nr_dimensions_;
public:
  MyPointRepresentation ()
  {
    // Define the number of dimensions
    nr_dimensions_ = 4;
  }

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

MultiBodyICP::MultiBodyICP() {
  // empty
}

/*
vector<arma::Mat<double>> MultiBodyICP::SingleFrameICP(frames &frame_in, model &mod) {

  vector<arma::Mat<double>> M_align_vec;

  arma::Mat<double> body_pcl = frame_in.pcl_init_single_frame[0];
  arma::Mat<double> wing_L_pcl = frame_in.pcl_init_single_frame[1];
  arma::Mat<double> wing_R_pcl = frame_in.pcl_init_single_frame[2];

  arma::Mat<double> M_body_init = frame_in.M_init_single_frame[0];
  arma::Mat<double> M_wing_L_init = frame_in.M_init_single_frame[1];
  arma::Mat<double> M_wing_R_init = frame_in.M_init_single_frame[2];

  // Calculate transformation matrices for all initial orientations:

  // body:

  arma::Mat<double> M_thorax_init = MultiBodyICP::CalculateMeshTransform(mod, M_body_init, 0);
  arma::Mat<double> M_head_init = MultiBodyICP::CalculateMeshTransform(mod, M_thorax_init, 1);
  arma::Mat<double> M_abdomen_init = MultiBodyICP::CalculateMeshTransform(mod, M_thorax_init, 2);

  pcl::PointCloud<pcl::PointXYZ> thorax_src_pcl = MultiBodyICP::GetModelSRCPointCloud(mod, M_thorax_init, 0);
  PointCloudT thorax_src = MultiBodyICP::TransferPointXYZ2PointNT(thorax_src_pcl);
  pcl::PointCloud<pcl::PointXYZ> head_src_pcl = MultiBodyICP::GetModelSRCPointCloud(mod, M_head_init, 1);
  PointCloudT head_src = MultiBodyICP::TransferPointXYZ2PointNT(head_src_pcl);
  pcl::PointCloud<pcl::PointXYZ> abdomen_src_pcl = MultiBodyICP::GetModelSRCPointCloud(mod, M_abdomen_init, 2);
  PointCloudT abdomen_src = MultiBodyICP::TransferPointXYZ2PointNT(abdomen_src_pcl);

  pcl::PointCloud<pcl::PointXYZ> body_dest_pcl = MultiBodyICP::Convert_Mat_2_PCL_XYZ(body_pcl);
  PointCloudT body_dest = MultiBodyICP::TransferPointXYZ2PointNT(body_dest_pcl);

  // Perform ICP on thorax
  //tuple<arma::Mat<double>,double> icp_result_thorax = MultiBodyICP::ICP_on_single_segment(body_dest_pcl, thorax_src_pcl);
  //tuple<arma::Mat<double>,double> icp_result_head = MultiBodyICP::ICP_on_single_segment(body_dest_pcl, head_src_pcl);
  //tuple<arma::Mat<double>,double> icp_result_abdomen = MultiBodyICP::ICP_on_single_segment(body_dest_pcl, abdomen_src_pcl);
  tuple<arma::Mat<double>,double> icp_result_thorax = MultiBodyICP::RobustPoseEstimation(body_dest, thorax_src);
  tuple<arma::Mat<double>,double> icp_result_head = MultiBodyICP::RobustPoseEstimation(body_dest, head_src);
  tuple<arma::Mat<double>,double> icp_result_abdomen = MultiBodyICP::RobustPoseEstimation(body_dest, abdomen_src);

  M_align_vec.push_back(get<0>(icp_result_thorax));
  M_align_vec.push_back(get<0>(icp_result_head));
  M_align_vec.push_back(get<0>(icp_result_abdomen));

  // left wing:

  arma::Mat<double> M_wing_L1_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_L_init.submat(0,0,3,3), 3);
  arma::Mat<double> M_wing_L2_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_L_init.submat(0,4,3,7), 3);
  arma::Mat<double> M_wing_L3_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_L_init.submat(0,8,3,11), 3);
  arma::Mat<double> M_wing_L4_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_L_init.submat(0,12,3,15), 3);

  pcl::PointCloud<pcl::PointXYZ> wing_L1_src_pcl = MultiBodyICP::GetModelSRCPointCloud(mod, M_wing_L1_init, 3);
  PointCloudT wing_L1_src = MultiBodyICP::TransferPointXYZ2PointNT(wing_L1_src_pcl);
  pcl::PointCloud<pcl::PointXYZ> wing_L2_src_pcl = MultiBodyICP::GetModelSRCPointCloud(mod, M_wing_L2_init, 3);
  PointCloudT wing_L2_src = MultiBodyICP::TransferPointXYZ2PointNT(wing_L2_src_pcl);
  pcl::PointCloud<pcl::PointXYZ> wing_L3_src_pcl = MultiBodyICP::GetModelSRCPointCloud(mod, M_wing_L3_init, 3);
  PointCloudT wing_L3_src = MultiBodyICP::TransferPointXYZ2PointNT(wing_L3_src_pcl);
  pcl::PointCloud<pcl::PointXYZ> wing_L4_src_pcl = MultiBodyICP::GetModelSRCPointCloud(mod, M_wing_L4_init, 3);
  PointCloudT wing_L4_src = MultiBodyICP::TransferPointXYZ2PointNT(wing_L4_src_pcl);

  pcl::PointCloud<pcl::PointXYZ> wing_L_dest_pcl = MultiBodyICP::Convert_Mat_2_PCL_XYZ(wing_L_pcl);
  PointCloudT wing_L_dest = MultiBodyICP::TransferPointXYZ2PointNT(wing_L_dest_pcl);

  // Perform ICP on the left wing:

  //tuple<arma::Mat<double>,double> icp_result_wing_L1 = MultiBodyICP::ICP_on_single_segment(wing_L_dest_pcl, wing_L1_src_pcl);
  //tuple<arma::Mat<double>,double> icp_result_wing_L2 = MultiBodyICP::ICP_on_single_segment(wing_L_dest_pcl, wing_L2_src_pcl);
  //tuple<arma::Mat<double>,double> icp_result_wing_L3 = MultiBodyICP::ICP_on_single_segment(wing_L_dest_pcl, wing_L3_src_pcl);
  //tuple<arma::Mat<double>,double> icp_result_wing_L4 = MultiBodyICP::ICP_on_single_segment(wing_L_dest_pcl, wing_L4_src_pcl);
  tuple<arma::Mat<double>,double> icp_result_wing_L1 = MultiBodyICP::RobustPoseEstimation(wing_L_dest, wing_L1_src);
  tuple<arma::Mat<double>,double> icp_result_wing_L2 = MultiBodyICP::RobustPoseEstimation(wing_L_dest, wing_L2_src);
  tuple<arma::Mat<double>,double> icp_result_wing_L3 = MultiBodyICP::RobustPoseEstimation(wing_L_dest, wing_L3_src);
  tuple<arma::Mat<double>,double> icp_result_wing_L4 = MultiBodyICP::RobustPoseEstimation(wing_L_dest, wing_L4_src);

  arma::Col<double> score_vec_L = {get<1>(icp_result_wing_L1),get<1>(icp_result_wing_L2),get<1>(icp_result_wing_L3),get<1>(icp_result_wing_L4)};

  if (score_vec_L.index_max()==0) {
    M_align_vec.push_back(get<0>(icp_result_wing_L1));
  }
  else if (score_vec_L.index_max()==1) {
    M_align_vec.push_back(get<0>(icp_result_wing_L2));
  }
  else if (score_vec_L.index_max()==2) {
    M_align_vec.push_back(get<0>(icp_result_wing_L3));
  }
  else if (score_vec_L.index_max()==3) {
    M_align_vec.push_back(get<0>(icp_result_wing_L4));
  }
  else {
    arma::Mat<double> zero_mat(4,4);
    zero_mat.zeros();
    M_align_vec.push_back(zero_mat);
  }

  // right wing:

  arma::Mat<double> M_wing_R1_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_R_init.submat(0,0,3,3), 4);
  arma::Mat<double> M_wing_R2_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_R_init.submat(0,4,3,7), 4);
  arma::Mat<double> M_wing_R3_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_R_init.submat(0,8,3,11), 4);
  arma::Mat<double> M_wing_R4_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_R_init.submat(0,12,3,15), 4);

  pcl::PointCloud<pcl::PointXYZ> wing_R1_src_pcl = MultiBodyICP::GetModelSRCPointCloud(mod, M_wing_R1_init, 4);
  PointCloudT wing_R1_src = MultiBodyICP::TransferPointXYZ2PointNT(wing_R1_src_pcl);
  pcl::PointCloud<pcl::PointXYZ> wing_R2_src_pcl = MultiBodyICP::GetModelSRCPointCloud(mod, M_wing_R2_init, 4);
  PointCloudT wing_R2_src = MultiBodyICP::TransferPointXYZ2PointNT(wing_R2_src_pcl);
  pcl::PointCloud<pcl::PointXYZ> wing_R3_src_pcl = MultiBodyICP::GetModelSRCPointCloud(mod, M_wing_R3_init, 4);
  PointCloudT wing_R3_src = MultiBodyICP::TransferPointXYZ2PointNT(wing_R3_src_pcl);
  pcl::PointCloud<pcl::PointXYZ> wing_R4_src_pcl = MultiBodyICP::GetModelSRCPointCloud(mod, M_wing_R4_init, 4);
  PointCloudT wing_R4_src = MultiBodyICP::TransferPointXYZ2PointNT(wing_R4_src_pcl);

  pcl::PointCloud<pcl::PointXYZ> wing_R_dest_pcl = MultiBodyICP::Convert_Mat_2_PCL_XYZ(wing_R_pcl);
  PointCloudT wing_R_dest = MultiBodyICP::TransferPointXYZ2PointNT(wing_R_dest_pcl);

  // Perform ICP on the right wing:

  //tuple<arma::Mat<double>,double> icp_result_wing_R1 = MultiBodyICP::ICP_on_single_segment(wing_R_dest_pcl, wing_R1_src_pcl);
  //tuple<arma::Mat<double>,double> icp_result_wing_R2 = MultiBodyICP::ICP_on_single_segment(wing_R_dest_pcl, wing_R2_src_pcl);
  //tuple<arma::Mat<double>,double> icp_result_wing_R3 = MultiBodyICP::ICP_on_single_segment(wing_R_dest_pcl, wing_R3_src_pcl);
  //tuple<arma::Mat<double>,double> icp_result_wing_R4 = MultiBodyICP::ICP_on_single_segment(wing_R_dest_pcl, wing_R4_src_pcl);
  tuple<arma::Mat<double>,double> icp_result_wing_R1 = MultiBodyICP::RobustPoseEstimation(wing_R_dest, wing_R1_src);
  tuple<arma::Mat<double>,double> icp_result_wing_R2 = MultiBodyICP::RobustPoseEstimation(wing_R_dest, wing_R2_src);
  tuple<arma::Mat<double>,double> icp_result_wing_R3 = MultiBodyICP::RobustPoseEstimation(wing_R_dest, wing_R3_src);
  tuple<arma::Mat<double>,double> icp_result_wing_R4 = MultiBodyICP::RobustPoseEstimation(wing_R_dest, wing_R4_src);

  arma::Col<double> score_vec_R = {get<1>(icp_result_wing_R1),get<1>(icp_result_wing_R2),get<1>(icp_result_wing_R3),get<1>(icp_result_wing_R4)};

  if (score_vec_R.index_max()==0) {
    M_align_vec.push_back(get<0>(icp_result_wing_R1));
  }
  else if (score_vec_R.index_max()==1) {
    M_align_vec.push_back(get<0>(icp_result_wing_R2));
  }
  else if (score_vec_R.index_max()==2) {
    M_align_vec.push_back(get<0>(icp_result_wing_R3));
  }
  else if (score_vec_R.index_max()==3) {
    M_align_vec.push_back(get<0>(icp_result_wing_R4));
  }
  else {
    arma::Mat<double> zero_mat(4,4);
    zero_mat.zeros();
    M_align_vec.push_back(zero_mat);
  }

  return M_align_vec;
}
*/

pcl::PointCloud<pcl::PointXYZ> MultiBodyICP::GetModelSRCPointCloud(model &mod, arma::Mat<double> M_init, int seg_ind) {

  pcl::PolygonMesh part_mesh = mod.parts[seg_ind];

  // Scale the part:

  double part_scale = mod.scale[seg_ind];

  arma::Mat<double> M_part = {{M_init(0,0)*part_scale, M_init(0,1)*part_scale, M_init(0,2)*part_scale, M_init(0,3)},
    {M_init(1,0)*part_scale, M_init(1,1)*part_scale, M_init(1,2)*part_scale, M_init(1,3)},
    {M_init(2,0)*part_scale, M_init(2,1)*part_scale, M_init(2,2)*part_scale, M_init(2,3)},
    {0.0, 0.0, 0.0, 1.0}};

  // Transform the mesh:

  pcl::PolygonMesh trans_mesh = MultiBodyICP::TransformMesh(part_mesh, M_part);

  // Get Pointcloud of the mesh:

  pcl::PointCloud<pcl::PointXYZ> pcl_out = MultiBodyICP::GetPointCloudFromMesh(trans_mesh);

  return pcl_out;
}

arma::Mat<double> MultiBodyICP::CalculateMeshTransform(model &mod, arma::Mat<double> M_init, int seg_ind) {

  arma::Mat<double> R_init = M_init.submat(0,0,2,2);
  arma::Col<double> T_init = {M_init(0,3), M_init(1,3), M_init(2,3)};

  double seg_scale = mod.scale[seg_ind];

  arma::Col<double> bbox_config = mod.bounding_box_config[seg_ind];
  arma::Mat<double> M_bbox = MultiBodyICP::GetM(bbox_config);
  arma::Mat<double> R_bbox = M_bbox.submat(0,0,2,2);
  arma::Col<double> T_bbox = {M_bbox(0,3)*seg_scale, M_bbox(1,3)*seg_scale, M_bbox(2,3)*seg_scale};

  arma::Mat<double> R_out = R_init*R_bbox;
  arma::Col<double> T_out = R_init*T_bbox+T_init;
  arma::Mat<double> M_out = {{R_out(0,0), R_out(0,1), R_out(0,2), T_out(0)},
    {R_out(1,0), R_out(1,1), R_out(1,2), T_out(1)},
    {R_out(2,0), R_out(2,1), R_out(2,2), T_out(2)},
    {0.0, 0.0, 0.0, 1.0}};
  mod.M_vec.push_back(M_out);

  return M_out;
}

tuple<arma::Mat<double>,double> MultiBodyICP::RobustPoseEstimation(PointCloudT &dest_pcl, PointCloudT &src_pcl) {

  int max_iter = 50000;
  double leaf_size = 0.10;
  double search_radius = 0.25;
  int N_samples = 3;
  int N_features = 5;

  // Pointclouds:
  PointCloudT::Ptr src (new PointCloudT);
  PointCloudT::Ptr src_aligned (new PointCloudT);
  PointCloudT::Ptr dest (new PointCloudT);
  FeatureCloudT::Ptr src_features (new FeatureCloudT);
  FeatureCloudT::Ptr dest_features (new FeatureCloudT);

  *src = src_pcl;
  *dest = dest_pcl;

  // Downsampling:
  pcl::VoxelGrid<PointNT> grid;
  grid.setLeafSize (leaf_size, leaf_size, leaf_size);
  grid.setInputCloud (src);
  grid.filter (*src);
  grid.setInputCloud (dest);
  grid.filter (*dest);

  // Estimate normals:
  pcl::NormalEstimationOMP<PointNT,PointNT> nest;
  nest.setRadiusSearch (search_radius);
  nest.setInputCloud (src);
  nest.compute (*src);
  nest.setInputCloud (dest);
  nest.compute (*dest);

  // Estimate features:
  FeatureEstimationT fest;
  fest.setRadiusSearch (search_radius);
  fest.setInputCloud (src);
  fest.setInputNormals (src);
  fest.compute (*src_features);
  fest.setInputCloud (dest);
  fest.setInputNormals (dest);
  fest.compute (*dest_features);

  // Perform alignment:
  pcl::SampleConsensusPrerejective<PointNT,PointNT,FeatureT> align;
  align.setInputSource (src);
  align.setSourceFeatures (src_features);
  align.setInputTarget (dest);
  align.setTargetFeatures (dest_features);
  align.setMaximumIterations (max_iter); // Number of RANSAC iterations
  align.setNumberOfSamples (N_samples); // Number of points to sample for generating/prejecting a pose
  align.setCorrespondenceRandomness (N_features); // Number of nearest features to use
  align.setSimilarityThreshold (0.5f); // Polygonal edge length similarity threshold
  align.setMaxCorrespondenceDistance (5.0f * leaf_size); // Inlier threshold
  align.setInlierFraction (0.1f); // Required inlier fraction ofr accepting a pose hypothesis

  {
    pcl::ScopeTime t("Alignment");
    align.align (*src_aligned);
  }

  arma::Mat<double> M_out(4,4);
  double score;

  if (align.hasConverged ()) {

    Eigen::Matrix4f transformation = align.getFinalTransformation ();

    M_out(0,0) = transformation(0,0);
    M_out(0,1) = transformation(0,1);
    M_out(0,2) = transformation(0,2);
    M_out(0,3) = transformation(0,3);
    M_out(1,0) = transformation(1,0);
    M_out(1,1) = transformation(1,1);
    M_out(1,2) = transformation(1,2);
    M_out(1,3) = transformation(1,3);
    M_out(2,0) = transformation(2,0);
    M_out(2,1) = transformation(2,1);
    M_out(2,2) = transformation(2,2);
    M_out(2,3) = transformation(2,3);
    M_out(3,0) = transformation(3,0);
    M_out(3,1) = transformation(3,1);
    M_out(3,2) = transformation(3,2);
    M_out(3,3) = transformation(3,3);

    score = 1.0;

    cout << "transformation matrix" << endl;
    cout << M_out << endl;
    cout << "inliers" << endl;
    cout << align.getInliers().size() << endl;
    cout << "" << endl;

  }
  else {
    cout << "aligned failed" << endl;
  }

  return make_tuple(M_out,score);
}

tuple<arma::Mat<double>,double> MultiBodyICP::ICP_on_single_segment(pcl::PointCloud<pcl::PointXYZ> &dest_pcl, pcl::PointCloud<pcl::PointXYZ> &src_pcl) {

  pcl::PointCloud<pcl::PointXYZ>::Ptr dest_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud (new pcl::PointCloud<pcl::PointXYZ>);

  *dest_cloud = dest_pcl;
  *src_cloud = src_pcl;

  pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
  icp.setInputCloud(src_cloud);
  icp.setInputTarget(dest_cloud);

  pcl::PointCloud<pcl::PointXYZ> Final;
  icp.align(Final);

  int icp_converged = icp.hasConverged();
  double icp_score = icp.getFitnessScore();
  Eigen::Matrix4f M_final = icp.getFinalTransformation();

  cout << "has converged:" << icp_converged << " score: " << icp_score << endl;
  cout << M_final << endl;

  arma::Mat<double> M_out(4,4);

  M_out(0,0) = M_final(0,0);
  M_out(0,1) = M_final(0,1);
  M_out(0,2) = M_final(0,2);
  M_out(0,3) = M_final(0,3);
  M_out(1,0) = M_final(1,0);
  M_out(1,1) = M_final(1,1);
  M_out(1,2) = M_final(1,2);
  M_out(1,3) = M_final(1,3);
  M_out(2,0) = M_final(2,0);
  M_out(2,1) = M_final(2,1);
  M_out(2,2) = M_final(2,2);
  M_out(2,3) = M_final(2,3);
  M_out(3,0) = M_final(3,0);
  M_out(3,1) = M_final(3,1);
  M_out(3,2) = M_final(3,2);
  M_out(3,3) = M_final(3,3);

  return make_tuple(M_out,icp_score);

}

vector<arma::Mat<int>> MultiBodyICP::ReturnProjectedImages(frames &frame_in, model &mod, vox_grid &vox, FocalGrid &fcg) {

  // Compute initial orientation matrices:

  arma::Mat<double> M_body_init = frame_in.M_init_single_frame[0];
  arma::Mat<double> M_wing_L_init = frame_in.M_init_single_frame[1];
  arma::Mat<double> M_wing_R_init = frame_in.M_init_single_frame[2];

  // body:
  arma::Mat<double> M_thorax_init  = MultiBodyICP::CalculateMeshTransform(mod, M_body_init, 0);
  arma::Mat<double> M_head_init    = MultiBodyICP::CalculateMeshTransform(mod, M_thorax_init, 1);
  arma::Mat<double> M_abdomen_init = MultiBodyICP::CalculateMeshTransform(mod, M_thorax_init, 2);

  // left wing:
  arma::Mat<double> M_wing_L1_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_L_init.submat(0,0,3,3), 3);

  // right wing:
  arma::Mat<double> M_wing_R1_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_R_init.submat(0,0,3,3), 4);

  // Project the model segments from 3d to 2d and back to 3d:

  vector<arma::Col<int>> mdl_pcl_thorax  = MultiBodyICP::GetProjectedImages(mod, vox, fcg, M_thorax_init, 0, 10);
  vector<arma::Col<int>> mdl_pcl_head    = MultiBodyICP::GetProjectedImages(mod, vox, fcg, M_head_init, 1, 10);
  vector<arma::Col<int>> mdl_pcl_abdomen = MultiBodyICP::GetProjectedImages(mod, vox, fcg, M_abdomen_init, 2, 10);
  vector<arma::Col<int>> mdl_pcl_wing_L1 = MultiBodyICP::GetProjectedImages(mod, vox, fcg, M_wing_L1_init, 3, 3);
  vector<arma::Col<int>> mdl_pcl_wing_R1 = MultiBodyICP::GetProjectedImages(mod, vox, fcg, M_wing_R1_init, 4, 3);

  vector<arma::Mat<int>> proj_frames;

  int N_frames = mdl_pcl_thorax.size();

  for (int i=0; i<N_frames; i++) {

    arma::Col<int> frame = mdl_pcl_thorax[i]+mdl_pcl_head[i]+mdl_pcl_abdomen[i]+mdl_pcl_wing_L1[i]+mdl_pcl_wing_R1[i];

    int N_row = get<0>(frame_in.image_size[i]);
    int N_col = get<1>(frame_in.image_size[i]);

    arma::Mat<int> frame_mat(N_row,N_col);

    for (int j=0; j<N_col; j++) {
      for (int k=0; k<N_row; k++) {
        if (frame(k*N_col+j)>10) {
          frame_mat(k,j) = 10;
        }
        else {
          frame_mat(k,j) = frame(k*N_col+j);
        }
      }
    }

    proj_frames.push_back(frame_mat);

  }

  return proj_frames;
}

vector<arma::Col<int>> MultiBodyICP::GetProjectedImages(model &mod, vox_grid &vox, FocalGrid &fcg, arma::Mat<double> M_init, int seg_ind, int int_val) {

  vector<arma::Mat<double>> proj_images;

  pcl::PointCloud<pcl::PointXYZ> seg_pcl = MultiBodyICP::GetModelSRCPointCloud(mod, M_init, seg_ind);

  vector<tuple<double,double,double,int>> seg_pcl_vec = MultiBodyICP::Convert_PCL_XYZ_2_Vec(seg_pcl, int_val);

  vector<arma::Col<int>> proj_frames = fcg.ProjectCloud2Image(seg_pcl_vec);

  return proj_frames;
}

arma::Mat<double> MultiBodyICP::ReturnSRCPCL(frames &frame_in, model &mod, vox_grid &vox, FocalGrid &fcg) {

  // Compute initial orientation matrices:

  arma::Mat<double> M_body_init   = frame_in.M_init_single_frame[0];
  arma::Mat<double> M_wing_L_init = frame_in.M_init_single_frame[1];
  arma::Mat<double> M_wing_R_init = frame_in.M_init_single_frame[2];

  // body:
  arma::Mat<double> M_thorax_init  = MultiBodyICP::CalculateMeshTransform(mod, M_body_init, 0);
  arma::Mat<double> M_head_init    = MultiBodyICP::CalculateMeshTransform(mod, M_thorax_init, 1);
  arma::Mat<double> M_abdomen_init = MultiBodyICP::CalculateMeshTransform(mod, M_thorax_init, 2);

  // left wing:
  arma::Mat<double> M_wing_L1_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_L_init.submat(0,0,3,3), 3);
  arma::Mat<double> M_wing_L2_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_L_init.submat(0,4,3,7), 3);
  arma::Mat<double> M_wing_L3_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_L_init.submat(0,8,3,11), 3);
  arma::Mat<double> M_wing_L4_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_L_init.submat(0,12,3,15), 3);

  // right wing:
  arma::Mat<double> M_wing_R1_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_R_init.submat(0,0,3,3), 4);
  arma::Mat<double> M_wing_R2_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_R_init.submat(0,4,3,7), 4);
  arma::Mat<double> M_wing_R3_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_R_init.submat(0,8,3,11), 4);
  arma::Mat<double> M_wing_R4_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_R_init.submat(0,12,3,15), 4);

  // Project the model segments from 3d to 2d and back to 3d:

  arma::Mat<double> mdl_pcl_thorax  = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_thorax_init,  0, 1);
  arma::Mat<double> mdl_pcl_head    = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_head_init,    1, 1);
  arma::Mat<double> mdl_pcl_abdomen = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_abdomen_init, 2, 1);
  arma::Mat<double> mdl_pcl_wing_L1 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_L1_init, 3, 1);
  arma::Mat<double> mdl_pcl_wing_L2 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_L2_init, 3, 1);
  arma::Mat<double> mdl_pcl_wing_L3 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_L3_init, 3, 1);
  arma::Mat<double> mdl_pcl_wing_L4 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_L4_init, 3, 1);
  arma::Mat<double> mdl_pcl_wing_R1 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_R1_init, 4, 1);
  arma::Mat<double> mdl_pcl_wing_R2 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_R2_init, 4, 1);
  arma::Mat<double> mdl_pcl_wing_R3 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_R3_init, 4, 1);
  arma::Mat<double> mdl_pcl_wing_R4 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_R4_init, 4, 1);

  arma::Mat<double> pcl_mat_out = mdl_pcl_thorax;
  pcl_mat_out = arma::join_rows(pcl_mat_out,mdl_pcl_head);
  pcl_mat_out = arma::join_rows(pcl_mat_out,mdl_pcl_abdomen);
  pcl_mat_out = arma::join_rows(pcl_mat_out,mdl_pcl_wing_L1);
  pcl_mat_out = arma::join_rows(pcl_mat_out,mdl_pcl_wing_L2);
  pcl_mat_out = arma::join_rows(pcl_mat_out,mdl_pcl_wing_L3);
  pcl_mat_out = arma::join_rows(pcl_mat_out,mdl_pcl_wing_L4);
  pcl_mat_out = arma::join_rows(pcl_mat_out,mdl_pcl_wing_R1);
  pcl_mat_out = arma::join_rows(pcl_mat_out,mdl_pcl_wing_R2);
  pcl_mat_out = arma::join_rows(pcl_mat_out,mdl_pcl_wing_R3);
  pcl_mat_out = arma::join_rows(pcl_mat_out,mdl_pcl_wing_R4);

  return pcl_mat_out;
}

arma::Mat<double> MultiBodyICP::SingleFrameICP(frames &frame_in, model &mod, vox_grid &vox, FocalGrid &fcg) {

  clock_t start;
  double duration;

  start = clock();

  arma::Mat<double> body_pcl = frame_in.pcl_init_single_frame[0];
  arma::Mat<double> M_body_init   = frame_in.M_init_single_frame[0];

  arma::Mat<double> M_thorax_init  = MultiBodyICP::CalculateMeshTransform(mod, M_body_init, 0);
  arma::Mat<double> M_head_init    = MultiBodyICP::CalculateMeshTransform(mod, M_thorax_init, 1);
  arma::Mat<double> M_abdomen_init = MultiBodyICP::CalculateMeshTransform(mod, M_thorax_init, 2);

  tuple<arma::Mat<double>,arma::Mat<double>,double> thorax_icp_results  = MultiBodyICP::ICPAlgorithm(mod, vox, fcg, body_pcl, M_thorax_init,  0, 1);
  tuple<arma::Mat<double>,arma::Mat<double>,double> head_icp_results    = MultiBodyICP::ICPAlgorithm(mod, vox, fcg, body_pcl, M_head_init,  1, 3);
  tuple<arma::Mat<double>,arma::Mat<double>,double> abdomen_icp_results = MultiBodyICP::ICPAlgorithm(mod, vox, fcg, body_pcl, M_abdomen_init,  2, 5);

  arma::Mat<double> pcl_mat_out = get<0>(thorax_icp_results);
  pcl_mat_out = arma::join_rows(pcl_mat_out,get<0>(head_icp_results));
  pcl_mat_out = arma::join_rows(pcl_mat_out,get<0>(abdomen_icp_results));

  duration = (clock()-start)/ (double) CLOCKS_PER_SEC;

  cout << "ICP took" << endl;
  cout << duration << endl;

  return pcl_mat_out;

}

/*
arma::Mat<double> MultiBodyICP::SingleFrameICP(frames &frame_in, model &mod, vox_grid &vox, FocalGrid &fcg) {

  // Compute initial orientation matrices:

  arma::Mat<double> body_pcl = frame_in.pcl_init_single_frame[0];
  arma::Mat<double> wing_L_pcl = frame_in.pcl_init_single_frame[1];
  arma::Mat<double> wing_R_pcl = frame_in.pcl_init_single_frame[2];

  arma::Mat<double> M_body_init   = frame_in.M_init_single_frame[0];
  arma::Mat<double> M_wing_L_init = frame_in.M_init_single_frame[1];
  arma::Mat<double> M_wing_R_init = frame_in.M_init_single_frame[2];

  // body:
  arma::Mat<double> M_thorax_init  = MultiBodyICP::CalculateMeshTransform(mod, M_body_init, 0);
  arma::Mat<double> M_head_init    = MultiBodyICP::CalculateMeshTransform(mod, M_thorax_init, 1);
  arma::Mat<double> M_abdomen_init = MultiBodyICP::CalculateMeshTransform(mod, M_thorax_init, 2);

  // left wing:
  arma::Mat<double> M_wing_L1_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_L_init.submat(0,0,3,3), 3);
  arma::Mat<double> M_wing_L2_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_L_init.submat(0,4,3,7), 3);
  arma::Mat<double> M_wing_L3_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_L_init.submat(0,8,3,11), 3);
  arma::Mat<double> M_wing_L4_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_L_init.submat(0,12,3,15), 3);

  // right wing:
  arma::Mat<double> M_wing_R1_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_R_init.submat(0,0,3,3), 4);
  arma::Mat<double> M_wing_R2_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_R_init.submat(0,4,3,7), 4);
  arma::Mat<double> M_wing_R3_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_R_init.submat(0,8,3,11), 4);
  arma::Mat<double> M_wing_R4_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_R_init.submat(0,12,3,15), 4);

  // Project the model segments from 3d to 2d and back to 3d:

  arma::Mat<double> mdl_pcl_thorax  = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_thorax_init,  0, 1);
  arma::Mat<double> mdl_pcl_head    = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_head_init,    1, 1);
  arma::Mat<double> mdl_pcl_abdomen = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_abdomen_init, 2, 1);
  arma::Mat<double> mdl_pcl_wing_L1 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_L1_init, 3, 2);
  arma::Mat<double> mdl_pcl_wing_L2 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_L2_init, 3, 2);
  arma::Mat<double> mdl_pcl_wing_L3 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_L3_init, 3, 2);
  arma::Mat<double> mdl_pcl_wing_L4 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_L4_init, 3, 2);
  arma::Mat<double> mdl_pcl_wing_R1 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_R1_init, 4, 3);
  arma::Mat<double> mdl_pcl_wing_R2 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_R2_init, 4, 3);
  arma::Mat<double> mdl_pcl_wing_R3 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_R3_init, 4, 3);
  arma::Mat<double> mdl_pcl_wing_R4 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_R4_init, 4, 3);

  // Perform ICP:
  tuple<arma::Mat<double>,arma::Mat<double>,double> thorax_icp_results  = MultiBodyICP::ICPAlgorithm(mod, vox, fcg, body_pcl, mdl_pcl_thorax,  M_thorax_init,  0, 1);
  tuple<arma::Mat<double>,arma::Mat<double>,double> head_icp_results    = MultiBodyICP::ICPAlgorithm(mod, vox, fcg, body_pcl, mdl_pcl_head,     M_head_init,    1, 2);
  tuple<arma::Mat<double>,arma::Mat<double>,double> abdomen_icp_results = MultiBodyICP::ICPAlgorithm(mod, vox, fcg, body_pcl, mdl_pcl_abdomen, M_abdomen_init, 2, 3);
  tuple<arma::Mat<double>,arma::Mat<double>,double> wing_L1_icp_results = MultiBodyICP::ICPAlgorithm(mod, vox, fcg, wing_L_pcl, mdl_pcl_wing_L1, M_wing_L1_init, 3, 5);
  tuple<arma::Mat<double>,arma::Mat<double>,double> wing_L2_icp_results = MultiBodyICP::ICPAlgorithm(mod, vox, fcg, wing_L_pcl, mdl_pcl_wing_L2, M_wing_L2_init, 3, 5);
  tuple<arma::Mat<double>,arma::Mat<double>,double> wing_L3_icp_results = MultiBodyICP::ICPAlgorithm(mod, vox, fcg, wing_L_pcl, mdl_pcl_wing_L3, M_wing_L1_init, 3, 5);
  tuple<arma::Mat<double>,arma::Mat<double>,double> wing_L4_icp_results = MultiBodyICP::ICPAlgorithm(mod, vox, fcg, wing_L_pcl, mdl_pcl_wing_L4, M_wing_L2_init, 3, 5);
  tuple<arma::Mat<double>,arma::Mat<double>,double> wing_R1_icp_results = MultiBodyICP::ICPAlgorithm(mod, vox, fcg, wing_R_pcl, mdl_pcl_wing_R1, M_wing_R1_init, 4, 10);
  tuple<arma::Mat<double>,arma::Mat<double>,double> wing_R2_icp_results = MultiBodyICP::ICPAlgorithm(mod, vox, fcg, wing_R_pcl, mdl_pcl_wing_R2, M_wing_R2_init, 4, 10);
  tuple<arma::Mat<double>,arma::Mat<double>,double> wing_R3_icp_results = MultiBodyICP::ICPAlgorithm(mod, vox, fcg, wing_R_pcl, mdl_pcl_wing_R3, M_wing_R1_init, 4, 10);
  tuple<arma::Mat<double>,arma::Mat<double>,double> wing_R4_icp_results = MultiBodyICP::ICPAlgorithm(mod, vox, fcg, wing_R_pcl, mdl_pcl_wing_R4, M_wing_R2_init, 4, 10);

  // Find best fit:
  arma::Mat<double> pcl_mat_out = get<0>(thorax_icp_results);
  pcl_mat_out = arma::join_rows(pcl_mat_out,get<0>(head_icp_results));
  pcl_mat_out = arma::join_rows(pcl_mat_out,get<0>(abdomen_icp_results));

  arma::Row<double> score_vec_L = {get<2>(wing_L1_icp_results), get<2>(wing_L2_icp_results),get<2>(wing_L3_icp_results), get<2>(wing_L4_icp_results)};
  arma::Row<double> score_vec_R = {get<2>(wing_R1_icp_results), get<2>(wing_R2_icp_results),get<2>(wing_R3_icp_results), get<2>(wing_R4_icp_results)};

  if (score_vec_L.index_min()==0) {
    pcl_mat_out = arma::join_rows(pcl_mat_out,get<0>(wing_L1_icp_results));
  }
  else if (score_vec_L.index_min()==1) {
    pcl_mat_out = arma::join_rows(pcl_mat_out,get<0>(wing_L2_icp_results));
  }
  else if (score_vec_L.index_min()==2) {
    pcl_mat_out = arma::join_rows(pcl_mat_out,get<0>(wing_L3_icp_results));
  }
  else if (score_vec_L.index_min()==3) {
    pcl_mat_out = arma::join_rows(pcl_mat_out,get<0>(wing_L4_icp_results));
  }

  if (score_vec_R.index_min()==0) {
    pcl_mat_out = arma::join_rows(pcl_mat_out,get<0>(wing_R1_icp_results));
  }
  else if (score_vec_R.index_min()==1) {
    pcl_mat_out = arma::join_rows(pcl_mat_out,get<0>(wing_R2_icp_results));
  }
  else if (score_vec_R.index_min()==2) {
    pcl_mat_out = arma::join_rows(pcl_mat_out,get<0>(wing_R3_icp_results));
  }
  else if (score_vec_R.index_min()==3) {
    pcl_mat_out = arma::join_rows(pcl_mat_out,get<0>(wing_R4_icp_results));
  }

  return pcl_mat_out;
}
*/

//tuple<arma::Mat<double>,double> MultiBodyICP::PairAlign(arma::Mat<double> &dest_pcl, arma::Mat<double> &src_pcl) {
tuple<arma::Mat<double>,double> MultiBodyICP::PairAlign(arma::Mat<double> &dest_pcl, model &mod, arma::Mat<double> M_init, int seg_ind) {

  //PointCloud cloud_src = MultiBodyICP::Convert_Mat_2_PCL_XYZ(src_pcl);
  //PointCloud cloud_tgt = MultiBodyICP::Convert_Mat_2_PCL_XYZ(dest_pcl);

  PointCloud cloud_src = MultiBodyICP::GetModelSRCPointCloud(mod, M_init, seg_ind);
  PointCloud cloud_tgt = MultiBodyICP::Convert_Mat_2_PCL_XYZ(dest_pcl);

  PointCloud::Ptr src (new PointCloud);
  PointCloud::Ptr tgt (new PointCloud);

  PointCloud::Ptr cloud_src2 (new PointCloud);
  PointCloud::Ptr cloud_tgt2 (new PointCloud);

  *cloud_src2 = cloud_src;
  *cloud_tgt2 = cloud_tgt;

  pcl::VoxelGrid<PointT> grid;

  grid.setLeafSize (0.06, 0.06, 0.06);
  grid.setInputCloud (cloud_src2);
  grid.filter (*src);

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

  pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
  //pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> icp;

  icp.setMaxCorrespondenceDistance(0.2);
  icp.setMaximumIterations(50);
  icp.setTransformationEpsilon(0.001);
  //icp.setEuclideanFitnessEpsilon(1);
  icp.setRANSACIterations(50);
  icp.setRANSACOutlierRejectionThreshold(0.06);

  icp.setInputCloud(src);
  icp.setInputTarget(tgt);

  pcl::PointCloud<pcl::PointXYZ> Final;

  icp.align(Final);

  double score = icp.getFitnessScore();

    // Get the transformation from target to source
  //targetToSource = Ti.inverse();

  Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity ();

  Ti = icp.getFinalTransformation();

  arma::Mat<double> M_transform(4,4);

  M_transform(0,0) = Ti(0,0);
  M_transform(0,1) = Ti(0,1);
  M_transform(0,2) = Ti(0,2);
  M_transform(0,3) = Ti(0,3);
  M_transform(1,0) = Ti(1,0);
  M_transform(1,1) = Ti(1,1);
  M_transform(1,2) = Ti(1,2);
  M_transform(1,3) = Ti(1,3);
  M_transform(2,0) = Ti(2,0);
  M_transform(2,1) = Ti(2,1);
  M_transform(2,2) = Ti(2,2);
  M_transform(2,3) = Ti(2,3);
  M_transform(3,0) = 0.0;
  M_transform(3,1) = 0.0;
  M_transform(3,2) = 0.0;
  M_transform(3,3) = 1.0;

  return make_tuple(M_transform,score);

}

/*
//tuple<arma::Mat<double>,double> MultiBodyICP::PairAlign(arma::Mat<double> &dest_pcl, arma::Mat<double> &src_pcl) {
tuple<arma::Mat<double>,double> MultiBodyICP::PairAlign(arma::Mat<double> &dest_pcl, model &mod, arma::Mat<double> M_init, int seg_ind) {

  //PointCloud cloud_src = MultiBodyICP::Convert_Mat_2_PCL_XYZ(src_pcl);
  PointCloud cloud_src = MultiBodyICP::GetModelSRCPointCloud(mod, M_init, seg_ind);
  PointCloud cloud_tgt = MultiBodyICP::Convert_Mat_2_PCL_XYZ(dest_pcl);

  PointCloud::Ptr src (new PointCloud);
  PointCloud::Ptr tgt (new PointCloud);

  PointCloud::Ptr cloud_src2 (new PointCloud);
  PointCloud::Ptr cloud_tgt2 (new PointCloud);

  *cloud_src2 = cloud_src;
  *cloud_tgt2 = cloud_tgt;

  //*src = cloud_src;
  //*tgt = cloud_tgt;

  pcl::VoxelGrid<PointT> grid;

  grid.setLeafSize (0.06, 0.06, 0.06);
  grid.setInputCloud (cloud_src2);
  grid.filter (*src);

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

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

  //pcl::NormalEstimation<PointT, PointNT> norm_est;
  pcl::NormalEstimationOMP<PointT, PointNT> norm_est;
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
  norm_est.setSearchMethod (tree);
  norm_est.setKSearch (30);

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

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

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

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

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

  // Run the same optimization in a loop and visualize the results
  Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity (), prev, targetToSource;
  PointCloudT::Ptr reg_result = points_with_normals_src;
  reg.setMaximumIterations (30);

  double score = 1.0;

  clock_t start;
  double duration;

  start = clock();

  for (int i = 0; i < 30; ++i) {
    // save cloud for visualization purpose
    points_with_normals_src = reg_result;

    // Estimate
    reg.setInputSource (points_with_normals_src);
    reg.align (*reg_result);

    //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 (fabs ((reg.getLastIncrementalTransformation () - prev).sum ()) < reg.getTransformationEpsilon ())
      reg.setMaxCorrespondenceDistance (reg.getMaxCorrespondenceDistance () - 0.001);

    prev = reg.getLastIncrementalTransformation ();

    score = reg.getFitnessScore();
  }

  duration = (clock()-start)/ (double) CLOCKS_PER_SEC;

  cout << "iteration time" << endl;
  cout << duration << endl;

  // Get the transformation from target to source
  //targetToSource = Ti.inverse();

  arma::Mat<double> M_transform(4,4);

  M_transform(0,0) = Ti(0,0);
  M_transform(0,1) = Ti(0,1);
  M_transform(0,2) = Ti(0,2);
  M_transform(0,3) = Ti(0,3);
  M_transform(1,0) = Ti(1,0);
  M_transform(1,1) = Ti(1,1);
  M_transform(1,2) = Ti(1,2);
  M_transform(1,3) = Ti(1,3);
  M_transform(2,0) = Ti(2,0);
  M_transform(2,1) = Ti(2,1);
  M_transform(2,2) = Ti(2,2);
  M_transform(2,3) = Ti(2,3);
  M_transform(3,0) = 0.0;
  M_transform(3,1) = 0.0;
  M_transform(3,2) = 0.0;
  M_transform(3,3) = 1.0;

  return make_tuple(M_transform,score);
}

/*
tuple<arma::Mat<double>,arma::Mat<double>,double> MultiBodyICP::ICPAlgorithm(model &mod, vox_grid &vox, FocalGrid &fcg, arma::Mat<double> &dest_pcl, arma::Mat<double> M_init, int seg_ind, int int_val) {

  arma::Mat<double> src_pcl_i;

  PointCloud cloud_src;
  PointCloud cloud_tgt;

  PointCloud::Ptr src (new PointCloud);
  PointCloud::Ptr tgt (new PointCloud);

  // Non-linear closest point:

  pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> icp;

  icp.setMaxCorrespondenceDistance(0.3);
  icp.setMaximumIterations(30);
  icp.setTransformationEpsilon(0.001);
  icp.setEuclideanFitnessEpsilon(1);
  icp.setRANSACIterations(30);
  icp.setRANSACOutlierRejectionThreshold(0.04);

  // Set target:
  cloud_tgt = MultiBodyICP::Convert_Mat_2_PCL_XYZ(dest_pcl);
  *tgt = cloud_tgt;
  icp.setInputTarget(tgt);

  // Iteration parameters:
  int max_iter = 10;
  double prev_score = 1e6;
  int i = 0;

  // Output parameters:
  arma::Mat<double> M_out;
  double score = 1.0;
  pcl::PointCloud<pcl::PointXYZ> Final;
  Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity ();
  arma::Mat<double> M_delta(4,4);
  arma::Mat<double> M_update;
  arma::Mat<double> src_pcl_out;
  M_update = M_init;

  while ((i < max_iter) && ((score/prev_score)<1.02)) {

    // Set src:
    src_pcl_i = MultiBodyICP::GetProjectedModelPCL(mod,vox,fcg,M_update,seg_ind,int_val);
    cloud_src = MultiBodyICP::Convert_Mat_2_PCL_XYZ(src_pcl_i);
    *src = cloud_src;
    icp.setInputCloud(src);

    // Align:
    icp.align(Final);

    score = icp.getFitnessScore();

    Ti = icp.getFinalTransformation();

    M_delta(0,0) = Ti(0,0);
    M_delta(0,1) = Ti(0,1);
    M_delta(0,2) = Ti(0,2);
    M_delta(0,3) = Ti(0,3);
    M_delta(1,0) = Ti(1,0);
    M_delta(1,1) = Ti(1,1);
    M_delta(1,2) = Ti(1,2);
    M_delta(1,3) = Ti(1,3);
    M_delta(2,0) = Ti(2,0);
    M_delta(2,1) = Ti(2,1);
    M_delta(2,2) = Ti(2,2);
    M_delta(2,3) = Ti(2,3);
    M_delta(3,0) = 0.0;
    M_delta(3,1) = 0.0;
    M_delta(3,2) = 0.0;
    M_delta(3,3) = 1.0;

    M_update = M_delta*M_update;

    if ((score/prev_score)<1.02) {
      src_pcl_out = src_pcl_i;
      M_out = M_update;
      prev_score = score;
    }
    else {
      i = max_iter;
    }

    i++;
  }

  return make_tuple(src_pcl_out,M_out,score);
}
*/

tuple<arma::Mat<double>,arma::Mat<double>,double> MultiBodyICP::ICPAlgorithm(model &mod, vox_grid &vox, FocalGrid &fcg, arma::Mat<double> &dest_pcl, arma::Mat<double> M_init, int seg_ind, int int_val) {

  arma::Mat<double> M_update;
  M_update = M_init;

  tuple<arma::Mat<double>,double> pair_update = MultiBodyICP::PairAlign(dest_pcl, mod, M_init, seg_ind);

  double score = get<1>(pair_update);

  cout << "score" << endl;
  cout << score << endl;

  arma::Mat<double> M_delta = get<0>(pair_update);

  cout << M_delta << endl;

  M_update.submat(0,0,2,2) = M_delta.submat(0,0,2,2)*M_update.submat(0,0,2,2);
  M_update(0,3) = M_update(0,3)+M_delta(0,3);
  M_update(1,3) = M_update(1,3)+M_delta(1,3);
  M_update(2,3) = M_update(2,3)+M_delta(2,3);

  arma::Mat<double> src_pcl_out = MultiBodyICP::GetProjectedModelPCL(mod,vox,fcg,M_update,seg_ind,int_val);

  return make_tuple(src_pcl_out,M_update,score);
}

/*
tuple<arma::Mat<double>,arma::Mat<double>,double> MultiBodyICP::ICPAlgorithm(model &mod, vox_grid &vox, FocalGrid &fcg, arma::Mat<double> &dest_pcl, arma::Mat<double> M_init, int seg_ind, int int_val) {

  double score = 1.0;
  arma::Mat<double> src_pcl_i;
  arma::Mat<double> src_pcl_out;
  arma::Mat<double> M_update;
  M_update = M_init;
  arma::Mat<double> M_out;
  M_out.eye(4,4);

  int max_iter = 30;
  double prev_score = 1e6;
  int i = 0;

  //for (int i=0; i<20; i++) {
  while ((i < max_iter) && ((score/prev_score)<1.05)) {

    cout << "iteration" << endl;
    cout << i << endl;

    src_pcl_i = MultiBodyICP::GetProjectedModelPCL(mod,vox,fcg,M_update,seg_ind,int_val);

    tuple<arma::Mat<double>,double> pair_update = MultiBodyICP::PairAlign(dest_pcl, src_pcl_i);

    //score = get<1>(pair_update);
    score = get<1>(pair_update);
    cout << "score" << endl;
    cout << score << endl;

    arma::Mat<double> M_delta = get<0>(pair_update);

    M_update.submat(0,0,2,2) = M_update.submat(0,0,2,2)*M_delta.submat(0,0,2,2);
    M_update(0,3) = M_update(0,3)+M_delta(0,3);
    M_update(1,3) = M_update(1,3)+M_delta(1,3);
    M_update(2,3) = M_update(2,3)+M_delta(2,3);

    if ((score/prev_score)<1.05) {

      M_out = M_update;
      src_pcl_out = src_pcl_i;
      prev_score = score;

    }
    else {
      i = max_iter;
    }

    i++;
  }

  return make_tuple(src_pcl_out,M_out,score);
}
*/

double MultiBodyICP::ProjectedModelScore(model &mod, vox_grid &vox, FocalGrid &fcg, arma::Mat<double> &dest_pcl, arma::Mat<double> M_init, int seg_ind, int int_val) {

  pcl::PointCloud<pcl::PointXYZ> seg_pcl = MultiBodyICP::GetModelSRCPointCloud(mod, M_init, seg_ind);

  vector<tuple<double,double,double,int>> seg_pcl_vec = MultiBodyICP::Convert_PCL_XYZ_2_Vec(seg_pcl, int_val);

  vector<tuple<double,double,double,int>> dest_pcl_vec = MultiBodyICP::Convert_Mat_2_PCL_Vec(dest_pcl, int_val);

  vector<arma::Col<int>> proj_mdl_frames = fcg.ProjectCloud2Image(seg_pcl_vec);

  vector<arma::Col<int>> proj_dest_frames = fcg.ProjectCloud2Image(dest_pcl_vec);

  int N_views = proj_mdl_frames.size();

  double score = 0.0;

  for (int i=0; i<N_views; i++) {

    arma::Col<int> frame_diff = arma::abs(proj_dest_frames[i]-proj_mdl_frames[i]);

    score += (1.0*arma::sum(frame_diff))/(1.0*arma::sum(proj_dest_frames[i]));

  }

  return score;

}

arma::Mat<double> MultiBodyICP::GetProjectedModelPCL(model &mod, vox_grid &vox, FocalGrid &fcg, arma::Mat<double> M_init, int seg_ind, int int_val) {

  // Project model pointcloud to 2d images:

  pcl::PointCloud<pcl::PointXYZ> seg_pcl = MultiBodyICP::GetModelSRCPointCloud(mod, M_init, seg_ind);

  vector<tuple<double,double,double,int>> seg_pcl_vec = MultiBodyICP::Convert_PCL_XYZ_2_Vec(seg_pcl, int_val);

  vector<arma::Col<int>> proj_mdl_frames = fcg.ProjectCloud2Image(seg_pcl_vec);

  // Project 2d images to pointcloud:

  vector<tuple<int,double,double,double,double,double,double>> proj_pcl = fcg.ProjectImage2Cloud(proj_mdl_frames, vox);

  arma::Mat<double> pcl_out = MultiBodyICP::Convert_PCL_Vec_2_Mat(proj_pcl);

  return pcl_out;
}

pcl::PolygonMesh MultiBodyICP::TransformMesh(pcl::PolygonMesh mesh_in, arma::Mat<double> transform_mat) {

  pcl::PCLHeader header = mesh_in.header;

  pcl::PCLPointCloud2 cloud2_in = mesh_in.cloud;

  vector<pcl::Vertices> polygons = mesh_in.polygons;

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromPCLPointCloud2(cloud2_in, *cloud_in);

  // Construct the transformation matrix

  Eigen::Matrix4f trans_mat = Eigen::Matrix4f::Identity();

  trans_mat(0,0) = transform_mat(0,0);
  trans_mat(0,1) = transform_mat(0,1);
  trans_mat(0,2) = transform_mat(0,2);
  trans_mat(0,3) = transform_mat(0,3);
  trans_mat(1,0) = transform_mat(1,0);
  trans_mat(1,1) = transform_mat(1,1);
  trans_mat(1,2) = transform_mat(1,2);
  trans_mat(1,3) = transform_mat(1,3);
  trans_mat(2,0) = transform_mat(2,0);
  trans_mat(2,1) = transform_mat(2,1);
  trans_mat(2,2) = transform_mat(2,2);
  trans_mat(2,3) = transform_mat(2,3);

  // Transform the pointcloud

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);

  pcl::transformPointCloud(*cloud_in, *cloud_out, trans_mat);

  pcl::PCLPointCloud2::Ptr cloud2_out (new pcl::PCLPointCloud2);

  pcl::toPCLPointCloud2(*cloud_out,*cloud2_out);

  pcl::PolygonMesh mesh_out;

  mesh_out.header = header;
  mesh_out.polygons = polygons;
  mesh_out.cloud = *cloud2_out;

  return mesh_out;

}

pcl::PointCloud<pcl::PointXYZ> MultiBodyICP::GetPointCloudFromMesh(pcl::PolygonMesh mesh_in) {

  pcl::PointCloud<pcl::PointXYZ> cloud_out;

  pcl::PCLPointCloud2 cloud2_in = mesh_in.cloud;

  pcl::fromPCLPointCloud2(cloud2_in, cloud_out);

  return cloud_out;
}

arma::Mat<double> MultiBodyICP::MultiplyMat(arma::Mat<double> M_a, arma::Mat<double> M_b) {

  arma::Mat<double> mult_mat;

  mult_mat.zeros(4,4);

  mult_mat.submat(0,0,2,2) = M_a.submat(0,0,2,2)*M_b.submat(0,0,2,2);

  mult_mat.submat(0,0,2,0) = M_a.submat(0,0,2,2)*M_b.submat(0,0,2,0)+M_a.submat(0,0,2,0);

  //mult_mat(0,3) = M_a(0,3)+M_b(0,3);
  //mult_mat(1,3) = M_a(1,3)+M_b(1,3);
  //mult_mat(2,3) = M_a(2,3)+M_b(2,3);

  mult_mat(3,3) = 1.0;

  return mult_mat;

}

arma::Mat<double> MultiBodyICP::TransformMat(arma::Col<double> q_in, double scale) {

  arma::Mat<double> start_mat;

  start_mat.zeros(4,4);

  start_mat(0,0) = (2.0*pow(q_in(0),2)-1.0+2.0*pow(q_in(1),2));
  start_mat(0,1) = (2.0*q_in(1)*q_in(2)+2.0*q_in(0)*q_in(3));
  start_mat(0,2) = (2.0*q_in(1)*q_in(3)-2.0*q_in(0)*q_in(2));
  start_mat(0,3) = scale*(q_in(4));
  start_mat(1,0) = (2.0*q_in(1)*q_in(2)-2.0*q_in(0)*q_in(3));
  start_mat(1,1) = (2.0*pow(q_in(0),2)-1.0+2.0*pow(q_in(2),2));
  start_mat(1,2) = (2.0*q_in(2)*q_in(3)+2.0*q_in(0)*q_in(1));
  start_mat(1,3) = scale*(q_in(5));
  start_mat(2,0) = (2.0*q_in(1)*q_in(3)+2.0*q_in(0)*q_in(2));
  start_mat(2,1) = (2.0*q_in(2)*q_in(3)-2.0*q_in(0)*q_in(1));
  start_mat(2,2) = (2.0*pow(q_in(0),2)-1.0+2.0*pow(q_in(3),2));
  start_mat(2,3) = scale*(q_in(6));
  start_mat(3,3) = 1.0;

  return start_mat;

}

arma::Mat<double> MultiBodyICP::Convert_PCL_Vec_2_Mat(vector<tuple<int,double,double,double,double,double,double>> pcl_in) {

  int N_points = pcl_in.size();

  arma::Mat<double> pcl_out(7,N_points);

  for (int i=0; i<N_points; i++) {

    pcl_out(0,i) = (double) get<0>(pcl_in[i]);
    pcl_out(1,i) = get<1>(pcl_in[i]);
    pcl_out(2,i) = get<2>(pcl_in[i]);
    pcl_out(3,i) = get<3>(pcl_in[i]);
    pcl_out(4,i) = get<4>(pcl_in[i]);
    pcl_out(5,i) = get<5>(pcl_in[i]);
    pcl_out(6,i) = get<6>(pcl_in[i]);

  }

  return pcl_out;
}

vector<tuple<double,double,double,int>> MultiBodyICP::Convert_PCL_XYZ_2_Vec(pcl::PointCloud<pcl::PointXYZ> pcl_in, int ind) {

  vector<tuple<double,double,double,int>> pcl_out;

  for (int i=0; i<pcl_in.points.size(); i++) {

    pcl_out.push_back(make_tuple(pcl_in.points[i].x,pcl_in.points[i].y,pcl_in.points[i].z,ind));

  }

  return pcl_out;
}

vector<tuple<double,double,double,int>> MultiBodyICP::Convert_Mat_2_PCL_Vec(arma::Mat<double> &pcl_in, int ind) {

  vector<tuple<double,double,double,int>> pcl_out;

  for (int i=0; i<pcl_in.n_cols; i++) {

    pcl_out.push_back(make_tuple(pcl_in(1,i),pcl_in(2,i),pcl_in(3,i),ind));

  }

  return pcl_out;
}

pcl::PointCloud<pcl::PointXYZ> MultiBodyICP::Convert_Mat_2_PCL_XYZ(arma::Mat<double> pcl_mat) {

  // Convert an armadillo matrix to a PointCloud vector

  pcl::PointCloud<pcl::PointXYZ> pcl_vec;

  int N_points = pcl_mat.n_cols;

  for (int i=0; i<N_points; i++) {

    pcl::PointXYZ point;
    point.x = pcl_mat(1,i);
    point.y = pcl_mat(2,i);
    point.z = pcl_mat(3,i);

    pcl_vec.push_back(point);

  }

  return pcl_vec;

}

arma::Mat<double> MultiBodyICP::GetM(arma::Col<double> state_in) {

  arma::Col<double> q_vec = {state_in(0),state_in(1),state_in(2),state_in(3)};
  q_vec = q_vec/sqrt(pow(q_vec(0),2)+pow(q_vec(1),2)+pow(q_vec(2),2)+pow(q_vec(3),2));

  double q0 = q_vec(0);
  double q1 = q_vec(1);
  double q2 = q_vec(2);
  double q3 = q_vec(3);
  double tx = state_in(4);
  double ty = state_in(5);
  double tz = state_in(6);

  arma::Mat<double> M = {{2.0*pow(q0,2.0)-1.0+2.0*pow(q1,2.0), 2.0*q1*q2-2.0*q0*q3, 2.0*q1*q3+2.0*q0*q2, tx},
    {2.0*q1*q2+2.0*q0*q3, 2.0*pow(q0,2.0)-1.0+2.0*pow(q2,2.0), 2.0*q2*q3-2.0*q0*q1, ty},
    {2.0*q1*q3-2.0*q0*q2, 2.0*q2*q3+2.0*q0*q1, 2.0*pow(q0,2.0)-1.0+2.0*pow(q3,2.0), tz},
    {0.0, 0.0, 0.0, 1.0}};

  return M;

}

PointCloudT MultiBodyICP::TransferPointXYZ2PointNT(pcl::PointCloud<pcl::PointXYZ> pcl_in) {

  PointCloudT pcl_out;

  for (int i=0; i<pcl_in.points.size(); i++) {

    PointNT point_out;

    point_out.x = pcl_in.points[i].x;
    point_out.y = pcl_in.points[i].y;
    point_out.z = pcl_in.points[i].z;
    point_out.normal_x = 1.0;
    point_out.normal_y = 0.0;
    point_out.normal_z = 0.0;

    pcl_out.push_back(point_out);

  }
  return pcl_out;
}

arma::Mat<double> MultiBodyICP::TransferPointCloudT2Mat(PointCloudT pcl_in, int int_val) {

  int N_points = pcl_in.points.size();

  arma::Mat<double> pcl_out(7,N_points);

  for (int i=0; i<N_points; i++) {
    pcl_out(0,i) = (double) int_val;
    pcl_out(1,i) = pcl_in.points[i].x;
    pcl_out(2,i) = pcl_in.points[i].y;
    pcl_out(3,i) = pcl_in.points[i].z;
    pcl_out(4,i) = 1.0;
    pcl_out(5,i) = 0.0;
    pcl_out(6,i) = 0.0;
  }

  return pcl_out;
}


/*
 * Type and constant definitions
 * Copyright (C) 2019  Robin Scheibler
 *
 * 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.
 *
 * You should have received a copy of the MIT License along with this program. If
 * not, see <https://opensource.org/licenses/MIT>.
 */

/* This file contains type and constants definitions */
#ifndef __COMMON_HPP__
#define __COMMON_HPP__

#include <iostream>
#include <Eigen/Dense>

extern float libroom_eps;  // epsilon is the precision for floating point computations. It is defined in libroom.cpp

template<size_t D>
using Vectorf = Eigen::Matrix<float, D, 1>;

using MatrixXf = Eigen::MatrixXf;
typedef Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> MatrixXb;
typedef Eigen::Matrix<bool, Eigen::Dynamic, 1> VectorXb;

/* The 'entry' type is simply defined as an array of 2 floats.
 * It represents an entry that is logged by the microphone
 * during the ray_tracing execution.
 * The first one of those float will be the travel time of a ray reaching
 * the microphone. The second one will be the energy of this ray.*/

struct Hit
{
  float distance = 0.f;
  Eigen::ArrayXf transmitted;  // vector of transmitted energy over frequency bands

  Hit(int _nfreq)
  {
    transmitted.resize(_nfreq);
    transmitted.setOnes();
  };
  Hit(const float _d, const Eigen::ArrayXf &_t)
    : distance(_d), transmitted(_t) {}
};

typedef std::vector<std::list<Hit>> HitLog;

size_t get_new_size(size_t val, size_t cur_size)
{
  size_t new_size = cur_size;
  while (val >= new_size)
    new_size *= 2;
  return new_size;
}

class Histogram2D
{
  size_t rows, cols;
  Eigen::ArrayXXf array;
  Eigen::ArrayXXi counts;

  public:
    Histogram2D() {}  // empty constructor
    Histogram2D(int _r, int _c) : rows(_r), cols(_c)
    {
      init(rows, cols);
    }

    void init(int rows, int cols)
    {
      array.resize(rows, cols);
      array.setZero();
      counts.resize(rows, cols);
      counts.setZero();
    }

    void reset()
    {
      array.setZero();
      counts.setZero();
    }

    void resize_rows(int new_rows)
    {
      auto old_rows = array.rows();
      // this will resize the array while preserving the content
      array.conservativeResize(new_rows, Eigen::NoChange);
      counts.conservativeResize(new_rows, Eigen::NoChange);
      // We need to initialize the new elements
      if (new_rows > old_rows)
      {
        array.bottomRows(new_rows - old_rows).setZero();
        counts.bottomRows(new_rows - old_rows).setZero();
      }
    }

    void resize_cols(int new_cols)
    {
      auto old_cols = array.cols();
      // this will resize the array while preserving the content
      array.conservativeResize(Eigen::NoChange, new_cols);
      counts.conservativeResize(Eigen::NoChange, new_cols);
      // We need to initialize the new elements
      if (new_cols > old_cols)
      {
        array.rightCols(new_cols - old_cols).setZero();
        counts.rightCols(new_cols - old_cols).setZero();
      }
    }

    void log(Eigen::Index row, Eigen::Index col, float val)
    {
      if (row >= array.rows())
        resize_rows(get_new_size(row, array.rows()));

      if (col >= array.cols())
        resize_cols(get_new_size(col, array.cols()));

      array.coeffRef(row, col) += val;
      counts.coeffRef(row, col)++;
    }

    void log_col(Eigen::Index col, const Eigen::ArrayXf &val)
    {
      if (col >= array.cols())
        resize_cols(get_new_size(col, array.cols()));

      array.col(col) += val;
      counts.col(col) += 1;
    }

    void log_row(Eigen::Index row, const Eigen::ArrayXf &val)
    {
      if (row >= array.rows())
        resize_rows(get_new_size(row, array.rows()));

      array.row(row) += val;
      counts.row(row) += 1;
    }

    float bin(Eigen::Index row, Eigen::Index col) const
    {
      if (counts.coeff(row, col) != 0)
        return array.coeff(row, col) / counts.coeff(row, col);
      else
        return 0.f;
    }

    Eigen::ArrayXXf get_hist() const
    {
      return array;
    }
};

#endif // __COMMON_HPP__


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

// Copyright (c) 2014-2021, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISTANCE_SEGMENT_TO_SEGMENT_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISTANCE_SEGMENT_TO_SEGMENT_HPP

#include <algorithm>
#include <iterator>

#include <boost/core/addressof.hpp>

#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/algorithms/detail/distance/is_comparable.hpp>
#include <boost/geometry/algorithms/detail/distance/strategy_utils.hpp>
#include <boost/geometry/algorithms/dispatch/distance.hpp>
#include <boost/geometry/algorithms/intersects.hpp>

#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/tags.hpp>

#include <boost/geometry/util/condition.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace distance
{

// compute segment-segment distance
template<typename Segment1, typename Segment2, typename Strategies>
class segment_to_segment
{
    typedef distance::strategy_t<Segment1, Segment2, Strategies> strategy_type;

public:
    typedef distance::return_t<Segment1, Segment2, Strategies> return_type;

    static inline return_type apply(Segment1 const& segment1, Segment2 const& segment2,
                                    Strategies const& strategies)
    {
        if (geometry::intersects(segment1, segment2, strategies))
        {
            return 0;
        }

        typename point_type<Segment1>::type p[2];
        detail::assign_point_from_index<0>(segment1, p[0]);
        detail::assign_point_from_index<1>(segment1, p[1]);

        typename point_type<Segment2>::type q[2];
        detail::assign_point_from_index<0>(segment2, q[0]);
        detail::assign_point_from_index<1>(segment2, q[1]);

        strategy_type const strategy = strategies.distance(segment1, segment2);

        auto const cstrategy = strategy::distance::services::get_comparable
                                <
                                    strategy_type
                                >::apply(strategy);

        distance::creturn_t<Segment1, Segment2, Strategies> d[4];
        d[0] = cstrategy.apply(q[0], p[0], p[1]);
        d[1] = cstrategy.apply(q[1], p[0], p[1]);
        d[2] = cstrategy.apply(p[0], q[0], q[1]);
        d[3] = cstrategy.apply(p[1], q[0], q[1]);

        std::size_t imin = std::distance(boost::addressof(d[0]),
                                         std::min_element(d, d + 4));

        if (BOOST_GEOMETRY_CONDITION(is_comparable<strategy_type>::value))
        {
            return d[imin];
        }

        switch (imin)
        {
        case 0:
            return strategy.apply(q[0], p[0], p[1]);
        case 1:
            return strategy.apply(q[1], p[0], p[1]);
        case 2:
            return strategy.apply(p[0], q[0], q[1]);
        default:
            return strategy.apply(p[1], q[0], q[1]);
        }
    }
};

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

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

// segment-segment
template <typename Segment1, typename Segment2, typename Strategy>
struct distance
    <
        Segment1, Segment2, Strategy, segment_tag, segment_tag,
        strategy_tag_distance_point_segment, false
    >
    : detail::distance::segment_to_segment<Segment1, Segment2, Strategy>
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISTANCE_SEGMENT_TO_SEGMENT_HPP