/*! \file Peridigm_EnergyReleaseDamageCorrepondenceModel.cpp */

//@HEADER
// ************************************************************************
//
//                             Peridigm
//                 Copyright (2011) Sandia Corporation
//
// Under the terms of Contract DE-AC04-94AL85000 with Sandia Corporation,
// the U.S. Government retains certain rights in this software.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// 3. Neither the name of the Corporation nor the names of the
// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY SANDIA CORPORATION "AS IS" AND ANY
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL SANDIA CORPORATION OR THE
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// Questions?
// David J. Littlewood   djlittl@sandia.gov
// John A. Mitchell      jamitch@sandia.gov
// Michael L. Parks      mlparks@sandia.gov
// Stewart A. Silling    sasilli@sandia.gov
//
// ************************************************************************
// Author of this Routine
// Christian Willberg   christian.willberg@dlr.de
// German Aerospace Center
//@HEADER

#include "Peridigm_EnergyReleaseDamageCorrepondenceModel.hpp"
#include "Peridigm_Field.hpp"
#include "material_utilities.h"
#include "correspondence.h"
#include <thread>
#include <Teuchos_Assert.hpp>
#include <Epetra_SerialComm.h>
#include <Sacado.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

using namespace std;

PeridigmNS::EnergyReleaseDamageCorrepondenceModel::EnergyReleaseDamageCorrepondenceModel(const Teuchos::ParameterList& params)
: DamageModel(params),
m_applyThermalStrains(false),
m_modelCoordinatesFieldId(-1),
m_coordinatesFieldId(-1),
m_damageFieldId(-1),
m_bondDamageFieldId(-1),
m_deltaTemperatureFieldId(-1),
m_dilatationFieldId(-1),
m_weightedVolumeFieldId(-1),
m_horizonFieldId(-1),
m_piolaStressTimesInvShapeTensorXId(-1),
m_piolaStressTimesInvShapeTensorYId(-1),
m_piolaStressTimesInvShapeTensorZId(-1),
m_detachedNodesFieldId(-1),
m_forceDensityFieldId(-1),
m_deformationGradientFieldId(-1),
m_hourglassStiffId(-1),
m_OMEGA(PeridigmNS::InfluenceFunction::self().getInfluenceFunction()) {

    if (params.isParameter("Critical Energy")) {
        m_criticalEnergyTension = params.get<double>("Critical Energy");

    }
    if (params.isParameter("Degradation Factor")){
        degradationFactor = 1;
        degradationFactor = params.get<double>("Degradation Factor");

    }else{
        degradationFactor = 1;
    }
    m_incremental = true;
    if (params.isParameter("Incremental")){
       m_incremental = params.get<bool>("Incremental");
    }
    m_criticalEnergyTension = params.get<double>("Critical Energy");
    m_hourglassCoefficient = params.get<double>("Hourglass Coefficient");
    m_planeStrain=false;
    m_planeStress=false;
    detachedNodesCheck = false;
    if (params.isParameter("Detached Nodes Check"))
        detachedNodesCheck = params.get<bool>("Detached Nodes Check");
    if (params.isParameter("Plane Strain"))
      m_planeStrain = params.get<bool>("Plane Strain");
    if (params.isParameter("Plane Stress"))
      m_planeStress = params.get<bool>("Plane Stress");
    m_plane = false;

    m_onlyTension = false;
    if(params.isParameter("Only Tension")){
        m_onlyTension = params.get<bool>("Only Tension");

    }

  //************************************
  // wie komme ich an den Namen??
  //************************************
  //if (params.isParameter("Linear Elastic Correspondence")){

    //std::cout<<"Use Material: Linear Elastic Correspondence"<<std::endl;

    if (m_planeStrain==true){
        //m_plane=true;
        m_plane = true;
        m_Thickness = params.get<double>("Thickness");
        //std::cout<<"Method 2D Plane Strain"<<std::endl;
    }
    if (m_planeStress==true){
       // m_plane=true;
        m_plane = true;
        m_Thickness = params.get<double>("Thickness");
        //std::cout<<"WRN: Method 2D Plane Stress --> not fully implemented yet"<<std::endl;
    }

// Params anpassen. Irgendwo muss das befuellt werden, da bspw. die thermische Verformung bei Material
// und Schaden existiert
//
//

    m_pi = 3.14159;

    if (params.isParameter("Thermal Expansion Coefficient")) {
        m_alpha = params.get<double>("Thermal Expansion Coefficient");
        m_applyThermalStrains = true;
    }

    PeridigmNS::FieldManager& fieldManager = PeridigmNS::FieldManager::self();
    m_modelCoordinatesFieldId = fieldManager.getFieldId(PeridigmField::NODE, PeridigmField::VECTOR, PeridigmField::CONSTANT,"Model_Coordinates");
    m_coordinatesFieldId = fieldManager.getFieldId(PeridigmField::NODE, PeridigmField::VECTOR, PeridigmField::TWO_STEP, "Coordinates");
    m_volumeFieldId = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::SCALAR, PeridigmField::CONSTANT, "Volume");
    m_weightedVolumeFieldId = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::SCALAR, PeridigmField::CONSTANT, "Weighted_Volume");
    m_dilatationFieldId = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::SCALAR, PeridigmField::TWO_STEP, "Dilatation");
    m_damageFieldId = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::SCALAR, PeridigmField::TWO_STEP, "Damage");
    m_bondDamageFieldId = fieldManager.getFieldId(PeridigmField::BOND, PeridigmField::SCALAR, PeridigmField::TWO_STEP, "Bond_Damage");
    m_horizonFieldId = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::SCALAR, PeridigmField::CONSTANT, "Horizon");
    m_piolaStressTimesInvShapeTensorXId   = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::VECTOR, PeridigmField::TWO_STEP, "PiolaStressTimesInvShapeTensorX");
    m_piolaStressTimesInvShapeTensorYId   = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::VECTOR, PeridigmField::TWO_STEP, "PiolaStressTimesInvShapeTensorY");
    m_piolaStressTimesInvShapeTensorZId   = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::VECTOR, PeridigmField::TWO_STEP, "PiolaStressTimesInvShapeTensorZ");
    m_detachedNodesFieldId              = fieldManager.getFieldId(PeridigmField::NODE,    PeridigmField::SCALAR, PeridigmField::TWO_STEP, "Detached_Nodes");
    m_forceDensityFieldId               = fieldManager.getFieldId(PeridigmField::NODE,    PeridigmField::VECTOR, PeridigmField::TWO_STEP, "Force_Density");
    m_deformationGradientFieldId        = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::FULL_TENSOR, PeridigmField::CONSTANT, "Deformation_Gradient");
    m_hourglassStiffId                  = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::FULL_TENSOR, PeridigmField::CONSTANT, "Hourglass_Stiffness");

    m_fieldIds.push_back(m_volumeFieldId);
    m_fieldIds.push_back(m_modelCoordinatesFieldId);
    m_fieldIds.push_back(m_coordinatesFieldId);
    m_fieldIds.push_back(m_weightedVolumeFieldId);
    m_fieldIds.push_back(m_dilatationFieldId);
    m_fieldIds.push_back(m_volumeFieldId);
    m_fieldIds.push_back(m_damageFieldId);
    m_fieldIds.push_back(m_detachedNodesFieldId);
    m_fieldIds.push_back(m_bondDamageFieldId);
    m_fieldIds.push_back(m_horizonFieldId);
    m_fieldIds.push_back(m_piolaStressTimesInvShapeTensorXId);
    m_fieldIds.push_back(m_piolaStressTimesInvShapeTensorYId);
    m_fieldIds.push_back(m_piolaStressTimesInvShapeTensorZId);
    m_fieldIds.push_back(m_forceDensityFieldId);
    m_fieldIds.push_back(m_hourglassStiffId);
    m_fieldIds.push_back(m_deformationGradientFieldId);

}

PeridigmNS::EnergyReleaseDamageCorrepondenceModel::~EnergyReleaseDamageCorrepondenceModel() {
}

void
PeridigmNS::EnergyReleaseDamageCorrepondenceModel::initialize(const double dt,
        const int numOwnedPoints,
        const int* ownedIDs,
        const int* neighborhoodList,
        PeridigmNS::DataManager& dataManager) const {
    double *damage, *bondDamage;

    dataManager.getData(m_damageFieldId, PeridigmField::STEP_NP1)->ExtractView(&damage);
    dataManager.getData(m_bondDamageFieldId, PeridigmField::STEP_NP1)->ExtractView(&bondDamage);

    dataManager.getData(m_piolaStressTimesInvShapeTensorXId, PeridigmField::STEP_NP1)->PutScalar(0.0);
    dataManager.getData(m_piolaStressTimesInvShapeTensorYId, PeridigmField::STEP_NP1)->PutScalar(0.0);
    dataManager.getData(m_piolaStressTimesInvShapeTensorZId, PeridigmField::STEP_NP1)->PutScalar(0.0);
    dataManager.getData(m_detachedNodesFieldId, PeridigmField::STEP_NP1)->PutScalar(0.0);
    // Initialize damage to zero
    int neighborhoodListIndex(0);
    int bondIndex(0);
    int nodeId, numNeighbors;
    int iID, iNID;

    for (iID = 0; iID < numOwnedPoints; ++iID) {
        nodeId = ownedIDs[iID];
        damage[nodeId] = 0.0;
        numNeighbors = neighborhoodList[neighborhoodListIndex++];
        neighborhoodListIndex += numNeighbors;
        for (iNID = 0; iNID < numNeighbors; ++iNID) {
            bondDamage[bondIndex] = 0.0;
            bondIndex += 1;
        }
    }

}

