/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2008 Lorella Fatone
 Copyright (C) 2008 Francesca Mariani
 Copyright (C) 2008 Maria Cristina Recchioni
 Copyright (C) 2008 Francesco Zirilli
 Copyright (C) 2008 StatPro Italia srl

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/experimental/varianceoption/integralhestonvarianceoptionengine.hpp>
#include <ql/errors.hpp>
#include <ql/function.hpp>
#include <boost/scoped_array.hpp>
#include <complex>

namespace QuantLib {

    namespace {

    /*
     *****************************************************************
     **
     ** Parameters defining the initial condition of the Heston model
     ** and the European call option
     **
     *****************************************************************
     */
    /*
     *****************************************************************
     ** Assign: v0, eprice, tau, rtax
     ******************************************************************
     ******************************************************************
     **     v0: initial variance
     ** eprice: realized variance strike price
     **    tau: time to maturity
     *    rtax: risk free interest rate
     ****************************************************************
     */

    typedef std::complex<Real> Complex;

    Real IvopOneDim(Real eps, Real chi, Real theta, Real /*rho*/,
                      Real v0, Real eprice, Time tau, Real rtax)
    {
        Real ss=0.0;
        boost::scoped_array<double> xiv(new double[2048*2048+1]);
        double nris=0.0;
        int j=0,mm=0;
        double pi=0,pi2=0;
        double dstep=0;
        Real option=0, impart=0;

        boost::scoped_array<Complex> ff(new Complex[2048*2048]);
        Complex xi;
        Complex ui,beta,zita,gamma,csum,vero;
        Complex contrib, caux, caux1,caux2,caux3;

        ui=Complex(0.0,1.0);

        /*
         **********************************************************
         **   i0: initial integrated variance i0=0
         **********************************************************
         */
        Real i0=0.0;
        //s=2.0*chi*theta/(eps*eps)-1.0;

        //s=s+1;

        /*
         *************************************************
         ** Start integration procedure
         *************************************************
         */

        pi= 3.14159265358979324;
        pi2=2.0*pi;
        Real s=2.0*chi*theta/(eps*eps)-1.0;
        /*
         ****************************************
         ** Note that s must be greater than zero
         ****************************************
         */

        if(s<=0)
        {
            QL_FAIL("this parameter must be greater than zero-> " << s);
        }

        ss=s+1;

        /*
         *************************************************
         ** Start integration procedure
         *************************************************

         **************************************************************
         ** The oscillatory integral that approximates the price of
         ** the realized variance option is computed using the method
         ** proposed by Bailey, Swarztrauber in the paper published in
         ** Siam Journal on Scientific Computing Vol 15(5) 1994
         ** p. 1105-1110
         **************************************************************

         **************************************************************
         ** dstep: real number, generally a power of two, that must be
         **        assigned to determine the grid of
         **        integration. Hint: dstep=256 or 512 (dstep<=2048)
         **************************************************************
         */
        dstep=256.0;
        nris=std::sqrt(pi2)/dstep;
        mm=(int)(pi2/(nris*nris));

        /*
         ******************************************
         **  Definition of the integration grid  **
         ******************************************
         */
        for (j=0;j<=mm-1;j++)
        {
            xiv[j+1]=(double)(j-mm/2)*nris;
        }

        for (j=0;j<=mm-1;j++)
        {
            xi=xiv[j+1];
            caux=chi*chi;
            caux1=2.0*eps*eps;
            caux1=caux1*xi;
            caux1=caux1*ui;
            caux2=caux1+caux;

            zita=0.5*std::sqrt(caux2);

            caux1=std::exp(-2.0*tau*zita);

            beta=0.5*chi+zita;
            beta=beta+caux1*(zita-0.5*chi);
            gamma=1.0-caux1;

            caux=-ss*tau;
            caux2=caux*(zita-0.5*chi);
            caux=ss*std::log(2.0*(zita/beta));
            caux3=-v0*ui*xi*(gamma/beta);
            caux=caux+caux3;
            caux=caux+caux2;

            ff[j+1]=std::exp(caux);
            if(std::sqrt(std::imag(xi)*std::imag(xi)+std::real(xi)*std::real(xi))>1.e-06)
            {
                contrib=-eprice/(ui*xi);
                caux=ui*xi;
                caux=caux*eprice;
                caux=std::exp(caux);
                caux=caux-1.0;
                caux2=ui*xi*ui*xi;
                contrib=contrib+caux/caux2;
            }
            else
            {
                contrib=eprice*eprice*0.5;
            }
            ff[j+1]=ff[j+1]*contrib;
        }
        csum=0.0;
        for (j=0;j<=mm-1;j++)
        {
            caux=std::pow(-1.0,j);
            caux2=-2.0*pi*(double)mm*(double)j*0.5/(double)mm;
            caux3=ui*caux2;
            csum=csum+ff[j+1]*caux*std::exp(caux3);
        }
        csum=csum*std::sqrt(std::pow(-1.0,mm))*nris/pi2;
        vero=i0-eprice+theta*tau+(1.0-std::exp(-chi*tau))*(v0-theta)/chi;
        csum=csum+vero;
        option=std::exp(-rtax*tau)*std::real(csum);
        impart=std::imag(csum);
        QL_ENSURE(impart <= 1e-12,
                  "imaginary part option (must be zero) = " << impart);
        return option;
    }

    Real IvopTwoDim(Real eps, Real chi, Real theta, Real /*rho*/,
                    Real v0, Time tau, Real rtax,
                    const ext::function<Real(Real)>& payoff) {

        Real ss=0.0;
        boost::scoped_array<double> xiv(new double[2048*2048+1]);
        boost::scoped_array<double> ivet(new double[2048 * 2048 + 1]);
        double nris=0.0;
        int j=0,mm=0,k=0;
        double pi=0,pi2=0;

        double dstep=0;
        Real ip=0;
        Real payoffval=0;
        Real option=0/*, impart=0*/;

        Real sumr=0;//,sumi=0;
        Complex dxi,z;

        boost::scoped_array<Complex> ff(new Complex[2048*2048]);
        Complex xi;
        Complex ui,beta,zita,gamma,csum;
        Complex caux,caux1,caux2,caux3;

        ui=Complex(0.0,1.0);

        /*
         **********************************************************
         **   i0: initial integrated variance i0=0
         **********************************************************
         */
        Real i0=0.0;

        /*
         *************************************************
         ** Start integration procedure
         *************************************************
         */

        pi= 3.14159265358979324;
        pi2=2.0*pi;

        Real s=2.0*chi*theta/(eps*eps)-1.0;
        /*
         ****************************************
         ** Note that s must be greater than zero
         ****************************************
         */

        if(s<=0)
        {
            QL_FAIL("this parameter must be greater than zero-> " << s);
        }

        ss=s+1;

        /*
         *************************************************
         ** Start integration procedure
         *************************************************

         **************************************************************
         ** The oscillatory integral that approximates the price of
         ** the realized variance option is computed using the method
         ** proposed by Bailey, Swarztrauber in the paper published in
         ** Siam Journal on Scientific Computing Vol 15(5) 1994
         ** p. 1105-1110
         **************************************************************

         **************************************************************
         ** dstep: real number, generally a power of two that must be
         **        assigned to determine the grid of
         **        integration. Hint: dstep=256 or 512 (dstep<=2048)
         **************************************************************
         */
        dstep=64.0;
        nris=std::sqrt(pi2)/dstep;
        mm=(int)(pi2/(nris*nris));

        /*
         ******************************************
         **  Definition of the integration grid  **
         ******************************************
         */

        for (j=0;j<=mm-1;j++)
        {
            xiv[j+1]=(double)(j-mm/2)*nris;
            ivet[j+1]=(double)(j-mm/2)*pi2/((double)mm*nris);
        }

        for (j=0;j<=mm-1;j++)
        {
            xi=xiv[j+1];

            caux=chi*chi;
            caux1=2.0*eps*eps;
            caux1=caux1*xi;
            caux1=caux1*ui;
            caux2=caux1+caux;

            zita=0.5*std::sqrt(caux2);
            caux1=std::exp(-2.0*tau*zita);

            beta=0.5*chi+zita;
            beta=beta+caux1*(zita-0.5*chi);

            gamma=1.0-caux1;

            caux=-ss*tau;
            caux2=caux*(zita-0.5*chi);
            caux=ss*std::log(2.0*(zita/beta));
            caux3=-v0*ui*xi*(gamma/beta);
            caux=caux+caux3;
            caux=caux+caux2;
            ff[j+1]=std::exp(caux);
        }

        sumr=0.0;
        //sumi=0.0;
        for (k=0;k<=mm-1;k++)
        {
            ip=i0-ivet[k+1];
            payoffval=payoff(ip);

            dxi=2.0*pi*(double)k/(double)mm*ui;
            csum=0.0;
            for (j=0;j<=mm-1;j++)
            {
                z=-(double)j*dxi;
                caux=std::pow(-1.0,j);
                csum=csum+ff[j+1]*caux*std::exp(z);
            }
            csum=csum*std::pow(-1.0,k)*nris/pi2;

            sumr=sumr+payoffval*std::real(csum);
            //sumi=sumi+payoffval*std::imag(csum);
        }
        sumr=sumr*nris;
        //sumi=sumi*nris;

        option=std::exp(-rtax*tau)*sumr;
        //impart=sumi;
        //QL_ENSURE(impart <= 1e-3,
        //          "imaginary part option (must be close to zero) = " << impart);
        return option;
    }

    struct payoff_adapter {
        ext::shared_ptr<QuantLib::Payoff> payoff;
        explicit payoff_adapter(ext::shared_ptr<QuantLib::Payoff> payoff)
        : payoff(payoff) {}
        Real operator()(Real S) const {
            return (*payoff)(S);
        }
    };

    }

    IntegralHestonVarianceOptionEngine::IntegralHestonVarianceOptionEngine(
                              const ext::shared_ptr<HestonProcess>& process)
    : process_(process) {
        registerWith(process_);
    }

    void IntegralHestonVarianceOptionEngine::calculate() const {

        QL_REQUIRE(process_->dividendYield().empty(),
                   "this engine does not manage dividend yields");

        Handle<YieldTermStructure> riskFreeRate = process_->riskFreeRate();

        Real epsilon = process_->sigma();
        Real chi = process_->kappa();
        Real theta = process_->theta();
        Real rho = process_->rho();
        Real v0 = process_->v0();

        Time tau = riskFreeRate->dayCounter().yearFraction(
                                        Settings::instance().evaluationDate(),
                                        arguments_.maturityDate);
        Rate r = riskFreeRate->zeroRate(arguments_.maturityDate,
                                        riskFreeRate->dayCounter(),
                                        Continuous);

        ext::shared_ptr<PlainVanillaPayoff> plainPayoff =
            ext::dynamic_pointer_cast<PlainVanillaPayoff>(arguments_.payoff);
        if (plainPayoff && plainPayoff->optionType() == Option::Call) {
            // a specialization for Call options is available
            Real strike = plainPayoff->strike();
            results_.value = IvopOneDim(epsilon, chi, theta, rho,
                                        v0, strike, tau, r)
                * arguments_.notional;
        } else {
            results_.value = IvopTwoDim(epsilon, chi, theta, rho, v0, tau, r,
                                        payoff_adapter(arguments_.payoff))
                * arguments_.notional;
        }
    }

}


#include "LonLatToXYTransformation.h"
#include "State.h"
#include <boost/move/make_unique.hpp>
#include <gis/SpatialReference.h>
#include <ogr_spatialref.h>

