/*! \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;
}


#include <boost/foreach.hpp>
#include <map>

#include "vector.h"
#include "bulk_operate.h"
#include "data_frame.h"
#include "factor.h"
#include "vector_vector.h"
#include "local_vec_store.h"
#include "local_vv_store.h"
#include "mem_vv_store.h"

using namespace fm;

class adj_apply: public gr_apply_operate<local_vec_store>
{
public:
	virtual void run(const void *key, const local_vec_store &val,
			local_vec_store &vec) const {
		vec.resize(val.get_length());
		memcpy(vec.get_raw_arr(), val.get_raw_arr(),
				val.get_length() * val.get_entry_size());
	}

	virtual const scalar_type &get_key_type() const {
		return get_scalar_type<int>();
	}
	virtual const scalar_type &get_output_type() const {
		return get_scalar_type<int>();
	}
	virtual size_t get_num_out_eles() const {
		return 0;
	}
};

class set_label_operate: public type_set_vec_operate<factor_value_t>
{
	factor f;
	size_t num_same_label;
public:
	set_label_operate(const factor &_f, size_t tot_num_eles): f(_f) {
		num_same_label = tot_num_eles / f.get_num_levels();
	}

	virtual void set(factor_value_t *arr, size_t num_eles, off_t start_idx) const {
		for (size_t i = 0; i < num_eles; i++)
			arr[i] = (start_idx + i) / num_same_label;
	}
};

class part_apply_operate: public gr_apply_operate<local_vv_store>
{
	const scalar_type &val_type;
public:
	part_apply_operate(const scalar_type &type): val_type(type) {
	}

	virtual void run(const void *key, const local_vv_store &val,
			local_vec_store &vec) const {
		assert(val.get_type() == val_type);
		assert(vec.get_type() == get_scalar_type<size_t>());
		vec.resize(1);
		size_t num = 0;
		for (size_t i = 0; i < val.get_length(); i++)
			num += val.get_length(i);
		vec.set<size_t>(0, num);
	}
	virtual const scalar_type &get_key_type() const {
		return get_scalar_type<factor_value_t>();
	}
	virtual const scalar_type &get_output_type() const {
		return get_scalar_type<size_t>();
	}
	virtual size_t get_num_out_eles() const {
		return 1;
	}
};

void test_groupby()
{
	printf("test groupby\n");
	detail::smp_vec_store::ptr store = detail::smp_vec_store::create(1000000,
			get_scalar_type<int>());
#pragma omp parallel for
	for (size_t i = 0; i < store->get_length(); i++)
		store->set<int>(i, random() % 1000);
	printf("set the vector store\n");
	vector::ptr vec = vector::create(store);
	data_frame::ptr res = vec->groupby(adj_apply(), false);
	printf("size: %ld\n", res->get_num_entries());

	vector_vector::ptr vv = vector_vector::create(
			detail::mem_vv_store::cast(res->get_vec("agg")));

	factor f(50);
	factor_vector::ptr labels = factor_vector::create(f, res->get_num_entries(),
			-1, true, set_label_operate(f, res->get_num_entries()));
	labels->sort();
	vector_vector::ptr gr_res = vv->groupby(*labels,
			part_apply_operate(vv->get_type()));
	const detail::mem_vv_store &res_store
		= dynamic_cast<const detail::mem_vv_store &>(gr_res->get_data());
	printf("There are %ld vectors\n", gr_res->get_num_vecs());

	std::map<factor_value_t, size_t> label_map;
	const detail::smp_vec_store &label_store
		= dynamic_cast<const detail::smp_vec_store &>(labels->get_data());
	for (size_t i = 0; i < labels->get_length(); i++) {
		factor_value_t label = label_store.get<factor_value_t>(i);
		if (label_map.find(label) == label_map.end())
			label_map.insert(std::pair<factor_value_t, size_t>(label, 0));
		label_map[label]++;
	}
	assert(gr_res->get_num_vecs() == label_map.size());

	size_t vec_idx = 0;
	size_t label_idx = 0;
	for (auto it = label_map.begin(); it != label_map.end(); it++) {
		factor_value_t label = it->first;
		size_t num_label_vecs = it->second;
		size_t num_eles = 0;
		for (size_t i = 0; i < num_label_vecs; i++) {
			assert(label_store.get<factor_value_t>(vec_idx) == label);
			num_eles += vv->get_length(vec_idx);
			vec_idx++;
		}
		assert(num_eles == *(size_t *) res_store.get_raw_arr(label_idx));
		label_idx++;
	}
	assert(vec_idx == vv->get_num_vecs());
	assert(label_idx == gr_res->get_num_vecs());
}

detail::vec_store::ptr create_mem_vec(size_t len)
{
	detail::smp_vec_store::ptr v = detail::smp_vec_store::create(
			len, get_scalar_type<int>());
	for (size_t i = 0; i < len; i++)
		v->set<int>(i, random() % 10000);
	return v;
}

detail::mem_vv_store::ptr create_mem_vv(size_t num_vecs, size_t max_vec_len)
{
	std::vector<detail::vec_store::const_ptr> vecs(num_vecs);
	for (size_t i = 0; i < vecs.size(); i++)
		vecs[i] = create_mem_vec(random() % max_vec_len);

	detail::mem_vv_store::ptr vv = detail::mem_vv_store::create(get_scalar_type<int>());
	vv->append(vecs.begin(), vecs.end());
	return vv;
}

void verify_data(const int *buf1, const int *buf2, size_t len)
{
	for (size_t i = 0; i < len; i++)
		assert(buf1[i] == buf2[i]);
}

void test_append_vecs()
{
	printf("test appending vectors\n");
	std::vector<detail::vec_store::const_ptr> vecs(1000);
	for (size_t i = 0; i < vecs.size(); i++)
		vecs[i] = create_mem_vec(random() % 1000);

	detail::mem_vv_store::ptr vv1 = detail::mem_vv_store::create(get_scalar_type<int>());
	BOOST_FOREACH(detail::vec_store::const_ptr v, vecs)
		vv1->append(*v);
	assert(vv1->get_num_vecs() == vecs.size());
	for (size_t i = 0; i < vecs.size(); i++) {
		assert(vv1->get_length(i) == vecs[i]->get_length());
		verify_data((const int *) vv1->get_raw_arr(i),
				(const int *) detail::mem_vec_store::cast(vecs[i])->get_raw_arr(),
				vecs[i]->get_length());
	}

	detail::mem_vv_store::ptr vv2 = detail::mem_vv_store::create(get_scalar_type<int>());
	vv2->append(vecs.begin(), vecs.end());
	assert(vv2->get_num_vecs() == vecs.size());
	for (size_t i = 0; i < vecs.size(); i++) {
		assert(vv2->get_length(i) == vecs[i]->get_length());
		verify_data((const int *) vv2->get_raw_arr(i),
				(const int *) detail::mem_vec_store::cast(vecs[i])->get_raw_arr(),
				vecs[i]->get_length());
	}
}

void test_append_vvs()
{
	printf("test append vector vectors\n");
	std::vector<detail::vec_store::const_ptr> vvs(100);
	std::vector<size_t> vec_lens;
	std::vector<const char *> vec_data;
	for (size_t i = 0; i <vvs.size(); i++) {
		detail::mem_vv_store::ptr vv = create_mem_vv(10, 1000);
		vvs[i] = vv;
		for (size_t j = 0; j < vv->get_num_vecs(); j++) {
			vec_lens.push_back(vv->get_length(j));
			vec_data.push_back(vv->get_raw_arr(j));
		}
	}

	detail::mem_vv_store::ptr vv1 = detail::mem_vv_store::create(get_scalar_type<int>());
	BOOST_FOREACH(detail::vec_store::const_ptr vv, vvs)
		vv1->append(*vv);
	assert(vv1->get_num_vecs() == vec_lens.size());
	for (size_t i = 0; i < vec_lens.size(); i++) {
		assert(vv1->get_length(i) == vec_lens[i]);
		verify_data((const int *) vv1->get_raw_arr(i),
				(const int *) vec_data[i], vv1->get_length(i));
	}

	detail::mem_vv_store::ptr vv2 = detail::mem_vv_store::create(get_scalar_type<int>());
	vv2->append(vvs.begin(), vvs.end());
	assert(vv2->get_num_vecs() == vec_lens.size());
	for (size_t i = 0; i < vec_lens.size(); i++) {
		assert(vv2->get_length(i) == vec_lens[i]);
		verify_data((const int *) vv2->get_raw_arr(i),
				(const int *) vec_data[i], vv2->get_length(i));
	}
}

class time2_apply_operate: public arr_apply_operate
{
public:
	virtual void run(const local_vec_store &in,
			local_vec_store &out) const {
		out.resize(in.get_length());
		assert(out.get_type() == get_scalar_type<long>());
		for (size_t i = 0; i < out.get_length(); i++) {
			out.set<long>(i, in.get<int>(i) * 2);
			assert(out.get<long>(i) == in.get<int>(i) * 2);
		}
	}
	virtual const scalar_type &get_input_type() const {
		return get_scalar_type<int>();
	}
	virtual const scalar_type &get_output_type() const {
		return get_scalar_type<long>();
	}
	virtual size_t get_num_out_eles(size_t num_input) const {
		return 0;
	}
};

void test_flatten()
{
	printf("test flatten a sub vector vector\n");
	detail::mem_vv_store::ptr vv_store = create_mem_vv(100, 1000);
	vector_vector::ptr vv = vector_vector::create(vv_store);
	vector::ptr vec = vv->cat();
	assert(memcmp(dynamic_cast<const detail::mem_vec_store &>(
					vec->get_data()).get_raw_arr(),
				vv_store->get_raw_arr(0),
				vec->get_length() * vec->get_entry_size()) == 0);
}

void test_apply()
{
	printf("test apply to each vector\n");
	detail::mem_vv_store::ptr vv_store = create_mem_vv(100, 1000);
	vector_vector::ptr vv = vector_vector::create(vv_store);

	vector_vector::ptr vv2 = vector_vector::cast(vv->apply(time2_apply_operate()));
	assert(vv->get_type() == get_scalar_type<int>());
	assert(vv2->get_type() == get_scalar_type<long>());
	for (size_t i = 0; i < vv->get_length(); i++) {
		assert(vv->get_length(i) == vv2->get_length(i));
		int *vec = (int *) vv_store->get_raw_arr(i);
		long *vec2 = (long *) dynamic_cast<const detail::mem_vv_store &>(
				vv2->get_data()).get_raw_arr(i);
		for (size_t k = 0; k < vv->get_length(i); k++)
			assert(vec[k] * 2 == vec2[k]);
	}

	vector::ptr vec = vv->cat();
	vector::ptr vec2 = vv2->cat();
	printf("vec len: %ld, vec2 len: %ld\n", vec->get_length(), vec2->get_length());
	assert(vec->get_length() == vec2->get_length());
	const detail::smp_vec_store &vstore1
		= dynamic_cast<const detail::smp_vec_store &>(vec->get_data());
	const detail::smp_vec_store &vstore2
		= dynamic_cast<const detail::smp_vec_store &>(vec2->get_data());
	for (size_t i = 0; i < vec->get_length(); i++)
		assert(vstore1.get<int>(i) * 2 == vstore2.get<long>(i));
}

void verify_portion(local_vv_store::const_ptr lvv, detail::mem_vv_store::ptr vv_store)
{
	size_t global_start = lvv->get_global_start();
	for (size_t i = 0; i < lvv->get_length(); i++) {
		const char *lvec = lvv->get_raw_arr(i);
		const char *vec = vv_store->get_raw_arr(global_start + i);
		printf("%ld: %ld, %ld: %ld\n", i, lvv->get_length(i), global_start + i,
				vv_store->get_length(global_start + i));
		assert(lvv->get_length(i) == vv_store->get_length(global_start + i));
		assert(memcmp(lvec, vec,
					lvv->get_length(i) * lvv->get_type().get_size()) == 0);
		lvec = lvv->get_sub_arr(i, i + 1);
		assert(memcmp(lvec, vec,
					lvv->get_length(i) * lvv->get_type().get_size()) == 0);
	}
}

void test_portion()
{
	printf("test getting portions\n");
	detail::mem_vv_store::ptr vv_store = create_mem_vv(100, 1000);
	local_vv_store::ptr lvv = local_vv_store::cast(vv_store->get_portion(9, 7));
	assert(lvv->get_length() == 7);
	assert(lvv->get_global_start() == 9);
	verify_portion(lvv, vv_store);

	printf("test sub vector of a portion\n");
	lvv->expose_sub_vec(2, 3);
	assert(lvv->get_global_start() == 9 + 2);
	assert(lvv->get_length() == 3);
	verify_portion(lvv, vv_store);

	printf("test reset of a portion\n");
	lvv->reset_expose();
	assert(lvv->get_global_start() == 9);
	assert(lvv->get_length() == 7);
	verify_portion(lvv, vv_store);

	printf("test get_portion of a portion\n");
	local_vv_store::ptr llvv = local_vv_store::cast(lvv->get_portion(2, 3));
	assert(llvv->get_length() == 3);
	assert(llvv->get_global_start() == 11);
	verify_portion(llvv, vv_store);

	printf("test a const portion\n");
	detail::mem_vv_store::const_ptr cvv_store = vv_store;
	local_vv_store::const_ptr clvv = local_vv_store::cast(cvv_store->get_portion(9, 7));
	verify_portion(clvv, vv_store);
}

int main()
{
	test_portion();
	test_groupby();
	test_append_vecs();
	test_append_vvs();
	test_flatten();
	test_apply();
}


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

#ifndef MTL__OPERATION_COMPUTE_SUMMAND_INCLUDE
#define MTL__OPERATION_COMPUTE_SUMMAND_INCLUDE

#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/utility/copy_expression_const_ref_container.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl { namespace operation {

/// Compute a summand in an expression
/** For instance matrix vector products are transformed into mult function calls
    when assigned to a column vector. Adding such a vector to other vector expressions
    requires to compute (evaluate) the summand first and then add the resulting
    vector. The current implementation assumes that the result of the operation
    can be represented by the multiplied column vector. This is for instance wrong
    when the matrix is complex and the vector real. To handle this is signifantly
    more complicated and is planned for the future.
**/
template <typename Expr>
struct compute_summand
{
    typedef Expr type;
    compute_summand(const Expr& expr) : value(expr) {}
    // value is a const& if Expr is a true vector and a copy if it is an expression
    typename mtl::traits::copy_expression_const_ref_container<Expr>::type value;
};

/// Specialization for matrix vector products
template <typename Matrix, typename CVector>
struct compute_summand< mat_cvec_times_expr<Matrix, CVector> >
{
    typedef CVector    type;

    compute_summand(const mat_cvec_times_expr<Matrix, CVector>& expr)
      : value(num_rows(expr.first))
#ifndef NDEBUG // might be helpful, e.g. for generating AST
      , first(expr.first), second(expr.second)
#endif
    {
	vampir_trace<3005> tracer;
	value= expr.first * expr.second;
    }

    CVector value;
#ifndef NDEBUG
    const Matrix&  first;
    const CVector& second;
#endif
};

/// Specialization for matrix vector products
template <typename Matrix, typename CVector>
struct compute_summand< vector::mat_cvec_multiplier<Matrix, CVector> >
{
    typedef CVector    type;

    compute_summand(const vector::mat_cvec_multiplier<Matrix, CVector>& expr)
      : value(num_rows(expr.A))
    {
	vampir_trace<3005> tracer;
	expr.assign_to(value);
    }

    CVector value;
};

}} // namespace mtl::operation

#endif // MTL__OPERATION_COMPUTE_SUMMAND_INCLUDE


#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