/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */
#include "DavidsonDiagonalizer.h"
#include "../MathUtils.h"
#include "DiagonalizerSettings.h"
#include "PreconditionerEvaluator.h"
#include "SubspaceOrthogonalizer.h"
#include <Core/Log.h>
#include <Eigen/Eigenvalues>

namespace Scine {
namespace Utils {

NonOrthogonalDavidson::NonOrthogonalDavidson(int eigenvaluesToCompute, int totalDimension)
  : KrylovDiagonalizer(eigenvaluesToCompute, totalDimension) {
}

NonOrthogonalDavidson::~NonOrthogonalDavidson() = default;

EigenContainer NonOrthogonalDavidson::eigenDecomposition(const Eigen::MatrixXd& projectedMatrix) const {
  auto algorithm = settings_->getString(gepAlgorithmForBalancedMethodOption);
  if (algorithm == "standard")
    return MathUtils::stabilizedGeneralizedEigendecomposition<MathUtils::GepAlgorithm::Standard>(
        projectedMatrix, basisOverlap_.selfadjointView<Eigen::Upper>());
  else if (algorithm == "cholesky")
    return MathUtils::stabilizedGeneralizedEigendecomposition<MathUtils::GepAlgorithm::Cholesky>(
        projectedMatrix, basisOverlap_.selfadjointView<Eigen::Upper>());
  else if (algorithm == "simultaneous_diag")
    return MathUtils::stabilizedGeneralizedEigendecomposition<MathUtils::GepAlgorithm::SimultaneousDiagonalization>(
        projectedMatrix, basisOverlap_.selfadjointView<Eigen::Upper>());
  else
    throw InvalidDiagonalizerInputException("Algorithm " + algorithm + " not available for stable GEP solution.");
}

inline void NonOrthogonalDavidson::onSigmaMatrixEvaluation(const Eigen::MatrixXd& projector) {
  decltype(basisOverlap_)::Index newCols = subspaceDimension_ - basisOverlap_.cols();
  basisOverlap_.conservativeResize(subspaceDimension_, subspaceDimension_);
  Eigen::MatrixXd rightCols = projector.rightCols(newCols);
  basisOverlap_.rightCols(newCols) = projector.transpose() * rightCols;
}

inline void NonOrthogonalDavidson::filterCorrectionVectors(const Eigen::MatrixXd& projector,
                                                           Eigen::MatrixXd& newGuessVectors) const {
  double absNorm =
      std::abs(newGuessVectors.col(newGuessVectors.cols() - 1).transpose() * projector.col(projector.cols() - 1));
  if (absNorm > 1.0 - settings_->getDouble(correctionToleranceOption)) {
    newGuessVectors.col(newGuessVectors.cols() - 1) = residualVectors_.col(newGuessVectors.cols() - 1);
  }
}

void NonOrthogonalDavidson::callCollapserImpl() {
  guessVectors_ = subspaceCollapser_->getCollapsedNonOrthogonalSubspace();
  subspaceDimension_ = guessVectors_.cols();
  basisOverlap_.resize(0, 0);
}

OrthogonalDavidson::OrthogonalDavidson(int eigenvaluesToCompute, int totalDimension)
  : KrylovDiagonalizer(eigenvaluesToCompute, totalDimension) {
}

OrthogonalDavidson::~OrthogonalDavidson() = default;

inline void OrthogonalDavidson::filterCorrectionVectors(const Eigen::MatrixXd& projector, Eigen::MatrixXd& newGuessVectors) const {
  Eigen::MatrixXd orthogonalizedCorrectionVector = newGuessVectors;
  Eigen::VectorXd correctionVectorsNorms = newGuessVectors.colwise().norm();

  int dimension = 0;
  double maxNorm = 0;
  Eigen::VectorXd maxNormOrthogonalizedVector;
  for (int i = 0; i < orthogonalizedCorrectionVector.cols(); ++i) {
    Eigen::VectorXd orthogonalizedVector;
    SubspaceOrthogonalizer::orthogonalizeToSubspace(orthogonalizedCorrectionVector.col(i), projector, orthogonalizedVector);

    double norm = orthogonalizedVector.norm() / correctionVectorsNorms(i);
    if (norm > maxNorm) {
      std::swap(norm, maxNorm);
      maxNormOrthogonalizedVector = orthogonalizedVector;
    }

    if (norm / correctionVectorsNorms(i) > settings_->getDouble(correctionToleranceOption)) {
      newGuessVectors.col(dimension++) = orthogonalizedVector;
    }
  }
  // Retain one at least if everything is thrown away, the one that corrects the most
  if (dimension == 0) {
    newGuessVectors.col(0) = maxNormOrthogonalizedVector;
    dimension = 1;
  }
  newGuessVectors.conservativeResize(Eigen::NoChange, dimension);
}

inline void OrthogonalDavidson::onIterationStart() {
  KrylovDiagonalizer::onIterationStart();
  SubspaceOrthogonalizer::qrOrthogonalize(guessVectors_, subspaceDimension_);
  guessVectors_.leftCols(subspaceDimension_).colwise().normalize();
}

EigenContainer OrthogonalDavidson::eigenDecomposition(const Eigen::MatrixXd& projectedMatrix) const {
  EigenContainer result;
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> subspaceDiagonalizer(projectedMatrix);
  result.eigenVectors = subspaceDiagonalizer.eigenvectors();
  result.eigenValues = subspaceDiagonalizer.eigenvalues();
  return result;
}

void OrthogonalDavidson::callCollapserImpl() {
  guessVectors_ = subspaceCollapser_->getCollapsedOrthogonalSubspace();
  subspaceDimension_ = guessVectors_.cols();
}
} // namespace Utils
} // namespace Scine


#include <mex.h>
#include <math.h>
#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;
using namespace std;

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{

    mxArray *output_mex;

    double *tri_num, *obj_tri, *q_target;
    double *output;

    tri_num = mxGetPr(prhs[0]);
    obj_tri = mxGetPr(prhs[1]);
    q_target = mxGetPr(prhs[2]);

    int tri_n = tri_num[0];

    output_mex = plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL);

    output = mxGetPr(output_mex);

    int tri[3];

    output[0] = 1;

    tri[0] = obj_tri[0] - 1;
    tri[1] = obj_tri[0 + tri_n] - 1;
    tri[2] = obj_tri[0 + 2 * tri_n] - 1;

    Vector3d va(q_target[2 * tri[0]], q_target[2 * tri[0] + 1], 0);
    Vector3d vb(q_target[2 * tri[1]], q_target[2 * tri[1] + 1], 0);
    Vector3d vc(q_target[2 * tri[2]], q_target[2 * tri[2] + 1], 0);

    Vector3d e1 = va - vb;
    Vector3d e2 = vc - vb;
    Vector3d flag = e1.cross(e2);

    for(int i = 0; i < tri_n; i++)
    {
        tri[0] = obj_tri[i] - 1;
        tri[1] = obj_tri[i + tri_n] - 1;
        tri[2] = obj_tri[i + 2 * tri_n] - 1;

        Vector3d va(q_target[2 * tri[0]], q_target[2 * tri[0] + 1], 0);
        Vector3d vb(q_target[2 * tri[1]], q_target[2 * tri[1] + 1], 0);
        Vector3d vc(q_target[2 * tri[2]], q_target[2 * tri[2] + 1], 0);

        Vector3d e1 = va - vb;
        Vector3d e2 = vc - vb;
        Vector3d f1 = e1.cross(e2);

        if(f1.dot(flag) < 0)
        {
            output[0] = 0;
        }
    }

    return;

}


// Created by enenra on 19.11.2021.

#ifndef KURSOVAYA_1_HEADER_HPP
#define KURSOVAYA_1_HEADER_HPP

#include "nlohmann/json.hpp"
#include <boost/filesystem.hpp>
#include <algorithm>
#include <regex>
#include <string>
#include <iomanip>
#include <cmath>
#include <ctime>

using json = nlohmann::json;

size_t power(std::string &X);

size_t number_degree(const size_t &val);

int number_degree(const int &val);

class boolean {
private:
    std::vector<int> vec;

public:
    boolean()=default;

    explicit boolean(std::string &init) {
        for (size_t i = 0; i < init.size(); ++i) {
            if ((init[i] == '0') || (init[i] == '1')) {
                if (i == 0)
                    if ((init[i + 1] == '0') || (init[i + 1] == '1')) {
                        if (init[i] == '0')
                            vec.push_back(0);
                        else vec.push_back(1);
                    }
                if ((i > 0) && (i < init.size() - 1))
                    if ((init[i + 1] == '0') || (init[i + 1] == '1') || (init[i - 1] == '0') || (init[i - 1] == '1')) {
                        if (init[i] == '0')
                            vec.push_back(0);
                        else vec.push_back(1);
                    }
                if (i == init.size() - 1) {
                    if ((init[i - 1] == '0') || (init[i - 1] == '1')) {
                        if (init[i] == '0')
                            vec.push_back(0);
                        else vec.push_back(1);
                    }
                }
            }
        }
    }

    boolean(int val, int deg) {
        for (int i = 0; i < deg; ++i)
            vec.push_back(0);
        while (deg > 0) {
            vec[deg - 1] = val % 2;
            val /= 2;
            --deg;
        }
    }

    ~boolean() = default;

    void push_back(int val) {
        vec.push_back(val);
    }

    bool empty() {
        if (vec.empty())
            return true;
        return false;
    }

    int degree() {
        int deg = 0;
        while (pow(2.0, deg) != vec.size())
            deg += 1;
        return deg;
    }

    boolean operator+=(boolean &r) {
        for (size_t i = 0; i < vec.size(); ++i)
            vec[i] ^= r.vec[i];
        return *this;
    }

    boolean operator*=(boolean &r) {
        for (size_t i = 0; i < vec.size(); ++i)
            vec[i] &= r.vec[i];
        return *this;
    }

    int &operator[](size_t &index) {
        return vec[index];
    }

    int &operator[](int index) {
        return vec[index];
    }