namespace SmartMet
{
namespace Plugin
{
namespace Dali
{
LonLatToXYTransformation::LonLatToXYTransformation(const Projection& projection)
    : transformation(Fmi::SpatialReference("WGS84"), projection.getCRS()), box(projection.getBox())
{
}

bool LonLatToXYTransformation::transform(double longitude, double latitude, double& x, double& y)
{
  if (!transformation.transform(longitude, latitude))
    return false;

  box.transform(longitude, latitude);
  x = longitude;
  y = latitude;

  return true;
}

bool LonLatToXYTransformation::transform(double& inoutX, double& inoutY)
{
  return transform(inoutX, inoutY, inoutX, inoutY);
}

}  // namespace Dali
}  // namespace Plugin
}  // namespace SmartMet


#include <complex>
#include <exception>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>
#include <boost/circular_buffer.hpp>
#include <range/v3/all.hpp>
#include <cstdint>
#include <initializer_list>
#include <iterator>
namespace views = ranges::views;
using Buffer = boost::circular_buffer<int>;
class Knothash
{
  Buffer ring_;
  std::vector<int> sequencelen_;
  int skiplen_ = 0;
  int curpos_ = 0;
  void reverse(int start, int len)
  {
    for (auto i = 0; i < len / 2; ++i) {
      int b = (start + i) % ring_.size();
      int r = (start + len - 1 - i) % ring_.size();
      std::swap(ring_[b], ring_[r]);
    }
  }

public:
  Knothash(int n, std::initializer_list<int> l) : ring_(n), sequencelen_(l)
  {
    for (int i = 0; i < n; ++i) {
      ring_.push_back(i);
    }
  }
  void hash()
  {
    for (auto l : sequencelen_) {
      reverse(curpos_, l);
      curpos_ += l + skiplen_;
      ++skiplen_;
    }
  }
  int final() const
  {
    return ring_[0] * ring_[1];
  }
};
int main()
{
  //Knothash h(5, { 3, 4, 1, 5 });
  Knothash h(256, { 187, 254, 0, 81, 169, 219, 1, 190, 19, 102, 255, 56, 46, 32, 2, 216 });
  h.hash();
  int n = h.final();
  std::cout << n << std::endl;
}


/*
//
// Created by root on 9/18/21.
//

#include "vio/bundle_adjustment.h"

// This file is part of VIO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (Robotics and Perception Group, University of Zurich, Switzerland).
//
// VIO is free software: you can redistribute it and/or modify it under the
// terms of the GNU General Public License as published by the Free Software
// Foundation, either version 3 of the License, or any later version.
//
// VIO is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

#include <vio/math_utils.h>
#include <boost/thread.hpp>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <g2o/solvers/structure_only/structure_only_solver.h>
#include <vio/bundle_adjustment.h>
#include <vio/frame.h>
#include <vio/feature.h>
#include <vio/point.h>
#include <vio/config.h>
#include <vio/map.h>

#define SCHUR_TRICK 1

namespace vio {
        void twoViewBA(
                FramePtr frame1,
                FramePtr frame2,
                double reproj_thresh,
                Map* map)
        {
            // scale reprojection threshold in pixels to unit plane
            reproj_thresh /= frame1->cam_->errorMultiplier2();

            // init g2o
            g2o::SparseOptimizer optimizer;
            setupG2o(&optimizer);

            list<EdgeContainerSE3> edges;
            size_t v_id = 0;

            // New Keyframe Vertex 1: This Keyframe is set to fixed!
            std::shared_ptr<g2oFrameSE2> v_frame1 = createG2oFrameSE2(frame1, v_id++, true);
            optimizer.addVertex(v_frame1);

            // New Keyframe Vertex 2
            std::shared_ptr<g2oFrameSE2> v_frame2 = createG2oFrameSE2(frame2, v_id++, false);
            optimizer.addVertex(v_frame2);

            // Create Point Vertices
            for(Features::iterator it_ftr=frame1->fts_.begin(); it_ftr!=frame1->fts_.end(); ++it_ftr)
            {
                std::shared_ptr<Point> pt = (*it_ftr)->point;
                if(pt == NULL)
                    continue;
                std::shared_ptr<g2oPoint> v_pt = createG2oPoint(pt->pos_, v_id++, false);
                optimizer.addVertex(v_pt);
                pt->v_pt_ = v_pt;
                std::shared_ptr<g2oEdgeSE3> e = createG2oEdgeSE3(v_frame1, v_pt, vk::project2d((*it_ftr)->f), true, reproj_thresh*Config::lobaRobustHuberWidth());
                optimizer.addEdge(e);
                edges.push_back(EdgeContainerSE3(e, frame1, *it_ftr)); // TODO feature now links to frame, so we can simplify edge container!

                // find at which index the second frame observes the point
                std::shared_ptr<Feature> ftr_frame2 = pt->findFrameRef(frame2);
                e = createG2oEdgeSE3(v_frame2, v_pt, vk::project2d(ftr_frame2->f), true, reproj_thresh*Config::lobaRobustHuberWidth());
                optimizer.addEdge(e);
                edges.push_back(EdgeContainerSE3(e, frame2, ftr_frame2));
            }

            // Optimization
            double init_error, final_error;
            runSparseBAOptimizer(&optimizer, Config::lobaNumIter(), init_error, final_error);
            printf("2-View BA: Error before/after = %f / %f\n", init_error, final_error);

            // Update Keyframe Positions
            frame1->T_f_w_.rotation_matrix() = v_frame1->estimate().rotation().toRotationMatrix();
            frame1->T_f_w_.translation() = v_frame1->estimate().translation();
            frame2->T_f_w_.rotation_matrix() = v_frame2->estimate().rotation().toRotationMatrix();
            frame2->T_f_w_.translation() = v_frame2->estimate().translation();

            // Update Mappoint Positions
            for(Features::iterator it=frame1->fts_.begin(); it!=frame1->fts_.end(); ++it)
            {
                if((*it)->point == NULL)
                    continue;
                (*it)->point->pos_ = (*it)->point->v_pt_->estimate();
                (*it)->point->v_pt_ = NULL;
            }

            // Find Mappoints with too large reprojection error
            const double reproj_thresh_squared = reproj_thresh*reproj_thresh;
            size_t n_incorrect_edges = 0;
            for(list<EdgeContainerSE3>::iterator it_e = edges.begin(); it_e != edges.end(); ++it_e)
                if(it_e->edge->chi2() > reproj_thresh_squared)
                {
                    if(it_e->feature->point != NULL)
                    {
                        map->safeDeletePoint(it_e->feature->point);
                        it_e->feature->point = NULL;
                    }
                    ++n_incorrect_edges;
                }

            printf("2-View BA: Wrong edges =  %zu\n", n_incorrect_edges);
        }

        void localBA(
                FramePtr center_kf,
                set<FramePtr>* core_kfs,
                Map* map,
                size_t& n_incorrect_edges_1,
                size_t& n_incorrect_edges_2,
                double& init_error,
                double& final_error)
        {

            // init g2o
            g2o::SparseOptimizer optimizer;
            setupG2o(&optimizer);

            list<EdgeContainerSE3> edges;
            set<std::shared_ptr<Point>> mps;
            list<FramePtr> neib_kfs;
            size_t v_id = 0;
            size_t n_mps = 0;
            size_t n_fix_kfs = 0;
            size_t n_var_kfs = 1;
            size_t n_edges = 0;
            n_incorrect_edges_1 = 0;
            n_incorrect_edges_2 = 0;

            // Add all core keyframes
            for(set<FramePtr>::iterator it_kf = core_kfs->begin(); it_kf != core_kfs->end(); ++it_kf)
            {
                std::shared_ptr<g2oFrameSE2> v_kf = createG2oFrameSE2(it_kf->get(), v_id++, false);
                (*it_kf)->v_kf_ = v_kf;
                ++n_var_kfs;
                assert(optimizer.addVertex(v_kf));

                // all points that the core keyframes observe are also optimized:
                for(Features::iterator it_pt=(*it_kf)->fts_.begin(); it_pt!=(*it_kf)->fts_.end(); ++it_pt)
                    if((*it_pt)->point != NULL)
                        mps.insert((*it_pt)->point);
            }

            // Now go throug all the points and add a measurement. Add a fixed neighbour
            // Keyframe if it is not in the set of core kfs
            double reproj_thresh_2 = Config::lobaThresh() / center_kf->cam_->errorMultiplier2();
            double reproj_thresh_1 = Config::poseOptimThresh() / center_kf->cam_->errorMultiplier2();
            double reproj_thresh_1_squared = reproj_thresh_1*reproj_thresh_1;
            for(set<Point*>::iterator it_pt = mps.begin(); it_pt!=mps.end(); ++it_pt)
            {
                // Create point vertex
                g2oPoint* v_pt = createG2oPoint((*it_pt)->pos_, v_id++, false);
                (*it_pt)->v_pt_ = v_pt;
                assert(optimizer.addVertex(v_pt));
                ++n_mps;

                // Add edges
                list<Feature*>::iterator it_obs=(*it_pt)->obs_.begin();
                while(it_obs!=(*it_pt)->obs_.end())
                {
                    Vector2d error = vk::project2d((*it_obs)->f) - vk::project2d((*it_obs)->frame->w2f((*it_pt)->pos_));

                    if((*it_obs)->frame->v_kf_ == NULL)
                    {
                        // frame does not have a vertex yet -> it belongs to the neib kfs and
                        // is fixed. create one:
                        std::shared_ptr<g2oFrameSE2> v_kf = createG2oFrameSE2((*it_obs)->frame, v_id++, true);
                        (*it_obs)->frame->v_kf_ = v_kf;
                        ++n_fix_kfs;
                        assert(optimizer.addVertex(v_kf));
                        neib_kfs.push_back((*it_obs)->frame);
                    }

                    // create edge
                    std::shared_ptr<g2oEdgeSE3> e = createG2oEdgeSE3((*it_obs)->frame->v_kf_, v_pt,
                                                     vk::project2d((*it_obs)->f),
                                                     true,
                                                     reproj_thresh_2*Config::lobaRobustHuberWidth(),
                                                     1.0 / (1<<(*it_obs)->level));
                    assert(optimizer.addEdge(e));
                    edges.push_back(EdgeContainerSE3(e, (*it_obs)->frame, *it_obs));
                    ++n_edges;
                    ++it_obs;
                }
            }

            // structure only
            g2o::StructureOnlySolver<3> structure_only_ba;
            g2o::OptimizableGraph::VertexContainer points;
            for (g2o::OptimizableGraph::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it)
            {
                g2o::OptimizableGraph::Vertex* v = static_cast<g2o::OptimizableGraph::Vertex*>(it->second);
                if (v->dimension() == 3 && v->edges().size() >= 2)
                    points.push_back(v);
            }
            structure_only_ba.calc(points, 10);

            // Optimization
            if(Config::lobaNumIter() > 0)
                runSparseBAOptimizer(&optimizer, Config::lobaNumIter(), init_error, final_error);

            // Update Keyframes
            for(set<FramePtr>::iterator it = core_kfs->begin(); it != core_kfs->end(); ++it)
            {
                (*it)->T_f_w_ = SE3( (*it)->v_kf_->estimate().rotation(),
                                     (*it)->v_kf_->estimate().translation());
                (*it)->v_kf_ = NULL;
            }

            for(list<FramePtr>::iterator it = neib_kfs.begin(); it != neib_kfs.end(); ++it)
                (*it)->v_kf_ = NULL;

            // Update Mappoints
            for(set<std::shared_ptr<Point>>::iterator it = mps.begin(); it != mps.end(); ++it)
            {
                (*it)->pos_ = (*it)->v_pt_->estimate();
                (*it)->v_pt_ = NULL;
            }

            // Remove Measurements with too large reprojection error
            double reproj_thresh_2_squared = reproj_thresh_2*reproj_thresh_2;
            for(list<EdgeContainerSE3>::iterator it = edges.begin(); it != edges.end(); ++it)
            {
                if(it->edge->chi2() > reproj_thresh_2_squared) //*(1<<it->feature_->level))
                {
                    map->removePtFrameRef(it->frame, it->feature);
                    ++n_incorrect_edges_2;
                }
            }

            // TODO: delete points and edges!
            init_error = sqrt(init_error)*center_kf->cam_->errorMultiplier2();
            final_error = sqrt(final_error)*center_kf->cam_->errorMultiplier2();
        }

        void globalBA(Map* map)
        {
            // init g2o
            g2o::SparseOptimizer optimizer;
            setupG2o(&optimizer);

            list<EdgeContainerSE3> edges;
            list< pair<FramePtr,Feature*> > incorrect_edges;

            // Go through all Keyframes
            size_t v_id = 0;
            double reproj_thresh_2 = Config::lobaThresh() / map->lastKeyframe()->cam_->errorMultiplier2();
            double reproj_thresh_1_squared = Config::poseOptimThresh() / map->lastKeyframe()->cam_->errorMultiplier2();
            reproj_thresh_1_squared *= reproj_thresh_1_squared;
            for(list<FramePtr>::iterator it_kf = map->keyframes_.begin();
                it_kf != map->keyframes_.end(); ++it_kf)
            {
                // New Keyframe Vertex
                std::shared_ptr<g2oFrameSE2> v_kf = createG2oFrameSE2(it_kf->get(), v_id++, false);
                (*it_kf)->v_kf_ = v_kf;
                optimizer.addVertex(v_kf);
                for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr)
                {
                    // for each keyframe add edges to all observed mapoints
                    std::shared_ptr<Point> mp = (*it_ftr)->point;
                    if(mp == NULL)
                        continue;
                    g2oPoint* v_mp = mp->v_pt_;
                    if(v_mp == NULL)
                    {
                        // mappoint-vertex doesn't exist yet. create a new one:
                        v_mp = createG2oPoint(mp->pos_, v_id++, false);
                        mp->v_pt_ = v_mp;
                        optimizer.addVertex(v_mp);
                    }

                    // Due to merging of mappoints it is possible that references in kfs suddenly
                    // have a very large reprojection error which may result in distorted results.
                    Vector2d error = vk::project2d((*it_ftr)->f) - vk::project2d((*it_kf)->w2f(mp->pos_));
                    if(error.squaredNorm() > reproj_thresh_1_squared)
                        incorrect_edges.push_back(pair<FramePtr,Feature*>(*it_kf, *it_ftr));
                    else
                    {
                        std::shared_ptr<g2oEdgeSE3> e = createG2oEdgeSE3(v_kf, v_mp, vk::project2d((*it_ftr)->f),
                                                         true,
                                                         reproj_thresh_2*Config::lobaRobustHuberWidth());

                        edges.push_back(EdgeContainerSE3(e, it_kf->get(), *it_ftr));
                        optimizer.addEdge(e);
                    }
                }
            }

            // Optimization
            double init_error=0.0, final_error=0.0;
            if(Config::lobaNumIter() > 0)
                runSparseBAOptimizer(&optimizer, Config::lobaNumIter(), init_error, final_error);

            // Update Keyframe and MapPoint Positions
            for(list<FramePtr>::iterator it_kf = map->keyframes_.begin();
                it_kf != map->keyframes_.end(); ++it_kf)
            {
                (*it_kf)->T_f_w_ = SE3( (*it_kf)->v_kf_->estimate().rotation(),
                                        (*it_kf)->v_kf_->estimate().translation());
                (*it_kf)->v_kf_ = NULL;
                for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr)
                {
                    std::shared_ptr<Point> mp = (*it_ftr)->point;
                    if(mp == NULL)
                        continue;
                    if(mp->v_pt_ == NULL)
                        continue;       // mp was updated before
                    mp->pos_ = mp->v_pt_->estimate();
                    mp->v_pt_ = NULL;
                }
            }

            // Remove Measurements with too large reprojection error
            for(list< pair<FramePtr,std::shared_ptr<Feature>> >::iterator it=incorrect_edges.begin();
                    it!=incorrect_edges.end(); ++it)
            map->removePtFrameRef(it->first.get(), it->second);

            double reproj_thresh_2_squared = reproj_thresh_2*reproj_thresh_2;
            for(list<EdgeContainerSE3>::iterator it = edges.begin(); it != edges.end(); ++it)
            {
                if(it->edge->chi2() > reproj_thresh_2_squared)
                {
                    map->removePtFrameRef(it->frame, it->feature);
                }
            }
        }

        void setupG2o(g2o::SparseOptimizer * optimizer)
        {
            // TODO: What's happening with all this HEAP stuff? Memory Leak?
            optimizer->setVerbose(false);

#if SCHUR_TRICK
            // solver
            g2o::BlockSolver_6_3::LinearSolverType* linearSolver;
            linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();
            //linearSolver = new g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType>();

            g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
            g2o::OptimizationAlgorithmLevenberg * solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
#else
            g2o::BlockSolverX::LinearSolverType * linearSolver;
  linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolverX::PoseMatrixType>();
  //linearSolver = new g2o::LinearSolverCSparse<g2o::BlockSolverX::PoseMatrixType>();
  g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver);
  g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
#endif

            solver->setMaxTrialsAfterFailure(5);
            optimizer->setAlgorithm(solver);

            // setup camera
            g2o::CameraParameters * cam_params = new g2o::CameraParameters(1.0, Vector2d(0.,0.), 0.);
            cam_params->setId(0);
            if (!optimizer->addParameter(cam_params)) {
                assert(false);
            }
        }

        void
        runSparseBAOptimizer(g2o::SparseOptimizer* optimizer,
                             unsigned int num_iter,
                             double& init_error, double& final_error)
        {
            optimizer->initializeOptimization();
            optimizer->computeActiveErrors();
            init_error = optimizer->activeChi2();
            optimizer->optimize(num_iter);
            final_error = optimizer->activeChi2();
        }

        g2oFrameSE3*
        createG2oFrameSE3(FramePtr frame, size_t id, bool fixed)
        {
            g2oFrameSE3* v = new g2oFrameSE3();
            v->setId(id);
            v->setFixed(fixed);
            v->setEstimate(g2o::SE3Quat(frame->T_f_w_.unit_quaternion(), frame->T_f_w_.translation()));
            return v;
        }

        g2oPoint*
        createG2oPoint(Vector3d pos,
                       size_t id,
                       bool fixed)
        {
            g2oPoint* v = new g2oPoint();
            v->setId(id);
#if SCHUR_TRICK
            v->setMarginalized(true);
#endif
            v->setFixed(fixed);
            v->setEstimate(pos);
            return v;
        }

        g2oEdgeSE3*
        createG2oEdgeSE3( g2oFrameSE3* v_frame,
                          g2oPoint* v_point,
                          const Vector2d& f_up,
                          bool robust_kernel,
                          double huber_width,
                          double weight)
        {
            g2oEdgeSE3* e = new g2oEdgeSE3();
            e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_point));
            e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_frame));
            e->setMeasurement(f_up);
            e->information() = weight * Eigen::Matrix2d::Identity(2,2);
            g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber();      // TODO: memory leak
            rk->setDelta(huber_width);
            e->setRobustKernel(rk);
            e->setParameterId(0, 0); //old: e->setId(v_point->id());
            return e;
        }

} // namespace VIO
*/


