// -*- C++ -*-

// The MIT License (MIT)
//
// Copyright (c) 2017 Alexander Samoilov
//
// 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.

// ==========================================
//  FDLIB, CFDLAB, BEMLIB
//
//  Copyright by C. Pozrikidis, 1999
//  All rights reserved.
//
//  This program is to be used only under the
//  stipulations of the licensing agreement.
// ==========================================
//
// ---------------------------------------------------
//  Axisymmetric streaming (uniform)
//  potential flow past a stationary body.
//
//  This program solves
//  an integral equation of the second kind
//  for the disturbance harmonic potential
//  over the body contour
//  and computes streamlines
//
//  The contour of the body in the xy upper half-plane
//  consists of a Number of SeGments (NSG)
//
//  The segments may be straight lines or circular arcs.
//
//  Each segment is discretized into a number of elements
//
//
//  Symbols:
//  --------
//
//  NSG: Number of segments defining the body contour
//
//  NE(i): Number of elements on the ith segment
//
//  RT(i): Stretch ratio of elements on ith segment
//
//  Itp(i): Index for shape of the ith segment:
//          1 for a straight segment
//          2 for a circular arc
//
//  (Xe, Ye):  end-nodes of elements on a segment
//  (Xm, Ym):  mid-nodes of elements on a segment
//  (Xw, Yw):  end-nodes of elements on all segments
//
//  (X0, Y0):  coordinates of collocation points
//
//  T0(i): angle subtended from the center of a circular element
//         at ith collocation point
//
//  arel(i):  axisymmetric surface area of ith element
//
//  phi: disturbance potential at collocation points
//
//  dphidn0: normal derivative of disturbance potential
//                  at collocation points
//
//  dphids0: tangential derivative of potential
//                  at collocation points
//
//  cp:      pressure coefficient at collocation points
//
//  NGL: Number of Gaussian points for integration
//       over each element
//
//  Icross: Index for stopping the computation of the streamlines
//          Default value is 0
//          If Icross = 1, computation stops when a streamline
//          crosses the yz plane
//
//  Notes:
//  ------
//
//  Normal vector points into the flow
//
// ----------------------------

#include <iostream>
#include <fstream>
#include <string>
#include <chrono>
#include <boost/program_options.hpp>
#include "parameters.hpp"
#include "program_options.hpp"
#include "lin_alg_types.hpp"
#include "body_ax_geo.hpp"
#include "body_ax_sdlp.tcc"
#include "body_ax_vel.tcc"

program_options parse_command_line(int argc, char** argv)
{
    namespace po = boost::program_options;
    po::options_description desc("allowed options");
    desc.add_options()
            ("help",                   "describe arguments")
            ("verbose",                "be verbose")
            ("flow_type",              po::value<int>(), "legacy Fortran constants for flow type, 50 for sphere, 51 for torus")
            ("input_data",             po::value<std::string>(), "name of the input file e.g. `sphere.dat`, `torus_trgl.dat`")
            ("asy_name",               po::value<std::string>(), "dump streamlines to vector `.asy` file if name is given");
    try
    {
        po::variables_map vm;
        po::store(po::parse_command_line(argc, argv, desc), vm);
        po::notify(vm);
        program_options popt;

        if (vm.count("help")) {
            std::cout << desc << std::endl;
            std::exit(1);
        }

        popt.verbose = vm.count("verbose");
        if (vm.count("flow_type")) {
            popt.flow_type = vm["flow_type"].as<int>();
        }
        if (vm.count("input_data")) {
            popt.input_data = vm["input_data"].as<std::string>();
        }
        if (vm.count("asy_name")) {
            popt.asy_name = vm["asy_name"].as<std::string>();
        }

        return popt;
    }
    catch(const std::exception& e)
    {
        std::cerr << e.what() << std::endl;
        std::cout << desc << std::endl;
        std::exit(-1);
    }
}

using Time = std::chrono::high_resolution_clock;
using ms = std::chrono::milliseconds;
using fsec = std::chrono::duration<float>;

int main(int argc, char **argv)
{
    auto popt = parse_command_line(argc, argv);
    if (popt.verbose) {
        std::cout << popt;
    }

    matg_t<FLOATING_TYPE> phi;
    vecg_t<FLOATING_TYPE> velt, veln, cp;
    matg_t<FLOATING_TYPE> al;      // for the linear system
    vecg_t<FLOATING_TYPE> bl, sol; // ditto

    auto const& run_params = body_ax_geo<FLOATING_TYPE>(popt);
    if (popt.verbose) {
        std::cout << "-I- ngl: " << run_params.ngl << "\n";
        std::cout << "-I- vx: " << run_params.vx << "\n";
        std::cout << "-I- cr: " << run_params.cr << "\n";
        std::cout << "-I- ne[0]: " << run_params.ne[0] << "\n";
        std::cout << "-I- xwmin: " << run_params.xwmin << "\n";
        std::cout << "-I- ywmin: " << run_params.ywmin << "\n";
    }

    // ...

    //---------------------------------------------
    // Generate the linear system
    // for the potential at the collocation points
    //
    // Generate the influence matrix
    // consisting of integrals of the
    // single-layer potential
    //
    // Compute the rhs by evaluating the dlp
    //-------------------------------------------------------

    // resizing
    al.resize(run_params.ncl, run_params.ncl);
    bl.resize(run_params.ncl);
    // stiffness matrix and rhs
    for (int i = 0; i < run_params.ncl; ++i) {           // loop over collocation points
        bl(i) = ZERO<FLOATING_TYPE>;
        int j = -1;
        for (int k = 0; k < run_params.nsg; ++k) {       // loop over segments
            FLOATING_TYPE rad = NaN<FLOATING_TYPE>, xcnt = NaN<FLOATING_TYPE>, ycnt = NaN<FLOATING_TYPE>;
            if (run_params.itp[k] == 2) {
                rad  = run_params.actis[k];
                xcnt = run_params.xcntr[k];
                ycnt = run_params.ycntr[k];
            }
            for (int l = 0; l < run_params.ne[k]; ++l) { // loop over elements
                FLOATING_TYPE x1 = run_params.xw(k,l);
                FLOATING_TYPE y1 = run_params.yw(k,l);
                FLOATING_TYPE t1 = run_params.tw(k,l);

                FLOATING_TYPE x2 = run_params.xw(k,l+1);
                FLOATING_TYPE y2 = run_params.yw(k,l+1);
                FLOATING_TYPE t2 = run_params.tw(k,l+1);

                j = j+1;
                bool ising = (i == j);
                FLOATING_TYPE qqq, www;
                body_ax_sdlp (run_params.x0[i], run_params.y0[i], run_params.t0[i],
                              x1, y1, t1, x2, y2, t2, run_params.gl, ising,
                              run_params.itp[k], rad,xcnt,ycnt,
                              qqq, www);

                al(i, j) = www;
                bl(i) += qqq * run_params.dphidn0[j];

            }

        }
        al(i, i) -= HALF<FLOATING_TYPE>;

    }

    auto t0 = Time::now();
    //------------------------
    // Solve the linear system
    //------------------------
    sol = al.lu().solve(bl.transpose());
    auto t1 = Time::now();
    fsec fs = t1 - t0;
    ms d = std::chrono::duration_cast<ms>(fs);
    std::cout << "linear system solution took " << fs.count() << "s\n";
    std::cout <<  "linear system solution took " << d.count() << "ms\n";

    if (popt.verbose) {
        for (size_t i = 0; i < run_params.ncl; ++i) {
            std::cout << "sol(" << i << ") : " << sol(i) << std::endl;
        }
    }

    //------------------------
    // Distribute the solution
    //------------------------

    size_t max_elems = run_params.ne.maxCoeff();
    phi.resize(run_params.nsg, max_elems);
    velt.resize(run_params.nsg * max_elems);
    veln.resize(run_params.nsg * max_elems);
    cp.resize(run_params.nsg * max_elems);

    size_t k = 0;        // counter
    for (size_t i = 0; i < run_params.nsg; ++i) {
        for (size_t j = 0; j < run_params.ne[i]; ++j) {
            phi(i, j) = sol(k++);
        }
    }

    //------------------------------------------------
    // Compute the tangential disturbance velocity
    //             by taking tangential derivative of \phi
    //
    // Compute the pressure coefficient cp
    //             drag force
    //------------------------------------------------

    FLOATING_TYPE forcex = ZERO<FLOATING_TYPE>, dphids0;
    k = 0;        // counter
    for (size_t i = 0; i < run_params.nsg; ++i) {
        for (size_t j = 0; j < run_params.ne[i]; ++j) {
            //--------------------------------
            // tangential disturbance velocity
            // computed by differentiating phi
            //--------------------------------
            if (j == 0) {                          // forward differencing
                dphids0 = (phi(i,j+1)-phi(i,j))/(run_params.s0(k+1)-run_params.s0(k));
            } else if(j == run_params.ne(i) - 1) { // backward differencing
                dphids0 = (phi(i,j)-phi(i,j-1))/(run_params.s0(k)-run_params.s0(k-1));
            } else {                               // second-order differencing
                FLOATING_TYPE g1 = phi(i,j-1);
                FLOATING_TYPE g2 = phi(i,j);
                FLOATING_TYPE g3 = phi(i,j+1);
                FLOATING_TYPE h1 = run_params.s0(k-1);
                FLOATING_TYPE h2 = run_params.s0(k);
                FLOATING_TYPE h3 = run_params.s0(k+1);
                FLOATING_TYPE aa = ((g3-g2)/(h3-h2)-(g1-g2)/(h1-h2))/(h3-h1);
                FLOATING_TYPE bb = (g3-g2)/(h3-h2)-aa*(h3-h2);
                dphids0 = bb;
            }
            //---------------
            // total velocity
            //---------------

            FLOATING_TYPE velx = run_params.dphidn0(k)*run_params.vnx0(k)+dphids0*run_params.tnx0(k);
            FLOATING_TYPE vely = run_params.dphidn0(k)*run_params.vny0(k)+dphids0*run_params.tny0(k);

            velx = velx + run_params.vx;        // add incident flow

            //---
            // add line vortex ring
            //---

            int iopt = 1;
            FLOATING_TYPE ulvr, vlvr, psi;
            lvr_fs<FLOATING_TYPE>(iopt, run_params.x0[k], run_params.y0[k], run_params.xlvr, run_params.ylvr, ulvr, vlvr, psi);

            velx = velx + run_params.cr*ulvr;
            vely = vely + run_params.cr*vlvr;

            //-------------------------------
            // tangential and normal velocity
            // and pressure coefficient
            //-------------------------------

            // ...
            velt(k) = velx*run_params.tnx0(k)+vely*run_params.tny0(k);
            veln(k) = velx*run_params.vnx0(k)+vely*run_params.vny0(k);

            //------------
            // axial force
            //------------

            cp(k) = ONE<FLOATING_TYPE> - (velt(k)*velt(k)) / (run_params.vx*run_params.vx);

            forcex = forcex + cp(k)*run_params.vnx0(k)*run_params.arel(k);
            if (popt.verbose) {
                std::cout << "velt(" << k << "): " << velt(k) << " veln(" << k << "): " << veln(k)
                          << " cp(" << k << "): " << cp(k) << std::endl;
            }

            ++k;
        }
    }

    std::cout << "Axial Force: " << forcex << std::endl;

    //-------------------
    // print the solution
    //-------------------
    std::ofstream sol_out("body_ax.out");
    if (sol_out) {
        k = 0;
        for (size_t i = 0; i < run_params.nsg; ++i) {
            sol_out << run_params.ne[i] << std::endl;
            for (size_t j = 0; j < run_params.ne[i]; ++j) {
                sol_out << j << " "
                        << run_params.x0(k) << " "
                        << run_params.y0(k) << " "
                        << run_params.s0(k) << " "
                        << phi(i, j) << " " << velt(k) << " " << veln(k) << " " << cp(k) << std::endl;

                ++k;
            }
        }
    }
    sol_out << 0 << std::endl;
    sol_out.close();

    bool detailed_iso = true; //false;
    FLOATING_TYPE dl, ux1, uy1, ux2, uy2;
    int irk;

    if (detailed_iso) {
        dl    = static_cast<FLOATING_TYPE>(0.05) * static_cast<FLOATING_TYPE>(0.125);
        irk   = 4;   // Runge-Kutta 4th order
    } else {
        dl    = static_cast<FLOATING_TYPE>(0.05);
        irk   = 2;
    }

    //--------------------------
    // prepare for crossing test
    //--------------------------
    bool icross;
    if (popt.flow_type == to_underlying(FlowType::SPHERE)) {
        icross = true;
    } else if (popt.flow_type == to_underlying(FlowType::THORUS)) {
        icross = false;
    }

    //--------------------------
    // begin drawing streamlines
    //--------------------------
    std::ofstream fstr;
    fstr.open("body_ax.asy");
    if (fstr) {
        fstr <<
                "import graph;\n"
                "import patterns;\n"
                "size(350,350,IgnoreAspect);\n"
                "add(\"crosshatch\",crosshatch(3mm));\n"
                "\n"
                "pair reflect_y(pair pt)\n"
                "{\n"
                "  return (pt.x, -pt.y);\n"
                "}\n"
                "\n"
                "pair[] reflect_y(pair[] pts)\n"
                "{\n"
                "  pair[] ret;\n"
                "  for (int i = 0; i < pts.length; ++i) {\n"
                "    ret.push((pts[i].x, -pts[i].y));\n"
                "  }\n"
                "  return ret;\n"
                "}\n"
                "\n"
                "pair[][] toru = {\n";

        for (size_t i = 0; i < run_params.nsg; ++i) {
            fstr << "  {\n";
            for (size_t j = 0; j < run_params.ne[i]; ++j) {
                fstr << "(" << run_params.xw(i,j) << ","  << run_params.yw(i,j) << "),\n";
            }
            fstr << "  },\n";
        }

        fstr << "};\n"
                "pair[][] stream_lines = {\n";
    }

    size_t n_streamlines = run_params.x00.size();
    constexpr size_t mstr = 1200;
    std::vector<FLOATING_TYPE> xstr, ystr; // for streamlines
    xstr.reserve(mstr);
    ystr.reserve(mstr);
    for (size_t i = 0; i < n_streamlines; ++i) {
        FLOATING_TYPE x00s = run_params.x00[i], y00s = run_params.y00[i];
        FLOATING_TYPE xcross = x00s;   // to be used for crossing check

        //------
        xstr.clear();
        ystr.clear();
        for (size_t k = 0; k < mstr; ++k) {

            xstr.push_back(x00s);
            ystr.push_back(y00s);

            //---------------
            // integrate ODEs
            //---------------

            velocity<FLOATING_TYPE>(run_params, phi, run_params.dphidn0, x00s,y00s, ux1,uy1);

            FLOATING_TYPE step = dl / std::sqrt(ux1*ux1 + uy1*uy1);     // set the frozen-time step

            FLOATING_TYPE xsv = x00s;  // save
            FLOATING_TYPE ysv = y00s;  // save

            //----------------------
            if (irk == 2) {
            //----------------------

                FLOATING_TYPE steph = HALF<FLOATING_TYPE> * step;

                x00s = xsv + step * ux1;
                y00s = ysv + step * uy1;

                velocity<FLOATING_TYPE>(run_params, phi, run_params.dphidn0, x00s,y00s, ux2,uy2);

                x00s = xsv + steph * (ux1 + ux2);
                y00s = ysv + steph * (uy1 + uy2);

            //---------------------------
            } else if (irk == 4) {
            //---------------------------

                FLOATING_TYPE steph = HALF<FLOATING_TYPE> * step, ux3, uy3, ux4, uy4;
                FLOATING_TYPE step6 = step / FLOATING_TYPE(6.0);

                x00s = xsv + steph * ux1;
                y00s = ysv + steph * uy1;

                velocity<FLOATING_TYPE>(run_params, phi, run_params.dphidn0, x00s,y00s, ux2,uy2);

                x00s = xsv + steph * ux2;
                y00s = ysv + steph * uy2;

                velocity<FLOATING_TYPE>(run_params, phi, run_params.dphidn0, x00s,y00s, ux3,uy3);

                x00s = xsv + step * ux3;
                y00s = ysv + step * uy3;

                velocity<FLOATING_TYPE>(run_params, phi, run_params.dphidn0, x00s,y00s, ux4,uy4);

                x00s = xsv + step/FLOATING_TYPE(6.0) * (ux1+FLOATING_TYPE(2.0)*ux2+FLOATING_TYPE(2.0)*ux3+ux4);
                y00s = ysv + step/FLOATING_TYPE(6.0) * (uy1+FLOATING_TYPE(2.0)*uy2+FLOATING_TYPE(2.0)*uy3+uy4);

            //-----------
            }
            //-----------

            //---------------------------
            // test for x=0 plane crossing
            //---------------------------

            if (icross) {
                auto test = xcross*x00s;
                if (test < ZERO<FLOATING_TYPE>) {
                    std::cout << " Crossed the x=0 plane: I will stop\n";
                    break; // to the next streamline
                }
            }

            //-------------------------
            // test for sphere crossing
            //-------------------------
            if (icross) {
                FLOATING_TYPE crosss = std::hypot(x00s-run_params.xcntr[0], y00s-run_params.ycntr[0]);
                if (crosss < run_params.actis[0]) {
                    xstr.pop_back();
                    ystr.pop_back();
                    goto finish_streamline;
                }
            }

            //-----------------------
            // window crossing checks
            //-----------------------

            if (x00s < run_params.xwmin) break;
            if (x00s > run_params.xwmax) break;
            if (y00s < run_params.ywmin) break;
            if (y00s > run_params.ywmax) break;

        } // k loop

        xstr.push_back(x00s);
        ystr.push_back(y00s);

finish_streamline:;
        std::cout << " One streamline with " << xstr.size() << " points completed\n";
        if (fstr) {
            fstr << "{\n";
            for (size_t i = 0; i < xstr.size(); ++i) {
                fstr << '(' << xstr[i] << ',' << ystr[i] << "),\n";
            }
            fstr << "},\n";
        }
    }

    if (fstr) {
        fstr <<
            "};\n"
            "\n"
            "filldraw((toru[0][0]--toru[1][0]--toru[2][0]--cycle),magenta+0.7);\n"
            "filldraw((reflect_y(toru[0][0])--reflect_y(toru[1][0])--reflect_y(toru[2][0])--cycle),magenta+0.7);\n"
            "\n"
            "for (int i = 0; i < stream_lines.length; ++i) {\n"
            "  draw(graph(stream_lines[i]),            heavygreen+0.2);\n"
            "  draw(graph(reflect_y(stream_lines[i])), heavygreen+0.2);\n"
            "}\n"
            "\n"
            "real ticks[] = {-2,-1.5,-1,-0.5,0,0.5,1,1.5,2};\n"
            "\n"
            "xaxis(\"X\", BottomTop, LeftTicks(Label(fontsize(6pt)), ticks));\n"
            "yaxis(rotate(90)*\"Y\", LeftRight, RightTicks(Label(fontsize(6pt)), ticks));\n";
    }

}