    size_t to_number() {
        size_t result = 0;
        for (size_t i = 0; i < vec.size(); ++i)
            result += vec[i] * pow(2.0, vec.size() - 1 - i);
        return result;
    }

    bool operator==(const boolean& r) const {
        if (vec.size() != r.vec.size())
            return false;
        else
            for (size_t i = 0; i < vec.size(); ++i)
                if (vec[i] != r.vec[i])
                    return false;
        return true;
    }

    bool operator!=(const boolean &r) const {
        if (vec.size() != r.vec.size())
            return true;
        else
            for (size_t i = 0; i < vec.size(); ++i)
                if (vec[i] != r.vec[i])
                    return true;
        return false;
    }
};

class cryptalgorithm {
private:
    std::string text;
    std::string cipher;
    std::vector<int> keys;
    std::vector<std::string> functions;

public:
    cryptalgorithm() = default;

    cryptalgorithm(std::string &t, std::string &c, std::vector<int> &k, std::vector<std::string> &f) {
        text = t;
        cipher = c;
        keys = k;
        functions = f;
    }

    ~cryptalgorithm() = default;

    void absolute_stability(std::ofstream &out) {
        out << '\n' << " - ANALYZING ABSOLUTE STABILITY:" << '\n';
        if (power(text) <= keys.size())
            out << '\n' << "Your algorithm is absolutely stable" << '\n';
        else out << '\n' << "Your algorithm is not absolutely stable" << '\n';
        out << '\n';
    }

    void differential_attack(std::ofstream &out) {
        out << '\n' << " - DIFFERENTIAL ATTACK:" << '\n';
        if (functions.size() == 1) {
            out
                    << "Your algorithm does not cannot ba attacked with differential attack, or entered functions are incorrect"
                    << '\n';
            out
                    << "Make sure. that, if Your encryption function is a permutation, You enter it as a system of coordinate boolean functions"
                    << '\n';
            out << "Boolean functions are entered as a vector of 0 and 1" << '\n';
        } else {
            std::vector<boolean> system;
            for (auto &function: functions) {
                boolean f(function);
                if (!f.empty())
                    system.push_back(f);
            }
            if (system.empty()) {
                out
                        << "Your algorithm does not cannot ba attacked with differential attack, or entered functions are incorrect"
                        << '\n';
                out
                        << "Make sure. that, if Your encryption function is a permutation, You enter it as a system of coordinate boolean functions"
                        << '\n';
                out << "Boolean functions are entered as a vector of 0 and 1" << '\n';
            } else {
                unsigned int start = clock();
                int deg = system[0].degree();
                size_t n;
                if (text == "ASCII")
                    n = 256;
                else n = text.size();
                auto **table = new size_t *[n];
                for (size_t k = 0; k < n; ++k) {
                    table[k] = new size_t[n];
                    boolean delta_x(k, deg);
                    for (size_t i = 0; i < n; ++i) {
                        boolean delta_y, y;
                        for (auto &j: system)
                            y.push_back(j[i]);
                        boolean x(i, deg);
                        x += delta_x;
                        size_t index = x.to_number();
                        for (auto &j: system)
                            delta_y.push_back(j[index]);
                        delta_y += y;
                        size_t eq = delta_y.to_number();
                        table[k][i] = eq;
                    }
                    std::vector<size_t> v;
                    for (size_t i = 0; i < n; ++i)
                        v.push_back(0);
                    for (size_t r = 0; r < n; ++r)
                        v[table[k][r]] += 1;
                    for (size_t i = 0; i < n; ++i)
                        table[k][i] = v[i];
                }
                unsigned int end = clock();
                unsigned int time = end - start;
                // Вывод данных
                out << '\n';
                for (size_t i = 0; i <= n; ++i) {
                    for (size_t j = 0; j <= n; ++j) {
                        if (i * j == 0) {
                            if ((i == 0) && (j == 0))
                                out << "|     ";
                            if ((i == 0) && (j != 0)) {
                                size_t val = j - 1;
                                boolean output(val, deg);
                                out << ' ';
                                for (int k = 0; k < deg; ++k)
                                    out << output[k];
                            }
                            if ((i != 0) && (j == 0)) {
                                size_t val = i - 1;
                                boolean output(val, deg);
                                out << "| ";
                                for (int k = 0; k < deg; ++k)
                                    out << output[k];
                            }
                            out << " |";
                        } else {
                            out << "  " << table[i - 1][j - 1];
                            for (size_t h = 0; h < deg - number_degree(table[i - 1][j - 1]); ++h)
                                out << ' ';
                            out << '|';
                        }
                    }
                    out << '\n';
                }
                out
                        << '\n'
                        << "Estimated time of analyzing given S-block to find most probable outcomes from certain entered text is "
                        << time << " ns" << '\n';
                for (size_t i = 0; i < n; ++i)
                    delete[] table[i];
                delete[] table;
            }
        }
    }

    void brute_force(std::ofstream &out) {
        out << '\n' << " - BRUTE FORCE ATTACK:" << '\n';
        size_t n;
        if (text == "ASCII")
            n = 256;
        else
            n = text.size();
        out << '\n' << "This attack is always very long and needs many resources, in your case it will take about ";
        out << n << " factorial, multiplied by the time of one tact." << '\n';
    }

    void linear_attack(std::ofstream &out) {
        out << '\n' << " - LINEAR ATTACK:" << '\n';
        if (functions.size() == 1) {
            out
                    << "Your algorithm does not cannot ba attacked with linear attack, or entered functions are incorrect"
                    << '\n';
            out
                    << "Make sure. that, if Your encryption function is a permutation, You enter it as a system of coordinate boolean functions"
                    << '\n';
            out << "Boolean functions are entered as a vector of 0 and 1" << '\n';
        } else {
            std::vector<boolean> system;
            for (auto &function: functions) {
                boolean f(function);
                if (!f.empty())
                    system.push_back(f);
            }
            if (system.empty()) {
                out
                        << "Your algorithm does not cannot ba attacked with linear attack, or entered functions are incorrect"
                        << '\n';
                out
                        << "Make sure. that, if Your encryption function is a permutation, You enter it as a system of coordinate boolean functions"
                        << '\n';
                out << "Boolean functions are entered as a vector of 0 and 1" << '\n';
            } else {
                unsigned int start = clock();
                size_t n, deg = system[0].degree();
                if (text == "ASCII")
                    n = 256;
                else n = text.size();
                auto **table = new int *[n];
                for (size_t i = 0; i < n; ++i) {
                    table[i] = new int[n];
                    boolean x_linear(i, deg);
                    for (size_t j = 0; j < n; ++j) {
                        table[i][j] = 0;
                        boolean y_linear(j, deg);
                        for (size_t h = 0; h < n; ++h) {
                            boolean alfa(h, deg);
                            boolean beta;
                            for (auto &k: system)
                                beta.push_back(k[h]);
                            alfa *= x_linear;
                            beta *= y_linear;
                            int x = 0, y = 0;
                            for (size_t t = 0; t < deg; ++t) {
                                x ^= alfa[t];
                                y ^= beta[t];
                            }
                            if (x == y)
                                table[i][j] += 1;
                        }
                        table[i][j] = table[i][j] - n / 2;
                    }
                }
                unsigned int end = clock();
                unsigned int time = end - start;
                // Вывод данных
                out << '\n';
                for (size_t i = 0; i <= n; ++i) {
                    for (size_t j = 0; j <= n; ++j) {
                        if (i * j == 0) {
                            if ((i == 0) && (j == 0))
                                out << "|     ";
                            if ((i == 0) && (j != 0)) {
                                size_t val = j - 1;
                                boolean output(val, deg);
                                out << ' ';
                                for (size_t k = 0; k < deg; ++k)
                                    out << output[k];
                            }
                            if ((i != 0) && (j == 0)) {
                                size_t val = i - 1;
                                boolean output(val, deg);
                                out << "| ";
                                for (size_t k = 0; k < deg; ++k)
                                    out << output[k];
                            }
                            out << " |";
                        } else {
                            if (table[i - 1][j - 1] > 0) {
                                out << " +" << table[i - 1][j - 1];
                                for (size_t h = 0; h < deg - number_degree(table[i - 1][j - 1]); ++h)
                                    out << ' ';
                                out << '|';
                            }
                            if (table[i - 1][j - 1] == 0) {
                                out << "  " << table[i - 1][j - 1];
                                for (size_t h = 0; h < deg - 1; ++h)
                                    out << ' ';
                                out << '|';
                            }
                            if (table[i - 1][j - 1] < 0) {
                                out << ' ' << table[i - 1][j - 1];
                                for (size_t h = 0; h < deg - number_degree(table[i - 1][j - 1]); ++h)
                                    out << ' ';
                                out << '|';
                            }
                        }
                    }
                    out << '\n';
                }
                out
                        << '\n'
                        << "Estimated time to build needed approximations and analyse the probability of destabilization of given S-block is "
                        << time << " ns" << '\n';
                for (size_t i = 0; i < n; ++i)
                    delete[] table[i];
                delete[] table;
            }
        }
    }
};

#endif //KURSOVAYA_1_HEADER_HPP


/*====================================================================
   bouncingBallBenchmark.cc
   Copyright (c) 2019 Matthew Millard <matthew.millard@iwr.uni-heidelberg.de>
   Licensed under the zlib license. See LICENSE for more details.
 *///=================================================================

#include <string>
#include <iostream>
#include <stdio.h>
#include <rbdl/rbdl.h>
#include <rbdl/addons/luamodel/luamodel.h>
#include <rbdl/addons/geometry/geometry.h>
#include "csvtools.h"

#include "ContactToolkit.h"

#include <boost/numeric/odeint/stepper/runge_kutta_cash_karp54.hpp>
#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
#include <boost/numeric/odeint/integrate/integrate_adaptive.hpp>
#include <boost/numeric/odeint/stepper/generation/make_controlled.hpp>
//using namespace std;
using namespace boost::numeric::odeint;