/*
 * Copyright 2020 Tier IV, Inc. All rights reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include <scene_module/crosswalk/util.h>
#include <utilization/util.h>

#include <cmath>
#include <string>
#include <vector>

#include <boost/assert.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

#define EIGEN_MPL2_ONLY
#include <Eigen/Core>
#include <Eigen/Geometry>

#include <autoware_perception_msgs/DynamicObjectArray.h>

namespace bg = boost::geometry;
using Point = bg::model::d2::point_xy<double>;
using Polygon = bg::model::polygon<Point, false>;
using Line = bg::model::linestring<Point>;

bool getBackwordPointFromBasePoint(
  const Eigen::Vector2d & line_point1, const Eigen::Vector2d & line_point2,
  const Eigen::Vector2d & base_point, const double backward_length, Eigen::Vector2d & output_point)
{
  Eigen::Vector2d line_vec = line_point2 - line_point1;
  Eigen::Vector2d backward_vec = backward_length * line_vec.normalized();
  output_point = base_point + backward_vec;
  return true;
}

bool insertTargetVelocityPoint(
  const autoware_planning_msgs::PathWithLaneId & input, const Polygon & polygon,
  const double & margin, const double & velocity, const PlannerData & planner_data,
  autoware_planning_msgs::PathWithLaneId & output, DebugData & debug_data,
  boost::optional<int> & first_stop_path_point_index)
{
  output = input;
  for (size_t i = 0; i < output.points.size() - 1; ++i) {
    const auto p0 = output.points.at(i).point.pose.position;
    const auto p1 = output.points.at(i + 1).point.pose.position;
    const Line line{{p0.x, p0.y}, {p1.x, p1.y}};
    std::vector<Point> collision_points;
    bg::intersection(polygon, line, collision_points);

    if (collision_points.empty()) continue;
    // -- debug code --
    for (const auto & cp : collision_points) {
      Eigen::Vector3d point3d(cp.x(), cp.y(), planner_data.current_pose.pose.position.z);
      debug_data.collision_points.push_back(point3d);
    }
    std::vector<Eigen::Vector3d> line3d;
    line3d.emplace_back(p0.x, p0.y, p0.z);
    line3d.emplace_back(p1.x, p1.y, p1.z);
    debug_data.collision_lines.push_back(line3d);
    // ----------------

    // check nearest collision point
    Point nearest_collision_point;
    double min_dist = 0.0;
    for (size_t j = 0; j < collision_points.size(); ++j) {
      double dist = bg::distance(Point(p0.x, p0.y), collision_points.at(j));
      if (j == 0 || dist < min_dist) {
        min_dist = dist;
        nearest_collision_point = collision_points.at(j);
        debug_data.nearest_collision_point =
          planning_utils::toRosPoint(collision_points.at(j), p0.z);
      }
    }

    // search target point index
    size_t insert_target_point_idx = 0;
    const double base_link2front = planner_data.base_link2front;
    double length_sum = 0;

    const double target_length = margin + base_link2front;
    Eigen::Vector2d point1, point2;
    point1 << nearest_collision_point.x(), nearest_collision_point.y();
    point2 << p0.x, p0.y;
    length_sum += (point2 - point1).norm();
    for (size_t j = i; 0 < j; --j) {
      if (target_length < length_sum) {
        insert_target_point_idx = j + 1;
        break;
      }
      const auto pj1 = output.points.at(j).point.pose.position;
      const auto pj2 = output.points.at(j - 1).point.pose.position;
      point1 << pj1.x, pj1.y;
      point2 << pj2.x, pj2.y;
      length_sum += (point2 - point1).norm();
    }

    // create target point
    Eigen::Vector2d target_point;
    autoware_planning_msgs::PathPointWithLaneId target_point_with_lane_id;
    getBackwordPointFromBasePoint(point2, point1, point2, length_sum - target_length, target_point);
    const int target_velocity_point_idx =
      std::max(static_cast<int>(insert_target_point_idx) - 1, 0);
    target_point_with_lane_id = output.points.at(target_velocity_point_idx);
    target_point_with_lane_id.point.pose.position.x = target_point.x();
    target_point_with_lane_id.point.pose.position.y = target_point.y();
    if (insert_target_point_idx > 0) {
      //calculate z-position of the target point (Internal division point of point1/point2)
      //if insert_target_point_idx is zero, use z-position of target_velocity_point_idx
      const double internal_div_ratio =
        (point1 - target_point).norm() /
        ((point1 - target_point).norm() + (point2 - target_point).norm());
      target_point_with_lane_id.point.pose.position.z =
        output.points.at(insert_target_point_idx).point.pose.position.z * (1 - internal_div_ratio) +
        output.points.at(insert_target_point_idx - 1).point.pose.position.z * internal_div_ratio;
    }

    if ((point1 - point2).norm() > 1.0E-3) {
      const double yaw = std::atan2(point1.y() - point2.y(), point1.x() - point2.x());
      target_point_with_lane_id.point.pose.orientation = planning_utils::getQuaternionFromYaw(yaw);
    }
    target_point_with_lane_id.point.twist.linear.x = velocity;
    if (velocity == 0.0 && target_velocity_point_idx < first_stop_path_point_index) {
      first_stop_path_point_index = target_velocity_point_idx;
      // -- debug code --
      debug_data.first_stop_pose = target_point_with_lane_id.point.pose;
      // ----------------
    }
    // -- debug code --
    if (velocity == 0.0)
      debug_data.stop_poses.push_back(target_point_with_lane_id.point.pose);
    else
      debug_data.slow_poses.push_back(target_point_with_lane_id.point.pose);
    // ----------------

    // insert target point
    output.points.insert(
      output.points.begin() + insert_target_point_idx, target_point_with_lane_id);

    // insert 0 velocity after target point
    for (size_t j = insert_target_point_idx; j < output.points.size(); ++j)
      output.points.at(j).point.twist.linear.x =
        std::min(velocity, output.points.at(j).point.twist.linear.x);
    return true;
  }
  return false;
}


/*
 * Copyright (c) 2017 Louis Langholtz https://github.com/louis-langholtz/Box2D
 *
 * This software is provided 'as-is', without any express or implied
 * warranty. In no event will the authors be held liable for any damages
 * arising from the use of this software.
 *
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 *
 * 1. The origin of this software must not be misrepresented; you must not
 *    claim that you wrote the original software. If you use this software
 *    in a product, an acknowledgment in the product documentation would be
 *    appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 *    misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */

/**
 * @file
 * @brief Units file.
 */

#ifndef Units_hpp
#define Units_hpp

#include <Box2D/Common/RealNum.hpp>
#include <Box2D/Common/Templates.hpp>

// #define USE_BOOST_UNITS
#ifdef USE_BOOST_UNITS
#include <boost/units/io.hpp>
#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/si/time.hpp>
#include <boost/units/systems/si/velocity.hpp>
#include <boost/units/systems/si/acceleration.hpp>
#include <boost/units/systems/si/frequency.hpp>
#include <boost/units/systems/si/velocity.hpp>
#include <boost/units/systems/si/mass.hpp>
#include <boost/units/systems/si/momentum.hpp>
#include <boost/units/systems/si/inverse_mass.hpp>
#include <boost/units/systems/si/area.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
#include <boost/units/systems/si/angular_momentum.hpp>
#include <boost/units/systems/si/angular_velocity.hpp>
#include <boost/units/systems/si/angular_acceleration.hpp>
#include <boost/units/systems/si/second_moment_of_area.hpp>
#include <boost/units/systems/si/surface_density.hpp>
#include <boost/units/systems/si/moment_of_inertia.hpp>
#include <boost/units/systems/si/inverse_moment_of_inertia.hpp>
#include <boost/units/systems/si/force.hpp>
#include <boost/units/systems/si/torque.hpp>
#include <boost/units/systems/angle/degrees.hpp>
#endif

namespace box2d
{
#ifdef USE_BOOST_UNITS

    using Time = boost::units::quantity<boost::units::si::time, Real>;
    constexpr auto Second = Time{boost::units::si::second * Real{1}};

    using Frequency = boost::units::quantity<boost::units::si::frequency, Real>;
    constexpr auto Hertz = Frequency{boost::units::si::hertz * Real{1}};

    using Length = boost::units::quantity<boost::units::si::length, Real>;
    constexpr auto Meter = Length{boost::units::si::meter * Real{1}};

    using LinearVelocity = boost::units::quantity<boost::units::si::velocity, Real>;
    constexpr auto MeterPerSecond = LinearVelocity{boost::units::si::meter_per_second * Real{1}};

    using LinearAcceleration = boost::units::quantity<boost::units::si::acceleration, Real>;
    constexpr auto MeterPerSquareSecond = LinearAcceleration{boost::units::si::meter_per_second_squared * Real{1}};

    using Mass = boost::units::quantity<boost::units::si::mass, Real>;
    constexpr auto Kilogram = Mass{boost::units::si::kilogram * Real{1}};

    using InvMass = boost::units::quantity<boost::units::si::inverse_mass, Real>;

    using Area = boost::units::quantity<boost::units::si::area, Real>;
    constexpr auto SquareMeter = Area{boost::units::si::square_meter * Real{1}};

    using Density = boost::units::quantity<boost::units::si::surface_density, Real>;
    constexpr auto KilogramPerSquareMeter = Density{boost::units::si::kilogram_per_square_meter * Real{1}};

    using Angle = boost::units::quantity<boost::units::si::plane_angle, Real>;
    constexpr auto Radian = Angle{boost::units::si::radian * Real{1}};
    constexpr auto Degree = Angle{boost::units::degree::degree * Real{1}};
    constexpr auto SquareRadian = Radian * Radian;

    using AngularVelocity = boost::units::quantity<boost::units::si::angular_velocity, Real>;
    constexpr auto RadianPerSecond = AngularVelocity{boost::units::si::radian_per_second * Real{1}};
    constexpr auto DegreePerSecond = AngularVelocity{RadianPerSecond * Degree / Radian};

    using AngularAcceleration = boost::units::quantity<boost::units::si::angular_acceleration, Real>;
    constexpr auto RadianPerSquareSecond = Radian / (Second * Second);

    using Force = boost::units::quantity<boost::units::si::force, Real>;
    constexpr auto Newton = Force{boost::units::si::newton * Real{1}};

    using Torque = boost::units::quantity<boost::units::si::torque, Real>;
    constexpr auto NewtonMeter = Torque{boost::units::si::newton_meter * Real{1}};

    using SecondMomentOfArea = boost::units::quantity<boost::units::si::second_moment_of_area, Real>;

    using RotInertia = boost::units::quantity<boost::units::si::moment_of_inertia, Real>;
    using InvRotInertia = boost::units::quantity<boost::units::si::inverse_moment_of_inertia, Real>;

    using Momentum = boost::units::quantity<boost::units::si::momentum, Real>;
    constexpr auto NewtonSecond = Newton * Second;

    /// @brief Angular momentum.
    /// @note Units of L^2 M T^-1 QP^-1.
    using AngularMomentum = boost::units::quantity<boost::units::si::angular_momentum, Real>;

#else // USE_BOOST_UNITS

    using Time = Real;
    constexpr auto Second = Real{1};

    using Frequency = Real;
    constexpr auto Hertz = Real{1};

    using Length = Real;
    constexpr auto Meter = Real{1};

    using LinearVelocity = Real;
    constexpr auto MeterPerSecond = Real{1};

    using LinearAcceleration = Real;
    constexpr auto MeterPerSquareSecond = Real{1};

    using Mass = Real;
    constexpr auto Kilogram = Real{1};

    using InvMass = Real;

    using Area = Real;
    constexpr auto SquareMeter = Real{1};

    using Density = Real;
    constexpr auto KilogramPerSquareMeter = Real{1};

    using Angle = Real;
    constexpr auto Radian = Real{1};
    constexpr auto Degree = Pi / Real{180};
    constexpr auto SquareRadian = Radian * Radian;

    using AngularVelocity = Real;
    constexpr auto RadianPerSecond = Real{1};
    constexpr auto DegreePerSecond = Degree;

    using AngularAcceleration = Real;
    constexpr auto RadianPerSquareSecond = Real{1};

    using Force = Real;
    constexpr auto Newton = Real{1};

    using Torque = Real;
    constexpr auto NewtonMeter = Real{1};

    using SecondMomentOfArea = Real;

    using RotInertia = Real;
    using InvRotInertia = Real;

    using Momentum = Real;
    constexpr auto NewtonSecond = Real{1};

    using AngularMomentum = Real;

#endif // USE_BOOST_UNITS

    constexpr inline Real StripUnit(const Real value)
    {
        return value;
    }

#ifdef USE_BOOST_UNITS

    constexpr inline Real StripUnit(const Angle value)
    {
        return Real{value / Radian};
    }

    constexpr inline Real StripUnit(const Length value)
    {
        return Real{value / Meter};
    }

    constexpr inline Real StripUnit(const Area value)
    {
        return Real{value / SquareMeter};
    }

    constexpr inline Real StripUnit(const Mass value)
    {
        // InvMass has units of M^-1
        return Real{value / Kilogram};
    }

    constexpr inline Real StripUnit(const InvMass value)
    {
        // InvMass has units of M^-1
        return Real{value * Kilogram};
    }

    constexpr inline Real StripUnit(const RotInertia value)
    {
        return Real{value * SquareRadian / (SquareMeter * Kilogram)};
    }

    constexpr inline Real StripUnit(const InvRotInertia value)
    {
        // InvRotInertia has units of L^-2 M^-1 QP^2
        return Real{value * SquareMeter * Kilogram / SquareRadian};
    }

    constexpr inline Real StripUnit(const Momentum value)
    {
        // Momentum has units of M L T^-1
        return Real{value * Second / (Kilogram * Meter)};
    }

    constexpr inline Real StripUnit(const LinearVelocity value)
    {
        return Real{value / MeterPerSecond};
    }

    constexpr inline Real StripUnit(const AngularVelocity value)
    {
        return Real{value / RadianPerSecond};
    }

    constexpr inline Real StripUnit(const Density value)
    {
        return Real{value / KilogramPerSquareMeter};
    }

    constexpr inline Real StripUnit(const Force value)
    {
        // Force has units of Newtons - which are M L T^2
        return Real{value / Newton};
    }

    constexpr inline Real StripUnit(const Torque value)
    {
        return Real{value / NewtonMeter};
    }

    template <>
    constexpr Angle GetInvalid() noexcept
    {
        return GetInvalid<Real>() * Radian;
    }

    template <>
    constexpr inline bool IsValid(const Angle& x) noexcept
    {
        return IsValid(Real{x / Radian});
    }

    template <>
    constexpr Frequency GetInvalid() noexcept
    {
        return GetInvalid<Real>() * Hertz;
    }

    template <>
    constexpr inline bool IsValid(const Frequency& x) noexcept
    {
        return IsValid(Real{x / Hertz});
    }

    template <>
    constexpr AngularVelocity GetInvalid() noexcept
    {
        return GetInvalid<Real>() * RadianPerSecond;
    }

    template <>
    constexpr inline bool IsValid(const AngularVelocity& x) noexcept
    {
        return IsValid(Real{x / RadianPerSecond});
    }

    template <>
    constexpr Time GetInvalid() noexcept
    {
        return GetInvalid<Real>() * Second;
    }

    template <>
    constexpr inline bool IsValid(const Time& x) noexcept
    {
        return IsValid(Real{x / Second});
    }

    template <>
    constexpr Length GetInvalid() noexcept
    {
        return GetInvalid<Real>() * Meter;
    }

    template <>
    constexpr inline bool IsValid(const Length& x) noexcept
    {
        return IsValid(Real{x / Meter});
    }

    template <>
    constexpr Mass GetInvalid() noexcept
    {
        return GetInvalid<Real>() * Kilogram;
    }

    template <>
    constexpr inline bool IsValid(const Mass& x) noexcept
    {
        return IsValid(Real{x / Kilogram});
    }

    template <>
    constexpr InvMass GetInvalid() noexcept
    {
        return GetInvalid<Real>() / Kilogram;
    }

    template <>
    constexpr inline bool IsValid(const InvMass& x) noexcept
    {
        return IsValid(Real{x * Kilogram});
    }