void
PeridigmNS::EnergyReleaseDamageCorrepondenceModel::computeDamage(const double dt,
        const int numOwnedPoints,
        const int* ownedIDs,
        const int* neighborhoodList,
        PeridigmNS::DataManager& dataManager) const {

    double *x, *y, *damage, *bondDamageNP1, *horizon, *vol, *detachedNodes;

    double criticalEnergyTension(-1.0);
    // for temperature dependencies easy to extent
    double *deltaTemperature = NULL;
    double *tempStressX, *tempStressY, *tempStressZ;
    dataManager.getData(m_damageFieldId, PeridigmField::STEP_NP1)->ExtractView(&damage);

    dataManager.getData(m_modelCoordinatesFieldId, PeridigmField::STEP_NONE)->ExtractView(&x);

    dataManager.getData(m_coordinatesFieldId, PeridigmField::STEP_NP1)->ExtractView(&y);
    dataManager.getData(m_volumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&vol);

    dataManager.getData(m_horizonFieldId, PeridigmField::STEP_NONE)->ExtractView(&horizon);
    dataManager.getData(m_bondDamageFieldId, PeridigmField::STEP_NP1)->ExtractView(&bondDamageNP1);

    dataManager.getData(m_piolaStressTimesInvShapeTensorXId, PeridigmField::STEP_NP1)->ExtractView(&tempStressX);
    dataManager.getData(m_piolaStressTimesInvShapeTensorYId, PeridigmField::STEP_NP1)->ExtractView(&tempStressY);
    dataManager.getData(m_piolaStressTimesInvShapeTensorZId, PeridigmField::STEP_NP1)->ExtractView(&tempStressZ);
    dataManager.getData(m_detachedNodesFieldId, PeridigmField::STEP_NP1)->ExtractView(&detachedNodes);
    ////////////////////////////////////////////////////////
    // Hourglass control
    // not synchronized yet; Hourglass correction is used only from nodeId site
    //
    double *defGrad, *hourglassStiff;
    vector<double> TSvector(3), hourglassStiffVector(9);
    double* hStiff = &hourglassStiffVector[0];
    double* TS = &TSvector[0];
    ///////////////////////////////////////////////////////
    dataManager.getData(m_hourglassStiffId, PeridigmField::STEP_NONE)->ExtractView(&hourglassStiff);
    dataManager.getData(m_deformationGradientFieldId, PeridigmField::STEP_NONE)->ExtractView(&defGrad);
    //std::cout<< "heredam"<<std::endl;
    // Set the bond damage to the previous value --> needed for iteration in implicit time integration
    *(dataManager.getData(m_bondDamageFieldId, PeridigmField::STEP_NP1)) = *(dataManager.getData(m_bondDamageFieldId, PeridigmField::STEP_N));
     double *forceDensity;
    dataManager.getData(m_forceDensityFieldId, PeridigmField::STEP_NP1)->ExtractView(&forceDensity);

    ////////////////////////////////////////////////////
    double trialDamage(0.0);
    int neighborhoodListIndex(0), bondIndex(0);
    int nodeId, numNeighbors, neighborID, iID, iNID;
    double totalDamage;
    double nodeInitialX[3], nodeCurrentX[3];
    double omegaP1, omegaP2;
    double critIso;

    double quadhorizon;
    // bond force density state
    double TX, TY, TZ, TXN, TYN, TZN;
    // bond force density state projected to deformed bond
    double TPX, TPY, TPZ, TPXN, TPYN, TPZN;
    double factor, factorN;
    // deformed state
    double Y_dx, Y_dy, Y_dz;
    // initial state
    double X_dx, X_dy, X_dz;
    // displacement vector state and abolute value
    double etaX, etaY, etaZ, dEta, normEtaSq;
    double bondEnergy;
    double dX, dY;//, dYSq;

    //---------------------------
    // INITIALIZE PROCESS STEP t
    //---------------------------
    // Foster 2009 Journal for Multiscale Computational Engineering
    // m_criticalEnergy = 4 G / (pi delta^4)
    // what if two horizons meet? two criterions will hit the same bond..
    //double quadhorizon =  4 /( m_pi * m_horizon * m_horizon * m_horizon * m_horizon );

    if (m_criticalEnergyTension > 0.0)
        criticalEnergyTension = m_criticalEnergyTension;

    // Update the bond damage
    // Break bonds if the bond energy potential is greater than the critical bond energy potential
    //---------------------------
    // DAMAGE ANALYSIS
    //---------------------------
    bondIndex = 0;

    for (iID = 0; iID < numOwnedPoints; ++iID, defGrad+=9, hourglassStiff+=9) {
        numNeighbors = neighborhoodList[neighborhoodListIndex++];

        nodeId = ownedIDs[iID];
        nodeInitialX[0] = x[nodeId*3];
        nodeInitialX[1] = x[nodeId*3+1];
        nodeInitialX[2] = x[nodeId*3+2];
        nodeCurrentX[0] = y[nodeId*3];
        nodeCurrentX[1] = y[nodeId*3+1];
        nodeCurrentX[2] = y[nodeId*3+2];

        for (iNID = 0; iNID < numNeighbors; ++iNID) {

            neighborID = neighborhoodList[neighborhoodListIndex++];
            if (detachedNodes[nodeId]!=0) continue;
            if (detachedNodes[neighborID]!=0) continue;
            X_dx = x[neighborID*3]   - nodeInitialX[0];
            X_dy = x[neighborID*3+1] - nodeInitialX[1];
            X_dz = x[neighborID*3+2] - nodeInitialX[2];

            Y_dx = y[neighborID*3]   - nodeCurrentX[0];
            Y_dy = y[neighborID*3+1] - nodeCurrentX[1];
            Y_dz = y[neighborID*3+2] - nodeCurrentX[2];
            etaX  = (Y_dx-X_dx);
            etaY  = (Y_dy-X_dy);
            etaZ  = (Y_dz-X_dz);

            //double uNx = y[neighborID*3]   - x[neighborID*3];
            //double uNy = y[neighborID*3+1] - x[neighborID*3+1];
            //double uNz = y[neighborID*3+2] - x[neighborID*3+2];

            dX = distance(nodeInitialX[0], nodeInitialX[1], nodeInitialX[2], x[neighborID*3], x[neighborID*3+1], x[neighborID*3+2]);
            dY = distance(nodeCurrentX[0], nodeCurrentX[1], nodeCurrentX[2], y[neighborID*3], y[neighborID*3+1], y[neighborID*3+2]);
            dEta = dY-dX;

            normEtaSq = etaX*etaX+etaY*etaY+etaZ*etaZ;

            bool Tension = true;

            if (m_onlyTension == true && dEta<0) Tension = false;
            if (Tension == true){
                if (normEtaSq>0){
                    omegaP1 = MATERIAL_EVALUATION::scalarInfluenceFunction(dX, horizon[nodeId]);
                    omegaP2 = MATERIAL_EVALUATION::scalarInfluenceFunction(-dX, horizon[neighborID]);
                    // average Force has to be taken
                    // if not the case where
                    if (m_plane == true) X_dz = 0;
                    double temp = 0.5*(1-bondDamageNP1[bondIndex]);

                    double FxsiX = *(defGrad)   * X_dx + *(defGrad+1) * X_dy + *(defGrad+2) * X_dz;
                    double FxsiY = *(defGrad+3) * X_dx + *(defGrad+4) * X_dy + *(defGrad+5) * X_dz;
                    double FxsiZ = *(defGrad+6) * X_dx + *(defGrad+7) * X_dy + *(defGrad+8) * X_dz;
                    hStiff[0] = *(hourglassStiff  ); hStiff[1] = *(hourglassStiff+1);  hStiff[2] = *(hourglassStiff+2);
                    hStiff[3] = *(hourglassStiff+3); hStiff[4] = *(hourglassStiff+4);  hStiff[5] = *(hourglassStiff+5);
                    hStiff[6] = *(hourglassStiff+6); hStiff[7] = *(hourglassStiff+7);  hStiff[8] = *(hourglassStiff+8);
                    CORRESPONDENCE::computeCorrespondenceStabilityWanEtAlShort(FxsiX,FxsiY,FxsiZ,Y_dx,Y_dy,Y_dz,hStiff,TS);
               // std::cout<< TS[0]<<" dam"<< TS[1]<<std::endl;
                    // volumes, or volume relation to include? should be inside via the shape tensor, which is included in tempStress
                    TX  =   omegaP1 * ( tempStressX[3*nodeId]     * X_dx + tempStressX[3*nodeId+1]     * X_dy + tempStressX[3*nodeId+2]     * X_dz + m_hourglassCoefficient*TS[0]);
                    TY  =   omegaP1 * ( tempStressY[3*nodeId]     * X_dx + tempStressY[3*nodeId+1]     * X_dy + tempStressY[3*nodeId+2]     * X_dz + m_hourglassCoefficient*TS[1]);
                    TZ  =   omegaP1 * ( tempStressZ[3*nodeId]     * X_dx + tempStressZ[3*nodeId+1]     * X_dy + tempStressZ[3*nodeId+2]     * X_dz + m_hourglassCoefficient*TS[2]);
                    // undeformedBondX, undeformedBondY, undeformedBondZ of bond 1-2 equal to -undeformedBondX, -undeformedBondY, -undeformedBondZ of bond 2-1
                    TXN =   omegaP2 * ( tempStressX[3*neighborID] * X_dx + tempStressX[3*neighborID+1] * X_dy + tempStressX[3*neighborID+2] * X_dz + m_hourglassCoefficient*TS[0]);
                    TYN =   omegaP2 * ( tempStressY[3*neighborID] * X_dx + tempStressY[3*neighborID+1] * X_dy + tempStressY[3*neighborID+2] * X_dz + m_hourglassCoefficient*TS[1]);
                    TZN =   omegaP2 * ( tempStressZ[3*neighborID] * X_dx + tempStressZ[3*neighborID+1] * X_dy + tempStressZ[3*neighborID+2] * X_dz + m_hourglassCoefficient*TS[2]);
                    //std::cout<< "here2"<<std::endl;
                    // orthogonal projection of T and TN to the relative displacement vector Foster et al. "An energy based .."
                    // --> die senkrecht zur Projektion stehenden Anteile entsprechen eventuell den Schubanteilen. D.h. man könnte das Kriterium hier splitten.
                    factor = (etaX*TX + etaY*TY + etaZ*TZ)/normEtaSq;
                    TPX = factor*etaX; TPY = factor*etaY; TPZ = factor*etaZ;

                    //factor = (Y_dx*TX + Y_dy*TY + Y_dz*TZ)/dYSq;
                    //TPX = factor*Y_dx; TPY = factor*Y_dy; TPZ = factor*Y_dz;

                    factorN = (etaX*TXN + etaY*TYN + etaZ*TZN)/normEtaSq;
                    TPXN = factorN*etaX; TPYN = factorN*etaY; TPZN = factorN*etaZ;

                    //factorN = (Y_dx*TXN + Y_dy*TYN + Y_dz*TZN)/dYSq;
                    //TPXN = factorN*Y_dx; TPYN = factorN*Y_dy; TPZN = factorN*Y_dz;

                    bondEnergy = 0.5*temp*(abs(TPX*etaX)+abs(TPXN*etaX)+abs(TPY*etaY)+abs(TPYN*etaY)+abs(TPZ*etaZ)+abs(TPZN*etaZ)) ;
                    //if (iID == 1) std::cout<<bondEnergy<<std::endl;
                }
                else
                {
                    bondEnergy = 0;
                }
                //bondEnergy = 0.5*(sqrt(TX*TX)+sqrt(TXN*TXN))*sqrt((uNx-ux)*(uNx-ux)) + 0.5*(sqrt(TY*TY)+sqrt(TYN*TYN))*sqrt((uNy-uy)*(uNy-uy)) + 0.5*(sqrt(TZ*TZ)+sqrt(TZN*TZN))*sqrt((uNz-uz)*(uNz-uz));

                double avgHorizon = 0.5*(horizon[nodeId]+horizon[neighborID]);
                if (bondEnergy<0){
                    std::cout<<TPX<<" "<< TPXN<<" BE "<<bondEnergy<<std::endl;
                }
                ////////////////////////////////////////////////////////////////
                //--> to check, depth is not included yet. How to handle??
                ////////////////////////////////////////////////////////////////
                if (m_planeStrain==false&&m_planeStress==false){
                   quadhorizon =  4 /( m_pi * avgHorizon * avgHorizon * avgHorizon * avgHorizon );
                }
                else
                {

                   quadhorizon =  3 /( avgHorizon * avgHorizon * avgHorizon * m_Thickness );
                }
                //quadhorizon =  4 /( m_pi * avgHorizon * avgHorizon * avgHorizon * avgHorizon );
                critIso = bondEnergy/(criticalEnergyTension*quadhorizon);
                //critIso = 0;

                //critIso = bondEnergy/(criticalEnergyTension*quadhorizon);

               // std::cout<<bondEnergy<<" EE "<<criticalEnergyTension*quadhorizon<<" Stress1 "<<dEta<<std::endl;
                trialDamage = 0.0;
                if (criticalEnergyTension > 0.0 && critIso > 1.0) {
                    trialDamage = bondDamageNP1[bondIndex] + degradationFactor;
                }

                if (trialDamage > bondDamageNP1[bondIndex]) {
                    if (trialDamage>1)trialDamage = 1;
                    bondDamageNP1[bondIndex] = trialDamage;

                }
            }
            bondIndex += 1;

            }

        }
    //  Update the element damage (percent of bonds broken)
    if (detachedNodesCheck == true){
        int check = 1;  //set check = 1 to start the loop
        //std::cout<< "detached"<<std::endl;
        while (check != 0){
            check = checkDetachedNodes(numOwnedPoints, ownedIDs, neighborhoodList, dataManager);
        }
    }
    neighborhoodListIndex = 0;
    bondIndex = 0;
    double volume;
    for (iID = 0; iID < numOwnedPoints; ++iID) {
        nodeId = ownedIDs[iID];
        numNeighbors = neighborhoodList[neighborhoodListIndex++];
        //neighborhoodListIndex += numNeighbors;
        totalDamage = 0.0;
        volume = vol[nodeId];
        for (iNID = 0; iNID < numNeighbors; ++iNID) {

            neighborID = neighborhoodList[neighborhoodListIndex++];
            // must be zero to avoid synchronization errors

            totalDamage += bondDamageNP1[bondIndex]*vol[neighborID];
            volume += vol[neighborID];
            bondIndex += 1;
        }
        if (numNeighbors > 0)
            totalDamage /= numNeighbors;
        else
            totalDamage = 0.0;

        damage[nodeId] = totalDamage/volume;

    }
}