using namespace RigidBodyDynamics;
using namespace RigidBodyDynamics::Math;

//====================================================================
// Boost stuff
//====================================================================

typedef std::vector< double > state_type;
typedef runge_kutta_cash_karp54< state_type > error_stepper_type;
typedef controlled_runge_kutta< error_stepper_type > controlled_stepper_type;

class rbdlToBoost {

  public:
    rbdlToBoost(Model* model,
                std::string &ballName,
                double ballRadius,
                Vector3d &pointOnPlane,
                Vector3d &planeNormal,
                double stiffness,
                double exponent,
                double damping,
                double staticFrictionSpeed,
                double staticFrictionCoefficient,
                double dynamicFrictionSpeed,
                double dynamicFrictionCoefficient,
                double viscousFrictionSlope,
                unsigned int numberOfWorkTermsInState
                ):model(model),r(ballRadius),r0P0(pointOnPlane),
                  eN0(planeNormal),k(stiffness),p(exponent),beta(damping),
                  staticFrictionSpeed(staticFrictionSpeed),
                  staticFrictionCoefficient(staticFrictionCoefficient),
                  dynamicFrictionSpeed(dynamicFrictionSpeed),
                  dynamicFrictionCoefficient(dynamicFrictionCoefficient),
                  viscousFrictionSlope(viscousFrictionSlope),
                  numberOfWorkTermsInState(numberOfWorkTermsInState)
    {

        q = VectorNd::Zero(model->dof_count);
        qd = VectorNd::Zero(model->dof_count);
        qdd = VectorNd::Zero(model->dof_count);
        tau = VectorNd::Zero(model->dof_count);
        fext.resize(model->mBodies.size());
        for(unsigned int i=0; i<fext.size();++i){
            fext[i]=SpatialVector::Zero();
        }
        ballId = model->GetBodyId(ballName.c_str());
        fK0n = Vector3dZero;
        tK0n = Vector3dZero;
        fK0t = Vector3dZero;
        tK0t = Vector3dZero;

        //1a. The regularized friction model is created and printed to file
        ContactToolkit::createRegularizedFrictionCoefficientCurve(
                          staticFrictionSpeed, staticFrictionCoefficient,
                          dynamicFrictionSpeed,dynamicFrictionCoefficient,
                          viscousFrictionSlope,"mu",frictionCoefficientCurve);

        frictionCoefficientCurve.printCurveToCSVFile("../output/",
                           "frictionCoefficientCurve",0,dynamicFrictionSpeed*2);
        printf("Wrote: ../output/frictionCoefficentCurve.csv\n");

        //Set the velocity at which the relaxed method is used to compute
        //the direction vector of the tangential velocity of the contact
        //point
        veps = staticFrictionSpeed/100.0;

        //If veps is too small, we really might have problems
        assert(veps > std::sqrt(std::numeric_limits<double>::epsilon()));

    }

    //1b. The state derivative for this model is called by Boost whenever the
    //    code for the 'operator()' function is called
    void operator() (const state_type &x,
                     state_type &dxdt,
                     const double t){

        //1c. The state x is split into generalized positions (q), generalized
        //    velocities (qdd). Here tau is set to 0 because we are not
        //    applying any generlized forces: the ball contact/friction forces
        //    are applied as external forces
        //q
        int j = 0;
        for(unsigned int i=0; i<model->q_size; i++){
            q[i] = double(x[j]);
            j++;
        }

        //qd
        for(unsigned int i=0; i<model->qdot_size; i++){
            qd[i] = double(x[j]);
            j++;
        }

        //tau = 0
        for(unsigned int i=0; i<model->qdot_size; i++){
            tau[i] = 0;
        }

        //2a. To evaluate the contact forces we must see if the ball is interpenetrating
        //    the ground. Outside of FEA modelling it is common to allow (non deforming)
        //    geometry to intepenetrate and to use the interpenetration to compute
        //    contact forces. Here we go with the simplest option and map the
        //    depth of penetration to a contact force.
        r0B0 = CalcBodyToBaseCoordinates(*model,q,ballId,Vector3dZero,true);
        ContactToolkit::calcSpherePlaneContactPointPosition(r0B0,r,eN0,r0K0);

        //if the contact point is in the sphere compute contact data
        z = (r0K0-r0P0).dot(eN0);
        dz=0.; //this is zero until contact is made
        if( z < 0. ){

          //2b. If the sphere is in contact with the plane, we must get the position and
          //velocity of the contact point.

          //Get the point of contact resolved in the coordinates of the ball
          EB0   = CalcBodyWorldOrientation(*model,q,ballId,true);
          rBKB  = EB0*(r0K0-r0B0);

          //Get the velocity of the point of contact
          v0K0 = CalcPointVelocity(*model,q,qd,ballId,rBKB,true);

          //Evaluate Hunt-Crossley Contact forces
          dz = (v0K0).dot(eN0); //assuming the plane is fixed.

          //2c. Now we can evaluate the contact forces using a Hunt-Crossley contact model
          //    This is a popular contact model because it is numerically well behaved.
          //    See the comments in ContactToolkit.h above the function definition
          //    for calcHuntCrossleyContactForce for further details.
          ContactToolkit::calcHuntCrossleyContactForce(z,dz,k,p,beta,hcInfo);

          //2d. To apply this contact force as an external force in RBDL, we must first
          //    transform it into a spatial wrench that is resolved in the Root frame.
          //    Here we turn the scalar contact force into a vector, and then evaluate
          //    the torque that this force vector produces about the Root frame
          fK0n = hcInfo.force*eN0;
          tK0n = VectorCrossMatrix(r0K0)*fK0n;

          //2e. Now we can use the tangential velocity of the contact point of the ball
          //    to evaluate the coefficient of friction, and then the friction forces
          v0K0t = v0K0 - dz*eN0;
          ContactToolkit::calcTangentialVelocityDirection(v0K0t,veps,eT0);

          mu = frictionCoefficientCurve.calcValue(v0K0t.norm());

          //2f. As with the contact model we evaluate the force the friction model applies
          //    to the ball and also the torque it generates about the ROOT frame
          fK0t = -mu*hcInfo.force*eT0;
          tK0t = VectorCrossMatrix(r0K0)*fK0t;

          //2g. The total wrench generated by the contact and friction models is applied to
          //    the entry in the vector fext that corresponds to the ball.
          fext[ballId][0] = tK0n[0] + tK0t[0];
          fext[ballId][1] = tK0n[1] + tK0t[1];
          fext[ballId][2] = tK0n[2] + tK0t[2];

          fext[ballId][3] = fK0n[0] + fK0t[0];
          fext[ballId][4] = fK0n[1] + fK0t[1];
          fext[ballId][5] = fK0n[2] + fK0t[2];

        }else{
          //zero the entry of fext associated with the ball.
          fext[ballId]=SpatialVector::Zero();
          hcInfo.force        = 0.;
          hcInfo.springForce  = 0.;
          hcInfo.dampingForce = 0.;
          fK0n = Vector3dZero;
          tK0n = Vector3dZero;
          fK0t = Vector3dZero;
          tK0t = Vector3dZero;
        }

        //3a. Now the generalized accelerations of the ball can be computed
        ForwardDynamics(*model,q,qd,tau,qdd,&fext);

        //3b. The state derivative dxdt is now formed using qd, and qdd. The
        //    derivatives of the  work done by the contact and friction models
        //    is also stored so that we  can track the system energy of the
        //    system through the simulation
        j = 0;
        for(unsigned int i = 0; i < model->q_size; i++){
            dxdt[j] = double(qd[i]);
            j++;
        }
        for(unsigned int i = 0; i < model->qdot_size; i++){
            dxdt[j] = double(qdd[i]);
            j++;
        }

        dworkN = hcInfo.force*dz;
        dxdt[j] = dworkN;
        j++;
        dxdt[j] = fK0t.dot(v0K0t);
        j++;
        assert((numberOfWorkTermsInState
                + model->q_size
                + model->qdot_size) == j);

    }

    /*
      Ascii Vector Notation:
      rBKB
        r: vector
        B: from the origin of the Ball frame
        K: to the contact point K
        B: expressed in the coordinates of the ball frame.
      EB0:
        E: rotation matrix
        B: to Frame B
        0: from Frame 0
      eN0:
        e: unit vector
        N: Normal direction
        0: expressed in the coordinates of the root frame (0)
      fK0
        f: force
        K: At point K
        0: expressed in the coordinates of the root frame
      tK0
        t: torque
        K: At point K
        0: expressed in the coordinates of the root frame

    */

    //Multibody Variables
    Model* model;
    VectorNd q, qd, qdd, tau;

    //Normal-Contact Model Working Variables
    unsigned int ballId;
    double z, dz; //pentration depth and velocity
    HuntCrossleyContactInfo hcInfo;
    std::vector< SpatialVector > fext;
    double dworkN, dworkT; //Work in the normal and tangential directions
    double mu;      //Friction coefficient
    Matrix3d EB0;   //Orientation of the ball expressed in the Root frame
    Vector3d r0B0;  //Position of the ball
    Vector3d rBKB;  //B : ball.
    Vector3d r0K0;  //K : contact point
    Vector3d v0K0;  //velocity of the contact point
    Vector3d v0K0t; //tangential velocity of the contact point
    Vector3d r0P0;  //Origin of the plane
    Vector3d eN0;   //Normal of the plane
    Vector3d eT0;   //Tangental direction of the plane: in 2d this isn't necessary
                    //here we compute it to show how this is done in a
                    //numerically stable way in 3d.
    Vector3d fK0n, tK0n; //contact force and moment
    Vector3d fK0t, tK0t; //tangential friction force and moment

    //Normal Contact-Model Parameters
    double r; //ball radius
    double k; //stiffness
    double p; //exponential power on the spring compression
    double beta; //damping