    template <>
    constexpr Momentum GetInvalid() noexcept
    {
        return GetInvalid<Real>() * Kilogram * MeterPerSecond;
    }

    template <>
    constexpr inline bool IsValid(const Momentum& x) noexcept
    {
        return IsValid(Real{x / (Kilogram * MeterPerSecond)});
    }

    template <>
    constexpr Force GetInvalid() noexcept
    {
        return GetInvalid<Real>() * Newton;
    }

    template <>
    constexpr inline bool IsValid(const Force& x) noexcept
    {
        return IsValid(Real{x / Newton});
    }

    template <>
    constexpr Torque GetInvalid() noexcept
    {
        return GetInvalid<Real>() * NewtonMeter;
    }

    template <>
    constexpr inline bool IsValid(const Torque& x) noexcept
    {
        return IsValid(Real{x / NewtonMeter});
    }

    template <>
    constexpr LinearVelocity GetInvalid() noexcept
    {
        return GetInvalid<Real>() * MeterPerSecond;
    }

    template <>
    constexpr inline bool IsValid(const LinearVelocity& x) noexcept
    {
        return IsValid(Real{x / MeterPerSecond});
    }

    template <>
    constexpr LinearAcceleration GetInvalid() noexcept
    {
        return GetInvalid<Real>() * MeterPerSquareSecond;
    }

    template <>
    constexpr inline bool IsValid(const LinearAcceleration& x) noexcept
    {
        return IsValid(Real{x / MeterPerSquareSecond});
    }

    template <>
    constexpr AngularAcceleration GetInvalid() noexcept
    {
        return GetInvalid<Real>() * RadianPerSquareSecond;
    }

    template <>
    constexpr inline bool IsValid(const AngularAcceleration& x) noexcept
    {
        return IsValid(Real{x / RadianPerSquareSecond});
    }

    template <>
    constexpr RotInertia GetInvalid() noexcept
    {
        // RotInertia is L^2  M    QP^-2
        return GetInvalid<Real>() * SquareMeter * Kilogram / SquareRadian;
    }

    template <>
    constexpr inline bool IsValid(const RotInertia& value) noexcept
    {
        return IsValid(Real{value / (SquareMeter * Kilogram / SquareRadian)});
    }

#endif

} // namespace box2d

#endif /* Units_hpp */