int PeridigmNS::EnergyReleaseDamageCorrepondenceModel::checkDetachedNodes(
                                                      const int numOwnedPoints,
                                                      const int* ownedIDs,
                                                      const int* neighborhoodList,
                                                      PeridigmNS::DataManager& dataManager
                                                      ) const
{
  double *bondDamage, *detachedNodes, *x, *volume, *horizon;
  dataManager.getData(m_modelCoordinatesFieldId, PeridigmField::STEP_NONE)->ExtractView(&x);
  dataManager.getData(m_volumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&volume);
  dataManager.getData(m_horizonFieldId, PeridigmField::STEP_NONE)->ExtractView(&horizon);
  dataManager.getData(m_bondDamageFieldId, PeridigmField::STEP_NP1)->ExtractView(&bondDamage);
  dataManager.getData(m_detachedNodesFieldId, PeridigmField::STEP_NP1)->ExtractView(&detachedNodes);

  int neighborhoodListIndex(0), bondIndex(0);
  int nodeId, numNeighbors, neighborID, iID, iNID;
  double nodeInitialX[3],  initialDistance;
  double checkShapeTensor[9], neighborVolume, checkShapeTensorInv[9];
  double omega;
  double determinant;
  int check = 0, matrixInversionReturnCode = 0;

  for(iID=0 ; iID<numOwnedPoints ; ++iID){
      nodeId = ownedIDs[iID];
      nodeInitialX[0] = x[nodeId*3];
      nodeInitialX[1] = x[nodeId*3+1];
      nodeInitialX[2] = x[nodeId*3+2];

      numNeighbors = neighborhoodList[neighborhoodListIndex++];

      checkShapeTensor[0] = 0.0;
      checkShapeTensor[1] = 0.0;
      checkShapeTensor[2] = 0.0;
      checkShapeTensor[3] = 0.0;
      checkShapeTensor[4] = 0.0;
      checkShapeTensor[5] = 0.0;
      checkShapeTensor[6] = 0.0;
      checkShapeTensor[7] = 0.0;
      checkShapeTensor[8] = 0.0;

      for(iNID=0 ; iNID<numNeighbors ; ++iNID){

          neighborID = neighborhoodList[neighborhoodListIndex++];

          if (detachedNodes[nodeId]==0){

              neighborVolume = volume[neighborID];
              initialDistance = distance(nodeInitialX[0], nodeInitialX[1], nodeInitialX[2],
                                  x[neighborID*3], x[neighborID*3+1], x[neighborID*3+2]);
              omega = MATERIAL_EVALUATION::scalarInfluenceFunction(initialDistance, horizon[iID]);
              //double omega = 1.0;
              double temp = (1.0 - bondDamage[bondIndex]) * omega * neighborVolume;

              double undeformedBondX =  x[neighborID*3]   - nodeInitialX[0];
              double undeformedBondY =  x[neighborID*3+1] - nodeInitialX[1];
              double undeformedBondZ =  x[neighborID*3+2] - nodeInitialX[2];

              checkShapeTensor[0]  += temp * undeformedBondX * undeformedBondX;
              checkShapeTensor[1]  += temp * undeformedBondX * undeformedBondY;
              checkShapeTensor[2]  += temp * undeformedBondX * undeformedBondZ;
              checkShapeTensor[3]  += temp * undeformedBondY * undeformedBondX;
              checkShapeTensor[4]  += temp * undeformedBondY * undeformedBondY;
              checkShapeTensor[5]  += temp * undeformedBondY * undeformedBondZ;
              checkShapeTensor[6]  += temp * undeformedBondZ * undeformedBondX;
              checkShapeTensor[7]  += temp * undeformedBondZ * undeformedBondY;
              checkShapeTensor[8]  += temp * undeformedBondZ * undeformedBondZ;
          }
          if (detachedNodes[neighborID]!=0&&bondDamage[bondIndex] != 1.0){// bondDamage check to avoid infinite loop; all bonds then destroyed
              bondDamage[bondIndex] = 1.0;
              check = 1;
          }

          bondIndex += 1;
      }

      if (detachedNodes[nodeId]!=0) continue;

      if (m_plane==true){
        matrixInversionReturnCode =
        CORRESPONDENCE::Invert2by2Matrix(checkShapeTensor, determinant, checkShapeTensorInv);
        }
      else{
        matrixInversionReturnCode =
        CORRESPONDENCE::Invert3by3Matrix(checkShapeTensor, determinant, checkShapeTensorInv);
        }

      if (matrixInversionReturnCode != 0){// to be checked
          check = 1;
          detachedNodes[nodeId]=1.;
          int bondIndex2 = bondIndex-numNeighbors; // set index back, to have the same correct bonds
          // delete all connected bonds
          for(iNID=0 ; iNID<numNeighbors ; ++iNID){
                  bondDamage[bondIndex2] = 1.0;
                  bondIndex2 += 1;
          }
        matrixInversionReturnCode = 0;
      }
  }

  return check;
}