    //Friction Model Parameters: see ContactToolkit
    // createRegularizedFrictionCoefficientCurve for details
    double staticFrictionSpeed;
    double staticFrictionCoefficient;
    double dynamicFrictionSpeed;
    double dynamicFrictionCoefficient;
    double viscousFrictionSlope;
    double veps; //Velocity at which the relaxed method is used to compute
                 //the direction vector of the tangential velocity
    RigidBodyDynamics::Addons::Geometry
        ::SmoothSegmentedFunction frictionCoefficientCurve;

    unsigned int numberOfWorkTermsInState;

};

struct pushBackStateAndTime
{
    std::vector< state_type >& states;
    std::vector< double >& times;

    pushBackStateAndTime( std::vector< state_type > &states ,
                              std::vector< double > &times )
    : states( states ) , times( times ) { }

    void operator()( const state_type &x , double t )
    {
        states.push_back( x );
        times.push_back( t );
    }
};

void f(const state_type &x, state_type &dxdt, const double t);

/* Problem Constants */
int main (int argc, char* argv[]) {
    rbdl_check_api_version (RBDL_API_VERSION);

  RigidBodyDynamics::Model model;

  std::string fileName("../model/ballPlaneContact.lua");
  std::string ballName("Ball");

  //0a. ballPlaneContact.lua model is read in
  if (!Addons::LuaModelReadFromFile(fileName.c_str(),&model)){
    std::cerr << "Error loading LuaModel: " << fileName << std::endl;
    abort();
  }

  //0b. Contact and friction parameters are set (more on these later)
  double radius = 0.5;
  Vector3d pointOnPlane = Vector3d(0.,0.,0.);
  Vector3d planeNormal  = Vector3d(0.,0.,1.);

  //Hunt-Crossley contact terms. See
  // ContactToolkit::calcHuntCrossleyContactForce for details
  double exponent = 2.0; //The spring force will increase with the deflection squared.
  double stiffness = 9.81/pow(0.01,2.); //The ball will settle to 1cm penetration
  double damping = 0.1; //lightly damped

  //Friction model terms. See
  // ContactToolkit::createRegularizedFrictionCoefficientCurve for details
  double staticFrictionSpeed        = 0.001;
  double staticFrictionCoefficient  = 0.8;
  double dynamicFrictionSpeed       = 0.01;
  double dynamicFrictionCoeffient   = 0.6;
  double viscousFrictionSlope       = 0.1;

  unsigned int numWorkTermsInState = 2; //1 normal work term
                                        //1 friction term

  VectorNd q, qd, x, tau;
  q.resize(model.dof_count);
  qd.resize(model.dof_count);
  tau.resize(model.dof_count);
  x.resize(model.dof_count*2);
  q.setZero();
  qd.setZero();
  x.setZero();
  tau.setZero();

  q[1] = 1.; //ball starts 1m off the ground
  qd[0]= 1.;

  for(unsigned int i=0; i<q.rows();++i){
    x[i] =q[i];
    x[i+q.rows()] = qd[i];
  }

  //0c. An object that Boost can integrate is instantiated:
  rbdlToBoost rbdlModel(&model,
                        ballName,
                        radius,
                        pointOnPlane,
                        planeNormal,
                        stiffness,
                        exponent,
                        damping,
                        staticFrictionSpeed,
                        staticFrictionCoefficient,
                        dynamicFrictionSpeed,
                        dynamicFrictionCoeffient,
                        viscousFrictionSlope,
                        numWorkTermsInState);

    //4a. The model state is initialized
    state_type xState(x.size()+numWorkTermsInState);
    state_type dxState(x.size()+numWorkTermsInState);
    for(unsigned int i=0; i<x.size(); ++i){
      xState[i]   = x[i];
    }

    //4b. This model is integrated forward in time from t0 to t1 and is evaluated
    //    at npts between these time points
    double t;
    double t0 = 0;
    double t1 = 1.0;
    unsigned int npts      = 100;

    double absTolVal = 1e-8;
    double relTolVal = 1e-8;

    double dt = (t1-t0)/(npts-1);
    double ke,pe,w =0;
    unsigned int k=0;

    std::vector<std::vector< double > > matrixData, matrixForceData;
    std::vector<std::vector< double > > matrixErrorData;
    std::vector< double > rowData(model.dof_count+1);
    std::vector< double > rowForceData(10);
    std::vector< double > rowErrorData(2);

    double a_x = 1.0 , a_dxdt = 1.0;
    controlled_stepper_type
    controlled_stepper(
        default_error_checker< double ,
                               range_algebra ,
                               default_operations >
        ( absTolVal , relTolVal , a_x , a_dxdt ) );

    double tp = 0;
    rowData[0] = 0;
    for(unsigned int z=0; z < model.dof_count; z++){
        rowData[z+1] = xState[z];
    }
    matrixData.push_back(rowData);

    SpatialVector fA0 = SpatialVector::Zero();
    for(unsigned int i=0; i<rowForceData.size();++i){
      rowForceData[i]=0.;
    }

    matrixForceData.push_back(rowForceData);

    double kepe0 = 0;
    double th,dth;

    //                 ,             ,             ,             ,             ,             ,             ,
    printf("Columns below:\n");
    printf("          t,        theta,   d/dt theta,           ke,           pe,            w, ke+pe-w-kepe0\n");

    for(unsigned int i=0; i<= npts; ++i){
      t = t0 + dt*i;

      integrate_adaptive(
          controlled_stepper ,
          rbdlModel , xState , tp , t , (t-tp)/10 );
      tp = t;

      //4c. At each time point the q's, qd's, and the work done on the ball by the
      //    contact & friction model is saved

      for(unsigned int j=0; j<x.rows();++j){
        x[j] = xState[j];
      }
      k=0;
      for(unsigned int j=0; j<model.q_size;++j){
        q[j] = xState[k];
        ++k;
      }
      for(unsigned int j=0; j<model.qdot_size;++j){
        qd[j] = xState[k];
        ++k;
      }
      w = 0.;
      for(unsigned int j=0; j<numWorkTermsInState;++j){
        w += xState[k];
        ++k;
      }

      //4d. The q's, qd's, and work terms are used to numerically evaluate the system
      //    energy of the ball less the work done on it: ke+pe-w, where
      //    ke is kinetic energy, pe is potential energy, and w is work.
      pe = Utils::CalcPotentialEnergy(model,
                                      q,true);

      ke = Utils::CalcKineticEnergy(model,
                                    q,
                                    qd,true);

      rowData[0] = t;
      for(unsigned int z=0; z < model.dof_count; z++){
          rowData[z+1] = xState[z];
      }
      matrixData.push_back(rowData);

    //4e. This is evaluated relative to the system energy of the ball at the
    //    beginning of the simulation. If the integrator were perfect the sum of
    //    ke+pe-w-kepe0 would be numerically zero. Because numerical integration is
    //    not perfect this quantity (printed to screen) will drift over time.
    //    You can see this if you adjust the absolute and relative tolerances
    //    (set between 4b and 4c) on the integrator and re-run the simulations.
    //    drift will be larger with looser integration tolerances.
      if(i==0) kepe0 = (ke+pe-w);

      rowErrorData[0] = t;
      rowErrorData[1] = (ke + pe -w) - kepe0;
      matrixErrorData.push_back(rowErrorData);

      printf("%e, %e, %e, %e, %e, %e, %e\n",
                  t, q[2],qd[2],ke,
                  pe,w,(ke+pe-w-kepe0));

      //Make sure the model state is up to date.
      rbdlModel(xState,dxState,tp);

      //4f. Here we grab quantities that we would like to save
      rowForceData[0] = t;
      //contact point location
      rowForceData[1] = rbdlModel.r0K0[0];
      rowForceData[2] = rbdlModel.r0K0[1];
      rowForceData[3] = rbdlModel.r0K0[2];
      //force at the contact point
      rowForceData[4] = rbdlModel.fK0n[0] + rbdlModel.fK0t[0];
      rowForceData[5] = rbdlModel.fK0n[1] + rbdlModel.fK0t[1];
      rowForceData[6] = rbdlModel.fK0n[2] + rbdlModel.fK0t[2];
      //moment at the contact point
      rowForceData[7] = 0.;//This contact model generates no contact moments
      rowForceData[8] = 0.;//This contact model generates no contact moments
      rowForceData[9] = 0.;//This contact model generates no contact moments

      matrixForceData.push_back(rowForceData);

      bool here=true;

    }
    printf("Columns above:\n");
    printf("          t,        theta,   d/dt theta,           ke,           pe,            w, ke+pe-w-kepe0\n");

    //5a. Finally we write the simulation data to file
    std::cout << std::endl;
    std::string emptyHeader("");
    std::string fileNameOut("../output/animation.csv");
    printMatrixToFile(matrixData,emptyHeader,fileNameOut);
    printf("Wrote: ../output/animation.csv (meshup animation file)\n");
    fileNameOut.assign("../output/animationForces.ff");
    printMatrixToFile(matrixForceData,emptyHeader,fileNameOut);
    printf("Wrote: ../output/animationForces.ff (meshup force file)\n");

    fileNameOut = "../output/kepe.csv";
    std::string header("time,systemEnergy,");
    printMatrixToFile(matrixErrorData,header,fileNameOut);
    printf("Wrote: ../output/kepe.csv (simulation data)\n");

   return 0;

}


#include "graph.h"
#include "grDB.h"
#include <map>
#include <vector>
#include <set>
#include <boost/unordered_map.hpp>
#include <boost/functional/hash.hpp>
#include <boost/property_map/property_map.hpp>
#include <chrono>
#include <cmath>

// Manhattan distance heuristic
template<typename Graph, typename CostType>
class distance_heuristic: public boost::astar_heuristic<Graph, CostType> {
public:
	distance_heuristic(const unordered_set<Vertex> &goals, Graph graph) :
			m_graph(graph), m_goals(goals) {
	}
	CostType operator()(Vertex u) {
		CostType result = INFINITY;
		for (Vertex m_goal : m_goals) {
			CostType dx = m_graph[m_goal].x - m_graph[u].x;
			CostType dy = m_graph[m_goal].y - m_graph[u].y;
			CostType dz = m_graph[m_goal].z - m_graph[u].z;
			CostType tmp = fabs(dx) + fabs(dy) + fabs(dz);
			if (tmp < result)
				result = tmp;
		}
		return result;
	}
private:
	Graph m_graph;
	unordered_set<Vertex> m_goals;
};