#define BIORBD_API_EXPORTS
#include "Utils/Quaternion.h"

#include <Eigen/Dense>
#include <rbdl/Quaternion.h>

biorbd::utils::Quaternion::Quaternion () :
    RigidBodyDynamics::Math::Quaternion(),
    m_Kstab(100)
{

}

biorbd::utils::Quaternion::Quaternion (const Eigen::Vector4d &vec4) :
    RigidBodyDynamics::Math::Quaternion(vec4),
    m_Kstab(100)
{

}

biorbd::utils::Quaternion::Quaternion (
        double x,
        double y,
        double z,
        double w) :
    RigidBodyDynamics::Math::Quaternion(x, y, z, w),
    m_Kstab(100)
{

}

biorbd::utils::Quaternion::Quaternion (
        const Eigen::Vector3d &vec4,
        double w) :
    RigidBodyDynamics::Math::Quaternion(vec4(0), vec4(1), vec4(2), w),
    m_Kstab(100)
{

}

biorbd::utils::Quaternion& biorbd::utils::Quaternion::operator=(
        const Eigen::Vector4d& vec4)
{
    if (this==&vec4) // check for self-assigment
        return *this;

    *this = vec4;
    return *this;
}

double biorbd::utils::Quaternion::w() const
{
    return (*this)(3);
}
double biorbd::utils::Quaternion::x() const
{
    return (*this)(0);
}
double biorbd::utils::Quaternion::y() const
{
    return (*this)(1);
}
double biorbd::utils::Quaternion::z() const
{
    return (*this)(2);
}