/*
Copyright 2010 Intel Corporation

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

//layout_database.hpp
#ifndef BOOST_POLYGON_TUTORIAL_LAYOUT_DATABASE_HPP
#define BOOST_POLYGON_TUTORIAL_LAYOUT_DATABASE_HPP
#include <boost/polygon/polygon.hpp>
#include <map>
#include "layout_rectangle.hpp"

typedef std::map<std::string, boost::polygon::polygon_90_set_data<int> > layout_database;

//map the layout rectangle data type to the boost::polygon::rectangle_concept
namespace boost { namespace polygon{
  template <>
  struct rectangle_traits<layout_rectangle> {
    typedef int coordinate_type;
    typedef interval_data<int> interval_type;
    static inline interval_type get(const layout_rectangle& rectangle, orientation_2d orient) {
      if(orient == HORIZONTAL)
        return interval_type(rectangle.xl, rectangle.xh);
      return interval_type(rectangle.yl, rectangle.yh);
    }
  };

  template <>
  struct geometry_concept<layout_rectangle> { typedef rectangle_concept type; };
}}

//insert layout rectangles into a layout database
inline void populate_layout_database(layout_database& layout, std::vector<layout_rectangle>& rects) {
  for(std::size_t i = 0; i < rects.size(); ++i) {
    layout[rects[i].layer].insert(rects[i]);
  }
}
#endif


//          Copyright (C) 2012, Michele Caini.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

//          Two Graphs Common Spanning Trees Algorithm
//      Based on academic article of Mint, Read and Tarjan
//     Efficient Algorithm for Common Spanning Tree Problem
// Electron. Lett., 28 April 1983, Volume 19, Issue 9, p.346-347

#ifndef BOOST_GRAPH_TWO_GRAPHS_COMMON_SPANNING_TREES_HPP
#define BOOST_GRAPH_TWO_GRAPHS_COMMON_SPANNING_TREES_HPP

#include <boost/config.hpp>

#include <boost/bimap.hpp>
#include <boost/type_traits.hpp>
#include <boost/concept/requires.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/undirected_dfs.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <vector>
#include <stack>
#include <map>

namespace boost
{

namespace detail {

  template
    <
      typename TreeMap,
      typename PredMap,
      typename DistMap,
      typename LowMap,
      typename Buffer
    >
  struct bridges_visitor: public default_dfs_visitor
  {
    bridges_visitor(
        TreeMap tree,
        PredMap pred,
        DistMap dist,
        LowMap low,
        Buffer& buffer
      ): mTree(tree), mPred(pred), mDist(dist), mLow(low), mBuffer(buffer)
    { mNum = -1; }

    template <typename Vertex, typename Graph>
    void initialize_vertex(const Vertex& u, const Graph& g)
    {
      put(mPred, u, u);
      put(mDist, u, -1);
    }

    template <typename Vertex, typename Graph>
    void discover_vertex(const Vertex& u, const Graph& g)
    {
      put(mDist, u, ++mNum);
      put(mLow, u, get(mDist, u));
    }

    template <typename Edge, typename Graph>
    void tree_edge(const Edge& e, const Graph& g)
    {
      put(mPred, target(e, g), source(e, g));
      put(mTree, target(e, g), e);
    }

    template <typename Edge, typename Graph>
    void back_edge(const Edge& e, const Graph& g)
    {
      put(mLow, source(e, g),
        (std::min)(get(mLow, source(e, g)), get(mDist, target(e, g))));
    }

    template <typename Vertex, typename Graph>
    void finish_vertex(const Vertex& u, const Graph& g)
    {
      Vertex parent = get(mPred, u);
      if(get(mLow, u) > get(mDist, parent))
        mBuffer.push(get(mTree, u));
      put(mLow, parent,
        (std::min)(get(mLow, parent), get(mLow, u)));
    }

    TreeMap mTree;
    PredMap mPred;
    DistMap mDist;
    LowMap mLow;
    Buffer& mBuffer;
    int mNum;
  };

  template <typename Buffer>
  struct cycle_finder: public base_visitor< cycle_finder<Buffer> >
  {
    typedef on_back_edge event_filter;
    cycle_finder(): mBuffer(0) { }
    cycle_finder(Buffer* buffer)
      : mBuffer(buffer) { }
    template <typename Edge, typename Graph>
    void operator()(const Edge& e, const Graph& g)
      {
        if(mBuffer)
          mBuffer->push(e);
      }
    Buffer* mBuffer;
  };

  template <typename DeletedMap>
  struct deleted_edge_status
  {
    deleted_edge_status() { }
    deleted_edge_status(DeletedMap map): mMap(map) { }
    template <typename Edge>
    bool operator()(const Edge& e) const
      { return (!get(mMap, e)); }
    DeletedMap mMap;
  };

  template <typename InLMap>
  struct inL_edge_status
  {
    inL_edge_status() { }
    inL_edge_status(InLMap map): mMap(map) { }
    template <typename Edge>
    bool operator()(const Edge& e) const
      { return get(mMap, e); }
    InLMap mMap;
  };

  template <
    typename Graph,
    typename Func,
    typename Seq,
    typename Map
  >
  void rec_two_graphs_common_spanning_trees
    (
      const Graph& iG,
      bimap<
          bimaps::set_of<int>,
          bimaps::set_of< typename graph_traits<Graph>::edge_descriptor >
        > iG_bimap,
      Map aiG_inL,
      Map diG,
      const Graph& vG,
      bimap<
          bimaps::set_of<int>,
          bimaps::set_of< typename graph_traits<Graph>::edge_descriptor >
        > vG_bimap,
      Map avG_inL,
      Map dvG,
      Func func,
      Seq inL
    )
  {
    typedef graph_traits<Graph> GraphTraits;

    typedef typename GraphTraits::vertex_descriptor vertex_descriptor;
    typedef typename GraphTraits::edge_descriptor edge_descriptor;

    typedef typename Seq::size_type seq_size_type;

    int edges = num_vertices(iG) - 1;
//
//  [ Michele Caini ]
//
//  Using the condition (edges != 0) leads to the accidental submission of
//    sub-graphs ((V-1+1)-fake-tree, named here fat-tree).
//  Remove this condition is a workaround for the problem of fat-trees.
//  Please do not add that condition, even if it improves performance.
//
//  Here is proposed the previous guard (that was wrong):
//     for(seq_size_type i = 0; (i < inL.size()) && (edges != 0); ++i)
//
    {
      for(seq_size_type i = 0; i < inL.size(); ++i)
        if(inL[i])
          --edges;

      if(edges < 0)
        return;
    }

    bool is_tree = (edges == 0);
    if(is_tree) {
      func(inL);
    } else {
      std::map<vertex_descriptor, default_color_type> vertex_color;
      std::map<edge_descriptor, default_color_type> edge_color;

      std::stack<edge_descriptor> iG_buf, vG_buf;
      bool found = false;

      seq_size_type m;
      for(seq_size_type j = 0; j < inL.size() && !found; ++j) {
        if(!inL[j]
            && !get(diG, iG_bimap.left.at(j))
            && !get(dvG, vG_bimap.left.at(j)))
        {
          put(aiG_inL, iG_bimap.left.at(j), true);
          put(avG_inL, vG_bimap.left.at(j), true);

          undirected_dfs(
              make_filtered_graph(iG,
                detail::inL_edge_status< associative_property_map<
                  std::map<edge_descriptor, bool> > >(aiG_inL)),
              make_dfs_visitor(
                detail::cycle_finder< std::stack<edge_descriptor> > (&iG_buf)),
              associative_property_map<
                std::map<vertex_descriptor, default_color_type> >(vertex_color),
              associative_property_map<
                std::map<edge_descriptor, default_color_type> >(edge_color)
            );
          undirected_dfs(
              make_filtered_graph(vG,
                detail::inL_edge_status< associative_property_map<
                  std::map<edge_descriptor, bool> > >(avG_inL)),
              make_dfs_visitor(
                detail::cycle_finder< std::stack<edge_descriptor> > (&vG_buf)),
              associative_property_map<
                std::map<vertex_descriptor, default_color_type> >(vertex_color),
              associative_property_map<
                std::map<edge_descriptor, default_color_type> >(edge_color)
            );

          if(iG_buf.empty() && vG_buf.empty()) {
            inL[j] = true;
            found = true;
            m = j;
          } else {
            while(!iG_buf.empty()) iG_buf.pop();
            while(!vG_buf.empty()) vG_buf.pop();
            put(aiG_inL, iG_bimap.left.at(j), false);
            put(avG_inL, vG_bimap.left.at(j), false);
          }
        }
      }

      if(found) {

        std::stack<edge_descriptor> iG_buf_copy, vG_buf_copy;
        for(seq_size_type j = 0; j < inL.size(); ++j) {
          if(!inL[j]
              && !get(diG, iG_bimap.left.at(j))
              && !get(dvG, vG_bimap.left.at(j)))
          {

            put(aiG_inL, iG_bimap.left.at(j), true);
            put(avG_inL, vG_bimap.left.at(j), true);

            undirected_dfs(
                make_filtered_graph(iG,
                  detail::inL_edge_status< associative_property_map<
                    std::map<edge_descriptor, bool> > >(aiG_inL)),
                make_dfs_visitor(
                  detail::cycle_finder<
                    std::stack<edge_descriptor> > (&iG_buf)),
               associative_property_map< std::map<
                  vertex_descriptor, default_color_type> >(vertex_color),
                associative_property_map<
                  std::map<edge_descriptor, default_color_type> >(edge_color)
              );
            undirected_dfs(
                make_filtered_graph(vG,
                  detail::inL_edge_status< associative_property_map<
                    std::map<edge_descriptor, bool> > >(avG_inL)),
                make_dfs_visitor(
                  detail::cycle_finder<
                    std::stack<edge_descriptor> > (&vG_buf)),
                associative_property_map< std::map<
                  vertex_descriptor, default_color_type> >(vertex_color),
                associative_property_map<
                  std::map<edge_descriptor, default_color_type> >(edge_color)
              );

            if(!iG_buf.empty() || !vG_buf.empty()) {
              while(!iG_buf.empty()) iG_buf.pop();
              while(!vG_buf.empty()) vG_buf.pop();
              put(diG, iG_bimap.left.at(j), true);
              put(dvG, vG_bimap.left.at(j), true);
              iG_buf_copy.push(iG_bimap.left.at(j));
              vG_buf_copy.push(vG_bimap.left.at(j));
            }

            put(aiG_inL, iG_bimap.left.at(j), false);
            put(avG_inL, vG_bimap.left.at(j), false);
          }
        }

        // REC
        detail::rec_two_graphs_common_spanning_trees<Graph, Func, Seq, Map>
          (iG, iG_bimap, aiG_inL, diG, vG, vG_bimap, aiG_inL, dvG, func, inL);

        while(!iG_buf_copy.empty()) {
          put(diG, iG_buf_copy.top(), false);
          put(dvG, vG_bimap.left.at(
            iG_bimap.right.at(iG_buf_copy.top())), false);
          iG_buf_copy.pop();
        }
        while(!vG_buf_copy.empty()) {
          put(dvG, vG_buf_copy.top(), false);
          put(diG, iG_bimap.left.at(
            vG_bimap.right.at(vG_buf_copy.top())), false);
          vG_buf_copy.pop();
        }

        inL[m] = false;
        put(aiG_inL, iG_bimap.left.at(m), false);
        put(avG_inL, vG_bimap.left.at(m), false);

        put(diG, iG_bimap.left.at(m), true);
        put(dvG, vG_bimap.left.at(m), true);

        std::map<vertex_descriptor, edge_descriptor> tree_map;
        std::map<vertex_descriptor, vertex_descriptor> pred_map;
        std::map<vertex_descriptor, int> dist_map, low_map;

        detail::bridges_visitor<
            associative_property_map<
                std::map<vertex_descriptor, edge_descriptor>
              >,
            associative_property_map<
                std::map<vertex_descriptor, vertex_descriptor>
              >,
            associative_property_map< std::map<vertex_descriptor, int> >,
            associative_property_map< std::map<vertex_descriptor, int> >,
            std::stack<edge_descriptor>
          >
        iG_vis(
            associative_property_map<
              std::map< vertex_descriptor, edge_descriptor> >(tree_map),
            associative_property_map<
              std::map< vertex_descriptor, vertex_descriptor> >(pred_map),
            associative_property_map<
              std::map< vertex_descriptor, int> >(dist_map),
            associative_property_map<
              std::map< vertex_descriptor, int> >(low_map),
            iG_buf
          ),
        vG_vis(
            associative_property_map<
              std::map< vertex_descriptor, edge_descriptor> >(tree_map),
            associative_property_map<
              std::map< vertex_descriptor, vertex_descriptor> >(pred_map),
            associative_property_map<
              std::map< vertex_descriptor, int> >(dist_map),
            associative_property_map<
              std::map< vertex_descriptor, int> >(low_map),
            vG_buf
          );

        undirected_dfs(make_filtered_graph(iG,
              detail::deleted_edge_status< associative_property_map<
              std::map<edge_descriptor, bool> > >(diG)),
            iG_vis,
            associative_property_map<
              std::map<vertex_descriptor, default_color_type> >(vertex_color),
            associative_property_map<
              std::map<edge_descriptor, default_color_type> >(edge_color)
          );
        undirected_dfs(make_filtered_graph(vG,
              detail::deleted_edge_status< associative_property_map<
              std::map<edge_descriptor, bool> > >(dvG)),
            vG_vis,
            associative_property_map<
              std::map<vertex_descriptor, default_color_type> >(vertex_color),
            associative_property_map<
              std::map<edge_descriptor, default_color_type> >(edge_color)
          );

        found = false;
        std::stack<edge_descriptor> iG_buf_tmp, vG_buf_tmp;
        while(!iG_buf.empty() && !found) {
          if(!inL[iG_bimap.right.at(iG_buf.top())]) {
            put(aiG_inL, iG_buf.top(), true);
            put(avG_inL, vG_bimap.left.at(
              iG_bimap.right.at(iG_buf.top())), true);

            undirected_dfs(
                make_filtered_graph(iG,
                  detail::inL_edge_status< associative_property_map<
                    std::map<edge_descriptor, bool> > >(aiG_inL)),
                make_dfs_visitor(
                  detail::cycle_finder<
                    std::stack<edge_descriptor> > (&iG_buf_tmp)),
                associative_property_map<
                  std::map<
                    vertex_descriptor, default_color_type> >(vertex_color),
                associative_property_map<
                  std::map<edge_descriptor, default_color_type> >(edge_color)
              );
            undirected_dfs(
                make_filtered_graph(vG,
                  detail::inL_edge_status< associative_property_map<
                    std::map<edge_descriptor, bool> > >(avG_inL)),
                make_dfs_visitor(
                  detail::cycle_finder<
                    std::stack<edge_descriptor> > (&vG_buf_tmp)),
                associative_property_map<
                  std::map<
                    vertex_descriptor, default_color_type> >(vertex_color),
                associative_property_map<
                  std::map<edge_descriptor, default_color_type> >(edge_color)
              );

            if(!iG_buf_tmp.empty() || !vG_buf_tmp.empty()) {
              found = true;
            } else {
              while(!iG_buf_tmp.empty()) iG_buf_tmp.pop();
              while(!vG_buf_tmp.empty()) vG_buf_tmp.pop();
              iG_buf_copy.push(iG_buf.top());
            }

            put(aiG_inL, iG_buf.top(), false);
            put(avG_inL, vG_bimap.left.at(
              iG_bimap.right.at(iG_buf.top())), false);
          }
          iG_buf.pop();
        }
        while(!vG_buf.empty() && !found) {
          if(!inL[vG_bimap.right.at(vG_buf.top())]) {
            put(avG_inL, vG_buf.top(), true);
            put(aiG_inL, iG_bimap.left.at(
              vG_bimap.right.at(vG_buf.top())), true);

            undirected_dfs(
                make_filtered_graph(iG,
                  detail::inL_edge_status< associative_property_map<
                    std::map<edge_descriptor, bool> > >(aiG_inL)),
                make_dfs_visitor(
                  detail::cycle_finder<
                    std::stack<edge_descriptor> > (&iG_buf_tmp)),
                associative_property_map<
                  std::map<
                    vertex_descriptor, default_color_type> >(vertex_color),
                associative_property_map<
                  std::map<edge_descriptor, default_color_type> >(edge_color)
              );
            undirected_dfs(
                make_filtered_graph(vG,
                  detail::inL_edge_status< associative_property_map<
                    std::map<edge_descriptor, bool> > >(avG_inL)),
                make_dfs_visitor(
                  detail::cycle_finder<
                    std::stack<edge_descriptor> > (&vG_buf_tmp)),
                associative_property_map<
                  std::map<
                    vertex_descriptor, default_color_type> >(vertex_color),
                associative_property_map<
                  std::map<edge_descriptor, default_color_type> >(edge_color)
              );

            if(!iG_buf_tmp.empty() || !vG_buf_tmp.empty()) {
              found = true;
            } else {
              while(!iG_buf_tmp.empty()) iG_buf_tmp.pop();
              while(!vG_buf_tmp.empty()) vG_buf_tmp.pop();
              vG_buf_copy.push(vG_buf.top());
            }

            put(avG_inL, vG_buf.top(), false);
            put(aiG_inL, iG_bimap.left.at(
              vG_bimap.right.at(vG_buf.top())), false);
          }
          vG_buf.pop();
        }

        if(!found) {

          while(!iG_buf_copy.empty()) {
            inL[iG_bimap.right.at(iG_buf_copy.top())] = true;
            put(aiG_inL, iG_buf_copy.top(), true);
            put(avG_inL, vG_bimap.left.at(
              iG_bimap.right.at(iG_buf_copy.top())), true);
            iG_buf.push(iG_buf_copy.top());
            iG_buf_copy.pop();
          }
          while(!vG_buf_copy.empty()) {
            inL[vG_bimap.right.at(vG_buf_copy.top())] = true;
            put(avG_inL, vG_buf_copy.top(), true);
            put(aiG_inL, iG_bimap.left.at(
              vG_bimap.right.at(vG_buf_copy.top())), true);
            vG_buf.push(vG_buf_copy.top());
            vG_buf_copy.pop();
          }

          // REC
          detail::rec_two_graphs_common_spanning_trees<
              Graph, Func, Seq, Map>
            (iG, iG_bimap, aiG_inL, diG, vG, vG_bimap, aiG_inL, dvG, func, inL);

          while(!iG_buf.empty()) {
            inL[iG_bimap.right.at(iG_buf.top())] = false;
            put(aiG_inL, iG_buf.top(), false);
            put(avG_inL, vG_bimap.left.at(
              iG_bimap.right.at(iG_buf.top())), false);
            iG_buf.pop();
          }
          while(!vG_buf.empty()) {
            inL[vG_bimap.right.at(vG_buf.top())] = false;
            put(avG_inL, vG_buf.top(), false);
            put(aiG_inL, iG_bimap.left.at(
              vG_bimap.right.at(vG_buf.top())), false);
            vG_buf.pop();
          }

        }

        put(diG, iG_bimap.left.at(m), false);
        put(dvG, vG_bimap.left.at(m), false);

      }
    }
  }

} // namespace detail

template <typename Coll, typename Seq>
struct tree_collector
{

public:
  BOOST_CONCEPT_ASSERT((BackInsertionSequence<Coll>));
  BOOST_CONCEPT_ASSERT((RandomAccessContainer<Seq>));
  BOOST_CONCEPT_ASSERT((CopyConstructible<Seq>));

  typedef typename Coll::value_type coll_value_type;
  typedef typename Seq::value_type seq_value_type;

  BOOST_STATIC_ASSERT((is_same<coll_value_type, Seq>::value));
  BOOST_STATIC_ASSERT((is_same<seq_value_type, bool>::value));

  tree_collector(Coll& seqs): mSeqs(seqs) { }

  inline void operator()(Seq seq)
    { mSeqs.push_back(seq); }

private:
  Coll& mSeqs;

};

template <
  typename Graph,
  typename Order,
  typename Func,
  typename Seq
>
BOOST_CONCEPT_REQUIRES(
  ((RandomAccessContainer<Order>))
  ((IncidenceGraphConcept<Graph>))
  ((UnaryFunction<Func, void, Seq>))
  ((Mutable_RandomAccessContainer<Seq>))
  ((VertexAndEdgeListGraphConcept<Graph>)),
  (void)
)
two_graphs_common_spanning_trees
  (
    const Graph& iG,
    Order iG_map,
    const Graph& vG,
    Order vG_map,
    Func func,
    Seq inL
  )
{
  typedef graph_traits<Graph> GraphTraits;

  typedef typename GraphTraits::directed_category directed_category;
  typedef typename GraphTraits::vertex_descriptor vertex_descriptor;
  typedef typename GraphTraits::edge_descriptor edge_descriptor;

  typedef typename GraphTraits::edges_size_type edges_size_type;
  typedef typename GraphTraits::edge_iterator edge_iterator;

  typedef typename Seq::const_iterator seq_const_iterator;
  typedef typename Seq::difference_type seq_diff_type;
  typedef typename Seq::value_type seq_value_type;
  typedef typename Seq::size_type seq_size_type;
  typedef typename Seq::iterator seq_iterator;

  typedef typename Order::const_iterator order_const_iterator;
  typedef typename Order::difference_type order_diff_type;
  typedef typename Order::value_type order_value_type;
  typedef typename Order::size_type order_size_type;
  typedef typename Order::iterator order_iterator;

  BOOST_STATIC_ASSERT((is_same<order_value_type, edge_descriptor>::value));
  BOOST_CONCEPT_ASSERT((Convertible<order_size_type, edges_size_type>));

  BOOST_CONCEPT_ASSERT((Convertible<seq_size_type, edges_size_type>));
  BOOST_STATIC_ASSERT((is_same<seq_value_type, bool>::value));

  BOOST_STATIC_ASSERT((is_same<directed_category, undirected_tag>::value));

  if(num_vertices(iG) != num_vertices(vG))
    return;

  if(inL.size() != num_edges(iG)
      || inL.size() != num_edges(vG))
    return;

  if(iG_map.size() != num_edges(iG)
      || vG_map.size() != num_edges(vG))
    return;

  typedef bimaps::bimap<
      bimaps::set_of< int >,
      bimaps::set_of< order_value_type >
    > bimap_type;
  typedef typename bimap_type::value_type bimap_value;

  bimap_type iG_bimap, vG_bimap;
  for(order_size_type i = 0; i < iG_map.size(); ++i)
    iG_bimap.insert(bimap_value(i, iG_map[i]));
  for(order_size_type i = 0; i < vG_map.size(); ++i)
    vG_bimap.insert(bimap_value(i, vG_map[i]));

  edge_iterator current, last;
  boost::tuples::tie(current, last) = edges(iG);
  for(; current != last; ++current)
    if(iG_bimap.right.find(*current) == iG_bimap.right.end())
      return;
  boost::tuples::tie(current, last) = edges(vG);
  for(; current != last; ++current)
    if(vG_bimap.right.find(*current) == vG_bimap.right.end())
      return;

  std::stack<edge_descriptor> iG_buf, vG_buf;

  std::map<vertex_descriptor, edge_descriptor> tree_map;
  std::map<vertex_descriptor, vertex_descriptor> pred_map;
  std::map<vertex_descriptor, int> dist_map, low_map;

  detail::bridges_visitor<
      associative_property_map<
          std::map<vertex_descriptor, edge_descriptor>
        >,
      associative_property_map<
          std::map<vertex_descriptor, vertex_descriptor>
        >,
      associative_property_map< std::map<vertex_descriptor, int> >,
      associative_property_map< std::map<vertex_descriptor, int> >,
      std::stack<edge_descriptor>
    >
  iG_vis(
      associative_property_map<
        std::map< vertex_descriptor, edge_descriptor> >(tree_map),
      associative_property_map<
        std::map< vertex_descriptor, vertex_descriptor> >(pred_map),
      associative_property_map<std::map< vertex_descriptor, int> >(dist_map),
      associative_property_map<std::map< vertex_descriptor, int> >(low_map),
      iG_buf
    ),
  vG_vis(
      associative_property_map<
        std::map< vertex_descriptor, edge_descriptor> >(tree_map),
      associative_property_map<
        std::map< vertex_descriptor, vertex_descriptor> >(pred_map),
      associative_property_map<std::map< vertex_descriptor, int> >(dist_map),
      associative_property_map<std::map< vertex_descriptor, int> >(low_map),
      vG_buf
    );

  std::map<vertex_descriptor, default_color_type> vertex_color;
  std::map<edge_descriptor, default_color_type> edge_color;

  undirected_dfs(iG, iG_vis,
      associative_property_map<
        std::map<vertex_descriptor, default_color_type> >(vertex_color),
      associative_property_map<
        std::map<edge_descriptor, default_color_type> >(edge_color)
    );
  undirected_dfs(vG, vG_vis,
      associative_property_map<
        std::map<vertex_descriptor, default_color_type> >(vertex_color),
      associative_property_map<
        std::map<edge_descriptor, default_color_type> >(edge_color)
    );

  while(!iG_buf.empty()) {
    inL[iG_bimap.right.at(iG_buf.top())] = true;
    iG_buf.pop();
  }
  while(!vG_buf.empty()) {
    inL[vG_bimap.right.at(vG_buf.top())] = true;
    vG_buf.pop();
  }

  std::map<edge_descriptor, bool> iG_inL, vG_inL;
  associative_property_map< std::map<edge_descriptor, bool> >
    aiG_inL(iG_inL), avG_inL(vG_inL);

  for(seq_size_type i = 0; i < inL.size(); ++i)
  {
    if(inL[i]) {
      put(aiG_inL, iG_bimap.left.at(i), true);
      put(avG_inL, vG_bimap.left.at(i), true);
    } else {
      put(aiG_inL, iG_bimap.left.at(i), false);
      put(avG_inL, vG_bimap.left.at(i), false);
    }
  }

  undirected_dfs(
      make_filtered_graph(iG,
        detail::inL_edge_status< associative_property_map<
          std::map<edge_descriptor, bool> > >(aiG_inL)),
      make_dfs_visitor(
        detail::cycle_finder< std::stack<edge_descriptor> > (&iG_buf)),
      associative_property_map<
        std::map<vertex_descriptor, default_color_type> >(vertex_color),
      associative_property_map<
        std::map<edge_descriptor, default_color_type> >(edge_color)
    );
  undirected_dfs(
      make_filtered_graph(vG,
        detail::inL_edge_status< associative_property_map<
          std::map<edge_descriptor, bool> > >(avG_inL)),
      make_dfs_visitor(
        detail::cycle_finder< std::stack<edge_descriptor> > (&vG_buf)),
      associative_property_map<
        std::map<vertex_descriptor, default_color_type> >(vertex_color),
      associative_property_map<
        std::map<edge_descriptor, default_color_type> >(edge_color)
    );

  if(iG_buf.empty() && vG_buf.empty()) {

    std::map<edge_descriptor, bool> iG_deleted, vG_deleted;
    associative_property_map< std::map<edge_descriptor, bool> > diG(iG_deleted);
    associative_property_map< std::map<edge_descriptor, bool> > dvG(vG_deleted);

    boost::tuples::tie(current, last) = edges(iG);
    for(; current != last; ++current)
      put(diG, *current, false);
    boost::tuples::tie(current, last) = edges(vG);
    for(; current != last; ++current)
      put(dvG, *current, false);

    for(seq_size_type j = 0; j < inL.size(); ++j) {
      if(!inL[j]) {
        put(aiG_inL, iG_bimap.left.at(j), true);
        put(avG_inL, vG_bimap.left.at(j), true);

        undirected_dfs(
            make_filtered_graph(iG,
              detail::inL_edge_status< associative_property_map<
                std::map<edge_descriptor, bool> > >(aiG_inL)),
            make_dfs_visitor(
              detail::cycle_finder< std::stack<edge_descriptor> > (&iG_buf)),
            associative_property_map<
              std::map<vertex_descriptor, default_color_type> >(vertex_color),
            associative_property_map<
              std::map<edge_descriptor, default_color_type> >(edge_color)
          );
        undirected_dfs(
            make_filtered_graph(vG,
              detail::inL_edge_status< associative_property_map<
                std::map<edge_descriptor, bool> > >(avG_inL)),
            make_dfs_visitor(
              detail::cycle_finder< std::stack<edge_descriptor> > (&vG_buf)),
            associative_property_map<
              std::map<vertex_descriptor, default_color_type> >(vertex_color),
            associative_property_map<
              std::map<edge_descriptor, default_color_type> >(edge_color)
          );

        if(!iG_buf.empty() || !vG_buf.empty()) {
          while(!iG_buf.empty()) iG_buf.pop();
          while(!vG_buf.empty()) vG_buf.pop();
          put(diG, iG_bimap.left.at(j), true);
          put(dvG, vG_bimap.left.at(j), true);
        }

        put(aiG_inL, iG_bimap.left.at(j), false);
        put(avG_inL, vG_bimap.left.at(j), false);
      }
    }

    int cc = 0;

    std::map<vertex_descriptor, int> com_map;
    cc += connected_components(
        make_filtered_graph(iG,
          detail::deleted_edge_status<associative_property_map<
            std::map<edge_descriptor, bool> > >(diG)),
        associative_property_map<std::map<vertex_descriptor, int> >(com_map)
      );
    cc += connected_components(
        make_filtered_graph(vG,
          detail::deleted_edge_status<associative_property_map<
            std::map<edge_descriptor, bool> > >(dvG)),
        associative_property_map< std::map<vertex_descriptor, int> >(com_map)
      );

    if(cc != 2)
      return;

    // REC
    detail::rec_two_graphs_common_spanning_trees<Graph, Func, Seq,
        associative_property_map< std::map<edge_descriptor, bool> > >
      (iG, iG_bimap, aiG_inL, diG, vG, vG_bimap, aiG_inL, dvG, func, inL);

  }

}

template <
  typename Graph,
  typename Func,
  typename Seq
>
BOOST_CONCEPT_REQUIRES(
  ((IncidenceGraphConcept<Graph>))
  ((EdgeListGraphConcept<Graph>)),
  (void)
)
two_graphs_common_spanning_trees
  (
    const Graph& iG,
    const Graph& vG,
    Func func,
    Seq inL
  )
{
  typedef graph_traits<Graph> GraphTraits;

  typedef typename GraphTraits::edge_descriptor edge_descriptor;
  typedef typename GraphTraits::edges_size_type edges_size_type;
  typedef typename GraphTraits::edge_iterator edge_iterator;

  std::vector<edge_descriptor> iGO, vGO;
  edge_iterator curr, last;

  boost::tuples::tie(curr, last) = edges(iG);
  for(; curr != last; ++curr)
    iGO.push_back(*curr);

  boost::tuples::tie(curr, last) = edges(vG);
  for(; curr != last; ++curr)
    vGO.push_back(*curr);

  two_graphs_common_spanning_trees(iG, iGO, vG, vGO, func, inL);
}

} // namespace boost

#endif // BOOST_GRAPH_TWO_GRAPHS_COMMON_SPANNING_TREES_HPP


/*=============================================================================
    Copyright (c) 2001-2014 Joel de Guzman
    Copyright (c) 2011 Jan Frederick Eick

    Distributed under the Boost Software License, Version 1.0. (See accompanying
    file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
==============================================================================*/
#if !defined(BOOST_SPIRIT_X3_UINT_APR_17_2006_0901AM)
#define BOOST_SPIRIT_X3_UINT_APR_17_2006_0901AM