#ifndef VIENNACL_LINALG_QR_METHOD_HPP_
#define VIENNACL_LINALG_QR_METHOD_HPP_

/* =========================================================================
   Copyright (c) 2010-2016, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

#include "viennacl/vector.hpp"
#include "viennacl/matrix.hpp"

#include "viennacl/linalg/qr-method-common.hpp"
#include "viennacl/linalg/tql2.hpp"
#include "viennacl/linalg/prod.hpp"

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>

/** @file viennacl/linalg/qr-method.hpp
    @brief Implementation of the QR method for eigenvalue computations. Experimental.
*/

namespace viennacl
{
namespace linalg
{
namespace detail
{
template <typename SCALARTYPE>
void final_iter_update_gpu(matrix_base<SCALARTYPE> & A,
                        int n,
                        int last_n,
                        SCALARTYPE q,
                        SCALARTYPE p
                        )
{
  (void)A; (void)n; (void)last_n; (void)q; (void)p;
#ifdef VIENNACL_WITH_OPENCL
    viennacl::ocl::context & ctx = const_cast<viennacl::ocl::context &>(viennacl::traits::opencl_handle(A).context());

    if(A.row_major())
    {
        viennacl::ocl::kernel& kernel = ctx.get_kernel(viennacl::linalg::opencl::kernels::svd<SCALARTYPE, row_major>::program_name(), SVD_FINAL_ITER_UPDATE_KERNEL);

        viennacl::ocl::enqueue(kernel(
                                      A,
                                      static_cast<cl_uint>(A.internal_size1()),
                                      static_cast<cl_uint>(n),
                                      static_cast<cl_uint>(last_n),
                                      q,
                                      p
                              ));
    }
    else
    {
        viennacl::ocl::kernel& kernel = ctx.get_kernel(viennacl::linalg::opencl::kernels::svd<SCALARTYPE, column_major>::program_name(), SVD_FINAL_ITER_UPDATE_KERNEL);

        viennacl::ocl::enqueue(kernel(
                                      A,
                                      static_cast<cl_uint>(A.internal_size1()),
                                      static_cast<cl_uint>(n),
                                      static_cast<cl_uint>(last_n),
                                      q,
                                      p
                              ));
    }
#endif
}
template <typename SCALARTYPE, typename VectorType>
void update_float_QR_column_gpu(matrix_base<SCALARTYPE> & A,
                        const VectorType& buf,
                        viennacl::vector<SCALARTYPE>& buf_vcl,
                        int m,
                        int n,
                        int last_n,
                        bool //is_triangular
                        )
{
  (void)A; (void)buf; (void)buf_vcl; (void)m; (void)n; (void)last_n;
#ifdef VIENNACL_WITH_OPENCL
  viennacl::ocl::context & ctx = const_cast<viennacl::ocl::context &>(viennacl::traits::opencl_handle(A).context());

    viennacl::fast_copy(buf, buf_vcl);

    if(A.row_major())
    {
        viennacl::ocl::kernel& kernel = ctx.get_kernel(viennacl::linalg::opencl::kernels::svd<SCALARTYPE, row_major>::program_name(), SVD_UPDATE_QR_COLUMN_KERNEL);

        viennacl::ocl::enqueue(kernel(
                                      A,
                                      static_cast<cl_uint>(A.internal_size1()),
                                      buf_vcl,
                                      static_cast<cl_uint>(m),
                                      static_cast<cl_uint>(n),
                                      static_cast<cl_uint>(last_n)
                              ));
    }
    else
    {
        viennacl::ocl::kernel& kernel = ctx.get_kernel(viennacl::linalg::opencl::kernels::svd<SCALARTYPE, column_major>::program_name(), SVD_UPDATE_QR_COLUMN_KERNEL);

        viennacl::ocl::enqueue(kernel(
                                      A,
                                      static_cast<cl_uint>(A.internal_size1()),
                                      buf_vcl,
                                      static_cast<cl_uint>(m),
                                      static_cast<cl_uint>(n),
                                      static_cast<cl_uint>(last_n)
                              ));
    }

#endif
}

    template<typename SCALARTYPE, typename MatrixT>
    void final_iter_update(MatrixT& A,
                            int n,
                            int last_n,
                            SCALARTYPE q,
                            SCALARTYPE p
                            )
    {
        for (int i = 0; i < last_n; i++)
        {
            SCALARTYPE v_in = A(i, n);
            SCALARTYPE z = A(i, n - 1);
            A(i, n - 1) = q * z + p * v_in;
            A(i, n) = q * v_in - p * z;
        }
    }

    template<typename SCALARTYPE, typename MatrixT>
    void update_float_QR_column(MatrixT& A,
                            const std::vector<SCALARTYPE>& buf,
                            int m,
                            int n,
                            int last_i,
                            bool is_triangular
                            )
    {
        for (int i = 0; i < last_i; i++)
        {
            int start_k = is_triangular?std::max(i + 1, m):m;

            SCALARTYPE* a_row = A.row(i);

            SCALARTYPE a_ik   = a_row[start_k];
            SCALARTYPE a_ik_1 = 0;
            SCALARTYPE a_ik_2 = 0;

            if (start_k < n)
                a_ik_1 = a_row[start_k + 1];

            for (int k = start_k; k < n; k++)
            {
                bool notlast = (k != n - 1);

                SCALARTYPE p = buf[5 * static_cast<vcl_size_t>(k)] * a_ik + buf[5 * static_cast<vcl_size_t>(k) + 1] * a_ik_1;

                if (notlast)
                {
                    a_ik_2 = a_row[k + 2];
                    p = p + buf[5 * static_cast<vcl_size_t>(k) + 2] * a_ik_2;
                    a_ik_2 = a_ik_2 - p * buf[5 * static_cast<vcl_size_t>(k) + 4];
                }

                a_row[k] = a_ik - p;
                a_ik_1 = a_ik_1 - p * buf[5 * static_cast<vcl_size_t>(k) + 3];

                a_ik = a_ik_1;
                a_ik_1 = a_ik_2;
            }

            if (start_k < n)
                a_row[n] = a_ik;
        }
    }

    /** @brief Internal helper class representing a row-major dense matrix used for the QR method for the purpose of computing eigenvalues. */
    template<typename SCALARTYPE>
    class FastMatrix
    {
    public:
        FastMatrix()
        {
            size_ = 0;
        }

        FastMatrix(vcl_size_t sz, vcl_size_t internal_size) : size_(sz), internal_size_(internal_size)
        {
            data.resize(internal_size * internal_size);
        }

        SCALARTYPE& operator()(int i, int j)
        {
            return data[static_cast<vcl_size_t>(i) * internal_size_ + static_cast<vcl_size_t>(j)];
        }

        SCALARTYPE* row(int i)
        {
            return &data[static_cast<vcl_size_t>(i) * internal_size_];
        }

        SCALARTYPE* begin()
        {
            return &data[0];
        }

        SCALARTYPE* end()
        {
            return &data[0] + data.size();
        }

        std::vector<SCALARTYPE> data;
    private:
        vcl_size_t size_;
        vcl_size_t internal_size_;
    };