void biorbd::utils::Quaternion::derivate(
        const Eigen::VectorXd &w)
{

    // Création du quaternion de "préproduit vectoriel"
    double qw = (*this)(3);
    double qx = (*this)(0);
    double qy = (*this)(1);
    double qz = (*this)(2);
    Eigen::Matrix4d Q;
    Q <<    qw, -qx, -qy, -qz,
            qx,  qw, -qz,  qy,
            qy,  qz,  qw, -qx,
            qz, -qy,  qx,  qw;

    // Ajout du paramètre de stabilisation
    Eigen::Vector4d w_tp (m_Kstab*w.norm()*(1-this->norm()), w(0), w(1), w(2));
    biorbd::utils::Quaternion quatDot(0.5 * Q * w_tp);
    biorbd::utils::Quaternion quatDot_tp(Eigen::Vector4d(quatDot(1), quatDot(2), quatDot(3), quatDot(0)));
    *this =  quatDot_tp; // Le quaternion est tourné à cause de la facon dont est faite la multiplication
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef ITL_ITL_FWD_INCLUDE
#define ITL_ITL_FWD_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>

namespace itl {

    template <class Real>                class basic_iteration;
    template <class Real, class OStream> class cyclic_iteration;
    template <class Real, class OStream> class noisy_iteration;

    template <typename Solver, typename VectorIn, bool trans> class solver_proxy;

    namespace pc {

	template <typename PC, typename Vector, bool> struct solver;
	template <typename Matrix, typename Value> class identity;
	// template <typename Matrix, typename Value, typename Vector> Vector solve(const identity<Matrix>&, const Vector& x);
	// template <typename Matrix, typename Vector> Vector adjoint_solve(const identity<Matrix>&, const Vector& x);

	template <typename Matrix, typename Value> class diagonal;
	// template <typename Matrix, typename Vector> Vector solve(const diagonal<Matrix>& P, const Vector& x);
	// template <typename Matrix, typename Vector> Vector adjoint_solve(const diagonal<Matrix>& P, const Vector& x);

	template <typename Matrix, typename Factorizer, typename Value> class ilu;
	template <typename Matrix, typename Value> class ilu_0; // Maybe we should declare the default here???
	template <typename Matrix, typename Value> class ilut; // Maybe we should declare the default here???
	// template <typename Matrix, typename Vector> Vector solve(const ilu_0<Matrix>& P, const Vector& x);
	// template <typename Matrix, typename Vector> Vector adjoint_solve(const ilu_0<Matrix>& P, const Vector& x);

	template <typename Matrix, typename Value> class ic_0; // Maybe we should declare the default here???
	// template <typename Matrix, typename Vector> Vector solve(const ic_0<Matrix>& P, const Vector& x);
	// template <typename Matrix, typename Value, typename Vector> Vector adjoint_solve(const ic_0<Matrix, Value>& P, const Vector& x);

    } //  namespace pc

    template < typename LinearOperator, typename HilbertSpaceX, typename HilbertSpaceB,
	       typename Preconditioner, typename Iteration >
    int cg(const LinearOperator& A, HilbertSpaceX& x, const HilbertSpaceB& b,
	   const Preconditioner& M, Iteration& iter);

    template < typename LinearOperator, typename Preconditioner= pc::identity<LinearOperator, double>,
	       typename RightPreconditioner= pc::identity<LinearOperator, double> >
    class cg_solver;

    template < typename LinearOperator, typename Vector,
	       typename Preconditioner, typename Iteration >
    int bicg(const LinearOperator &A, Vector &x, const Vector &b,
	     const Preconditioner &M, Iteration& iter);

    template < class LinearOperator, class HilbertSpaceX, class HilbertSpaceB,
	       class Preconditioner, class Iteration >
    int bicgstab(const LinearOperator& A, HilbertSpaceX& x, const HilbertSpaceB& b,
		 const Preconditioner& M, Iteration& iter);

    template < class LinearOperator, class HilbertSpaceX, class HilbertSpaceB,
	       class Preconditioner, class Iteration >
    int bicgstab_2(const LinearOperator& A, HilbertSpaceX& x, const HilbertSpaceB& b,
		   const Preconditioner& M, Iteration& iter);

    template < typename LinearOperator, typename Vector,
	       typename LeftPreconditioner, typename RightPreconditioner,
	       typename Iteration >
    int bicgstab_ell(const LinearOperator &A, Vector &x, const Vector &b,
		     const LeftPreconditioner &L, const RightPreconditioner &R,
		     Iteration& iter, size_t l);

    template < typename LinearOperator, typename Preconditioner= pc::identity<LinearOperator, double>,
	       typename RightPreconditioner= pc::identity<LinearOperator, double> >
    class bicgstab_ell_solver;

    template <typename Solver, unsigned N, bool Stored= false>
    class repeating_solver;

} // namespace itl

#endif // ITL_ITL_FWD_INCLUDE


#include <iostream>
#include <fstream>
#include <queue>
#include <vector>
#include <utility>
#include <stdlib.h>
#include <unordered_set>
#include <boost/graph/adjacency_list.hpp>
// #include <boost/graph/vf2_sub_graph_iso.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/subgraph.hpp>
#include <boost/graph/graph_utility.hpp>
#include "metis.h"

template <class Edge>
struct myhash
{
    std::size_t operator()(Edge const& e) const
    {
        return _h(e.idx);
    }
};

using namespace std;
using namespace boost;

// global declarations
const int K = 2;                                            // TODO: Change this to main param
idx_t ncon = 1;                                             // default METIS tuning param
idx_t nparts = K;                                           // set num partition to K

int main(int argc, char* argv[])
{
    // error check
    if (argc != 3)
    {
        cout << "usage: main.exe inputFile outputFile" << endl;
        cout << "inputFile must be in graphviz dot file format." << endl;
        return 1;
    }

    //********************************** Declarations **********************************

    // typedef declarations for templates
    typedef subgraph< adjacency_list<vecS, vecS, undirectedS, uint32_t,
        property< edge_index_t, int > > > graph_type;
    typedef graph_traits<graph_type>::vertex_iterator vertex_iter;
    typedef graph_traits<graph_type>::edge_iterator edge_iter;
    typedef graph_traits<graph_type>::out_edge_iterator out_edge_iter;
    typedef std::pair<vertex_iter, vertex_iter> vrange_t;
    typedef graph_traits<graph_type>::adjacency_iterator adj_iter;
    typedef std::pair<adj_iter, adj_iter> adjrange_t;
    typedef property_map<graph_type, vertex_index_t>::type IndexMap;
    typedef graph_traits<graph_type>::vertex_descriptor v_descriptor;
    typedef graph_traits<graph_type>::edge_descriptor e_descriptor;
    typedef std::vector<v_descriptor> vert_vec;
    typedef std::pair< v_descriptor, int > hop_pair_t;
    typedef std::vector<hop_pair_t> avt_vector_t;
    typedef std::queue<hop_pair_t> vert_que;
    typedef std::vector<bool> colormap;
    typedef std::vector<int> score_vec;
    typedef std::pair<v_descriptor, bool> found_t;
    //typedef std::unordered_set<e_descriptor, typename myhash> u_set_t;
    typedef std::set<e_descriptor> u_set_t;

    //*** variable declarations ***

    // The root graph
    graph_type graph1(0);

    // subgraphs from the root
    graph_type *subgraph_vect = new graph_type[K];

    // METIS library args
    idx_t nvert;
    int nedge;
    idx_t *xadj;
    idx_t *adjncy;
    idx_t objval;
    idx_t *part;
    idx_t options[METIS_NOPTIONS];

    // The graphviz dot file
    ifstream inputFile;

    // Output graphviz dot file
    ofstream outputFile;
    // ostream& os = cout;

    // iterators
    vrange_t vpair;
    int i = 0;
    int j = 0;
    graph_type::children_iterator ci;
    graph_type::children_iterator ci_end;
    vertex_iter v;
    vertex_iter v_end;
    //    vertex_iter v_next_col;
    edge_iter e;
    edge_iter e_end;
    out_edge_iter child_e;
    out_edge_iter child_e_end;
    out_edge_iter parent_e;
    out_edge_iter parent_e_end;
    adj_iter vi;
    adj_iter vi_end;

    // index for random access
    IndexMap index;

    // Alignment Vertex table
    vert_vec *avt = new vert_vec[K];
    int *avt_lookup;
    avt_vector_t *avt_unmatched = new avt_vector_t[K];      // to store intermediate results
    v_descriptor *avtrow = new v_descriptor[K];             // the initial row
    int avtRows;                                            // number of rows in table
    int hopcount;
    int degrees;
    int score;
    int bestscore;
    int pos_right;
    int best_position;
    int maxlength;
    score_vec *score_table = new score_vec[K];
    v_descriptor bestV;
    hop_pair_t hopPair;

    // colormap & que used in BFS
    colormap *clr_arr = new colormap[K];
    vert_que *vque_arr = new vert_que[K];

    // edge copy
    u_set_t edge_set;
    u_set_t added_edge_set;
    found_t found;
    int from;
    int to;

    // descriptors
    v_descriptor vLocalID;
    v_descriptor vGlobalID;
    v_descriptor orig_source;
    v_descriptor orig_target;
    v_descriptor copy_source;
    v_descriptor copy_target;
    e_descriptor eLocalID;
    e_descriptor eGlobalID;

    //********************************** Reading and Building Graph **********************************
    // Read in the graph from disk
    inputFile.open(argv[1]);
    dynamic_properties dp(ignore_other_properties);
    read_graphviz(inputFile, graph1, dp);
    inputFile.close();

    //Initialize Graph-Dependent Variables
    nvert = num_vertices(graph1);
    nedge = num_edges(graph1);
    xadj = new idx_t[nvert + 1];
    adjncy = new idx_t[nedge * 2];
    part = new idx_t[nvert];
    index = get(vertex_index, graph1);

    //********************************** Partition the graph with METIS algorithm **********************************
    // initialise
    METIS_SetDefaultOptions(options);
    xadj[0] = 0;

    // Transform the adjacency list into the Compressed Storage Format
    // TODO: consider using tie function - syntactic sugar
    for(vpair = vertices(graph1); vpair.first != vpair.second; ++vpair.first)
    {
        xadj[i+1] = xadj[i] + out_degree(*vpair.first, graph1);
        adjrange_t adjpair = adjacent_vertices(*vpair.first, graph1);
        j = xadj[i];
        for(adjpair.first; adjpair.first != adjpair.second; ++adjpair.first)
        {
            adjncy[j] = *adjpair.first;
            ++j;
        }
        ++i;
    }

    // Print out to verify
    cout << "METIS partitioning: " << endl;
    for(i = 0; i < nvert + 1; ++i)
    {
        cout << xadj[i] << ' ';
    }
    cout << endl;
    for(i = 0; i < nedge * 2; ++i)
    {
        cout << adjncy[i] << ' ';
    }
    cout << endl;

    // Use k-way graph partition algorithm
    objval = 0;
    METIS_PartGraphKway(&nvert, &ncon, xadj, adjncy, NULL, NULL, NULL, &nparts, NULL, NULL, NULL, &objval, part);

    // Print out the partition vector
    for(i=0; i < nvert; ++i)
    {
        cout << part[i] << ' ';
    }
    cout << endl;

	//********************************** Create Subgraphs based on METIS result **********************************
    // Create induced subgraphs based on partition vector
    for (i=0; i<K; ++i)
    {
        subgraph_vect[i] = graph1.create_subgraph();
    }
    for (i=0; i < nvert; ++i)
    {
        cout << "Mapping " << index[i] << " to subgraph " << part[i] << endl;
        add_vertex(index[i], subgraph_vect[part[i]]);
    }

	// print for testing

    //This will print out the global vertex IDs from the root graph
    cout << "root:" << endl;
    print_graph(graph1, get(vertex_index, graph1));
    cout << endl;
    for (i=0; i < K; ++i)
    {
        cout << "subgraph " << i << ":" << endl;
        cout << "vertices = ";
        for (boost::tie(v, v_end) = vertices(subgraph_vect[i]); v != v_end; ++v)
        {
            cout << subgraph_vect[i].local_to_global(*v) << ", ";
        }
        cout << endl;
        cout << "edges = ";
        for (boost::tie(e, e_end) = edges(subgraph_vect[i]); e != e_end; ++e)
        {
            cout << subgraph_vect[i].local_to_global(*e) << ", ";
        }
        cout << endl;
    }

    //********************************** Building the AVT **********************************
    // Start to build the AVT
    // Each vector will represent a column in the table
    // Each column in the table represents a subgraph

    // Build colormap
    // First step is to create the initial row
    for (i=0; i < K; ++i)
    {
        int maxdegree = 0;
        v_descriptor vertID = 0;
        // cout << "cp4" << endl;

        for (boost::tie(v, v_end) = vertices(subgraph_vect[i]); v != v_end; ++v)
        {
            int temp = out_degree(*v, subgraph_vect[i]);
            if (temp > maxdegree)
            {
                maxdegree = temp;
                vertID = *v;
                avtrow[i] = vertID;
            }
            // cout << "maxdegree = " << maxdegree << endl;
            // while we are processing every vertex in each subgraph
            // initialise the colormap
            clr_arr[i].push_back(false) ;
        }
    }

    //Print out the color array
    cout << std::boolalpha;
    for(i=0; i<K; ++i)
    {
        for(j=0; j < clr_arr[i].size(); ++j)
        {
            cout << clr_arr[i][j] << ' ';
        }
        cout << endl;
    }

    // Print out the starting point of AVT
    cout << endl << "AVT:" <<endl;
    for (i=0; i<K; ++i)
    {
        cout << subgraph_vect[i].local_to_global(avtrow[i]) << ' ';
    }
    cout << endl;

    // Load in the first row
    for(i=0; i<K; ++i)
    {
        avt_unmatched[i].push_back(std::make_pair(avtrow[i], 0));
    }

    // build ques for BFS
    for(i=0; i<K; ++i)
    {
        vque_arr[i].push(std::make_pair(avtrow[i], 0));
    }

    // Process using BFS
    // Mark all starting nodes as visited
    for (i=0; i < K; ++i)
    {
        index = get(vertex_index, subgraph_vect[i]);

        clr_arr[i][index[avtrow[i]]] = true;
    }

    // Print  out the color array
    cout << std::boolalpha;
    for(i=0; i<K; ++i)
    {
        for(j=0; j < clr_arr[i].size(); ++j)
        {
            cout << clr_arr[i][j] << ' ';
        }
        cout << endl;
    }

    // Process the que
	//*** Breadth First Search ***
    for(i=0; i < K; ++i)
    {
        while (vque_arr[i].empty()==false)
        {
            //  v_descriptor vGlobalID = vque_arr[i].front();
            //  v_descriptor vLocalID = subgraph_vect[i].global_to_local(vGlobalID);
            hopPair = vque_arr[i].front();

            vLocalID = hopPair.first;
            hopcount = hopPair.second;
            //  cout << "Processing BFS node: " << vLocalID << endl;
            cout << "Processing BFS node: " << index[vLocalID] << endl;
            index = get(vertex_index, subgraph_vect[i]);
            vque_arr[i].pop();
            for(boost::tie(vi, vi_end) = adjacent_vertices(vLocalID, subgraph_vect[i]); vi != vi_end; ++vi)
            {
                //  cout << subgraph_vect[i][*vi] << endl;
                //  cout << graph1[*vi] << endl;
                if(clr_arr[i][index[*vi]] == false)
                {
                    cout << "Processing adjacent node: " << index[*vi] << endl;
                    vque_arr[i].push(std::make_pair(*vi, hopcount+1));
                    avt_unmatched[i].push_back(std::make_pair(*vi, hopcount+1));
                    clr_arr[i][index[*vi]] = true;
                    // Print out the color map as updated
                    for(int z=0; z<K; ++z)
                    {
                        for(j=0; j < clr_arr[z].size(); ++j)
                        {
                            cout << clr_arr[z][j] << ' ';
                        }
                        cout << endl;
                    }
                }
            }
        }
    }

    // Print out the AVT
    cout << endl << "AVT (unmatched - UNbalanced) <read this sideways>:" << endl;
    for(i=0; i < K; ++i)
    {
        avtRows = avt_unmatched[i].size();
        for(j=0; j < avtRows; ++j)
        {
            cout << subgraph_vect[i].local_to_global(avt_unmatched[i][j].first) << '(' << avt_unmatched[i][j].second << ") ";
        }
        cout << endl;
    }
    cout << endl;

    // balance out the table if odd number of vertices
    // find the max length column
    maxlength = 0;
    for (i=0; i<K; ++i)
    {
        if (avt_unmatched[i].size() > maxlength)
        {
            maxlength = avt_unmatched[i].size();
        }
    }

    //Add vertices until all columns are maxlength
    for (i=0; i<K; ++i)
    {
		while (avt_unmatched[i].size() < maxlength)
    	{
	    	vLocalID = add_vertex(subgraph_vect[i]);
			avt_unmatched[i].push_back(std::make_pair(vLocalID, 99));
			clr_arr[i].push_back(false);
    	}
    }

    // Print out the AVT
    cout << endl << "AVT (unmatched - balanced) <read this sideways>:" << endl;
    for(i=0; i < K; ++i)
    {
        avtRows = avt_unmatched[i].size();
        for(j=0; j < avtRows; ++j)
        {
            cout << subgraph_vect[i].local_to_global(avt_unmatched[i][j].first) << '(' << avt_unmatched[i][j].second << ") ";
        }
        cout << endl;
    }
    cout << endl;

    //********************************** using local scores **********************************

    // use colormap for tracking matches
    // initialise
    for(i=0; i<K; ++i)
    {
        for(j=0; j < clr_arr[i].size(); ++j)
        {
            if(j==0)
            {
                clr_arr[i][j] = true;
            }
            else
            {
                clr_arr[i][j] = false;
            }
        }
    }

    // Print  out the color array
    for(i=0; i<K; ++i)
    {
        for(j=0; j < clr_arr[i].size(); ++j)
        {
            cout << clr_arr[i][j] << ' ';
        }
        cout << endl;
    }

    // create the avt_lookup table
    // We used a 2d array flattened to 1d
    // The values in this table are the index positions in the avt
    // Every time we push into avt we update avt_lookup
    avt_lookup = new int[K * avt_unmatched[0].size()];

    // copy the first column as is

    //use index to index into avt_lookup
    index = get(vertex_index, subgraph_vect[0]);
    for (i=0; i < avt_unmatched[0].size(); ++i)
    {
        avt[0].push_back(avt_unmatched[0][i].first);
        avt_lookup[index[avt_unmatched[0][i].first]] = i;
    }
    // match from left to right by score
    // score of 0 is considered a perfect match
    // as in same degree and same distance from hub

    // for each column, fill in next column row by row
    for (i=0; i < K-1; ++i)
    {
        // copy the first row as is
        avt[i+1].push_back(avt_unmatched[i+1][0].first);
        // get new index for each subgraph
        index = get(vertex_index, subgraph_vect[i]);
        avt_lookup[(i+1) * avt_unmatched[0].size() + (index[avt_unmatched[i+1][0].first])] = i;

        // for each item in left column, match against items in right column
        // skip over first iteration so j=1
        for(j=1; j < avt[0].size(); ++j)
        {
            cout << "avt size=" << avt[i].size() << endl;
            bestscore = 10000;  // initialise to silly high number; track best score so far
            pos_right = 0;      // current position of item on right
            best_position = 0;  // save position of best match
            // for each item in right column see if that item is a good match
            for(auto v_next_col = avt_unmatched[i+1].begin(); v_next_col != avt_unmatched[i+1].end(); ++v_next_col)
            {
                if(clr_arr[i+1][pos_right] == false)
                {
                    int dleft = out_degree(avt_unmatched[i][j].first, subgraph_vect[i]);
                    int dright = out_degree(v_next_col->first, subgraph_vect[i+1]);
                    cout << "comparing " << subgraph_vect[i].local_to_global(avt_unmatched[i][j].first) << " to " << subgraph_vect[i+1].local_to_global(v_next_col->first) << endl;
                    cout << "degree left: " << dleft
                        << " degree right: " << dright
                        << endl;
                    degrees = std::abs(dleft - dright);
                    hopcount = std::abs(avt_unmatched[i][j].second - v_next_col->second);
                    score = degrees + hopcount;  // can change to weighted score
                    cout << "degree diff: " << degrees << " hopcount diff: " << hopcount << " score: " << score << endl;
                    if(score < bestscore)
                    {
                        bestscore = score;
                        vLocalID = v_next_col->first;
                        best_position = pos_right;
                    }
                }
                ++pos_right;
            }

            // update the colormap, push in to avt, update lookup table
            clr_arr[i+1][best_position] = true;
            avt[i+1].push_back(vLocalID);
            avt_lookup[(i + 1) * avt_unmatched[i+1].size() + (index[avt_unmatched[i+1][best_position].first])] = j;
        }

    }

    // Print out the lookup table
    for(i = 0; i < avt[0].size() * K; ++i)
    {
       if(i % avt[0].size() == 0)
       {
           cout << endl;
       }
       cout << avt_lookup[i] << ' ';
    }
    cout << endl;

    // Print  out the color array
    for(i=0; i<K; ++i)
    {
        for(j=0; j < clr_arr[i].size(); ++j)
        {
            cout << clr_arr[i][j] << ' ';
        }
        cout << endl;
    }

    // Print out the AVT
    cout << endl << "AVT <read this sideways>:" << endl;
    for(i=0; i < K; ++i)
    {
        avtRows = avt[i].size();
        for(j=0; j < avtRows; ++j)
        {
            cout << subgraph_vect[i].local_to_global(avt[i][j]) << ' ';
        }
        cout << endl;
    }
    cout << endl;

	//********************************** Perform Block Alignment **********************************

	//*** First add all edges to the first column ***
	//*** first column will become the rolemodel ***
	adj_iter one;
	adj_iter one_end;
	adj_iter two;
	adj_iter two_end;
	bool matchFound;
	//*** Make the first column the rolemodel with all edges copied ***
	for(i = 0; i < avt[0].size(); ++i)
	{
		for(j = 1; j < K; ++j)
		{
			index = get(vertex_index ,subgraph_vect[j]);
			for(boost::tie(two, two_end) = adjacent_vertices(avt[j][i], subgraph_vect[j]); two != two_end; ++two)
			{
				//find pair to vertex in column one
				v_descriptor pair_vertex = index[avt[0][avt_lookup[j * avt[0].size() + (*two)]]];
				matchFound = false;
				for(boost::tie(one, one_end) = adjacent_vertices(avt[0][i], subgraph_vect[0]); one != one_end; ++one)
				{
					if(pair_vertex == *one)
					{
						matchFound = true;
					}
				}
				if(matchFound == false)
					add_edge(avt[0][i], pair_vertex, subgraph_vect[0]);
			}
		}
	}
	//*** compare all other columns to the rolemodel column and add edges ***
	for(i = 0; i < avt[0].size(); ++i)
	{
		for(j = 1; j < K; ++j)
		{
			index = get(vertex_index ,subgraph_vect[0]);
			for(boost::tie(one, one_end) = adjacent_vertices(avt[0][i], subgraph_vect[0]); one != one_end; ++one)
			{
				v_descriptor pair_vertex = index[avt[j][avt_lookup[*one]]];
				matchFound = false;
				for(boost::tie(two, two_end) = adjacent_vertices(avt[j][i], subgraph_vect[j]); two != two_end; ++two)
				{
					if(pair_vertex == *two)
					{
						matchFound = true;
					}
				}
				if(matchFound == false)
					add_edge(avt[j][i], pair_vertex, subgraph_vect[j]);
			}
		}
	}

	// print results of block alignment
    cout << "root:" << endl;
    print_graph(graph1, get(vertex_index, graph1));
    cout << endl;
    for (i=0; i < K; ++i)
    {
        cout << "subgraph " << i << ":" << endl;
        cout << "vertices = ";
        for (boost::tie(v, v_end) = vertices(subgraph_vect[i]); v != v_end; ++v)
        {
            cout << subgraph_vect[i].local_to_global(*v) << ", ";
        }
        cout << endl;
        cout << "edges = ";
        for (boost::tie(e, e_end) = edges(subgraph_vect[i]); e != e_end; ++e)
        {
            cout << subgraph_vect[i].local_to_global(*e) << ", ";
        }
        cout << endl;
    }

	//********************************** Perform Edge Copy **********************************
    // for each child subgraph
    for (i=0; i < K; ++i)
    {
        cout << "cp1" << endl;
        // for each vertex in subgraph, compare degree to parent vertex
        for(boost::tie(v, v_end) = vertices(subgraph_vect[i]); v != v_end; ++v)
        {
            // if degrees don't match then there is at least one crossing edge
            cout << "comparing subgraph " << i << ' ' << *v << " to " << subgraph_vect[i].local_to_global(*v) << endl;
            if (out_degree(*v, subgraph_vect[i]) != out_degree(subgraph_vect[i].local_to_global(*v), graph1))
            {
                cout << "Detected a crossing edge from vertex: " << subgraph_vect[i].local_to_global(*v) << endl;
                // for each child edge, load into hash table
                edge_set.clear();
                for(boost::tie(child_e, child_e_end) = out_edges(*v, subgraph_vect[i]); child_e != child_e_end; ++child_e)
                {
                    eGlobalID = subgraph_vect[i].local_to_global(*child_e);
                    edge_set.insert(eGlobalID);
                }
                // for each edge of parent vertex, check if it's in hash table
                for(boost::tie(parent_e, parent_e_end) = out_edges(subgraph_vect[i].local_to_global(*v), graph1); parent_e != parent_e_end; ++ parent_e)
                {
                    if(edge_set.find(*parent_e) == edge_set.end()
                        && added_edge_set.find(*parent_e) == added_edge_set.end())
                    {
                        // do edge copy
                        // get the source & target of the edge
                        orig_source = source(*parent_e, graph1);
                        orig_target = target(*parent_e, graph1);
                        cout << "edge source " << orig_source << " target " << orig_target << endl;
                        // find them in the avt
                        // use avt_lookup to find the index of the source
                        index = get(vertex_index, subgraph_vect[i]);
                        vLocalID = subgraph_vect[i].global_to_local(orig_source);
                        from = avt_lookup[i * avt[0].size() + index[vLocalID]];
                        // find the subgraph of the target & get it's local ID
                        for (j=0; j < K; ++j)
                        {
                            found = subgraph_vect[j].find_vertex(orig_target);
                            if(found.second)
                            {
                                // do lookup of the target
                                index = get(vertex_index, subgraph_vect[j]);
                                vLocalID = found.first;
                                to = avt_lookup[j * avt[0].size() + index[vLocalID]];
                                copy_source = subgraph_vect[j].local_to_global(avt[j][from]);
                                copy_target = subgraph_vect[i].local_to_global(avt[i][to]);
                                cout << "i=" << i << " j=" << j << " from=" << from << " to=" << to << endl;
                                if(edge(copy_source, copy_target, graph1).second == false)
                                {
                                    add_edge(copy_source, copy_target, graph1);
                                    added_edge_set.insert(eGlobalID);
                                }
                                break;
                            }
                        }

                    }
                }
            }
        }
    }

    // *******End***********

    cout << endl;
    // Dump the output to file
    outputFile.open(argv[2]);
    write_graphviz_dp(outputFile, graph1, dp.property("node_id", get(boost::vertex_index, graph1)));
    // uncomment this to print dot file to std out
    //write_graphviz_dp(cout, graph1, dp.property("node_id", get(boost::vertex_index, graph1)));
    outputFile.close();
    // also print out to std out
    cout << "root:" << endl;
    print_graph(graph1, get(vertex_index, graph1));
    cout << endl;
    for (i=0; i < K; ++i)
    {
        cout << "subgraph " << i << ":" << endl;
        cout << "vertices = ";
        for (boost::tie(v, v_end) = vertices(subgraph_vect[i]); v != v_end; ++v)
        {
            cout << subgraph_vect[i].local_to_global(*v) << ", ";
        }
        cout << endl;
        cout << "edges = ";
        for (boost::tie(e, e_end) = edges(subgraph_vect[i]); e != e_end; ++e)
        {
            cout << subgraph_vect[i].local_to_global(*e) << ", ";
        }
        cout << endl;
    }

    //cleanup
    // TODO: move these higher
    delete[] xadj;
    delete[] adjncy;
    delete[] part;
    delete[] subgraph_vect;
    delete[] avt;
    delete[] avt_lookup;
    delete[] avtrow;
    delete[] clr_arr;
    delete[] vque_arr;
    delete[] avt_unmatched;
    delete[] score_table;
    xadj = NULL;
    adjncy = NULL;
    part = NULL;
    subgraph_vect = NULL;
    avt = NULL;
    avtrow = NULL;
    clr_arr = NULL;
    vque_arr = NULL;
    avt_unmatched = NULL;
    score_table = NULL;
    avt_lookup = NULL;

    return 0;
}


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <ql/cashflows/iborcoupon.hpp>
#include <ql/indexes/iborindex.hpp>
#include <ql/math/solvers1d/brent.hpp>
#include <qle/pricingengines/analyticlgmswaptionengine.hpp>

#include <boost/bind.hpp>

namespace QuantExt {

AnalyticLgmSwaptionEngine::AnalyticLgmSwaptionEngine(const boost::shared_ptr<LinearGaussMarkovModel>& model,
                                                     const Handle<YieldTermStructure>& discountCurve,
                                                     const FloatSpreadMapping floatSpreadMapping)
    : GenericEngine<Swaption::arguments, Swaption::results>(), p_(model->parametrization()),
      c_(discountCurve.empty() ? p_->termStructure() : discountCurve), floatSpreadMapping_(floatSpreadMapping),
      caching_(false) {
    registerWith(model);
    registerWith(c_);
}

AnalyticLgmSwaptionEngine::AnalyticLgmSwaptionEngine(const boost::shared_ptr<CrossAssetModel>& model, const Size ccy,
                                                     const Handle<YieldTermStructure>& discountCurve,
                                                     const FloatSpreadMapping floatSpreadMapping)
    : GenericEngine<Swaption::arguments, Swaption::results>(), p_(model->irlgm1f(ccy)),
      c_(discountCurve.empty() ? p_->termStructure() : discountCurve), floatSpreadMapping_(floatSpreadMapping),
      caching_(false) {
    registerWith(model);
    registerWith(c_);
}

AnalyticLgmSwaptionEngine::AnalyticLgmSwaptionEngine(const boost::shared_ptr<IrLgm1fParametrization> irlgm1f,
                                                     const Handle<YieldTermStructure>& discountCurve,
                                                     const FloatSpreadMapping floatSpreadMapping)
    : GenericEngine<Swaption::arguments, Swaption::results>(), p_(irlgm1f),
      c_(discountCurve.empty() ? p_->termStructure() : discountCurve), floatSpreadMapping_(floatSpreadMapping),
      caching_(false) {
    registerWith(c_);
}

void AnalyticLgmSwaptionEngine::enableCache(const bool lgm_H_constant, const bool lgm_alpha_constant) {
    caching_ = true;
    lgm_H_constant_ = lgm_H_constant;
    lgm_alpha_constant_ = lgm_alpha_constant;
    clearCache();
}

void AnalyticLgmSwaptionEngine::clearCache() {
    S_.clear();             // indicates that H / alpha independent variables are not yet computed
    Hj_.clear();            // indicates that H dependent variables not not yet computed
    zetaex_ = Null<Real>(); // indicates that alpha dependent variables are not yet computed
}

void AnalyticLgmSwaptionEngine::calculate() const {

    QL_REQUIRE(arguments_.settlementType == Settlement::Physical, "cash-settled swaptions are not supported ...");

    Date reference = p_->termStructure()->referenceDate();

    Date expiry = arguments_.exercise->dates().back();

    if (expiry <= reference) {
        // swaption is expired, possibly generated swap is not
        // valued by this engine, so we set the npv to zero
        results_.value = 0.0;
        return;
    }

    if (!caching_ || S_.empty()) {

        Option::Type type = arguments_.type == VanillaSwap::Payer ? Option::Call : Option::Put;
        const Schedule& fixedSchedule = arguments_.swap->fixedSchedule();
        const Schedule& floatSchedule = arguments_.swap->floatingSchedule();

        j1_ = std::lower_bound(fixedSchedule.dates().begin(), fixedSchedule.dates().end(), expiry) -
              fixedSchedule.dates().begin();
        k1_ = std::lower_bound(floatSchedule.dates().begin(), floatSchedule.dates().end(), expiry) -
              floatSchedule.dates().begin();

        // compute S_i, i.e. equivalent fixed rate spreads compensating for
        // a) a possibly non-zero float spread and
        // b) a spread between the ibor indices forwarding curve and the
        //     discounting curve
        // here, we do not work with a spread corrections directly, but
        // with this multiplied by the nominal and accrual basis,
        // so S_i is really an amount correction.

        S_.resize(arguments_.fixedCoupons.size() - j1_);
        for (Size i = 0; i < S_.size(); ++i) {
            S_[i] = 0.0;
        }
        S_m1 = 0.0;
        Size ratio = static_cast<Size>(static_cast<Real>(arguments_.floatingCoupons.size()) /
                                           static_cast<Real>(arguments_.fixedCoupons.size()) +
                                       0.5);
        QL_REQUIRE(ratio >= 1, "floating leg's payment frequency must be equal or "
                               "higher than fixed leg's payment frequency in "
                               "analytic lgm swaption engine");

        Size k = k1_;
        // The method reduces the problem to a one curve configuration w.r.t. the discount curve and
        // apply a correction for the discount curve / forwarding curve spread. Furthermore the method
        // assumes that no historical fixings are present in the floating rate coupons.
        auto index = arguments_.swap->iborIndex();
        for (Size j = j1_; j < arguments_.fixedCoupons.size(); ++j) {
            Real sum1 = 0.0, sum2 = 0.0;
            for (Size rr = 0; rr < ratio && k < arguments_.floatingCoupons.size(); ++rr, ++k) {
                Real amount = arguments_.floatingCoupons[k];
                Real lambda1 = 0.0, lambda2 = 1.0;
                if (floatSpreadMapping_ == proRata) {
                    // we do not use the exact pay dates but the ratio to determine
                    // the distance to the adjacent payment dates
                    lambda2 = static_cast<Real>(rr + 1) / static_cast<Real>(ratio);
                    lambda1 = 1.0 - lambda2;
                }
                if (amount != Null<Real>()) {
                    Real flatAmount;
                    if (IborCoupon::usingAtParCoupons()) {
                        // if par coupons are used, we mimick the fixing estimation in IborCoupon; we make
                        // sure that the estimation period does not start in the past and we do not use
                        // historical fixings
                        Date fixingValueDate = index->fixingCalendar().advance(arguments_.floatingFixingDates[k],
                                                                               index->fixingDays(), Days);
                        fixingValueDate = std::max(fixingValueDate, reference);
                        auto cpn = boost::dynamic_pointer_cast<Coupon>(arguments_.swap->floatingLeg()[k]);
                        QL_REQUIRE(cpn, "AnalyticalLgmSwaptionEngine::calculate(): coupon expected on underlying swap "
                                        "floating leg, could not cast");
                        Date nextFixingDate = index->fixingCalendar().advance(
                            cpn->accrualEndDate(), -static_cast<Integer>(index->fixingDays()), Days);
                        Date fixingEndDate = index->fixingCalendar().advance(nextFixingDate, index->fixingDays(), Days);
                        fixingEndDate = std::max(fixingEndDate, fixingValueDate + 1);
                        Real spanningTime = index->dayCounter().yearFraction(fixingValueDate, fixingEndDate);
                        DiscountFactor disc1 = c_->discount(fixingValueDate);
                        DiscountFactor disc2 = c_->discount(fixingEndDate);
                        Real fixing = (disc1 / disc2 - 1.0) / spanningTime;
                        flatAmount = fixing * arguments_.floatingAccrualTimes[k] * arguments_.nominal;
                    } else {
                        // if indexed coupons are used, we use a proper fixing, but make sure that the fixing date
                        // is not in the past and we do not use a historical fixing for "today"
                        auto flatIbor = boost::make_shared<IborIndex>(
                            index->familyName() + " (no fixings)", index->tenor(), index->fixingDays(),
                            index->currency(), index->fixingCalendar(), index->businessDayConvention(),
                            index->endOfMonth(), index->dayCounter(), c_);
                        Date fixingDate =
                            flatIbor->fixingCalendar().adjust(std::max(arguments_.floatingFixingDates[k], reference));
                        flatAmount =
                            flatIbor->fixing(fixingDate) * arguments_.floatingAccrualTimes[k] * arguments_.nominal;
                    }
                    Real correction = (amount - flatAmount) * c_->discount(arguments_.floatingPayDates[k]);
                    sum1 += lambda1 * correction;
                    sum2 += lambda2 * correction;
                } else {
                    // if no amount is given, we do not need a spread correction
                    // due to different forward / discounting curves since then
                    // no curve is attached to the swap's ibor index and so we
                    // assume a one curve setup;
                    // but we can still have a float spread that has to be converted
                    // into a fixed leg's payment
                    Real correction = arguments_.nominal * arguments_.floatingSpreads[k] *
                                      arguments_.floatingAccrualTimes[k] * c_->discount(arguments_.floatingPayDates[k]);
                    sum1 += lambda1 * correction;
                    sum2 += lambda2 * correction;
                }
            }
            if (j > j1_) {
                S_[j - j1_ - 1] += sum1 / c_->discount(arguments_.fixedPayDates[j - 1]);
            } else {
                S_m1 += sum1 / c_->discount(arguments_.floatingResetDates[k1_]);
            }
            S_[j - j1_] += sum2 / c_->discount(arguments_.fixedPayDates[j]);
        }

        w_ = type == Option::Call ? -1.0 : 1.0;
        D0_ = c_->discount(arguments_.floatingResetDates[k1_]);
        Dj_.resize(arguments_.fixedCoupons.size() - j1_);
        for (Size j = j1_; j < arguments_.fixedCoupons.size(); ++j) {
            Dj_[j - j1_] = c_->discount(arguments_.fixedPayDates[j - j1_]);
        }
    }

    if (!caching_ || !lgm_H_constant_ || Hj_.empty()) {
        // it is a requirement that H' does not change its sign,
        // with u = -1.0 we handle the case H' < 0
        u_ = p_->Hprime(0.0) > 0.0 ? 1.0 : -1.0;

        H0_ = p_->H(p_->termStructure()->timeFromReference(arguments_.floatingResetDates[k1_]));
        Hj_.resize(arguments_.fixedCoupons.size() - j1_);
        for (Size j = j1_; j < arguments_.fixedCoupons.size(); ++j) {
            Hj_[j - j1_] = p_->H(p_->termStructure()->timeFromReference(arguments_.fixedPayDates[j]));
        }
    }

    if (!caching_ || !lgm_alpha_constant_ || zetaex_ == Null<Real>()) {
        zetaex_ = p_->zeta(p_->termStructure()->timeFromReference(expiry));
    }

    Brent b;
    Real yStar;
    try {
        yStar = b.solve(boost::bind(&AnalyticLgmSwaptionEngine::yStarHelper, this, _1), 1.0E-6, 0.0, 0.01);
    } catch (const std::exception& e) {
        QL_FAIL("AnalyticLgmSwaptionEngine, failed to compute yStar, " << e.what());
    }

    CumulativeNormalDistribution N;
    Real sqrt_zetaex = std::sqrt(zetaex_);
    Real sum = 0.0;
    for (Size j = j1_; j < arguments_.fixedCoupons.size(); ++j) {
        sum += w_ * (arguments_.fixedCoupons[j] - S_[j - j1_]) * Dj_[j - j1_] *
               N(u_ * w_ * (yStar + (Hj_[j - j1_] - H0_) * zetaex_) / sqrt_zetaex);
    }
    sum += -w_ * S_m1 * D0_ * N(u_ * w_ * yStar / sqrt_zetaex);
    sum += w_ * (arguments_.nominal * Dj_.back() * N(u_ * w_ * (yStar + (Hj_.back() - H0_) * zetaex_) / sqrt_zetaex) -
                 arguments_.nominal * D0_ * N(u_ * w_ * yStar / sqrt_zetaex));
    results_.value = sum;

    results_.additionalResults["fixedAmountCorrectionSettlement"] = S_m1;
    results_.additionalResults["fixedAmountCorrections"] = S_;

} // calculate

Real AnalyticLgmSwaptionEngine::yStarHelper(const Real y) const {
    Real sum = 0.0;
    for (Size j = j1_; j < arguments_.fixedCoupons.size(); ++j) {
        sum += (arguments_.fixedCoupons[j] - S_[j - j1_]) * Dj_[j - j1_] *
               std::exp(-(Hj_[j - j1_] - H0_) * y - 0.5 * (Hj_[j - j1_] - H0_) * (Hj_[j - j1_] - H0_) * zetaex_);
    }
    sum += -S_m1 * D0_;
    sum += Dj_.back() * arguments_.nominal *
           std::exp(-(Hj_.back() - H0_) * y - 0.5 * (Hj_.back() - H0_) * (Hj_.back() - H0_) * zetaex_);
    sum -= D0_ * arguments_.nominal;
    return sum;
}

} // namespace QuantExt


/**
 * @file 	main.cpp
 * @author 	Fabian Wegscheider
 * @date 	Jul 14, 2017
 */

#include <iostream>
#include <boost/program_options.hpp>
#include <boost/timer/timer.hpp>
#include <boost/graph/adjacency_list.hpp>
#include "GraphParser.h"
#include "PrimeNumbers.h"
#include "SteinerTreeHeuristic.h"

using std::string;
using std::cout;
using std::cerr;
using std::endl;
using namespace boost;
namespace po = boost::program_options;

using Graph = adjacency_list<vecS, vecS, undirectedS,
		no_property, property<edge_weight_t, double>>;

/**
 * The main function which reads in a graph from a .gph file, considers all
 * vertices with prime indices as terminals and calculates a steiner tree
 * using the an improved shortest-path-heuristic based on Dijkstra
 * @param numargs number of inputs on command line
 * @param args array of inputs on command line
 * @return whether the program operated successfully
 */
int main(int numargs, char* args[]) {

	timer::cpu_timer overallTimer;

	string input;
	bool printTreeSelected;
	int root = 1;

	/*parsing command line options*/
	po::options_description desc("Allowed options");
	desc.add_options()
			("help,h", "produce help message")
			("showtree,s", "print tree")
			("root,r", po::value<int>(&root)->default_value(1),
					"set root node (prime number)")
			("input-file", po::value<string>(), "input file");
	po::positional_options_description p;
	p.add("input-file", -1);
	po::variables_map vm;
	po::store(po::command_line_parser(numargs, args).
			options(desc).positional(p).run(), vm);
	po::notify(vm);

	if (vm.count("help")) {
		cout << desc << "\n";
		exit(EXIT_SUCCESS);
	}

	if (vm.count("input-file")) {
		input = vm["input-file"].as< string >();
	} else {
		cerr << "please specify an input file in the .gph format" << endl;
		exit(EXIT_FAILURE);
	}

	printTreeSelected = vm.count("showtree");
	/*end of parsing command line options*/

	int numVertices;
	int numEdges;

	GraphParser parser(input);
	if (!parser.openedSuccessfully) {
		cerr << "file could not be read" << endl;
		exit(EXIT_FAILURE);
	}

	//first line is read to get number of vertices and edges
	if (!parser.readFirstLine(numVertices, numEdges)) {
		cerr << "error while reading file, not the right format" << endl;
		exit(EXIT_FAILURE);
	}

	Edge* edges = new Edge[numEdges];
	double* weights = new double[numEdges];

	//rest of the file is read and parsed to a graph
	if (!parser.readEdgeData(edges, weights)) {
		cerr << "error while reading file, not the right format" << endl;
		exit(EXIT_FAILURE);
	}

	//undirected graph is constructed with all edges and their weights
	Graph g(edges, edges + numEdges , weights, numVertices);

	//In this array the steiner tree is implicitly stored by remembering the predecessor
	//of each vertex. A -1 means that the vertex is not contained in the tree
	int* treePredecessors = new int[numVertices];

	//all primes in {2,...,numVertices} are considered to be terminals
	vector<int> terminals = PrimeNumbers::findPrimes(numVertices);

	timer::cpu_timer algoTimer;

	//here the steiner tree is finally constructed
	double objectiveValue = SteinerTreeHeuristic::computeSteinerTree(g, numVertices,
			treePredecessors, terminals, root);

	if (objectiveValue == -1) {
		cerr << "the root you selected is not a valid prime number" << endl;
		exit(EXIT_FAILURE);
	}

	string algoTime = algoTimer.format(3, "%w");

	//to this string all edges in the tree are printed if user chose option -s
	string edgeString;
	if (printTreeSelected) {
		assert(SteinerTreeHeuristic::testAndPrintTree(g, numVertices, treePredecessors,
			terminals, root, edgeString));
	} else {
		assert(SteinerTreeHeuristic::testTree(g, numVertices, treePredecessors,
			terminals, root));
	}

	cout << "TLEN: " << objectiveValue << endl;
	if (printTreeSelected) cout << "TREE: " << edgeString << endl;
	cout << "TIME: " << overallTimer.format(3, "%t") << endl;
	cout << "WALL: " << algoTime << endl;
	cout << endl;

	delete[] edges;
	delete[] weights;
	delete[] treePredecessors;

	exit(EXIT_SUCCESS);
}


/*
  Copyright (c) 2010 Toru Tamaki

  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.
*/

#include <iostream>

#include <boost/shared_array.hpp>

#include <flann/flann.hpp>

// uncomment if you do not use the viewer.
//#define NOVIEWER

#include "3dregistration.h"

extern "C" {
  int sgemm_(const char *transa, const char *transb, const int *m, const int *n, const int *k,
             const float *alpha, const float *a, const int *lda, const float *b, const int *ldb,
             const float *beta, float *c, const int *ldc);
}

inline static float
distanceSquare(float x1, float y1, float z1,
	       float x2, float y2, float z2){

  float tmpx = (x1 - x2);
  float tmpy = (y1 - y2);
  float tmpz = (z1 - z2);

  return tmpx*tmpx + tmpy*tmpy + tmpz*tmpz;
}

inline static float
distanceSquareRT(float x1, float y1, float z1,
		 float x2, float y2, float z2, float* h_R, float* h_t){

  return distanceSquare(x1, y1, z1,
			(h_R[0]*x2 + h_R[1]*y2 + h_R[2]*z2) + h_t[0],
			(h_R[3]*x2 + h_R[4]*y2 + h_R[5]*z2) + h_t[1],
			(h_R[6]*x2 + h_R[7]*y2 + h_R[8]*z2) + h_t[2]);

}

#if 1
static void
findCenter(const float* h_X, const int Xsize,
	   float* h_Xc){

  const float* h_Xx = &h_X[Xsize*0];
  const float* h_Xy = &h_X[Xsize*1];
  const float* h_Xz = &h_X[Xsize*2];

  double Xcx = 0.0f, Xcy = 0.0f, Xcz = 0.0f;

#pragma omp parallel for reduction (+:Xcx,Xcy,Xcz)
  for(int i = 0; i < Xsize; i++){
    Xcx += h_Xx[i];
    Xcy += h_Xy[i];
    Xcz += h_Xz[i];
  }

  h_Xc[0] = Xcx / Xsize;
  h_Xc[1] = Xcy / Xsize;
  h_Xc[2] = Xcz / Xsize;

}

#else
static void
findCenter(const float* h_X, const int Xsize,
	   float* h_Xc){

  const float* h_Xx = &h_X[Xsize*0];
  const float* h_Xy = &h_X[Xsize*1];
  const float* h_Xz = &h_X[Xsize*2];

  h_Xc[0] = h_Xc[1] = h_Xc[2] = 0.0f;

  for(int i = 0; i < Xsize; i++){
    h_Xc[0] += h_Xx[i];
    h_Xc[1] += h_Xy[i];
    h_Xc[2] += h_Xz[i];
  }

  h_Xc[0] /= Xsize;
  h_Xc[1] /= Xsize;
  h_Xc[2] /= Xsize;

}
#endif

void icp(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target,
	 const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source,
	 float* h_R, float* h_t,
	 const registrationParameters &param)
{

  int Xsize, Ysize;
  boost::shared_array<float> h_X, h_Y;
  cloud2data(cloud_target, h_X, Xsize);
  cloud2data(cloud_source, h_Y, Ysize);

  //
  // initialize paramters
  //
  int maxIteration = param.maxIteration;

  //
  // memory allocation
  //

  float* h_Xx = h_X.get() + Xsize*0;
  float* h_Xy = h_X.get() + Xsize*1;
  float* h_Xz = h_X.get() + Xsize*2;

  float* h_Yx = h_Y.get() + Ysize*0;
  float* h_Yy = h_Y.get() + Ysize*1;
  float* h_Yz = h_Y.get() + Ysize*2;

  boost::shared_array<float> h_Xcorr ( new float[Ysize*3] ); // points in X corresponding to Y

  float h_S[9];
  float h_Xc[3];
  float h_Yc[3];

  findCenter(h_Y.get(), Ysize, h_Yc);

  // building flann index
  boost::shared_array<float> m_X ( new float [Xsize*3] );
  for (int i = 0; i < Xsize; i++)
  {
    m_X[i*3 + 0] = h_Xx[i];
    m_X[i*3 + 1] = h_Xy[i];
    m_X[i*3 + 2] = h_Xz[i];
  }
  flann::Matrix<float> mat_X(m_X.get(), Xsize, 3); // Xsize rows and 3 columns
  flann::Index< flann::L2<float> > index( mat_X, flann::KDTreeIndexParams() );
  index.buildIndex();

  boost::shared_array<float> m_Y ( new float [Ysize*3] );
  float* h_Xcorrx = h_Xcorr.get() + Ysize*0;
  float* h_Xcorry = h_Xcorr.get() + Ysize*1;
  float* h_Xcorrz = h_Xcorr.get() + Ysize*2;

  boost::shared_array<float> h_Ycentered ( new float [Ysize*3] );
  {
    float* h_Ycenteredx = h_Ycentered.get() + Ysize*0;
    float* h_Ycenteredy = h_Ycentered.get() + Ysize*1;
    float* h_Ycenteredz = h_Ycentered.get() + Ysize*2;
    for(int i = 0; i < Ysize; i++){
      h_Ycenteredx[i] = h_Yx[i] - h_Yc[0];
      h_Ycenteredy[i] = h_Yy[i] - h_Yc[1];
      h_Ycenteredz[i] = h_Yz[i] - h_Yc[2];
    }
  }

  // ICP main loop

  for(int iter=0; iter < maxIteration; iter++){

    // find closest points

    #pragma omp parallel for
    for (int i = 0; i < Ysize; i++)
    {
      m_Y[i*3 + 0] = (h_R[0]*h_Yx[i] + h_R[1]*h_Yy[i] + h_R[2]*h_Yz[i]) + h_t[0];
      m_Y[i*3 + 1] = (h_R[3]*h_Yx[i] + h_R[4]*h_Yy[i] + h_R[5]*h_Yz[i]) + h_t[1];
      m_Y[i*3 + 2] = (h_R[6]*h_Yx[i] + h_R[7]*h_Yy[i] + h_R[8]*h_Yz[i]) + h_t[2];
    }
    flann::Matrix<float> mat_Y(m_Y.get(), Ysize, 3); // Ysize rows and 3 columns

    std::vector< std::vector<size_t> > indices(Ysize);
    std::vector< std::vector<float> >  dists(Ysize);

    index.knnSearch(mat_Y,
		    indices,
		    dists,
		    1, // k of knn
		    flann::SearchParams() );

    #pragma omp parallel for
    for(int i = 0; i < Ysize; i++){
      // put the closest point to Ycorr
      h_Xcorrx[i] = h_Xx[indices[i][0]];
      h_Xcorry[i] = h_Xy[indices[i][0]];
      h_Xcorrz[i] = h_Xz[indices[i][0]];
    }

    findCenter(h_Xcorr.get(), Ysize, h_Xc);

    #pragma omp parallel for
    for(int i = 0; i < Ysize; i++){
      // put the closest point to Ycorr
      h_Xcorrx[i] -= h_Xc[0];
      h_Xcorry[i] -= h_Xc[1];
      h_Xcorrz[i] -= h_Xc[2];
    }

    // compute S

    {
      // SGEMM(TRANSA,TRANSB,M,N,K,ALPHA,A,LDA,B,LDB,BETA,C,LDC)
      //  C := alpha*op( A )*op( B ) + beta*C
      //
      //  h_X^T * h_Y => S
      //  m*k     k*n    m*n
      int three = 3;
      float one = 1.0f, zero = 0.0f;
      sgemm_((char*)"t", (char*)"n",
	     &three, &three, &Ysize, // m,n,k
	     &one, h_Xcorr.get(), &Ysize, // alpha, op(A), lda
	     h_Ycentered.get(), &Ysize,  // op(B), ldb
	     &zero, h_S, &three);  // beta, C, ldc
    }

    // find RT from S

    findRTfromS(h_Xc, h_Yc, h_S, h_R, h_t);

#ifndef NOVIEWER
    if(!param.noviewer){
      Eigen::Matrix4f transformation;
      transformation <<
      			h_R[0], h_R[1], h_R[2], h_t[0],
			h_R[3], h_R[4], h_R[5], h_t[1],
			h_R[6], h_R[7], h_R[8], h_t[2],
			0, 0, 0, 1;
      pcl::transformPointCloud ( *param.cloud_source, *param.cloud_source_trans, transformation );
      param.viewer->updatePointCloud ( param.cloud_source_trans, *param.source_trans_color, "source trans" );
      param.viewer->spinOnce();
    }
#endif

  }

}


// The MIT License (MIT)

// Copyright (c) 2014 Mohammad Dashti
// (www.mdashti.com - mohammad.dashti [at] epfl [dot] ch - mdashti [at] gmail [dot] com)

// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef KDOUBLE_H
#define KDOUBLE_H
#include <cmath>
#include <iostream>
// #include <boost/multiprecision/cpp_dec_float.hpp>

// typedef boost::multiprecision::number<boost::multiprecision::cpp_dec_float<1000, long long> > cpp_dec_float_1000;

#define DOUBLE_TYPE_STD_DOUBLE 1
#define DOUBLE_TYPE_KAHAN_DOUBLE 2
#define DOUBLE_TYPE_STD_LONG_DOUBLE 3
#define DOUBLE_TYPE_BOOST 4

// #ifndef DOUBLE_TYPE

//   //you can change the value of DOUBLE_TYPE_SYM
//   //to change the double type. Choices are:
//   // - DOUBLE_TYPE_STD_DOUBLE
//   // - DOUBLE_TYPE_KAHAN_DOUBLE
//   // - DOUBLE_TYPE_STD_LONG_DOUBLE
//   // - DOUBLE_TYPE_BOOST
//   #define DOUBLE_TYPE_SYM DOUBLE_TYPE_STD_DOUBLE

//   #if DOUBLE_TYPE_SYM == DOUBLE_TYPE_KAHAN_DOUBLE
//     #define DOUBLE_TYPE KDouble
//   #elif DOUBLE_TYPE_SYM == DOUBLE_TYPE_BOOST
//     #define DOUBLE_TYPE cpp_dec_float_1000
//   #elif DOUBLE_TYPE_SYM == DOUBLE_TYPE_STD_LONG_DOUBLE
//     #define DOUBLE_TYPE long double
//   #else
//     #define DOUBLE_TYPE double
//   #endif //DOUBLE_TYPE_SYM == DOUBLE_TYPE_KAHAN_DOUBLE
// #endif //DOUBLE_TYPE

namespace dbtoaster {

class KDouble
{
public:
  static double diff_p;
#if DOUBLE_TYPE_SYM == DOUBLE_TYPE_KAHAN_DOUBLE
private:
  double sum;
  double c;

protected:
  //friends
  friend bool operator==(const double, const KDouble &);
  friend bool operator!=(const double, const KDouble &);
  friend bool operator<(const double, const KDouble &);
  friend bool operator<=(const double, const KDouble &);
  friend bool operator>(const double, const KDouble &);
  friend bool operator>=(const double, const KDouble &);
  friend KDouble acos(const KDouble &);
  friend KDouble atan2(const KDouble &, const KDouble &);
  friend KDouble sqrt(const KDouble &);
  friend KDouble pow(const KDouble &, const KDouble &);
  // friend size_t hash_value(KDouble const &v);
  template <class T>
  friend void hash_combine(std::size_t& seed, const T& v);
  friend KDouble abs(const KDouble &dbl);

public:
  KDouble() : sum(0.0), c(0.0)
  {
  }

  KDouble(const KDouble &init) : sum(init.sum), c(init.c)
  {
  }

  KDouble(const double &init) : sum(init), c(0.0)
  {
  }

  ~KDouble()
  {
  }

  inline KDouble &operator=(const KDouble &other) { sum = other.sum; c = other.c; return *this; }
  inline KDouble &operator=(const double &other) { sum = other; c = 0.0; return *this; }
  inline KDouble &operator-=(const KDouble &other) {
    double y = -other.sum-c;
    double t = sum+y;
    c = static_cast<double>(t-sum)-y;
    sum = t;
    return *this;
  }
  inline KDouble &operator-=(const double &other) {
    double y = -other-c;
    double t = sum+y;
    c = static_cast<double>(t-sum)-y;
    sum = t;
    return *this;
  }
  inline KDouble &operator+=(const KDouble &other) {
    double y = other.sum-c;
    double t = sum+y;
    c = static_cast<double>(t-sum)-y;
    sum = t;
    return *this;
  }
  inline KDouble &operator+=(const double &other) {
    double y = other-c;
    double t = sum+y;
    c = static_cast<double>(t-sum)-y;
    sum = t;
    return *this;
  }

  //TODO Is this the right implementation?
  inline KDouble &operator*=(const KDouble &other) { sum *= other.sum; c *= other.sum; return *this; }
  inline KDouble &operator*=(const double &other) { sum *= other; c *= other; return *this; }
  inline KDouble &operator/=(const KDouble &other) { sum /= other.sum; c /= other.sum; return *this; }
  inline KDouble &operator/=(const double &other) { sum /= other; c /= other; return *this; }

  friend std::ostream &operator<<(std::ostream &out, const KDouble &kd) {
      return out << kd.sum;
  }

  //inline operator double() const { return sum; }
  // operator int() const { return static_cast<int>(sum); }
  // operator long() const { return static_cast<long>(sum); }

  inline bool operator==(const KDouble &other) const
  {
    return abs(sum-other.sum) < diff_p;
  }

  inline bool operator!=(const KDouble &other) const
  {
    return abs(sum-other.sum) >= diff_p;
  }

  // inline bool operator==(const double &other)
  // {
  //   return abs(sum-other) < diff_p;
  // }

  // inline bool operator!=(const double &other)
  // {
  //   return abs(sum-other) >= diff_p;
  // }

  inline bool operator<(const KDouble &other) const
  {
    return sum < other.sum;
  }

  inline bool operator<=(const KDouble &other) const
  {
    return sum <= other.sum;
  }

  inline bool operator>(const KDouble &other) const
  {
    return sum > other.sum;
  }

  inline bool operator>=(const KDouble &other) const
  {
    return sum >= other.sum;
  }
#endif //DOUBLE_TYPE_SYM == DOUBLE_TYPE_KAHAN_DOUBLE
};

#if DOUBLE_TYPE_SYM == DOUBLE_TYPE_KAHAN_DOUBLE

  inline bool operator==(const double sum, const KDouble & other)
  {
    return abs(sum-other.sum) < KDouble::diff_p;
  }
  inline bool operator!=(const double sum, const KDouble & other)
  {
    return abs(sum-other.sum) >= KDouble::diff_p;
  }

  inline bool operator<(const double sum, const KDouble &other)
  {
    return sum < other.sum;
  }

  inline bool operator<=(const double sum, const KDouble &other)
  {
    return sum <= other.sum;
  }

  inline bool operator>(const double sum, const KDouble &other)
  {
    return sum > other.sum;
  }

  inline bool operator>=(const double sum, const KDouble &other)
  {
    return sum >= other.sum;
  }

  inline KDouble operator-(const KDouble &g1, const KDouble &g2)
  {
      KDouble result(g1);
      result -= g2;
      return result;
  }

  inline KDouble operator+(const KDouble &g1, const KDouble &g2)
  {
      KDouble result(g1);
      result += g2;
      return result;
  }

  inline KDouble operator*(const KDouble &g1, const KDouble &g2)
  {
      KDouble result(g1);
      result *= g2;
      return result;
  }

  inline KDouble operator/(const KDouble &g1, const KDouble &g2)
  {
      KDouble result(g1);
      result /= g2;
      return result;
  }

  inline KDouble operator-(const double &g1, const KDouble &g2)
  {
      KDouble result(g1);
      result -= g2;
      return result;
  }

  inline KDouble operator+(const double &g1, const KDouble &g2)
  {
      KDouble result(g1);
      result += g2;
      return result;
  }

  inline KDouble operator/(const double &g1, const KDouble &g2)
  {
      KDouble result(g1);
      result /= g2;
      return result;
  }

  inline KDouble operator-(const long &g1, const KDouble &g2)
  {
      KDouble result(g1);
      result -= g2;
      return result;
  }

  inline KDouble operator+(const long &g1, const KDouble &g2)
  {
      KDouble result(g1);
      result += g2;
      return result;
  }

  inline KDouble operator/(const long &g1, const KDouble &g2)
  {
      KDouble result(g1);
      result /= g2;
      return result;
  }

  inline KDouble operator-(const int &g1, const KDouble &g2)
  {
      KDouble result(g1);
      result -= g2;
      return result;
  }

  inline KDouble operator+(const int &g1, const KDouble &g2)
  {
      KDouble result(g1);
      result += g2;
      return result;
  }

  inline KDouble operator*(const int &g1, const KDouble &g2)
  {
      KDouble result(g1);
      result *= g2;
      return result;
  }

  inline KDouble operator/(const int &g1, const KDouble &g2)
  {
      KDouble result(g1);
      result /= g2;
      return result;
  }

  inline KDouble operator-(const KDouble &g1, const double &g2)
  {
      KDouble result(g1);
      result -= g2;
      return result;
  }

  inline KDouble operator+(const KDouble &g1, const double &g2)
  {
      KDouble result(g1);
      result += g2;
      return result;
  }

  inline KDouble operator*(const KDouble &g1, const double &g2)
  {
      KDouble result(g1);
      result *= g2;
      return result;
  }

  inline KDouble operator/(const KDouble &g1, const double &g2)
  {
      KDouble result(g1);
      result /= g2;
      return result;
  }

  inline KDouble operator-(const KDouble &g1, const long &g2)
  {
      KDouble result(g1);
      result -= g2;
      return result;
  }

  inline KDouble operator+(const KDouble &g1, const long &g2)
  {
      KDouble result(g1);
      result += g2;
      return result;
  }

  inline KDouble operator*(const KDouble &g1, const long &g2)
  {
      KDouble result(g1);
      result *= g2;
      return result;
  }

  inline KDouble operator/(const KDouble &g1, const long &g2)
  {
      KDouble result(g1);
      result /= g2;
      return result;
  }

  inline KDouble operator-(const KDouble &g1, const int &g2)
  {
      KDouble result(g1);
      result -= g2;
      return result;
  }

  inline KDouble operator+(const KDouble &g1, const int &g2)
  {
      KDouble result(g1);
      result += g2;
      return result;
  }

  inline KDouble operator*(const KDouble &g1, const int &g2)
  {
      KDouble result(g1);
      result *= g2;
      return result;
  }

  inline KDouble operator/(const KDouble &g1, const int &g2)
  {
      KDouble result(g1);
      result /= g2;
      return result;
  }

  inline KDouble abs(const KDouble &dbl)
  {
      KDouble result(dbl);
      if(result.sum < 0.0) result.sum = -result.sum;
      return result;
  }

  inline KDouble acos(const KDouble &dbl)
  {
      return KDouble(acos(dbl.sum));
  }

  inline KDouble atan2(const KDouble &g1, const KDouble &g2)
  {
    return KDouble(atan2(g1.sum,g2.sum));
  }

  inline KDouble sqrt(const KDouble &dbl)
  {
    return KDouble(sqrt(dbl.sum));
  }

  inline KDouble pow(const KDouble &g1, const KDouble &g2)
  {
    return KDouble(pow(g1.sum, g2.sum));
  }
#endif //DOUBLE_TYPE_SYM == DOUBLE_TYPE_KAHAN_DOUBLE
}
#endif //KDOUBLE_H


#include <iostream>
#include <vector>
#include <cassert>
#include <cstring>
#include <limits>
#include <mpi.h>
#include <Eigen/Eigenvalues>

enum KernelType {CubicSpline = 0, WendlandC2 = 1, WendlandC4 = 2};

#include "particle_simulator.hpp"
#include "hdr_time.hpp"
#include "hdr_run.hpp"
#include "vector_x86.hpp"
#include "hdr_dimension.hpp"
#include "hdr_kernel.hpp"
#include "hdr_sph.hpp"
#include "hdr_hgas.hpp"
#include "hdr_bhns.hpp"

class SPHAnalysis : public HelmholtzGas {
public:
    SPHAnalysis() {
        this->id    = 0;
        this->istar = 0;
        this->mass  = 0.;
        this->pos   = 0.;
        this->vel   = 0.;
        this->uene  = 0.;
        this->alph  = 0.;
        this->alphu = 0.;
        this->ksr   = 0.;
        for(PS::S32 k = 0; k < NR::NumberOfNucleon; k++) {
            this->cmps[k] = 0.;
        }
    }

    void readAscii(FILE * fp) {
        fscanf(fp, "%lld%lld%lf", &this->id, &this->istar, &this->mass);      //  3
        fscanf(fp, "%lf%lf%lf", &this->pos[0], &this->pos[1], &this->pos[2]); //  6
        fscanf(fp, "%lf%lf%lf", &this->vel[0], &this->vel[1], &this->vel[2]); //  9
        fscanf(fp, "%lf%lf%lf", &this->acc[0], &this->acc[1], &this->acc[2]); // 12
        fscanf(fp, "%lf%lf%lf", &this->uene, &this->alph, &this->alphu);      // 15
        fscanf(fp, "%lf%lf%6d", &this->dens, &this->ksr,  &this->np);         // 18
        fscanf(fp, "%lf%lf%lf", &this->vsnd, &this->pres, &this->temp);       // 21
        fscanf(fp, "%lf%lf%lf", &this->divv, &this->rotv, &this->bswt);       // 24
        fscanf(fp, "%lf%lf%lf", &this->pot,  &this->abar, &this->zbar);       // 27
        fscanf(fp, "%lf",       &this->enuc);                                 // 28
        fscanf(fp, "%lf%lf%lf", &this->vsmx, &this->udot, &this->dnuc);       // 31
        for(PS::S32 k = 0; k < NuclearReaction::NumberOfNucleon; k++) {       // 32 -- 44
            fscanf(fp, "%lf", &this->cmps[k]);
        }
        fscanf(fp, "%lf", &this->pot3);
        fscanf(fp, "%lf%lf%lf", &this->tempmax[0], &this->tempmax[1], &this->tempmax[2]);
        fscanf(fp, "%lf", &this->entr);
    }

    /*
    void write(FILE * fp = stdout) {
        fprintf(fp, "%8d %2d %+e",  this->id, this->istar, this->mass);
        fprintf(fp, " %+e %+e %+e", this->pos[0], this->pos[1], this->pos[2]);
        fprintf(fp, " %+e %+e %+e", this->vel[0], this->vel[1], this->vel[2]);
        fprintf(fp, " %+e %+e %+e", this->uene, this->alph, this->alphu);
        fprintf(fp, " %+e", this->ksr);
        for(PS::S32 k = 0; k < NR::NumberOfNucleon; k++) {
            fprintf(fp, " %+.3e", this->cmps[k]);
        }
        fprintf(fp, "\n");
    }
    */

    inline PS::F64mat calcMomentOfInertia(PS::F64vec xc) {
        PS::F64mat qq = 0.;
        PS::F64vec dx = this->pos - xc;
        PS::F64 r2 = dx * dx;
        qq.xx = this->mass * (r2 - dx[0] * dx[0]);
        qq.xy = this->mass * (   - dx[0] * dx[1]);
        qq.xz = this->mass * (   - dx[0] * dx[2]);
        qq.yy = this->mass * (r2 - dx[1] * dx[1]);
        qq.yz = this->mass * (   - dx[1] * dx[2]);
        qq.zz = this->mass * (r2 - dx[2] * dx[2]);
        return qq;
    }
};

class BlackHoleNeutronStarAnalysis : public BlackHoleNeutronStar {
public:
    BlackHoleNeutronStarAnalysis() {
        this->id    = 0;
        this->istar = 0;
        this->mass  = 0.;
        this->pos   = 0.;
        this->vel   = 0.;
        this->acc   = 0.;
        this->eps   = 0.;
        this->pot   = 0.;
    }

    void readAscii(FILE * fp) {
        fscanf(fp, "%lld%lld%lf%lf%lf%lf%lf%lf%lf%lf%lf%lf%lf%lf",
               &this->id, &this->istar, &this->mass,
               &this->pos[0], &this->pos[1], &this->pos[2],
               &this->vel[0], &this->vel[1], &this->vel[2],
               &this->acc[0], &this->acc[1], &this->acc[2],
               &this->eps, &this->pot);
    }

    inline PS::F64mat calcMomentOfInertia(PS::F64vec xc) {
        PS::F64mat qq = 0.;
        PS::F64vec dx = this->pos - xc;
        PS::F64 r2 = dx * dx;
        qq.xx = this->mass * (r2 - dx[0] * dx[0]);
        qq.xy = this->mass * (   - dx[0] * dx[1]);
        qq.xz = this->mass * (   - dx[0] * dx[2]);
        qq.yy = this->mass * (r2 - dx[1] * dx[1]);
        qq.yz = this->mass * (   - dx[1] * dx[2]);
        qq.zz = this->mass * (r2 - dx[2] * dx[2]);
        return qq;
    }
};

int main(int argc, char ** argv) {
    MPI_Init(&argc, &argv);

    PS::ParticleSystem<SPHAnalysis> sph;
    sph.initialize();
    sph.createParticle(0);
    sph.setNumberOfParticleLocal(0);
    PS::ParticleSystem<BlackHoleNeutronStarAnalysis> bhns;
    bhns.initialize();
    bhns.createParticle(0);
    bhns.setNumberOfParticleLocal(0);

    char idir[1024], odir[1024];
    PS::F64 rcrit;
    PS::S64 ibgn, iend;
    FILE * fp = fopen(argv[1], "r");
    fscanf(fp, "%s", idir);
    fscanf(fp, "%s", odir);
    fscanf(fp, "%lf", &rcrit);
    fscanf(fp, "%lld%lld", &ibgn, &iend);
    fclose(fp);

    char ofile[1024];
    sprintf(ofile, "%s/axis.dat", odir);
    FILE * fout = fopen(ofile, "w");
    fprintf(fout, "# idir: %s\n", idir);
    fprintf(fout, "# crit: %+e\n", rcrit);
    fprintf(fout, "# time, Ixx, Iyy, Izz, Ixx, Iyy, Izz\n");
    for(PS::S64 itime = ibgn; itime <= iend; itime++) {
        char sfile[1024], bfile[1024];
        sprintf(sfile, "%s/sph_t%04d.dat",  idir, itime);
        sprintf(bfile, "%s/bhns_t%04d.dat", idir, itime);
        fp = fopen(sfile, "r");
        assert(fp);
        sph.readParticleAscii(sfile);
        fclose(fp);
        fp = fopen(bfile, "r");
        assert(fp);
        bhns.readParticleAscii(bfile);
        fclose(fp);

        PS::F64mat qq[2] = {0., 0.};
        for(PS::S64 i = 0; i < sph.getNumberOfParticleLocal(); i++) {
            PS::F64mat qtemp = sph[i].calcMomentOfInertia(bhns[0].pos);
            qq[0] = qq[0] + qtemp;
            PS::F64vec dx = sph[i].pos - bhns[0].pos;
            PS::F64    r2 = dx * dx;
            if(r2 < rcrit * rcrit) {
                qq[1] = qq[1] + qtemp;
            }
        }

        fprintf(fout, "%5d", itime);
        Eigen::Matrix3d MomentOfInertia[2];
        for(PS::S64 k = 0; k < 2; k++) {
            MomentOfInertia[k] <<
                qq[k].xx, qq[k].xy, qq[k].xz,
                qq[k].xy, qq[k].yy, qq[k].yz,
                qq[k].xz, qq[k].yz, qq[k].zz;
            Eigen::EigenSolver<Eigen::Matrix3d> es(MomentOfInertia[k], false);
            std::vector<PS::F64> ev;
            ev.push_back(es.eigenvalues()[0].real());
            ev.push_back(es.eigenvalues()[1].real());
            ev.push_back(es.eigenvalues()[2].real());
            std::sort(ev.begin(), ev.end());
            fprintf(fout, " %+e %+e %+e", ev[0], ev[1], ev[2]);
        }
        fprintf(fout, "\n");
        fflush(fout);

    }
    fclose(fout);

    MPI_Finalize();

    return 0;
}