#include <boost/spirit/home/x3/core/parser.hpp>
#include <boost/spirit/home/x3/core/skip_over.hpp>
#include <boost/spirit/home/x3/support/numeric_utils/extract_int.hpp>
#include <cstdint>

namespace boost { namespace spirit { namespace x3
{
    ///////////////////////////////////////////////////////////////////////////
    template <
        typename T
      , unsigned Radix = 10
      , unsigned MinDigits = 1
      , int MaxDigits = -1>
    struct uint_parser : parser<uint_parser<T, Radix, MinDigits, MaxDigits>>
    {
        // check template parameter 'Radix' for validity
        static_assert(
            (Radix >= 2 && Radix <= 36),
            "Error Unsupported Radix");

        typedef T attribute_type;
        static bool const has_attribute = true;

        template <typename Iterator, typename Context, typename Attribute>
        bool parse(Iterator& first, Iterator const& last
          , Context const& context, unused_type, Attribute& attr) const
        {
            typedef extract_uint<T, Radix, MinDigits, MaxDigits> extract;
            x3::skip_over(first, last, context);
            return extract::call(first, last, attr);
        }
    };

#define BOOST_SPIRIT_X3_UINT_PARSER(uint_type, name)                            \
    typedef uint_parser<uint_type> name##type;                                  \
    name##type const name = {};                                                 \
    /***/