    // Nonsymmetric reduction from Hessenberg to real Schur form.
    // This is derived from the Algol procedure hqr2, by Martin and Wilkinson, Handbook for Auto. Comp.,
    // Vol.ii-Linear Algebra, and the corresponding  Fortran subroutine in EISPACK.
    template <typename SCALARTYPE, typename VectorType>
    void hqr2(viennacl::matrix<SCALARTYPE>& vcl_H,
                viennacl::matrix<SCALARTYPE>& V,
                VectorType & d,
                VectorType & e)
    {
        transpose(V);

        int nn = static_cast<int>(vcl_H.size1());

        FastMatrix<SCALARTYPE> H(vcl_size_t(nn), vcl_H.internal_size2());//, V(nn);

        std::vector<SCALARTYPE>  buf(5 * vcl_size_t(nn));
        //boost::numeric::ublas::vector<float>  buf(5 * nn);
        viennacl::vector<SCALARTYPE> buf_vcl(5 * vcl_size_t(nn));

        viennacl::fast_copy(vcl_H, H.begin());

        int n = nn - 1;

        SCALARTYPE eps = 2 * static_cast<SCALARTYPE>(EPS);
        SCALARTYPE exshift = 0;
        SCALARTYPE p = 0;
        SCALARTYPE q = 0;
        SCALARTYPE r = 0;
        SCALARTYPE s = 0;
        SCALARTYPE z = 0;
        SCALARTYPE t;
        SCALARTYPE w;
        SCALARTYPE x;
        SCALARTYPE y;

        SCALARTYPE out1, out2;

        // compute matrix norm
        SCALARTYPE norm = 0;
        for (int i = 0; i < nn; i++)
        {
            for (int j = std::max(i - 1, 0); j < nn; j++)
                norm = norm + std::fabs(H(i, j));
        }

        // Outer loop over eigenvalue index
        int iter = 0;
        while (n >= 0)
        {
            // Look for single small sub-diagonal element
            int l = n;
            while (l > 0)
            {
                s = std::fabs(H(l - 1, l - 1)) + std::fabs(H(l, l));
                if (s <= 0)
                  s = norm;
                if (std::fabs(H(l, l - 1)) < eps * s)
                  break;

                l--;
            }

            // Check for convergence
            if (l == n)
            {
                // One root found
                H(n, n) = H(n, n) + exshift;
                d[vcl_size_t(n)] = H(n, n);
                e[vcl_size_t(n)] = 0;
                n--;
                iter = 0;
            }
            else if (l == n - 1)
            {
                // Two roots found
                w = H(n, n - 1) * H(n - 1, n);
                p = (H(n - 1, n - 1) - H(n, n)) / 2;
                q = p * p + w;
                z = static_cast<SCALARTYPE>(std::sqrt(std::fabs(q)));
                H(n, n) = H(n, n) + exshift;
                H(n - 1, n - 1) = H(n - 1, n - 1) + exshift;
                x = H(n, n);

                if (q >= 0)
                {
                    // Real pair
                    z = (p >= 0) ? (p + z) : (p - z);
                    d[vcl_size_t(n) - 1] = x + z;
                    d[vcl_size_t(n)] = d[vcl_size_t(n) - 1];
                    if (z <= 0 && z >= 0) // z == 0 without compiler complaints
                      d[vcl_size_t(n)] = x - w / z;
                    e[vcl_size_t(n) - 1] = 0;
                    e[vcl_size_t(n)] = 0;
                    x = H(n, n - 1);
                    s = std::fabs(x) + std::fabs(z);
                    p = x / s;
                    q = z / s;
                    r = static_cast<SCALARTYPE>(std::sqrt(p * p + q * q));
                    p = p / r;
                    q = q / r;

                    // Row modification
                    for (int j = n - 1; j < nn; j++)
                    {
                        SCALARTYPE h_nj = H(n, j);
                        z = H(n - 1, j);
                        H(n - 1, j) = q * z + p * h_nj;
                        H(n, j) = q * h_nj - p * z;
                    }

                    final_iter_update(H, n, n + 1, q, p);
                    final_iter_update_gpu(V, n, nn, q, p);
                }
                else
                {
                    // Complex pair
                    d[vcl_size_t(n) - 1] = x + p;
                    d[vcl_size_t(n)] = x + p;
                    e[vcl_size_t(n) - 1] = z;
                    e[vcl_size_t(n)] = -z;
                }

                n = n - 2;
                iter = 0;
            }
            else
            {
                // No convergence yet

                // Form shift
                x = H(n, n);
                y = 0;
                w = 0;
                if (l < n)
                {
                    y = H(n - 1, n - 1);
                    w = H(n, n - 1) * H(n - 1, n);
                }

                // Wilkinson's original ad hoc shift
                if (iter == 10)
                {
                    exshift += x;
                    for (int i = 0; i <= n; i++)
                        H(i, i) -= x;

                    s = std::fabs(H(n, n - 1)) + std::fabs(H(n - 1, n - 2));
                    x = y = SCALARTYPE(0.75) * s;
                    w = SCALARTYPE(-0.4375) * s * s;
                }

                // MATLAB's new ad hoc shift
                if (iter == 30)
                {
                    s = (y - x) / 2;
                    s = s * s + w;
                    if (s > 0)
                    {
                        s = static_cast<SCALARTYPE>(std::sqrt(s));
                        if (y < x) s = -s;
                        s = x - w / ((y - x) / 2 + s);
                        for (int i = 0; i <= n; i++)
                            H(i, i) -= s;
                        exshift += s;
                        x = y = w = SCALARTYPE(0.964);
                    }
                }

                iter = iter + 1;

                // Look for two consecutive small sub-diagonal elements
                int m = n - 2;
                while (m >= l)
                {
                    SCALARTYPE h_m1_m1 = H(m + 1, m + 1);
                    z = H(m, m);
                    r = x - z;
                    s = y - z;
                    p = (r * s - w) / H(m + 1, m) + H(m, m + 1);
                    q = h_m1_m1 - z - r - s;
                    r = H(m + 2, m + 1);
                    s = std::fabs(p) + std::fabs(q) + std::fabs(r);
                    p = p / s;
                    q = q / s;
                    r = r / s;
                    if (m == l)
                        break;
                    if (std::fabs(H(m, m - 1)) * (std::fabs(q) + std::fabs(r)) < eps * (std::fabs(p) * (std::fabs(H(m - 1, m - 1)) + std::fabs(z) + std::fabs(h_m1_m1))))
                        break;
                    m--;
                }

                for (int i = m + 2; i <= n; i++)
                {
                    H(i, i - 2) = 0;
                    if (i > m + 2)
                        H(i, i - 3) = 0;
                }

                // float QR step involving rows l:n and columns m:n
                for (int k = m; k < n; k++)
                {
                    bool notlast = (k != n - 1);
                    if (k != m)
                    {
                        p = H(k, k - 1);
                        q = H(k + 1, k - 1);
                        r = (notlast ? H(k + 2, k - 1) : 0);
                        x = std::fabs(p) + std::fabs(q) + std::fabs(r);
                        if (x > 0)
                        {
                            p = p / x;
                            q = q / x;
                            r = r / x;
                        }
                    }

                    if (x <= 0 && x >= 0) break;  // x == 0 without compiler complaints

                    s = static_cast<SCALARTYPE>(std::sqrt(p * p + q * q + r * r));
                    if (p < 0) s = -s;

                    if (s < 0 || s > 0)
                    {
                        if (k != m)
                            H(k, k - 1) = -s * x;
                        else
                            if (l != m)
                                H(k, k - 1) = -H(k, k - 1);

                        p = p + s;
                        y = q / s;
                        z = r / s;
                        x = p / s;
                        q = q / p;
                        r = r / p;

                        buf[5 * vcl_size_t(k)] = x;
                        buf[5 * vcl_size_t(k) + 1] = y;
                        buf[5 * vcl_size_t(k) + 2] = z;
                        buf[5 * vcl_size_t(k) + 3] = q;
                        buf[5 * vcl_size_t(k) + 4] = r;

                        SCALARTYPE* a_row_k = H.row(k);
                        SCALARTYPE* a_row_k_1 = H.row(k + 1);
                        SCALARTYPE* a_row_k_2 = H.row(k + 2);
                        // Row modification
                        for (int j = k; j < nn; j++)
                        {
                            SCALARTYPE h_kj = a_row_k[j];
                            SCALARTYPE h_k1_j = a_row_k_1[j];

                            p = h_kj + q * h_k1_j;
                            if (notlast)
                            {
                                SCALARTYPE h_k2_j = a_row_k_2[j];
                                p = p + r * h_k2_j;
                                a_row_k_2[j] = h_k2_j - p * z;
                            }

                            a_row_k[j] = h_kj - p * x;
                            a_row_k_1[j] = h_k1_j - p * y;
                        }

                        //H(k + 1, nn - 1) = h_kj;

                        // Column modification
                        for (int i = k; i < std::min(nn, k + 4); i++)
                        {
                            p = x * H(i, k) + y * H(i, k + 1);
                            if (notlast)
                            {
                                p = p + z * H(i, k + 2);
                                H(i, k + 2) = H(i, k + 2) - p * r;
                            }

                            H(i, k) = H(i, k) - p;
                            H(i, k + 1) = H(i, k + 1) - p * q;
                        }
                    }
                    else
                    {
                        buf[5 * vcl_size_t(k)] = 0;
                        buf[5 * vcl_size_t(k) + 1] = 0;
                        buf[5 * vcl_size_t(k) + 2] = 0;
                        buf[5 * vcl_size_t(k) + 3] = 0;
                        buf[5 * vcl_size_t(k) + 4] = 0;
                    }
                }

                // Timer timer;
                // timer.start();

                update_float_QR_column<SCALARTYPE>(H, buf, m, n, n, true);
                update_float_QR_column_gpu(V, buf, buf_vcl, m, n, nn, false);

                // std::cout << timer.get() << "\n";
            }
        }

        // Backsubstitute to find vectors of upper triangular form
        if (norm <= 0)
        {
            return;
        }

        for (n = nn - 1; n >= 0; n--)
        {
            p = d[vcl_size_t(n)];
            q = e[vcl_size_t(n)];

            // Real vector
            if (q <= 0 && q >= 0)
            {
                int l = n;
                H(n, n) = 1;
                for (int i = n - 1; i >= 0; i--)
                {
                    w = H(i, i) - p;
                    r = 0;
                    for (int j = l; j <= n; j++)
                        r = r + H(i, j) * H(j, n);

                    if (e[vcl_size_t(i)] < 0)
                    {
                        z = w;
                        s = r;
                    }
                    else
                    {
                        l = i;
                        if (e[vcl_size_t(i)] <= 0) // e[i] == 0 with previous if
                        {
                            H(i, n) = (w > 0 || w < 0) ? (-r / w) : (-r / (eps * norm));
                        }
                        else
                        {
                            // Solve real equations
                            x = H(i, i + 1);
                            y = H(i + 1, i);
                            q = (d[vcl_size_t(i)] - p) * (d[vcl_size_t(i)] - p) + e[vcl_size_t(i)] * e[vcl_size_t(i)];
                            t = (x * s - z * r) / q;
                            H(i, n) = t;
                            H(i + 1, n) = (std::fabs(x) > std::fabs(z)) ? ((-r - w * t) / x) : ((-s - y * t) / z);
                        }

                        // Overflow control
                        t = std::fabs(H(i, n));
                        if ((eps * t) * t > 1)
                            for (int j = i; j <= n; j++)
                                H(j, n) /= t;
                    }
                }
            }
            else if (q < 0)
            {
                // Complex vector
                int l = n - 1;

                // Last vector component imaginary so matrix is triangular
                if (std::fabs(H(n, n - 1)) > std::fabs(H(n - 1, n)))
                {
                    H(n - 1, n - 1) = q / H(n, n - 1);
                    H(n - 1, n) = -(H(n, n) - p) / H(n, n - 1);
                }
                else
                {
                    cdiv<SCALARTYPE>(0, -H(n - 1, n), H(n - 1, n - 1) - p, q, out1, out2);

                    H(n - 1, n - 1) = out1;
                    H(n - 1, n) = out2;
                }

                H(n, n - 1) = 0;
                H(n, n) = 1;
                for (int i = n - 2; i >= 0; i--)
                {
                    SCALARTYPE ra, sa, vr, vi;
                    ra = 0;
                    sa = 0;
                    for (int j = l; j <= n; j++)
                    {
                        SCALARTYPE h_ij = H(i, j);
                        ra = ra + h_ij * H(j, n - 1);
                        sa = sa + h_ij * H(j, n);
                    }

                    w = H(i, i) - p;

                    if (e[vcl_size_t(i)] < 0)
                    {
                        z = w;
                        r = ra;
                        s = sa;
                    }
                    else
                    {
                        l = i;
                        if (e[vcl_size_t(i)] <= 0) // e[i] == 0 with previous if
                        {
                            cdiv<SCALARTYPE>(-ra, -sa, w, q, out1, out2);
                            H(i, n - 1) = out1;
                            H(i, n) = out2;
                        }
                        else
                        {
                            // Solve complex equations
                            x = H(i, i + 1);
                            y = H(i + 1, i);
                            vr = (d[vcl_size_t(i)] - p) * (d[vcl_size_t(i)] - p) + e[vcl_size_t(i)] * e[vcl_size_t(i)] - q * q;
                            vi = (d[vcl_size_t(i)] - p) * 2 * q;
                            if ( (vr <= 0 && vr >= 0) && (vi <= 0 && vi >= 0) )
                                vr = eps * norm * (std::fabs(w) + std::fabs(q) + std::fabs(x) + std::fabs(y) + std::fabs(z));

                            cdiv<SCALARTYPE>(x * r - z * ra + q * sa, x * s - z * sa - q * ra, vr, vi, out1, out2);

                            H(i, n - 1) = out1;
                            H(i, n) = out2;

                            if (std::fabs(x) > (std::fabs(z) + std::fabs(q)))
                            {
                                H(i + 1, n - 1) = (-ra - w * H(i, n - 1) + q * H(i, n)) / x;
                                H(i + 1, n) = (-sa - w * H(i, n) - q * H(i, n - 1)) / x;
                            }
                            else
                            {
                                cdiv<SCALARTYPE>(-r - y * H(i, n - 1), -s - y * H(i, n), z, q, out1, out2);

                                H(i + 1, n - 1) = out1;
                                H(i + 1, n) = out2;
                            }
                        }

                        // Overflow control
                        t = std::max(std::fabs(H(i, n - 1)), std::fabs(H(i, n)));
                        if ((eps * t) * t > 1)
                        {
                            for (int j = i; j <= n; j++)
                            {
                                H(j, n - 1) /= t;
                                H(j, n) /= t;
                            }
                        }
                    }
                }
            }
        }

        viennacl::fast_copy(H.begin(), H.end(),  vcl_H);
        // viennacl::fast_copy(V.begin(), V.end(),  vcl_V);

        viennacl::matrix<SCALARTYPE> tmp = V;

        V = viennacl::linalg::prod(trans(tmp), vcl_H);
    }