struct found_goal {
	Vertex goal;
};

struct astar_goal_visitor: public boost::default_astar_visitor {
	astar_goal_visitor(const unordered_set<Vertex> &goals) :
			m_goals(goals) {
	}
	void examine_vertex(Vertex u, const Graph &g) {
		if (m_goals.find(u) != m_goals.end()) {
			throw found_goal( { u });
		}
	}
private:
	unordered_set<Vertex> m_goals;
	// boost::unordered_map<Vertex, Vertex, vertex_hash> m_predecessor;
};

std::pair<double, vector<Vertex>> RoutingDB::a_star(Graph &graph3D,
		const Vertex &s, const unordered_set<Vertex> &goals) const {
	using namespace boost;
	typedef boost::unordered_map<Vertex, Vertex, vertex_hash> pred_map;
	pred_map predecessor;
	boost::associative_property_map<pred_map> pred_pmap(predecessor);

	typedef boost::unordered_map<Vertex, double, vertex_hash> dist_map;
	dist_map distance;
	boost::associative_property_map<dist_map> dist_pmap(distance);

	typedef boost::unordered_map<Vertex, double, vertex_hash> cost_map;
	cost_map cost;
	boost::associative_property_map<cost_map> cost_pmap(cost);

	typedef boost::unordered_map<EdgeType, double, edge_hash> wt_map;
	wt_map weight;
	EdgeIter e, eend;
	for (tie(e, eend) = edges(graph3D); e != eend; ++e) {
		weight[*e] = graph3D[*e].weight;
	}
	Graph::out_edge_iterator oe, oeend;
	tie(oe, oeend) = out_edges(s, graph3D);
	boost::associative_property_map<wt_map> wt_pmap(weight);

	astar_goal_visitor visitor(goals);

	auto heur = distance_heuristic<Graph, double>(goals, graph3D);
	vector<Vertex> vecPath;
	try {
		VertexIter ui, ui_end;
		for (boost::tie(ui, ui_end) = vertices(graph3D); ui != ui_end; ++ui) {
			put(dist_pmap, *ui, INFINITY);
			put(cost_pmap, *ui, INFINITY);
			put(pred_pmap, *ui, *ui);
			visitor.initialize_vertex(*ui, graph3D);
		}
		put(dist_pmap, s, 0);
		put(cost_pmap, s, heur(s));
		boost::astar_search_no_init_tree(graph3D, s, heur,
				weight_map(wt_pmap).predecessor_map(pred_pmap).distance_map(
						dist_pmap).visitor(visitor));
		// printf("No feasible path.\n");
		return make_pair(INFINITY, vecPath);
	} catch (found_goal &fg) {
		// Walk backwards from the goal through the predecessor chain adding
		// vertices to the solution path.
		//printf("Path found: ");
		//isFeasible = true;
		double wl = 0;
		for (Vertex u = fg.goal; u != s; u = predecessor[u]) {
			//	printf("(%d,%d,%d)-", graph3D[u].x, graph3D[u].y, graph3D[u].z);
			// auto r = edge(u, predecessor[u], graph3D);
			// assert(r.second);
			vecPath.push_back(u);
			//	graph3D[r.first].isPath = true;
			++wl;
		}
		vecPath.push_back(s);
		// printf("(%d,%d,%d)\n", graph3D[s].x, graph3D[s].y, graph3D[s].z);
		// cout << "WL = " << wl << endl;
		return make_pair(wl, vecPath);
	}
}

unordered_set<Vertex> RoutingDB::build_goals(const Vpin &vp, const int xlLim,
		const int xuLim, const int ylLim, const int yuLim,
		const int zlLim) const {
	using namespace boost;
	std::unordered_set<Vertex> goals;
	int X = xuLim - xlLim + 1, Y = yuLim - ylLim + 1;
	for (auto gv : vp.gnetGrids) {
		goals.insert(
				(gv._z - zlLim) * Y * X + (gv._y - ylLim) * X
						+ (gv._x - xlLim));
	}
	return goals;
}

Graph RoutingDB::build_graph(const size_t gnetId,
		const std::unordered_set<Gcell, HashGcell3d> &bannedPts,
		const int xlLim, const int xuLim, const int ylLim, const int yuLim,
		const int zlLim, const int zuLim) const {
	using namespace boost;
	Graph graph3D;
	// vector<double> capacities;
	// vector<Location> locations;
	// boost::property_map<Graph, boost::edge_index_t>::type edge_index_map = get(boost::edge_index, graph2D);
	// boost::property_map<Graph, boost::vertex_index_t>::type vertex_index_map = get(boost::vertex_index, graph2D);
	int X = xuLim - xlLim + 1, Y = yuLim - ylLim + 1;
	for (int z = zlLim; z <= zuLim; ++z) {
		for (int y = ylLim; y <= yuLim; ++y) {
			for (int x = xlLim; x <= xuLim; ++x) {
				Vertex v = add_vertex(graph3D);
				graph3D[v].x = x;
				graph3D[v].y = y;
				graph3D[v].z = z;
			}
		}
	}
	int cap, dem, blk;
	bool vacant, banned;
	for (int x = xlLim; x <= xuLim; ++x) {
		for (int y = ylLim; y <= yuLim; ++y) {
			if (x > xlLim) {
				for (int z = zlLim; z <= zuLim; ++z) {
					if (getRoutingDir(z) == H) {
						size_t edgeId = findEdge(x - 1, y, z);
						cap = getEdgeCap(edgeId);
						dem = getEdgeDemand(edgeId);
						blk = getEdgeBlk(edgeId);
						vacant = _gnets[gnetId]._occupiedEdges.find(edgeId)
								== _gnets[gnetId]._occupiedEdges.end();
						banned = (bannedPts.find(Gcell(x - 1, y, z))
								!= bannedPts.end()
								|| bannedPts.find(Gcell(x, y, z))
										!= bannedPts.end());
					} else {
						cap = 0;
						dem = 0;
						blk = 0;
						vacant = false;
						banned = false;
					}
					if (cap > dem + blk && vacant && !banned) {
						auto r = add_edge(
								vertex(
										(z - zlLim) * Y * X + (y - ylLim) * X
												+ (x - xlLim), graph3D),
								vertex(
										(z - zlLim) * Y * X + (y - ylLim) * X
												+ (x - 1 - xlLim), graph3D),
								graph3D);
						assert(r.second);
					}
				}
			}
			if (y > ylLim) {
				for (int z = zlLim; z <= zuLim; ++z) {
					if (getRoutingDir(z) == V) {
						size_t edgeId = findEdge(x, y - 1, z);
						cap = getEdgeCap(edgeId);
						dem = getEdgeDemand(edgeId);
						blk = getEdgeBlk(edgeId);
						vacant = _gnets[gnetId]._occupiedEdges.find(edgeId)
								== _gnets[gnetId]._occupiedEdges.end();
						banned = (bannedPts.find(Gcell(x, y - 1, z))
								!= bannedPts.end()
								|| bannedPts.find(Gcell(x, y, z))
										!= bannedPts.end());
					} else {
						cap = 0;
						dem = 0;
						blk = 0;
						vacant = false;
						banned = false;
					}
					if (cap > dem + blk && vacant && !banned) {
						auto r = add_edge(
								vertex(
										(z - zlLim) * Y * X + (y - ylLim) * X
												+ (x - xlLim), graph3D),
								vertex(
										(z - zlLim) * Y * X
												+ (y - 1 - ylLim) * X
												+ (x - xlLim), graph3D),
								graph3D);
						assert(r.second);
					}
				}
			}
			for (int z = zlLim + 1; z <= zuLim; ++z) {
				size_t viaId = findVia(x, y, z - 1);
				cap = getViaCap(viaId);
				dem = getViaDemand(viaId);
				vacant = _gnets[gnetId]._occupiedVias.find(viaId)
						== _gnets[gnetId]._occupiedVias.end();
				banned = (bannedPts.find(Gcell(x, y, z - 1)) != bannedPts.end()
						|| bannedPts.find(Gcell(x, y, z)) != bannedPts.end());
				if (cap > dem && vacant && !banned) {
					auto r = add_edge(
							vertex(
									(z - zlLim) * Y * X + (y - ylLim) * X
											+ (x - xlLim), graph3D),
							vertex(
									(z - 1 - zlLim) * Y * X + (y - ylLim) * X
											+ (x - xlLim), graph3D), graph3D);
					assert(r.second);
				}
			}
		}
	}
	return graph3D;
}

bool comp_x(Gcell i, Gcell j) {
	return i._x < j._x;
}
bool comp_y(Gcell i, Gcell j) {
	return i._y < j._y;
}