    BOOST_SPIRIT_X3_UINT_PARSER(unsigned long, ulong_)
    BOOST_SPIRIT_X3_UINT_PARSER(unsigned short, ushort_)
    BOOST_SPIRIT_X3_UINT_PARSER(unsigned int, uint_)
    BOOST_SPIRIT_X3_UINT_PARSER(unsigned long long, ulong_long)

    BOOST_SPIRIT_X3_UINT_PARSER(uint8_t, uint8)
    BOOST_SPIRIT_X3_UINT_PARSER(uint16_t, uint16)
    BOOST_SPIRIT_X3_UINT_PARSER(uint32_t, uint32)
    BOOST_SPIRIT_X3_UINT_PARSER(uint64_t, uint64)

#undef BOOST_SPIRIT_X3_UINT_PARSER

#define BOOST_SPIRIT_X3_UINT_PARSER(uint_type, radix, name)                     \
    typedef uint_parser<uint_type, radix> name##type;                           \
    name##type const name = name##type();                                       \
    /***/

    BOOST_SPIRIT_X3_UINT_PARSER(unsigned, 2, bin)
    BOOST_SPIRIT_X3_UINT_PARSER(unsigned, 8, oct)
    BOOST_SPIRIT_X3_UINT_PARSER(unsigned, 16, hex)

#undef BOOST_SPIRIT_X3_UINT_PARSER

}}}

#endif


/* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */

#include "dsp/Resampler.h"

#include "dsp/Window.h"
#include "dsp/FFT.h"

#include <iostream>

#include <cmath>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(TestResampler)

using std::cout;
using std::endl;
using std::vector;

void
testResamplerOneShot(int sourceRate,
		     int targetRate,
		     int n,
		     cq_float *in,
		     int m,
		     cq_float *expected,
		     int skip)
{
    vector<cq_float> resampled = Resampler::resample(sourceRate, targetRate,
						   in, n);
    if (skip == 0) {
	BOOST_CHECK_EQUAL(resampled.size(), m);
    }
    for (int i = 0; i < m; ++i) {
	BOOST_CHECK_SMALL(resampled[i + skip] - expected[i], 1e-6);
    }
}

void
testResampler(int sourceRate,
	      int targetRate,
	      int n,
	      cq_float *in,
	      int m,
	      cq_float *expected)
{
    // Here we provide the input in chunks (of varying size)

    Resampler r(sourceRate, targetRate);
    int latency = r.getLatency();

    int m1 = m + latency;
    int n1 = int((m1 * sourceRate) / targetRate);

    cq_float *inPadded = new cq_float[n1];
    cq_float *outPadded = new cq_float[m1];

    for (int i = 0; i < n1; ++i) {
	if (i < n) inPadded[i] = in[i];
	else inPadded[i] = 0.0;
    }

    for (int i = 0; i < m1; ++i) {
	outPadded[i] = -999.0;
    }

    int chunkSize = 1;
    int got = 0;
    int i = 0;

    while (true) {
	got += r.process(inPadded + i, outPadded + got, chunkSize);
	i = i + chunkSize;
	chunkSize = chunkSize + 1;
	if (i >= n1) {
	    break;
	} else if (i + chunkSize >= n1) {
	    chunkSize = n1 - i;
	} else if (chunkSize > 15) {
	    chunkSize = 1;
	}
    }

    BOOST_CHECK_EQUAL(got, m1);

    for (int i = latency; i < m1; ++i) {
	BOOST_CHECK_SMALL(outPadded[i] - expected[i-latency], 1e-8);
    }

    delete[] outPadded;
    delete[] inPadded;
}

BOOST_AUTO_TEST_CASE(sameRateOneShot)
{
    cq_float d[] = { 0, 0.1, -0.3, -0.4, -0.3, 0, 0.5, 0.2, 0.8, -0.1 };
    testResamplerOneShot(4, 4, 10, d, 10, d, 0);
}

BOOST_AUTO_TEST_CASE(sameRate)
{
    cq_float d[] = { 0, 0.1, -0.3, -0.4, -0.3, 0, 0.5, 0.2, 0.8, -0.1 };
    testResampler(4, 4, 10, d, 10, d);
}

BOOST_AUTO_TEST_CASE(interpolatedMisc)
{
    // Interpolating any signal by N should give a signal in which
    // every Nth sample is the original signal
    cq_float in[] = { 0, 0.1, -0.3, -0.4, -0.3, 0, 0.5, 0.2, 0.8, -0.1 };
    int n = sizeof(in)/sizeof(in[0]);
    for (int factor = 2; factor < 10; ++factor) {
	vector<cq_float> out = Resampler::resample(6, 6 * factor, in, n);
	for (int i = 0; i < n; ++i) {
	    BOOST_CHECK_SMALL(out[i * factor] - in[i], 1e-5);
	}
    }
}

BOOST_AUTO_TEST_CASE(interpolatedSine)
{
    // Interpolating a sinusoid should give us a sinusoid, once we've
    // dropped the first few samples
    cq_float in[1000];
    cq_float out[2000];
    for (int i = 0; i < 1000; ++i) {
	in[i] = sin(i * M_PI / 2.0);
    }
    for (int i = 0; i < 2000; ++i) {
	out[i] = sin(i * M_PI / 4.0);
    }
    testResamplerOneShot(8, 16, 1000, in, 200, out, 512);
}

BOOST_AUTO_TEST_CASE(decimatedSine)
{
    // Decimating a sinusoid should give us a sinusoid, once we've
    // dropped the first few samples
    cq_float in[2000];
    cq_float out[1000];
    for (int i = 0; i < 2000; ++i) {
	in[i] = sin(i * M_PI / 8.0);
    }
    for (int i = 0; i < 1000; ++i) {
	out[i] = sin(i * M_PI / 4.0);
    }
    testResamplerOneShot(16, 8, 2000, in, 200, out, 256);
}

cq_float
measureSinFreq(const vector<cq_float> &v, int rate, int countCycles)
{
    int n = v.size();
    int firstPeak = -1;
    int lastPeak = -1;
    int nPeaks = 0;
    // count +ve peaks
    for (int i = v.size()/4; i + 1 < n; ++i) {
        // allow some fuzz
        int x0 = int(10000 * v[i-1]);
        int x1 = int(10000 * v[i]);
        int x2 = int(10000 * v[i+1]);
        if (x1 > 0 && x1 > x0 && x1 >= x2) {
            if (firstPeak < 0) firstPeak = i;
            lastPeak = i;
            ++nPeaks;
            if (nPeaks == countCycles) break;
        }
    }
    int nCycles = nPeaks - 1;
    if (nCycles <= 0) return 0.0;
    cq_float cycle = cq_float(lastPeak - firstPeak) / nCycles;
//    cout << "lastPeak = " << lastPeak << ", firstPeak = " << firstPeak << ", dist = " << lastPeak - firstPeak << ", nCycles = " << nCycles << ", cycle = " << cycle << endl;
    return rate / cycle;
}

void
testSinFrequency(int freq,
                 int sourceRate,
                 int targetRate)
{
    // Resampling a sinusoid and then resampling back should give us a
    // sinusoid of the same frequency as we started with

    int nCycles = 500;

    int duration = int(nCycles * cq_float(sourceRate) / cq_float(freq));
//    cout << "freq = " << freq << ", sourceRate = " << sourceRate << ", targetRate = " << targetRate << ", duration = " << duration << endl;

    vector<cq_float> in(duration, 0);
    for (int i = 0; i < duration; ++i) {
        in[i] = sin(i * M_PI * 2.0 * freq / sourceRate);
    }

    vector<cq_float> out = Resampler::resample(sourceRate, targetRate,
                                             in.data(), in.size());

    vector<cq_float> back = Resampler::resample(targetRate, sourceRate,
                                              out.data(), out.size());

    BOOST_CHECK_EQUAL(in.size(), back.size());

    cq_float inFreq = measureSinFreq(in, sourceRate, nCycles / 2);
    cq_float backFreq = measureSinFreq(back, sourceRate, nCycles / 2);

    BOOST_CHECK_SMALL(inFreq - backFreq, 1e-8);
}

// In each of the following we use a frequency that has an exact cycle
// length in samples at the lowest sample rate, so that we can easily
// rule out errors in measuring the cycle length after resampling. If
// the resampler gets its input or output rate wrong, that will show
// up no matter what the test signal's initial frequency is.

BOOST_AUTO_TEST_CASE(downUp2)
{
    testSinFrequency(441, 44100, 22050);
}

BOOST_AUTO_TEST_CASE(downUp5)
{
    testSinFrequency(300, 15000, 3000);
}

BOOST_AUTO_TEST_CASE(downUp16)
{
    testSinFrequency(300, 48000, 3000);
}

BOOST_AUTO_TEST_CASE(upDown2)
{
    testSinFrequency(441, 44100, 88200);
}

BOOST_AUTO_TEST_CASE(upDown5)
{
    testSinFrequency(300, 3000, 15000);
}

BOOST_AUTO_TEST_CASE(upDown16)
{
    testSinFrequency(300, 3000, 48000);
}

vector<cq_float>
squareWave(int rate, cq_float freq, int n)
{
    //!!! todo: hoist, test
    vector<cq_float> v(n, 0.0);
    for (int h = 0; h < (rate/4)/freq; ++h) {
	cq_float m = h * 2 + 1;
	cq_float scale = 1.0 / m;
	for (int i = 0; i < n; ++i) {
	    cq_float s = scale * sin((i * 2.0 * M_PI * m * freq) / rate);
	    v[i] += s;
	}
    }
    return v;
}

void
testSpectrum(int inrate, int outrate)
{
    // One second of a square wave
    int freq = 500;

    vector<cq_float> square =
	squareWave(inrate, freq, inrate);

    vector<cq_float> maybeSquare =
	Resampler::resample(inrate, outrate, square.data(), square.size());

    BOOST_CHECK_EQUAL(maybeSquare.size(), outrate);

    Window<cq_float>(HanningWindow, inrate).cut(square.data());
    Window<cq_float>(HanningWindow, outrate).cut(maybeSquare.data());

    // forward magnitude with size inrate, outrate

    vector<cq_float> inSpectrum(inrate, 0.0);
    FFTReal(inrate).forwardMagnitude(square.data(), inSpectrum.data());
    for (int i = 0; i < (int)inSpectrum.size(); ++i) {
	inSpectrum[i] /= inrate;
    }

    vector<cq_float> outSpectrum(outrate, 0.0);
    FFTReal(outrate).forwardMagnitude(maybeSquare.data(), outSpectrum.data());
    for (int i = 0; i < (int)outSpectrum.size(); ++i) {
	outSpectrum[i] /= outrate;
    }

    // Don't compare bins any higher than 96% of Nyquist freq of lower sr
    int lengthOfInterest = (inrate < outrate ? inrate : outrate) / 2;
    lengthOfInterest = lengthOfInterest - (lengthOfInterest / 25);

    for (int i = 0; i < lengthOfInterest; ++i) {
	BOOST_CHECK_SMALL(inSpectrum[i] - outSpectrum[i], 1e-7);
    }
}
/*
BOOST_AUTO_TEST_CASE(spectrum)
{
    int rates[] = { 8000, 22050, 44100, 48000 };
    for (int i = 0; i < (int)(sizeof(rates)/sizeof(rates[0])); ++i) {
	    for (int j = 0; j < (int)(sizeof(rates)/sizeof(rates[0])); ++j) {
	    testSpectrum(rates[i], rates[j]);
	}
    }
}
*/
BOOST_AUTO_TEST_SUITE_END()