    template <typename SCALARTYPE>
    bool householder_twoside(
                        matrix_base<SCALARTYPE>& A,
                        matrix_base<SCALARTYPE>& Q,
                        vector_base<SCALARTYPE>& D,
                        vcl_size_t start)
    {
        vcl_size_t A_size1 = static_cast<vcl_size_t>(viennacl::traits::size1(A));
        if(start + 2 >= A_size1)
            return false;

        prepare_householder_vector(A, D, A_size1, start + 1, start, start + 1, true);
        viennacl::linalg::house_update_A_left(A, D, start);
        viennacl::linalg::house_update_A_right(A, D);
        viennacl::linalg::house_update_QL(Q, D, A_size1);

        return true;
    }

    template <typename SCALARTYPE>
    void tridiagonal_reduction(matrix_base<SCALARTYPE>& A,
                               matrix_base<SCALARTYPE>& Q)
    {
        vcl_size_t sz = A.size1();

        viennacl::vector<SCALARTYPE> hh_vector(sz);

        for(vcl_size_t i = 0; i < sz; i++)
        {
            householder_twoside(A, Q, hh_vector, i);
        }

    }

    template <typename SCALARTYPE>
    void qr_method(viennacl::matrix<SCALARTYPE> & A,
                   viennacl::matrix<SCALARTYPE> & Q,
                   std::vector<SCALARTYPE> & D,
                   std::vector<SCALARTYPE> & E,
                   bool is_symmetric = true)
    {

        assert(A.size1() == A.size2() && bool("Input matrix must be square for QR method!"));
    /*    if (!viennacl::is_row_major<F>::value && !is_symmetric)
        {
          std::cout << "qr_method for non-symmetric column-major matrices not implemented yet!" << std::endl;
          exit(EXIT_FAILURE);
        }

        */
        vcl_size_t mat_size = A.size1();
        D.resize(A.size1());
        E.resize(A.size1());

        viennacl::vector<SCALARTYPE> vcl_D(mat_size), vcl_E(mat_size);
        //std::vector<SCALARTYPE> std_D(mat_size), std_E(mat_size);

        Q = viennacl::identity_matrix<SCALARTYPE>(Q.size1());

        // reduce to tridiagonal form
        detail::tridiagonal_reduction(A, Q);

        // pack diagonal and super-diagonal
        viennacl::linalg::bidiag_pack(A, vcl_D, vcl_E);
        copy(vcl_D, D);
        copy(vcl_E, E);

        // find eigenvalues of symmetric tridiagonal matrix
        if(is_symmetric)
        {
          viennacl::linalg::tql2(Q, D, E);

        }
        else
        {
              detail::hqr2(A, Q, D, E);
        }

        boost::numeric::ublas::matrix<SCALARTYPE> eigen_values(A.size1(), A.size1());
        eigen_values.clear();

        for (vcl_size_t i = 0; i < A.size1(); i++)
        {
            if(std::fabs(E[i]) < EPS)
            {
                eigen_values(i, i) = D[i];
            }
            else
            {
                eigen_values(i, i) = D[i];
                eigen_values(i, i + 1) = E[i];
                eigen_values(i + 1, i) = -E[i];
                eigen_values(i + 1, i + 1) = D[i];
                i++;
            }
        }

        copy(eigen_values, A);
    }
}

template <typename SCALARTYPE>
void qr_method_nsm(viennacl::matrix<SCALARTYPE>& A,
                   viennacl::matrix<SCALARTYPE>& Q,
                   std::vector<SCALARTYPE>& D,
                   std::vector<SCALARTYPE>& E
                  )
{
    detail::qr_method(A, Q, D, E, false);
}

template <typename SCALARTYPE>
void qr_method_sym(viennacl::matrix<SCALARTYPE>& A,
                   viennacl::matrix<SCALARTYPE>& Q,
                   std::vector<SCALARTYPE>& D
                  )
{
    std::vector<SCALARTYPE> E(A.size1());

    detail::qr_method(A, Q, D, E, true);
}

template <typename SCALARTYPE>
void qr_method_sym(viennacl::matrix<SCALARTYPE>& A,
                   viennacl::matrix<SCALARTYPE>& Q,
                   viennacl::vector_base<SCALARTYPE>& D
                  )
{
    std::vector<SCALARTYPE> std_D(D.size());
    std::vector<SCALARTYPE> E(A.size1());

    viennacl::copy(D, std_D);
    detail::qr_method(A, Q, std_D, E, true);
    viennacl::copy(std_D, D);
}

}
}

#endif


#ifndef PARMCB_MPI_SVA_SIGNED_HPP_
#define PARMCB_MPI_SVA_SIGNED_HPP_