std::tuple<double, vector<int>, vector<int>, int,
		std::unordered_set<Gcell, HashGcell3d>> RoutingDB::rerouteNet(
		const Layout &layout, Vpin &vp,
		const std::unordered_set<Gcell, HashGcell3d> &bannedPts,
		const int zlLim, const int zuLim, const int maxMargin,
		const bool singleMargin, const bool writeToDB, const bool updateRoot) {
	// Connect one v-pin to its remaining net
	using namespace boost;
	auto terminals = vp.gnetVertices;
	std::unordered_set<Gcell, HashGcell3d> addlBannedPts;
	terminals.insert(Gcell(vp.xCoord, vp.yCoord, vp.zCoord));
	int margin = singleMargin ? maxMargin : 0;
	int next_margin = 0;
	double wl = INFINITY;
	bool isFeasible = false;
	double minmhd = INFINITY;
	std::pair<double, vector<Vertex>> res;
	Graph g;
	std::vector<int> addedEdges, addedVias;
	int viaID = findVia(vp.xCoord, vp.yCoord, vp.zCoord);
	if (getViaDemand(viaID) >= getViaCap(viaID)) {
		return make_tuple(wl, addedEdges, addedVias, margin, addlBannedPts); // infeasible
	}
	// add new vpin
	// addViaDemand(viaID);
	// addedVias.push_back(viaID);
	while (margin <= maxMargin) {
// cout << "margin = " << margin << endl;
		int xlLim = max(
				min_element(terminals.begin(), terminals.end(), comp_x)->_x
						- margin, 0);
		int xuLim = min(
				max_element(terminals.begin(), terminals.end(), comp_x)->_x
						+ margin, (int) layout._numTilesX - 1);
		int ylLim = max(
				min_element(terminals.begin(), terminals.end(), comp_y)->_y
						- margin, 0);
		int yuLim = min(
				max_element(terminals.begin(), terminals.end(), comp_y)->_y
						+ margin, (int) layout._numTilesY - 1);
		g = build_graph(vp.gnetID, bannedPts, xlLim, xuLim, ylLim, yuLim, zlLim,
				zuLim);
		/*for (auto v : g.m_vertices) {
		 cerr << "Vertex: " << v.m_property.x << " "
		 << v.m_property.y << " " << v.m_property.z << endl;
		 }
		 for (auto e : g.m_edges) {
		 cerr << "Edge: " << g[e.m_source].id << " " << g[e.m_target].id
		 << endl;
		 }*/

		int X = xuLim - xlLim + 1, Y = yuLim - ylLim + 1;
		Vertex start = (vp.zCoord - zlLim) * Y * X + (vp.yCoord - ylLim) * X
				+ (vp.xCoord - xlLim);
		std::unordered_set<Vertex> goals = build_goals(vp, xlLim, xuLim, ylLim,
				yuLim, zlLim);
		if (minmhd == INFINITY) {
			for (auto goal : goals) {
				double mhd = fabs(g[start].x - g[goal].x)
						+ fabs(g[start].y - g[goal].y)
						+ fabs(g[start].z - g[goal].z);
				if (mhd < minmhd)
					minmhd = mhd;
			}
		}
		res = a_star(g, start, goals);
		if (res.first < INFINITY) { // found a-star path
			wl = res.first;
// cout << "wl = " << wl << endl;
			isFeasible = true;
			next_margin = std::max(next_margin,
					std::min(maxMargin, (int) ((res.first - minmhd) / 2.0)));
		} else { // not found
			next_margin = std::max(next_margin,
					std::min(maxMargin, margin * 2 + 1));
// cout << "next_margin = " << next_margin << endl;
		}
		if (next_margin > margin)
			margin = next_margin;
		else
			break;
	}
	if (!isFeasible)
		return make_tuple(INFINITY, addedEdges, addedVias, margin,
				addlBannedPts);
	// fill addedEdges / addedVias
	auto vecPath = res.second;
	if (writeToDB) {
		vp.wlToL1 += vecPath.size();
	}
	if (vecPath.size() > 0)
		addlBannedPts.insert(
				Gcell(g[vecPath[0]].x, g[vecPath[0]].y, g[vecPath[0]].z));
	for (int i = 1; i < static_cast<int>(vecPath.size()); i++) {
		addlBannedPts.insert(
				Gcell(g[vecPath[i]].x, g[vecPath[i]].y, g[vecPath[i]].z));
		if (g[vecPath[i]].z == g[vecPath[i - 1]].z) {
			if (_dirLayers[g[vecPath[i]].z] == H) {
				assert(g[vecPath[i]].y == g[vecPath[i - 1]].y);
				for (int x = min(g[vecPath[i]].x, g[vecPath[i - 1]].x);
						x < max(g[vecPath[i]].x, g[vecPath[i - 1]].x); x++) {
					if (writeToDB) {
						int edgeId = findEdge(x, g[vecPath[i]].y,
								g[vecPath[i]].z);
						if (!(_gnets[vp.gnetID].findCleanEdge(edgeId))) {
							_gnets[vp.gnetID].setCleanEdge(edgeId,
									_edges[edgeId]);
						}
						if (!(_gnets[vp.gnetID].findRUEdge(edgeId))) {
							_gnets[vp.gnetID].setRUEdge(edgeId, _edges[edgeId]);
						}
						restoreEdge(edgeId);
						_gnets[vp.gnetID]._occupiedEdges.insert(edgeId);
						_dirty = true;
					}
					addedEdges.push_back(
							findEdge(x, g[vecPath[i]].y, g[vecPath[i]].z));
				}
			} else if (_dirLayers[g[vecPath[i]].z] == V) {
				assert(g[vecPath[i]].x == g[vecPath[i - 1]].x);
				for (int y = min(g[vecPath[i]].y, g[vecPath[i - 1]].y);
						y < max(g[vecPath[i]].y, g[vecPath[i - 1]].y); y++) {
					if (writeToDB) {
						int edgeId = findEdge(g[vecPath[i]].x, y,
								g[vecPath[i]].z);
						if (!(_gnets[vp.gnetID].findCleanEdge(edgeId))) {
							_gnets[vp.gnetID].setCleanEdge(edgeId,
									_edges[edgeId]);
						}
						if (!(_gnets[vp.gnetID].findRUEdge(edgeId))) {
							_gnets[vp.gnetID].setRUEdge(edgeId, _edges[edgeId]);
						}
						restoreEdge(edgeId);
						_gnets[vp.gnetID]._occupiedEdges.insert(edgeId);
						_dirty = true;
					}
					addedEdges.push_back(
							findEdge(g[vecPath[i]].x, y, g[vecPath[i]].z));
				}
			} else
				assert(0);
		} else {
			assert(g[vecPath[i]].y == g[vecPath[i - 1]].y);
			assert(g[vecPath[i]].x == g[vecPath[i - 1]].x);
			for (int z = min(g[vecPath[i]].z, g[vecPath[i - 1]].z);
					z < max(g[vecPath[i]].z, g[vecPath[i - 1]].z); z++) {
				if (writeToDB) {
					int viaId = findVia(g[vecPath[i]].x, g[vecPath[i]].y, z);
					if (!(_gnets[vp.gnetID].findCleanVia(viaId))) {
						_gnets[vp.gnetID].setCleanVia(viaId, _vias[viaId]);
					}
					if (!(_gnets[vp.gnetID].findRUVia(viaId))) {
						_gnets[vp.gnetID].setRUVia(viaId, _vias[viaId]);
					}
					restoreVia(viaId);
					_gnets[vp.gnetID]._occupiedVias.insert(viaId);
					_dirty = true;
				}
				addedVias.push_back(
						findVia(g[vecPath[i]].x, g[vecPath[i]].y, z));
			}
		}
	}
	if (writeToDB) {
		auto &vecPath = res.second;
		// Erase points in the middle in the same direction along the path
		vector<bool> validPt;
		validPt.assign(vecPath.size(), true);
		char prevDir = 0; // 0 = default, 1 = +x, 2 = -x, 3 = +y, 4 = -y, 5 = +z, 6 = -z, 7 = other
		if (vecPath.size() > 2) {
			for (size_t i = 1; i < vecPath.size(); ++i) {
				if (g[vecPath[i]].x > g[vecPath[i - 1]].x
						&& g[vecPath[i]].y == g[vecPath[i - 1]].y
						&& g[vecPath[i]].z == g[vecPath[i - 1]].z) {
					if (prevDir == 1) {
						validPt[i - 1] = false;
					} else {
						prevDir = 1;
					}
				} else if (g[vecPath[i]].x < g[vecPath[i - 1]].x
						&& g[vecPath[i]].y == g[vecPath[i - 1]].y
						&& g[vecPath[i]].z == g[vecPath[i - 1]].z) {
					if (prevDir == 2) {
						validPt[i - 1] = false;
					} else {
						prevDir = 2;
					}
				} else if (g[vecPath[i]].x == g[vecPath[i - 1]].x
						&& g[vecPath[i]].y > g[vecPath[i - 1]].y
						&& g[vecPath[i]].z == g[vecPath[i - 1]].z) {
					if (prevDir == 3) {
						validPt[i - 1] = false;
					} else {
						prevDir = 3;
					}
				} else if (g[vecPath[i]].x == g[vecPath[i - 1]].x
						&& g[vecPath[i]].y < g[vecPath[i - 1]].y
						&& g[vecPath[i]].z == g[vecPath[i - 1]].z) {
					if (prevDir == 4) {
						validPt[i - 1] = false;
					} else {
						prevDir = 4;
					}
				} else if (g[vecPath[i]].x == g[vecPath[i - 1]].x
						&& g[vecPath[i]].y == g[vecPath[i - 1]].y
						&& g[vecPath[i]].z > g[vecPath[i - 1]].z) {
					if (prevDir == 5) {
						// validPt[i - 1] = false;
					} else {
						prevDir = 5;
					}
				} else if (g[vecPath[i]].x == g[vecPath[i - 1]].x
						&& g[vecPath[i]].y == g[vecPath[i - 1]].y
						&& g[vecPath[i]].z < g[vecPath[i - 1]].z) {
					if (prevDir == 6) {
						// validPt[i - 1] = false;
					} else {
						prevDir = 6;
					}
				} else {
					prevDir = 7;
				}
			}
			vector<Vertex> vecPath_tmp;
			for (size_t i = 0; i < vecPath.size(); ++i) {
				if (validPt[i]) {
					vecPath_tmp.push_back(vecPath[i]);
				}
			}
			vecPath.swap(vecPath_tmp);
		}
		Gnet &gnet = _gnets[vp.gnetID];
		gnet._gWires.reserve(gnet._gWires.size() + vecPath.size() + 1);
		int addedVertex = -1; // gWireID of the added vertex, -1 if n/a.

		vector<GlobalWire>::iterator gWireItr;
// check if there is a removed vertex due to broken wires and if so, add it back
		if (updateRoot) { // this can happen only when updateRoot == true
			vector<vector<GlobalWire>::iterator> rootItrs;
			for (gWireItr = gnet._gWires.begin(); gWireItr < gnet._gWires.end();
					++gWireItr) {
				if (gWireItr->_pWireId == -1
						&& gWireItr != gnet._gWires.begin() + gnet._root) {
					rootItrs.push_back(gWireItr);
				}
			}
			auto s = vp.gnetVertices;
			for (auto gw : gnet._gWires) {
//	cout << gw._x << " " << gw._y << " " << gw._z << endl;
				s.erase(Gcell(gw._x, gw._y, gw._z));
			}
//		cout << "s.size() = " << s.size() << endl;
			if (s.size() > 1) {
				cout << s.size() << endl;
				exit(-2);
			}
			if (s.size() == 1) { // have to make a new gWire that ALL roots (except gWire[0]) point to.
				gnet._gWires.push_back(
						GlobalWire(s.begin()->_x, s.begin()->_y, s.begin()->_z,
								-1, -1));
				addedVertex = gnet._gWires.size() - 1;
				for (auto &rootItr : rootItrs) {
					rootItr->_pWireId = addedVertex;
				}
			}
		}
// Identify goal type
		for (gWireItr = gnet._gWires.begin(); gWireItr < gnet._gWires.end();
				++gWireItr) {
			if (gWireItr->_x == g[vecPath[0]].x
					&& gWireItr->_y == g[vecPath[0]].y
					&& gWireItr->_z == g[vecPath[0]].z) {
				break; // Case 0: goal is one of endpoints of a gWire
			}
		}

		if (gWireItr == gnet._gWires.end()) { // Case 1: goal in the middle of a gWire
			for (gWireItr = gnet._gWires.begin(); gWireItr < gnet._gWires.end();
					++gWireItr) {
				if (gWireItr->_pWireId != -1) {
					int &x1 = gWireItr->_x, &y1 = gWireItr->_y, &z1 =
							gWireItr->_z;
					int &x2 = gnet._gWires[gWireItr->_pWireId]._x, &y2 =
							gnet._gWires[gWireItr->_pWireId]._y, &z2 =
							gnet._gWires[gWireItr->_pWireId]._z;
					int &x0 = g[vecPath[0]].x, &y0 = g[vecPath[0]].y, &z0 =
							g[vecPath[0]].z;
					if ((x1 != x2 && y0 == y1 && z0 == z1 && min(x1, x2) < x0
							&& x0 < max(x1, x2))
							|| (y1 != y2 && x0 == x1 && z0 == z1
									&& min(y1, y2) < y0 && y0 < max(y1, y2))
							|| (z1 != z2 && x0 == x1 && y0 == y1
									&& min(z1, z2) < z0 && z0 < max(z1, z2))) {
						// make a new gWire in the middle
						gnet._gWires.push_back(
								GlobalWire(x0, y0, z0, gWireItr->_pWireId, -1));
						gWireItr->_pWireId = gnet._gWires.size() - 1;
						gWireItr = gnet._gWires.end() - 1;
						break;
					}
				}
			}
		}
		assert(gWireItr < gnet._gWires.end());
		if (updateRoot) {
// Mode 2: move the root, make the root point to the new vertex, update the root
// search for all roots due to broken wires
			auto rootItr = gWireItr;
			vector<int> gWireIDsToReverse;
			if (rootItr < gnet._gWires.end()) { // Case 0 or Case 1
				while (rootItr->_pWireId != -1) {
					gWireIDsToReverse.push_back(rootItr - gnet._gWires.begin());
					rootItr = gnet._gWires.begin() + rootItr->_pWireId;
				}
				gWireIDsToReverse.push_back(rootItr - gnet._gWires.begin());
			}

			gnet._gWires[gWireIDsToReverse[0]]._pWireId = -1;
			for (auto itr = gWireIDsToReverse.begin() + 1;
					itr < gWireIDsToReverse.end(); ++itr) {
// Reverse the directions of each gWire
				gnet._gWires[(*itr)]._pWireId = *(itr - 1);
			}
			if (gnet._gWires[gnet._root]._pWireId != -1) {
				gnet._root = gWireIDsToReverse[0];
				cout << "<I> Root changed to: " << gnet._root << endl;
			}
// let the new gWire point to the last gwire in the list to reverse,
// finishing the reversing process.
//if (addedVertex != -1) {
//	gnet._gWires[addedVertex]._pWireId =
//			*(gWireIDsToReverse.end() - 1);
//}

// expose the new root and prepare for adding the new path.
			gWireItr = gnet._gWires.begin() + gWireIDsToReverse[0];

			for (auto itr = vecPath.begin() + 1; itr < vecPath.end(); ++itr) {
// Add edge (*itr, *(itr-1)) to DB (gnet/addEdgeDemand)
				gnet._gWires.push_back(
						GlobalWire(g[*itr].x, g[*itr].y, g[*itr].z, -1, -1));
				gWireItr->_pWireId = gnet._gWires.size() - 1;
				gWireItr = gnet._gWires.end() - 1;
			}
		} else {
// Mode 1: make the new vertex point to the root, keep the root
			int pWireId = gWireItr - gnet._gWires.begin();
			for (auto itr = vecPath.begin() + 1; itr < vecPath.end(); ++itr) {
// Add edge (*itr, *(itr+1)) to DB (gnet/addEdgeDemand)
				gnet._gWires.push_back(
						GlobalWire(g[*itr].x, g[*itr].y, g[*itr].z, pWireId,
								-1));
				pWireId = gnet._gWires.size() - 1;
			}
		}
	}
	return make_tuple(wl, addedEdges, addedVias, margin, addlBannedPts);
}

std::tuple<double, vector<int>, vector<int>, int> RoutingDB::rerouteNet(
		const Layout &layout, const Vpin &vp1, const Vpin &vp2, const int zlLim,
		const int zuLim, const int maxMargin, const bool singleMargin,
		const bool writeToDB) {
// Connect two v-pins
	using namespace boost;
	int margin = singleMargin ? maxMargin : 0;
	int next_margin = 0;
	double wl = INFINITY;
	bool isFeasible = false;
	double minmhd = INFINITY;
	std::pair<double, vector<Vertex>> res;
	std::vector<int> addedEdges, addedVias;
	Graph g;
	std::unordered_set<Gcell, HashGcell3d> bannedPts; // empty set
	while (margin <= maxMargin) {
// cout << "[2vp] margin = " << margin << endl;
		int xlLim = max(min(vp1.xCoord, vp2.xCoord) - margin, 0);
		int xuLim = min(max(vp1.xCoord, vp2.xCoord) + margin,
				(int) layout._numTilesX - 1);
		int ylLim = max(min(vp1.yCoord, vp2.yCoord) - margin, 0);
		int yuLim = min(max(vp1.yCoord, vp2.yCoord) + margin,
				(int) layout._numTilesY - 1);
		g = build_graph(vp1.gnetID, bannedPts, xlLim, xuLim, ylLim, yuLim,
				zlLim, zuLim);
		int X = xuLim - xlLim + 1, Y = yuLim - ylLim + 1;
		Vertex start = (vp1.zCoord + 1 - zlLim) * Y * X
				+ (vp1.yCoord - ylLim) * X + (vp1.xCoord - xlLim);
		Vertex goal = (vp2.zCoord + 1 - zlLim) * Y * X
				+ (vp2.yCoord - ylLim) * X + (vp2.xCoord - xlLim);
		if (minmhd == INFINITY) {
			minmhd = fabs(g[start].x - g[goal].x) + fabs(g[start].y - g[goal].y)
					+ fabs(g[start].z - g[goal].z);
		}
		std::unordered_set<Vertex> goals = { goal };
		res = a_star(g, start, goals);
		if (res.first < INFINITY) { // found a-star path
			wl = res.first;
// cout << "wl = " << wl << endl;
			isFeasible = true;
			next_margin = std::max(next_margin,
					std::min(maxMargin, (int) ((res.first - minmhd) / 2.0)));
		} else { // not found
			next_margin = std::max(next_margin,
					std::min(maxMargin, margin * 2 + 1));
// cout << "next_margin = " << next_margin << endl;
		}
		if (next_margin > margin)
			margin = next_margin;
		else
			break;
	}
	if (!isFeasible)
		return make_tuple(INFINITY, addedEdges, addedVias, margin);
// add the new vpin
	if (writeToDB) {
		int viaId = findVia(vp1.xCoord, vp1.yCoord, vp1.zCoord);
		if (!(_gnets[vp1.gnetID].findCleanVia(viaId))) {
			_gnets[vp1.gnetID].setCleanVia(viaId, _vias[viaId]);
		}
		if (!(_gnets[vp1.gnetID].findRUVia(viaId))) {
			_gnets[vp1.gnetID].setRUVia(viaId, _vias[viaId]);
		}
		restoreVia(viaId);
		_dirty = true;
	}
	addedVias.push_back(findVia(vp1.xCoord, vp1.yCoord, vp1.zCoord));

	auto &vecPath = res.second;
	// Erase points in the middle in the same direction along the path
	vector<bool> validPt;
	validPt.assign(vecPath.size(), true);
	char prevDir = 0; // 0 = default, 1 = +x, 2 = -x, 3 = +y, 4 = -y, 5 = +z, 6 = -z, 7 = other
	if (vecPath.size() > 2) {
		for (size_t i = 1; i < vecPath.size(); ++i) {
			if (g[vecPath[i]].x > g[vecPath[i - 1]].x
					&& g[vecPath[i]].y == g[vecPath[i - 1]].y
					&& g[vecPath[i]].z == g[vecPath[i - 1]].z) {
				if (prevDir == 1) {
					validPt[i - 1] = false;
				} else {
					prevDir = 1;
				}
			} else if (g[vecPath[i]].x < g[vecPath[i - 1]].x
					&& g[vecPath[i]].y == g[vecPath[i - 1]].y
					&& g[vecPath[i]].z == g[vecPath[i - 1]].z) {
				if (prevDir == 2) {
					validPt[i - 1] = false;
				} else {
					prevDir = 2;
				}
			} else if (g[vecPath[i]].x == g[vecPath[i - 1]].x
					&& g[vecPath[i]].y > g[vecPath[i - 1]].y
					&& g[vecPath[i]].z == g[vecPath[i - 1]].z) {
				if (prevDir == 3) {
					validPt[i - 1] = false;
				} else {
					prevDir = 3;
				}
			} else if (g[vecPath[i]].x == g[vecPath[i - 1]].x
					&& g[vecPath[i]].y < g[vecPath[i - 1]].y
					&& g[vecPath[i]].z == g[vecPath[i - 1]].z) {
				if (prevDir == 4) {
					validPt[i - 1] = false;
				} else {
					prevDir = 4;
				}
			} else if (g[vecPath[i]].x == g[vecPath[i - 1]].x
					&& g[vecPath[i]].y == g[vecPath[i - 1]].y
					&& g[vecPath[i]].z > g[vecPath[i - 1]].z) {
				if (prevDir == 5) {
					// validPt[i - 1] = false;
				} else {
					prevDir = 5;
				}
			} else if (g[vecPath[i]].x == g[vecPath[i - 1]].x
					&& g[vecPath[i]].y == g[vecPath[i - 1]].y
					&& g[vecPath[i]].z < g[vecPath[i - 1]].z) {
				if (prevDir == 6) {
					// validPt[i - 1] = false;
				} else {
					prevDir = 6;
				}
			} else {
				prevDir = 7;
			}
		}
		vector<Vertex> vecPath_tmp;
		for (size_t i = 0; i < vecPath.size(); ++i) {
			if (validPt[i]) {
				vecPath_tmp.push_back(vecPath[i]);
			}
		}
		vecPath.swap(vecPath_tmp);
	}
	for (int i = 1; i < static_cast<int>(vecPath.size()); i++) {
		if (g[vecPath[i]].z == g[vecPath[i - 1]].z) {
			if (_dirLayers[g[vecPath[i]].z] == H) {
				assert(g[vecPath[i]].y == g[vecPath[i - 1]].y);
				for (int x = min(g[vecPath[i]].x, g[vecPath[i - 1]].x);
						x < max(g[vecPath[i]].x, g[vecPath[i - 1]].x); x++) {
					if (writeToDB) {
						int edgeId = findEdge(x, g[vecPath[i]].y,
								g[vecPath[i]].z);
						if (!(_gnets[vp1.gnetID].findCleanEdge(edgeId))) {
							_gnets[vp1.gnetID].setCleanEdge(edgeId,
									_edges[edgeId]);
						}
						if (!(_gnets[vp1.gnetID].findRUEdge(edgeId))) {
							_gnets[vp1.gnetID].setRUEdge(edgeId,
									_edges[edgeId]);
						}
						restoreEdge(edgeId);
						_dirty = true;
					}
					addedEdges.push_back(
							findEdge(x, g[vecPath[i]].y, g[vecPath[i]].z));

				}
			} else if (_dirLayers[g[vecPath[i]].z] == V) {
				assert(g[vecPath[i]].x == g[vecPath[i - 1]].x);
				for (int y = min(g[vecPath[i]].y, g[vecPath[i - 1]].y);
						y < max(g[vecPath[i]].y, g[vecPath[i - 1]].y); y++) {
					if (writeToDB) {
						int edgeId = findEdge(g[vecPath[i]].x, y,
								g[vecPath[i]].z);
						if (!(_gnets[vp1.gnetID].findCleanEdge(edgeId))) {
							_gnets[vp1.gnetID].setCleanEdge(edgeId,
									_edges[edgeId]);
						}
						if (!(_gnets[vp1.gnetID].findRUEdge(edgeId))) {
							_gnets[vp1.gnetID].setRUEdge(edgeId,
									_edges[edgeId]);
						}
						restoreEdge(edgeId);
						_dirty = true;
					}
					addedEdges.push_back(
							findEdge(g[vecPath[i]].x, y, g[vecPath[i]].z));
				}
			} else
				assert(0);
		} else {
			assert(g[vecPath[i]].y == g[vecPath[i - 1]].y);
			assert(g[vecPath[i]].x == g[vecPath[i - 1]].x);
			for (int z = min(g[vecPath[i]].z, g[vecPath[i - 1]].z);
					z < max(g[vecPath[i]].z, g[vecPath[i - 1]].z); z++) {
				if (writeToDB) {
					int viaId = findVia(g[vecPath[i]].x, g[vecPath[i]].y, z);
					if (!(_gnets[vp1.gnetID].findCleanVia(viaId))) {
						_gnets[vp1.gnetID].setCleanVia(viaId, _vias[viaId]);
					}
					if (!(_gnets[vp1.gnetID].findRUVia(viaId))) {
						_gnets[vp1.gnetID].setRUVia(viaId, _vias[viaId]);
					}
					restoreVia(findVia(g[vecPath[i]].x, g[vecPath[i]].y, z));
					_dirty = true;
				}
				addedVias.push_back(
						findVia(g[vecPath[i]].x, g[vecPath[i]].y, z));
			}
		}
	}
	if (writeToDB) {
		auto vecPath = res.second;
		Gnet &gnet = _gnets[vp1.gnetID];
		gnet._gWires.reserve(gnet._gWires.size() + vecPath.size());
		vector<GlobalWire>::iterator gWireItr1, gWireItr2;
		// start
		for (gWireItr1 = gnet._gWires.begin(); gWireItr1 < gnet._gWires.end();
				++gWireItr1) {
			if (gWireItr1->_x == g[vecPath.back()].x
					&& gWireItr1->_y == g[vecPath.back()].y
					&& gWireItr1->_z == g[vecPath.back()].z - 1) {
				break;
			}
		}
		// goal
		for (gWireItr2 = gnet._gWires.begin(); gWireItr2 < gnet._gWires.end();
				++gWireItr2) {
			if (gWireItr2->_x == g[vecPath[0]].x
					&& gWireItr2->_y == g[vecPath[0]].y
					&& gWireItr2->_z == g[vecPath[0]].z) {
				break;
			}
		}
		assert((gWireItr1->_pWireId == -1) != (gWireItr2->_pWireId == -1));
		if (gWireItr2->_pWireId == -1) {
// cout << "Mode 2" << endl;
// Mode 2
			if (vecPath.size() == 1) {
				gWireItr2->_pWireId = gWireItr1 - gnet._gWires.begin();
			} else {
				for (auto itr = vecPath.begin() + 1; itr < vecPath.end() - 1;
						++itr) {
// Add edge (*itr, *(itr+1)) to DB (gnet/addEdgeDemand)
					gnet._gWires.push_back(
							GlobalWire(g[*itr].x, g[*itr].y, g[*itr].z, -1,
									-1));
					gWireItr2->_pWireId = gnet._gWires.size() - 1;
					gWireItr2 = gnet._gWires.end() - 1;
				}
				gnet._gWires.push_back(
						GlobalWire(g[vecPath.back()].x, g[vecPath.back()].y,
								g[vecPath.back()].z,
								gWireItr1 - gnet._gWires.begin(), -1));
				gWireItr2->_pWireId = gnet._gWires.size() - 1;
			}
		} else {
// cout << "Mode 1" << endl;
// Mode 1
			if (vecPath.size() == 1) {
				gWireItr1->_pWireId = gWireItr2 - gnet._gWires.begin();
			} else {
				std::reverse(vecPath.begin(), vecPath.end());
				for (auto itr = vecPath.begin(); itr < vecPath.end() - 2;
						++itr) {
// Add edge (*itr, *(itr+1)) to DB (gnet/addEdgeDemand)
					gnet._gWires.push_back(
							GlobalWire(g[*itr].x, g[*itr].y, g[*itr].z, -1,
									-1));
					gWireItr1->_pWireId = gnet._gWires.size() - 1;
					gWireItr1 = gnet._gWires.end() - 1;
				}
				gnet._gWires.push_back(
						GlobalWire(g[vecPath[vecPath.size() - 2]].x,
								g[vecPath[vecPath.size() - 2]].y,
								g[vecPath[vecPath.size() - 2]].z,
								gWireItr2 - gnet._gWires.begin(), -1));
				gWireItr1->_pWireId = gnet._gWires.size() - 1;
			}
		}
		restoreNet(vp1.gnetID, gnet, _gnets[vp1.gnetID]._cleanEdges,
				_gnets[vp1.gnetID]._cleanVias, false);
	}
	return make_tuple(wl + 1, addedEdges, addedVias, margin); // including WL of the new v-pin
}