//    Copyright (C) Dimitrios Michail 2019 - 2021.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          https://www.boost.org/LICENSE_1_0.txt)

#include <cstddef>
#include <functional>
#include <iostream>
#include <iterator>
#include <limits>
#include <set>
#include <vector>

#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/tuple/detail/tuple_basic.hpp>

#include <boost/mpi/environment.hpp>
#include <boost/mpi/communicator.hpp>
#include <boost/mpi/collectives.hpp>
#include <boost/mpi/timer.hpp>

#include <tbb/concurrent_vector.h>
#include <tbb/parallel_for.h>
#include <tbb/parallel_reduce.h>

#include <parmcb/detail/signed_dijkstra.hpp>
#include <parmcb/mpi/sptrees.hpp>
#include <parmcb/forestindex.hpp>
#include <parmcb/spvecgf2.hpp>
#include <parmcb/util.hpp>

namespace parmcb {

    namespace detail {

        template<class Graph, class WeightMap>
        std::tuple<std::set<typename boost::graph_traits<Graph>::edge_descriptor>,
                typename boost::property_traits<WeightMap>::value_type, bool> find_shortest_odd_cycle_mpi(
                const Graph &g, const WeightMap &weight_map,
                const std::vector<typename boost::graph_traits<Graph>::vertex_descriptor> &allVertices,
                const ForestIndex<Graph> &forest_index,
                const std::set<typename boost::graph_traits<Graph>::edge_descriptor> &signed_edges,
                boost::mpi::communicator &world) {

            typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
            typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
            typedef typename boost::property_traits<WeightMap>::value_type WeightType;

            std::less<WeightType> compare = std::less<WeightType>();
            std::tuple<std::set<Edge>, WeightType, bool> best = std::make_tuple(std::set<Edge> { },
                    (std::numeric_limits<WeightType>::max)(), false);
            typedef std::tuple<std::set<Edge>, WeightType, bool> cycle_t;
            auto cycle_min = [compare](const cycle_t &c1, const cycle_t &c2) {
                if (!std::get<2>(c1) || !std::get<2>(c2)) {
                    if (std::get<2>(c1)) {
                        return c1;
                    } else {
                        return c2;
                    }
                }
                // both valid, compare
                if (!compare(std::get<1>(c2), std::get<1>(c1))) {
                    return c1;
                }
                return c2;
            };

            if (signed_edges.size() == 1) {
                if (world.rank() == 0) {
                    auto se = *signed_edges.begin();
                    auto se_v = boost::source(se, g);
                    auto se_u = boost::target(se, g);
                    auto res = bidirectional_signed_dijkstra(g, weight_map, std::set<Edge> { }, signed_edges, true,
                            se_v, true, se_u, true, std::get<2>(best), std::get<1>(best));
                    if (std::get<2>(res) && std::get<0>(res).find(se) == std::get<0>(res).end()) {
                        std::get<1>(res) += boost::get(weight_map, se);
                        if (!std::get<2>(best) || compare(std::get<1>(res), std::get<1>(best))) {
                            std::get<0>(res).insert(se);
                            best = res;
                            assert(std::get<2>(best));
                        }
                    }
                }
            } else if (signed_edges.size() < boost::num_vertices(g)) {
                /*
                 * Heuristic in case number of signed edges is small compared to the number of vertices.
                 */
                std::map<Edge, std::set<Edge>> hidden_edges_per_edge;
                std::vector<Edge> signed_edges_as_vector;
                std::set<Edge> tmp_signed_edges = signed_edges;
                while (!tmp_signed_edges.empty()) {
                    auto bit = tmp_signed_edges.begin();
                    hidden_edges_per_edge.insert(std::make_pair(*bit, tmp_signed_edges));
                    signed_edges_as_vector.push_back(*bit);
                    tmp_signed_edges.erase(bit);
                }

                std::vector<Edge> local_signed_edges_as_vector;
                std::size_t total = signed_edges_as_vector.size();
                std::size_t stride = ceil((double) total / world.size());
                std::size_t istart = world.rank() * stride;
                std::size_t iend = istart + stride;
                for (std::size_t i = istart; i < iend && i < total; i++) {
                    local_signed_edges_as_vector.push_back(signed_edges_as_vector[i]);
                }

                std::tuple<std::set<Edge>, WeightType, bool> best_local_cycle = tbb::parallel_reduce(
                        tbb::blocked_range<std::size_t>(0, local_signed_edges_as_vector.size()),
                        std::make_tuple(std::set<Edge>(), (std::numeric_limits<WeightType>::max)(), false),
                        [&](tbb::blocked_range<std::size_t> r, auto running_min) {
                            for (std::size_t i = r.begin(); i < r.end(); i++) {
                                auto se = local_signed_edges_as_vector.at(i);
                                auto se_v = boost::source(se, g);
                                auto se_u = boost::target(se, g);
                                auto hidden_edges = hidden_edges_per_edge.at(se);
                                auto res = bidirectional_signed_dijkstra(g, weight_map, signed_edges, hidden_edges,
                                        true, se_v, true, se_u, true, std::get<2>(running_min),
                                        std::get<1>(running_min));
                                if (std::get<2>(res) && std::get<0>(res).find(se) == std::get<0>(res).end()) {
                                    std::get<1>(res) += boost::get(weight_map, se);
                                    if (!std::get<2>(running_min)
                                            || compare(std::get<1>(res), std::get<1>(running_min))) {
                                        std::get<0>(res).insert(se);
                                        running_min = res;
                                    }
                                }
                            }
                            return running_min;
                        },
                        cycle_min);

                std::vector<typename ForestIndex<Graph>::size_type> best_local_cycle_as_indices;
                convert_edges(std::get<0>(best_local_cycle),
                        std::inserter(best_local_cycle_as_indices, best_local_cycle_as_indices.end()), forest_index);
                SerializableMinOddCycle<Graph, WeightMap> local_min_odd_cycle(best_local_cycle_as_indices,
                        std::get<1>(best_local_cycle), std::get<2>(best_local_cycle));
                SerializableMinOddCycle<Graph, WeightMap> global_min_odd_cycle;

                boost::mpi::reduce(world, local_min_odd_cycle, global_min_odd_cycle,
                        SerializableMinOddCycleMinOp<Graph, WeightMap>(), 0);

                convert_edges(global_min_odd_cycle.edges, std::inserter(std::get<0>(best), std::get<0>(best).end()),
                        forest_index);
                std::get<1>(best) = global_min_odd_cycle.weight;
                std::get<2>(best) = global_min_odd_cycle.exists;
            } else {
                // split implicitly all vertices
                std::vector<Vertex> localVertices;
                std::size_t stride = ceil((double) allVertices.size() / world.size());
                std::size_t istart = world.rank() * stride;
                std::size_t iend = istart + stride;
                std::size_t total = allVertices.size();
                for (std::size_t i = istart; i < iend && i < total; i++) {
                    localVertices.push_back(allVertices[i]);
                }

                std::tuple<std::set<Edge>, WeightType, bool> best_local_cycle = tbb::parallel_reduce(
                        tbb::blocked_range<std::size_t>(0, localVertices.size()),
                        std::make_tuple(std::set<Edge>(), (std::numeric_limits<WeightType>::max)(), false),
                        [&](tbb::blocked_range<std::size_t> r, auto running_min) {
                            for (std::size_t i = r.begin(); i < r.end(); i++) {
                                auto v = localVertices[i];
                                const bool use_hidden_edges = false;
                                auto res = bidirectional_signed_dijkstra(g, weight_map, signed_edges,
                                        std::set<Edge> { }, use_hidden_edges, v, true, v, false,
                                        std::get<2>(running_min), std::get<1>(running_min));
                                if (std::get<2>(res)
                                        && (!std::get<2>(running_min)
                                                || compare(std::get<1>(res), std::get<1>(running_min)))) {
                                    running_min = res;
                                }
                            }
                            return running_min;
                        },
                        cycle_min);

                std::vector<typename ForestIndex<Graph>::size_type> best_local_cycle_as_indices;
                convert_edges(std::get<0>(best_local_cycle),
                        std::inserter(best_local_cycle_as_indices, best_local_cycle_as_indices.end()), forest_index);
                SerializableMinOddCycle<Graph, WeightMap> local_min_odd_cycle(best_local_cycle_as_indices,
                        std::get<1>(best_local_cycle), std::get<2>(best_local_cycle));
                SerializableMinOddCycle<Graph, WeightMap> global_min_odd_cycle;

                boost::mpi::reduce(world, local_min_odd_cycle, global_min_odd_cycle,
                        SerializableMinOddCycleMinOp<Graph, WeightMap>(), 0);

                convert_edges(global_min_odd_cycle.edges, std::inserter(std::get<0>(best), std::get<0>(best).end()),
                        forest_index);
                std::get<1>(best) = global_min_odd_cycle.weight;
                std::get<2>(best) = global_min_odd_cycle.exists;
            }

            return best;
        }

    } // detail

    template<class Graph, class WeightMap, class CycleOutputIterator>
    typename boost::property_traits<WeightMap>::value_type mcb_sva_signed_mpi(const Graph &g, WeightMap weight_map,
            CycleOutputIterator out, boost::mpi::communicator &world, const std::size_t hardware_concurrency_hint = 0) {

        typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
        typedef typename boost::graph_traits<Graph>::vertex_iterator VertexIt;
        typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
        typedef typename boost::property_traits<WeightMap>::value_type WeightType;

        /*
         * Index the graph
         */
        ForestIndex<Graph> forest_index(g);
        auto csd = forest_index.cycle_space_dimension();
        std::vector<Vertex> vertices;
        {
            VertexIt vi, viend;
            for (boost::tie(vi, viend) = boost::vertices(g); vi != viend; ++vi) {
                vertices.push_back(*vi);
            }
        }

        /*
         * Initialize support vectors
         */
        tbb::concurrent_vector<SpVecGF2<std::size_t>> support;
        tbb::parallel_for(tbb::blocked_range<std::size_t>(0, csd), [&](const tbb::blocked_range<std::size_t> &r) {
            for (std::size_t i = r.begin(); i != r.end(); ++i) {
                support.push_back(SpVecGF2<std::size_t> { i });
            }
        });

        boost::mpi::timer total_timer;

        /*
         * Main loop
         */
        WeightType mcb_weight = WeightType();
        for (std::size_t k = 0; k < csd; k++) {
            if (k % 250 == 0) {
                std::cout << "Rank " << world.rank() << " at cycle " << k << std::endl;
            }

            // TODO: check if sparsest support heuristic makes sense here

            // broadcast support vector
            if (world.rank() == 0) {
                boost::mpi::broadcast(world, support[k], 0);
            } else {
                SpVecGF2<std::size_t> received;
                boost::mpi::broadcast(world, received, 0);
                support[k] = received;
            }

            std::set<Edge> signed_edges;
            convert_edges(support[k], std::inserter(signed_edges, signed_edges.end()), forest_index);
            std::tuple<std::set<Edge>, WeightType, bool> best = parmcb::detail::find_shortest_odd_cycle_mpi(g, weight_map,
                    vertices, forest_index, signed_edges, world);

            if (world.rank() == 0) {
                /*
                 * Update support vectors
                 */
                std::set<std::size_t> cyclek;
                convert_edges(std::get<0>(best), std::inserter(cyclek, cyclek.end()), forest_index);
                tbb::parallel_for(tbb::blocked_range<std::size_t>(k + 1, csd),
                        [&](const tbb::blocked_range<std::size_t> &r) {
                            auto e = r.end();
                            for (std::size_t i = r.begin(); i != e; ++i) {
                                if (support[i] * cyclek == 1) {
                                    support[i] += support[k];
                                }
                            }
                        });

                /*
                 * Output cycles
                 */
                std::list<Edge> cyclek_edgelist;
                std::copy(std::get<0>(best).begin(), std::get<0>(best).end(), std::back_inserter(cyclek_edgelist));
                *out++ = cyclek_edgelist;
                mcb_weight += std::get<1>(best);
            }

        }

        if (world.rank() == 0) {
            std::cout << "Total time: " << total_timer.elapsed() << " (sec)" << std::endl;
        }

        return mcb_weight;
    }

} // namespace parmcb

#endif


// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017, 2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

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

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

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

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

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

#ifndef BOOST_GEOMETRY_PROJECTIONS_VANDG_HPP
#define BOOST_GEOMETRY_PROJECTIONS_VANDG_HPP

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

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>

namespace boost { namespace geometry
{

namespace srs { namespace par4
{
    struct vandg {};

}} //namespace srs::par4

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace vandg
    {

            static const double tolerance = 1.e-10;

            template <typename T>
            inline T C2_27() { return .07407407407407407407407407407407; }
            template <typename T>
            inline T PI4_3() { return boost::math::constants::four_thirds_pi<T>(); }
            template <typename T>
            inline T TPISQ() { return 19.739208802178717237668981999752; }
            template <typename T>
            inline T HPISQ() { return 4.9348022005446793094172454999381; }

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_vandg_spheroid
                : public base_t_fi<base_vandg_spheroid<T, Parameters>, T, Parameters>
            {
                inline base_vandg_spheroid(const Parameters& par)
                    : base_t_fi<base_vandg_spheroid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T& lp_lon, T& lp_lat, T& xy_x, T& xy_y) const
                {
                    static const T half_pi = detail::half_pi<T>();
                    static const T pi = detail::pi<T>();

                    T  al, al2, g, g2, p2;

                    p2 = fabs(lp_lat / half_pi);
                    if ((p2 - tolerance) > 1.) {
                        BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                    }
                    if (p2 > 1.)
                        p2 = 1.;
                    if (fabs(lp_lat) <= tolerance) {
                        xy_x = lp_lon;
                        xy_y = 0.;
                    } else if (fabs(lp_lon) <= tolerance || fabs(p2 - 1.) < tolerance) {
                        xy_x = 0.;
                        xy_y = pi * tan(.5 * asin(p2));
                        if (lp_lat < 0.) xy_y = -xy_y;
                    } else {
                        al = .5 * fabs(pi / lp_lon - lp_lon / pi);
                        al2 = al * al;
                        g = sqrt(1. - p2 * p2);
                        g = g / (p2 + g - 1.);
                        g2 = g * g;
                        p2 = g * (2. / p2 - 1.);
                        p2 = p2 * p2;
                        xy_x = g - p2; g = p2 + al2;
                        xy_x = pi * (al * xy_x + sqrt(al2 * xy_x * xy_x - g * (g2 - p2))) / g;
                        if (lp_lon < 0.) xy_x = -xy_x;
                        xy_y = fabs(xy_x / pi);
                        xy_y = 1. - xy_y * (xy_y + 2. * al);
                        if (xy_y < -tolerance) {
                            BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                        }
                        if (xy_y < 0.)
                            xy_y = 0.;
                        else
                            xy_y = sqrt(xy_y) * (lp_lat < 0. ? -pi : pi);
                    }
                }

                // INVERSE(s_inverse)  spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T& xy_x, T& xy_y, T& lp_lon, T& lp_lat) const
                {
                    static const T half_pi = detail::half_pi<T>();
                    static const T pi = detail::pi<T>();
                    static const T pi_sqr = detail::pi_sqr<T>();
                    static const T third = detail::third<T>();
                    static const T two_pi = detail::two_pi<T>();

                    static const T C2_27 = vandg::C2_27<T>();
                    static const T PI4_3 = vandg::PI4_3<T>();
                    static const T TPISQ = vandg::TPISQ<T>();
                    static const T HPISQ = vandg::HPISQ<T>();

                    T t, c0, c1, c2, c3, al, r2, r, m, d, ay, x2, y2;

                    x2 = xy_x * xy_x;
                    if ((ay = fabs(xy_y)) < tolerance) {
                        lp_lat = 0.;
                        t = x2 * x2 + TPISQ * (x2 + HPISQ);
                        lp_lon = fabs(xy_x) <= tolerance ? 0. :
                           .5 * (x2 - pi_sqr + sqrt(t)) / xy_x;
                            return;
                    }
                    y2 = xy_y * xy_y;
                    r = x2 + y2;    r2 = r * r;
                    c1 = - pi * ay * (r + pi_sqr);
                    c3 = r2 + two_pi * (ay * r + pi * (y2 + pi * (ay + half_pi)));
                    c2 = c1 + pi_sqr * (r - 3. *  y2);
                    c0 = pi * ay;
                    c2 /= c3;
                    al = c1 / c3 - third * c2 * c2;
                    m = 2. * sqrt(-third * al);
                    d = C2_27 * c2 * c2 * c2 + (c0 * c0 - third * c2 * c1) / c3;
                    if (((t = fabs(d = 3. * d / (al * m))) - tolerance) <= 1.) {
                        d = t > 1. ? (d > 0. ? 0. : pi) : acos(d);
                        lp_lat = pi * (m * cos(d * third + PI4_3) - third * c2);
                        if (xy_y < 0.) lp_lat = -lp_lat;
                        t = r2 + TPISQ * (x2 - y2 + HPISQ);
                        lp_lon = fabs(xy_x) <= tolerance ? 0. :
                           .5 * (r - pi_sqr + (t <= 0. ? 0. : sqrt(t))) / xy_x;
                    } else {
                        BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                    }
                }

                static inline std::string get_name()
                {
                    return "vandg_spheroid";
                }

            };

            // van der Grinten (I)
            template <typename Parameters>
            inline void setup_vandg(Parameters& par)
            {
                par.es = 0.;
            }

    }} // namespace detail::vandg
    #endif // doxygen

    /*!
        \brief van der Grinten (I) projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Miscellaneous
         - Spheroid
        \par Example
        \image html ex_vandg.gif
    */
    template <typename T, typename Parameters>
    struct vandg_spheroid : public detail::vandg::base_vandg_spheroid<T, Parameters>
    {
        inline vandg_spheroid(const Parameters& par) : detail::vandg::base_vandg_spheroid<T, Parameters>(par)
        {
            detail::vandg::setup_vandg(this->m_par);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::vandg, vandg_spheroid, vandg_spheroid)

        // Factory entry(s)
        template <typename T, typename Parameters>
        class vandg_entry : public detail::factory_entry<T, Parameters>
        {
            public :
                virtual base_v<T, Parameters>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<vandg_spheroid<T, Parameters>, T, Parameters>(par);
                }
        };

        template <typename T, typename Parameters>
        inline void vandg_init(detail::base_factory<T, Parameters>& factory)
        {
            factory.add_to_factory("vandg", new vandg_entry<T, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_VANDG_HPP