/*
 * car_model_propagator.hpp
 *
 * Created on: Oct 30, 2018 22:52
 * Description:
 *
 * Copyright (c) 2018 Ruixiang Du (rdu)
 */

#ifndef CAR_MODEL_PROPAGATOR_HPP
#define CAR_MODEL_PROPAGATOR_HPP

#include <cstdint>
#include <vector>

#include <boost/numeric/odeint.hpp>

#include "reachability/details/car_longitudinal_model.hpp"

namespace robotnav {
class CarModelPropagator {
 public:
  CarLongitudinalModel::state_type Propagate(CarLongitudinalModel::state_type init_state,
                       CarLongitudinalModel::control_type u, double t0,
                       double tf, double dt) {
    double t = t0;
    CarLongitudinalModel::state_type x = init_state;

    while (t <= tf) {
      //   integrator_(CarLongitudinalModel(u), x, t, dt);

      boost::numeric::odeint::integrate_const(
          boost::numeric::odeint::runge_kutta4<
              CarLongitudinalModel::state_type>(),
          CarLongitudinalModel(u), x, t, t+dt, dt/10.0);

      // add additional constraint to s, v: s >= s0, v >=0, v < v_max
      if (x[0] < init_state[0]) x[0] = init_state[0];
      if (x[1] < 0) x[1] = 0;
      if (x[1] > CarLongitudinalModel::v_max)
        x[1] = CarLongitudinalModel::v_max;
    }

    return x;
  }
};
}  // namespace robotnav

#endif /* CAR_MODEL_PROPAGATOR_HPP */