/*! \file Peridigm_CorrespondenceMaterial.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
//
// ************************************************************************
//@HEADER

#include "Peridigm_CorrespondenceMaterial.hpp"
#include "Peridigm_Field.hpp"
#include "elastic.h"
#include "correspondence.h"
#include "Peridigm_DegreesOfFreedomManager.hpp"
#include "elastic_correspondence.h"
#include <Teuchos_Assert.hpp>
#include <Epetra_SerialComm.h>
#include <Sacado.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
using namespace std;

PeridigmNS::CorrespondenceMaterial::CorrespondenceMaterial(const Teuchos::ParameterList& params)
  : Material(params),
    m_density(0.0), m_hourglassCoefficient(0.0),
        m_OMEGA(PeridigmNS::InfluenceFunction::self().getInfluenceFunction()),
    m_horizonFieldId(-1), m_volumeFieldId(-1),
    m_modelCoordinatesFieldId(-1), m_coordinatesFieldId(-1), m_velocitiesFieldId(-1),
    m_hourglassForceDensityFieldId(-1), m_forceDensityFieldId(-1), m_bondDamageFieldId(-1),
    m_deformationGradientFieldId(-1),
    m_shapeTensorInverseFieldId(-1),
    m_leftStretchTensorFieldId(-1),
    m_rotationTensorFieldId(-1),
    m_unrotatedCauchyStressFieldId(-1),
    m_cauchyStressFieldId(-1),
    m_unrotatedRateOfDeformationFieldId(-1),
    m_partialStressFieldId(-1),
    m_hourglassStiffId(-1)

{

  //! \todo Add meaningful asserts on material properties.
  m_bulkModulus = calculateBulkModulus(params);
  m_shearModulus = calculateShearModulus(params);
  m_density = params.get<double>("Density");

  m_stabilizationType = 3;

  m_plane = false;

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

    bool m_planeStrain = false, m_planeStress = false;
    if (params.isParameter("Plane Strain"))
        m_planeStrain = params.get<bool>("Plane Strain");

    if (params.isParameter("Plane Stress"))
        m_planeStress = params.get<bool>("Plane Stress");

    if (m_planeStrain==true){
        m_plane=true;

    }
    if (m_planeStress==true){
        m_plane=true;

    }
  m_tension = true;
  if (params.isParameter("Tension pressure separation for damage model")){
      m_tension = params.get<bool>("Tension pressure separation for damage model");

  }
  m_applyAutomaticDifferentiationJacobian = true;
  if(params.isParameter("Apply Automatic Differentiation Jacobian"))
    m_applyAutomaticDifferentiationJacobian = params.get<bool>("Apply Automatic Differentiation Jacobian");
  if (params.isParameter("Stabilizaton Type")){

    if (params.get<string>("Stabilizaton Type")=="Bond Based"){
        m_stabilizationType = 1;
        m_hourglassCoefficient = params.get<double>("Hourglass Coefficient");
    }
    if (params.get<string>("Stabilizaton Type")=="State Based"){
        m_stabilizationType = 2;
        m_hourglassCoefficient = params.get<double>("Hourglass Coefficient");
    }
    //if (params.get<string>("Stabilizaton Type")=="Sub Horizon"){
    //    m_stabilizationType = 4;
    //    // works only for linear elastic correspondence first
    //    // based on: Shubhankar Roy Chowdhury, Pranesh Roy, Debasish Roy and J N Reddy,
    //    // "A simple alteration of the peridynamics correspondence principle to eliminate zero-energy deformation"
    //}
    if (params.get<string>("Stabilizaton Type")=="Global Stiffness"){

        m_stabilizationType = 3;
        m_hourglassCoefficient = params.get<double>("Hourglass Coefficient");

        double C11 = 0.0, C12 = 0.0, C13 = 0.0, C14 = 0.0, C15 = 0.0, C16 = 0.0;
        double            C22 = 0.0, C23 = 0.0, C24 = 0.0, C25 = 0.0, C26 = 0.0;
        double                       C33 = 0.0, C34 = 0.0, C35 = 0.0, C36 = 0.0;
        double                                  C44 = 0.0, C45 = 0.0, C46 = 0.0;
        double                                             C55 = 0.0, C56 = 0.0;
        double                                                        C66 = 0.0;

        bool iso = false;
        if (params.isParameter("Material Symmetry")){
            if (params.get<string>("Material Symmetry")=="Isotropic"){
                C11 = params.get<double>("C11");
                C44 = params.get<double>("C44");
                C55 = params.get<double>("C44");
                C66 = params.get<double>("C44");
                C12 = C11 - 2*C55;
                C13 = C12;
                C14 = 0.0;
                C15 = 0.0;
                C16 = 0.0;
                C22 = params.get<double>("C11");
                C33 = params.get<double>("C11");
                C23 = C12;
                C24 = 0.0;
                C25 = 0.0;
                C26 = 0.0;
                C34 = 0.0;
                C35 = 0.0;
                C36 = 0.0;
                C45 = 0.0;
                C46 = 0.0;
                C56 = 0.0;
                iso  = true;
            }
            if (params.get<string>("Material Symmetry")=="Anisotropic"){
                C11 = params.get<double>("C11");
                C12 = params.get<double>("C12");
                C13 = params.get<double>("C13");
                C14 = params.get<double>("C14");
                C15 = params.get<double>("C15");
                C16 = params.get<double>("C16");
                C22 = params.get<double>("C22");
                C23 = params.get<double>("C23");
                C24 = params.get<double>("C24");
                C25 = params.get<double>("C25");
                C26 = params.get<double>("C26");
                C33 = params.get<double>("C33");
                C34 = params.get<double>("C34");
                C35 = params.get<double>("C35");
                C36 = params.get<double>("C36");
                C44 = params.get<double>("C44");
                C45 = params.get<double>("C45");
                C46 = params.get<double>("C46");
                C55 = params.get<double>("C55");
                C56 = params.get<double>("C56");
                C66 = params.get<double>("C66");

            }

        }
        else{
            m_bulkModulus = calculateBulkModulus(params);
            m_shearModulus = calculateShearModulus(params);

            C11 = (4*m_shearModulus*(3*m_bulkModulus + m_shearModulus))/(3*m_bulkModulus + 4*m_shearModulus);
            C44 = m_shearModulus;
            C55 = m_shearModulus;
            C66 = m_shearModulus;
            C12 = (2*(3*m_bulkModulus - 2*m_shearModulus)*m_shearModulus)/(3*m_bulkModulus + 4*m_shearModulus);
            C13 = C12;
            C14 = 0.0;
            C15 = 0.0;
            C16 = 0.0;
            C22 = C11;
            C33 = C11;
            C23 = C12;
            C24 = 0.0;
            C25 = 0.0;
            C26 = 0.0;
            C34 = 0.0;
            C35 = 0.0;
            C36 = 0.0;
            C45 = 0.0;
            C46 = 0.0;
            C56 = 0.0;
        }
        // Equation (8) Dipasquale, D., Sarego, G., Zaccariotto, M., Galvanetto, U., A discussion on failure criteria
          // for ordinary state-based Peridynamics, Engineering Fracture Mechanics (2017), doi: https://doi.org/10.1016/
          // j.engfracmech.2017.10.011
        if (m_planeStrain==true)m_plane=true;
        if (m_planeStress==true)m_plane=true;
        // have to be done after rotation if angles exist
        if (m_plane==false){
         C[0][0] = C11;C[0][1] = C12;C[0][2]= C13; C[0][3] = C14; C[0][4] = C15; C[0][5]= C16;
         C[1][0] = C12;C[1][1] = C22;C[1][2]= C23; C[1][3] = C24; C[1][4] = C25; C[1][5]= C26;
         C[2][0] = C13;C[2][1] = C23;C[2][2]= C33; C[2][3] = C34; C[2][4] = C35; C[2][5]= C36;
         C[3][0] = C14;C[3][1] = C24;C[3][2]= C34; C[3][3] = C44; C[3][4] = C45; C[3][5]= C46;
         C[4][0] = C15;C[4][1] = C25;C[4][2]= C35; C[4][3] = C45; C[4][4] = C55; C[4][5]= C56;
         C[5][0] = C16;C[5][1] = C26;C[5][2]= C36; C[5][3] = C46; C[5][4] = C56; C[5][5]= C66;
        }
        // tbd in future
        if (m_planeStress==true && iso == true){
            //only transversal isotropic in the moment --> definition of iso missing
         C[0][0] = C11-C13*C13/C22;C[0][1] = C12-C13*C23/C22;C[0][2] = 0.0; C[0][3] = 0.0; C[0][4] = 0.0; C[0][5] = 0.0;
         C[1][0] = C12-C13*C23/C22;C[1][1] = C22-C13*C23/C22;C[1][2] = 0.0; C[1][3] = 0.0; C[1][4] = 0.0; C[1][5] = 0.0;
         C[2][0] = 0.0;            C[2][1] = 0.0;            C[2][2] = 0.0; C[2][3] = 0.0; C[2][4] = 0.0; C[2][5] = 0.0;
         C[3][0] = 0.0;            C[3][1] = 0.0;            C[3][2] = 0.0; C[3][3] = 0.0; C[3][4] = 0.0; C[3][5] = 0.0;
         C[4][0] = 0.0;            C[4][1] = 0.0;            C[4][2] = 0.0; C[4][3] = 0.0; C[4][4] = 0.0; C[4][5] = 0.0;
         C[5][0] = 0.0;            C[5][1] = 0.0;            C[5][2] = 0.0; C[5][3] = 0.0; C[5][4] = 0.0; C[5][5] = C66;
        }
        // not correct for plane stress!!!
        if (m_plane==true && iso == false){
         C[0][0] = C11;C[0][1] = C12;C[0][2] = 0.0;C[0][3] = 0.0;C[0][4] = 0.0;C[0][5] = C16;
         C[1][0] = C12;C[1][1] = C22;C[1][2] = 0.0;C[1][3] = 0.0;C[1][4] = 0.0;C[1][5] = C26;
         C[2][0] = 0.0;C[2][1] = 0.0;C[2][2] = 0.0;C[2][3] = 0.0;C[2][4] = 0.0;C[2][5] = 0.0;
         C[3][0] = 0.0;C[3][1] = 0.0;C[3][2] = 0.0;C[3][3] = 0.0;C[3][4] = 0.0;C[3][5] = 0.0;
         C[4][0] = 0.0;C[4][1] = 0.0;C[4][2] = 0.0;C[4][3] = 0.0;C[4][4] = 0.0;C[4][5] = 0.0;
         C[5][0] = C16;C[5][1] = C26;C[5][2] = 0.0;C[5][3] = 0.0;C[5][4] = 0.0;C[5][5] = C66;

        }

    }
  }
  nonLin = false;
  lin = true;

  if (params.isParameter("Elastic Correspondence")){

      nonLin = true;
      lin = false;

    }

  //TEUCHOS_TEST_FOR_EXCEPT_MSG(params.isParameter("Apply Automatic Differentiation Jacobian"), "**** Error:  Automatic Differentiation is not supported for the ElasticCorrespondence material model.\n");
  TEUCHOS_TEST_FOR_EXCEPT_MSG(params.isParameter("Apply Shear Correction Factor"), "**** Error:  Shear Correction Factor is not supported for the ElasticCorrespondence material model.\n");
  TEUCHOS_TEST_FOR_EXCEPT_MSG(params.isParameter("Thermal Expansion Coefficient"), "**** Error:  Thermal expansion is not currently supported for the ElasticCorrespondence material model.\n");

  PeridigmNS::FieldManager& fieldManager = PeridigmNS::FieldManager::self();
  m_horizonFieldId                    = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::SCALAR, PeridigmField::CONSTANT, "Horizon");
  m_volumeFieldId                     = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::SCALAR, PeridigmField::CONSTANT, "Volume");
  m_modelCoordinatesFieldId           = fieldManager.getFieldId(PeridigmField::NODE,    PeridigmField::VECTOR, PeridigmField::CONSTANT, "Model_Coordinates");
  m_modelAnglesId                     = fieldManager.getFieldId(PeridigmField::NODE,    PeridigmField::VECTOR, PeridigmField::CONSTANT, "Local_Angles");
  m_coordinatesFieldId                = fieldManager.getFieldId(PeridigmField::NODE,    PeridigmField::VECTOR, PeridigmField::TWO_STEP, "Coordinates");
  m_velocitiesFieldId                 = fieldManager.getFieldId(PeridigmField::NODE,    PeridigmField::VECTOR, PeridigmField::TWO_STEP, "Velocity");
  m_forceDensityFieldId               = fieldManager.getFieldId(PeridigmField::NODE,    PeridigmField::VECTOR, PeridigmField::TWO_STEP, "Force_Density");
  m_hourglassForceDensityFieldId      = fieldManager.getFieldId(PeridigmField::NODE,    PeridigmField::VECTOR, PeridigmField::TWO_STEP, "Hourglass_Force_Density");
  m_bondDamageFieldId                 = fieldManager.getFieldId(PeridigmField::BOND,    PeridigmField::SCALAR, PeridigmField::TWO_STEP, "Bond_Damage");
  m_deformationGradientFieldId        = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::FULL_TENSOR, PeridigmField::CONSTANT, "Deformation_Gradient");
  m_leftStretchTensorFieldId          = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::FULL_TENSOR, PeridigmField::TWO_STEP, "Left_Stretch_Tensor");
  m_rotationTensorFieldId             = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::FULL_TENSOR, PeridigmField::TWO_STEP, "Rotation_Tensor");
  m_shapeTensorInverseFieldId         = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::FULL_TENSOR, PeridigmField::CONSTANT, "Shape_Tensor_Inverse");
  m_unrotatedCauchyStressFieldId      = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::FULL_TENSOR, PeridigmField::TWO_STEP, "Unrotated_Cauchy_Stress");
  m_cauchyStressFieldId               = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::FULL_TENSOR, PeridigmField::TWO_STEP, "Cauchy_Stress");
  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_unrotatedRateOfDeformationFieldId = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::FULL_TENSOR, PeridigmField::CONSTANT, "Unrotated_Rate_Of_Deformation");
  m_partialStressFieldId              = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::FULL_TENSOR, PeridigmField::TWO_STEP, "Partial_Stress");
  m_detachedNodesFieldId              = fieldManager.getFieldId(PeridigmField::NODE,    PeridigmField::SCALAR, PeridigmField::TWO_STEP, "Detached_Nodes");
  m_hourglassStiffId                  = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::FULL_TENSOR, PeridigmField::CONSTANT, "Hourglass_Stiffness");

  m_fieldIds.push_back(m_horizonFieldId);
  m_fieldIds.push_back(m_volumeFieldId);
  m_fieldIds.push_back(m_modelCoordinatesFieldId);
  m_fieldIds.push_back(m_coordinatesFieldId);
  m_fieldIds.push_back(m_velocitiesFieldId);
  m_fieldIds.push_back(m_hourglassForceDensityFieldId);
  m_fieldIds.push_back(m_forceDensityFieldId);
  m_fieldIds.push_back(m_bondDamageFieldId);
  m_fieldIds.push_back(m_deformationGradientFieldId);

  m_fieldIds.push_back(m_leftStretchTensorFieldId);
  m_fieldIds.push_back(m_rotationTensorFieldId);
  m_fieldIds.push_back(m_shapeTensorInverseFieldId);
  m_fieldIds.push_back(m_unrotatedCauchyStressFieldId);
  m_fieldIds.push_back(m_cauchyStressFieldId);
  m_fieldIds.push_back(m_piolaStressTimesInvShapeTensorXId);
  m_fieldIds.push_back(m_piolaStressTimesInvShapeTensorYId);
  m_fieldIds.push_back(m_piolaStressTimesInvShapeTensorZId);
  m_fieldIds.push_back(m_unrotatedRateOfDeformationFieldId);
  m_fieldIds.push_back(m_partialStressFieldId);
  m_fieldIds.push_back(m_detachedNodesFieldId);
  m_fieldIds.push_back(m_hourglassStiffId);
  m_fieldIds.push_back(m_modelAnglesId);

  }

PeridigmNS::CorrespondenceMaterial::~CorrespondenceMaterial()
{
}

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

  dataManager.getData(m_unrotatedRateOfDeformationFieldId, PeridigmField::STEP_NONE)->PutScalar(0.0);
  dataManager.getData(m_hourglassStiffId, PeridigmField::STEP_NONE)->PutScalar(0.0);
  dataManager.getData(m_leftStretchTensorFieldId, PeridigmField::STEP_N)->PutScalar(0.0);
  dataManager.getData(m_leftStretchTensorFieldId, PeridigmField::STEP_NP1)->PutScalar(0.0);
  dataManager.getData(m_rotationTensorFieldId, PeridigmField::STEP_N)->PutScalar(0.0);
  dataManager.getData(m_rotationTensorFieldId, PeridigmField::STEP_NP1)->PutScalar(0.0);
  dataManager.getData(m_unrotatedCauchyStressFieldId, PeridigmField::STEP_N)->PutScalar(0.0);
  dataManager.getData(m_unrotatedCauchyStressFieldId, PeridigmField::STEP_NP1)->PutScalar(0.0);
  dataManager.getData(m_cauchyStressFieldId, PeridigmField::STEP_N)->PutScalar(0.0);
  dataManager.getData(m_cauchyStressFieldId, PeridigmField::STEP_NP1)->PutScalar(0.0);
  dataManager.getData(m_detachedNodesFieldId, PeridigmField::STEP_NP1)->PutScalar(0.0);

  dataManager.getData(m_partialStressFieldId, PeridigmField::STEP_N)->PutScalar(0.0);
  dataManager.getData(m_partialStressFieldId, PeridigmField::STEP_NP1)->PutScalar(0.0);

  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);

  double *leftStretchTensorN;
  double *leftStretchTensorNP1;
  double *rotationTensorN;
  double *rotationTensorNP1;
  double *detachedNodes;
  dataManager.getData(m_leftStretchTensorFieldId, PeridigmField::STEP_N)->ExtractView(&leftStretchTensorN);
  dataManager.getData(m_leftStretchTensorFieldId, PeridigmField::STEP_NP1)->ExtractView(&leftStretchTensorNP1);
  dataManager.getData(m_rotationTensorFieldId, PeridigmField::STEP_N)->ExtractView(&rotationTensorN);
  dataManager.getData(m_rotationTensorFieldId, PeridigmField::STEP_NP1)->ExtractView(&rotationTensorNP1);
  dataManager.getData(m_detachedNodesFieldId, PeridigmField::STEP_NP1)->ExtractView(&detachedNodes);
  //Initialize the left stretch and rotation tenor to the identity matrix
  CORRESPONDENCE::setOnesOnDiagonalFullTensor(leftStretchTensorN, numOwnedPoints);
  CORRESPONDENCE::setOnesOnDiagonalFullTensor(leftStretchTensorNP1, numOwnedPoints);
  CORRESPONDENCE::setOnesOnDiagonalFullTensor(rotationTensorN, numOwnedPoints);
  CORRESPONDENCE::setOnesOnDiagonalFullTensor(rotationTensorNP1, numOwnedPoints);

  //Initialize the inverse of the shape tensor and the deformation gradient
  double *volume;
  double *horizon;
  double *modelCoordinates;
  double *coordinates;
  double *coordinatesNP1;
  double *shapeTensorInverse;
  double *deformationGradient;
  double *bondDamage;

  dataManager.getData(m_volumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&volume);
  dataManager.getData(m_horizonFieldId, PeridigmField::STEP_NONE)->ExtractView(&horizon);

  dataManager.getData(m_modelCoordinatesFieldId, PeridigmField::STEP_NONE)->ExtractView(&modelCoordinates);
  dataManager.getData(m_coordinatesFieldId, PeridigmField::STEP_N)->ExtractView(&coordinates);
  dataManager.getData(m_coordinatesFieldId, PeridigmField::STEP_NP1)->ExtractView(&coordinatesNP1);
  dataManager.getData(m_shapeTensorInverseFieldId, PeridigmField::STEP_NONE)->ExtractView(&shapeTensorInverse);
  dataManager.getData(m_deformationGradientFieldId, PeridigmField::STEP_NONE)->ExtractView(&deformationGradient);
  dataManager.getData(m_unrotatedRateOfDeformationFieldId, PeridigmField::STEP_NONE)->PutScalar(0.0);
  dataManager.getData(m_bondDamageFieldId, PeridigmField::STEP_NP1)->ExtractView(&bondDamage);

 int shapeTensorReturnCode = 0;
  shapeTensorReturnCode =
            CORRESPONDENCE::computeShapeTensorInverseAndApproximateDeformationGradient(volume,
                                                                                horizon,
                                                                                modelCoordinates,
                                                                                coordinates,
                                                                                coordinatesNP1,
                                                                                shapeTensorInverse,
                                                                                deformationGradient,
                                                                                bondDamage,
                                                                                neighborhoodList,
                                                                                numOwnedPoints,
                                                                                m_plane,
                                                                                detachedNodes);

  string shapeTensorErrorMessage =
    "**** Error:  CorrespondenceMaterial::initialize() failed to compute shape tensor.\n";
  shapeTensorErrorMessage +=
    "****         Note that all nodes must have a minimum of three neighbors.  Is the horizon too small?\n";
  TEUCHOS_TEST_FOR_EXCEPT_MSG(shapeTensorReturnCode != 0, shapeTensorErrorMessage);

}

void
PeridigmNS::CorrespondenceMaterial::computeForce(const double dt,
                                                 const int numOwnedPoints,
                                                 const int* ownedIDs,
                                                 const int* neighborhoodList,
                                                 PeridigmNS::DataManager& dataManager) const
{
  // Zero out the forces and partial stress

  dataManager.getData(m_forceDensityFieldId, PeridigmField::STEP_NP1)->PutScalar(0.0);
  dataManager.getData(m_partialStressFieldId, PeridigmField::STEP_NP1)->PutScalar(0.0);
  dataManager.getData(m_unrotatedRateOfDeformationFieldId, PeridigmField::STEP_NONE)->PutScalar(0.0);
  double *horizon, *volume, *modelCoordinates, *coordinates, *coordinatesNP1, *shapeTensorInverse, *deformationGradient, *bondDamage, *pointAngles, *detachedNodes;
  //double *deformationGradientNonInc;

  dataManager.getData(m_horizonFieldId, PeridigmField::STEP_NONE)->ExtractView(&horizon);
  dataManager.getData(m_volumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&volume);
  dataManager.getData(m_modelCoordinatesFieldId, PeridigmField::STEP_NONE)->ExtractView(&modelCoordinates);
  dataManager.getData(m_modelAnglesId,           PeridigmField::STEP_NONE)->ExtractView(&pointAngles);
  dataManager.getData(m_coordinatesFieldId, PeridigmField::STEP_N)->ExtractView(&coordinates);
  dataManager.getData(m_coordinatesFieldId, PeridigmField::STEP_NP1)->ExtractView(&coordinatesNP1);
  dataManager.getData(m_shapeTensorInverseFieldId, PeridigmField::STEP_NONE)->ExtractView(&shapeTensorInverse);
  dataManager.getData(m_deformationGradientFieldId, PeridigmField::STEP_NONE)->ExtractView(&deformationGradient);

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

  dataManager.getData(m_detachedNodesFieldId, PeridigmField::STEP_NP1)->ExtractView(&detachedNodes);
  // Compute the inverse of the shape tensor and the approximate deformation gradient
  // The approximate deformation gradient will be used by the derived class (specific correspondence material model)
  // to compute the Cauchy stress.
  // The inverse of the shape tensor is stored for later use after the Cauchy stress calculation

   // vier szenarien
   int shapeTensorReturnCode = 0;
  // if (lin == true){

    shapeTensorReturnCode =
        CORRESPONDENCE::computeShapeTensorInverseAndApproximateDeformationGradient(volume,
                                                                                horizon,
                                                                                modelCoordinates,
                                                                                coordinates,
                                                                                coordinatesNP1,
                                                                                shapeTensorInverse,
                                                                                deformationGradient,
                                                                                bondDamage,
                                                                                neighborhoodList,
                                                                                numOwnedPoints,
                                                                                m_plane,
                                                                                detachedNodes);

   //}

  string shapeTensorErrorMessage =
    "**** Error:  CorrespondenceMaterial::computeForce() failed to compute shape tensor.\n";
  shapeTensorErrorMessage +=
    "****         Note that all nodes must have a minimum of three neighbors.  Is the horizon too small?\n";
  TEUCHOS_TEST_FOR_EXCEPT_MSG(shapeTensorReturnCode != 0, shapeTensorErrorMessage);

  double *velocities, *leftStretchTensorN, *leftStretchTensorNP1, *rotationTensorN, *rotationTensorNP1, *unrotatedRateOfDeformation;
  if (nonLin==true){
      dataManager.getData(m_leftStretchTensorFieldId, PeridigmField::STEP_N)->ExtractView(&leftStretchTensorN);
      dataManager.getData(m_leftStretchTensorFieldId, PeridigmField::STEP_NP1)->ExtractView(&leftStretchTensorNP1);
      dataManager.getData(m_rotationTensorFieldId, PeridigmField::STEP_N)->ExtractView(&rotationTensorN);
      dataManager.getData(m_rotationTensorFieldId, PeridigmField::STEP_NP1)->ExtractView(&rotationTensorNP1);
      dataManager.getData(m_unrotatedRateOfDeformationFieldId, PeridigmField::STEP_NONE)->ExtractView(&unrotatedRateOfDeformation);
      dataManager.getData(m_velocitiesFieldId, PeridigmField::STEP_NP1)->ExtractView(&velocities);
      // Compute left stretch tensor, rotation tensor, and unrotated rate-of-deformation.
      // Performs a polar decomposition via Flanagan & Taylor (1987) algorithm.
      //"A non-ordinary state-based peridynamic method to model solid material deformation and fracture"
      // Thomas L. Warren, Stewart A. Silling, Abe Askari, Olaf Weckner, Michael A. Epton, Jifeng Xu

      int rotationTensorReturnCode = 0;

      rotationTensorReturnCode = CORRESPONDENCE::computeUnrotatedRateOfDeformationAndRotationTensor(volume,
                                                                                horizon,
                                                                                modelCoordinates,
                                                                                velocities,
                                                                                deformationGradient,
                                                                                shapeTensorInverse,
                                                                                leftStretchTensorN,
                                                                                rotationTensorN,
                                                                                leftStretchTensorNP1,
                                                                                rotationTensorNP1,
                                                                                unrotatedRateOfDeformation,
                                                                                neighborhoodList,
                                                                                numOwnedPoints,
                                                                                dt,
                                                                                bondDamage,
                                                                                m_plane,
                                                                                detachedNodes);

      string rotationTensorErrorMessage =
        "**** Error:  CorrespondenceMaterial::computeForce() failed to compute rotation tensor.\n";
      rotationTensorErrorMessage +=
        "****         Note that all nodes must have a minimum of three neighbors.  Is the horizon too small?\n";
      string rotationTensorErrorMessage2 =
        "**** Error:  CorrespondenceMaterial::computeForce() failed to invert deformation gradient tensor.\n";

      string rotationTensorErrorMessage3 =
        "**** Error:  CorrespondenceMaterial::computeForce() failed to invert temp.\n";

      TEUCHOS_TEST_FOR_EXCEPT_MSG(rotationTensorReturnCode == 1, rotationTensorErrorMessage);
      TEUCHOS_TEST_FOR_EXCEPT_MSG(rotationTensorReturnCode == 2, rotationTensorErrorMessage2);
      TEUCHOS_TEST_FOR_EXCEPT_MSG(rotationTensorReturnCode == 3, rotationTensorErrorMessage3);

     }

  // Evaluate the Cauchy stress using the routine implemented in the derived class (specific correspondence material model)
  // The general idea is to compute the stress based on:
  //   1) The unrotated rate-of-deformation tensor
  //   2) The time step
  //   3) Whatever state variables are managed by the derived class
  //
  // computeCauchyStress() typically uses the following fields which are accessed via the DataManager:
  //   Input:  unrotated rate-of-deformation tensor
  //   Input:  unrotated Cauchy stress at step N
  //   Input:  internal state data (managed in the derived class)
  //   Output: unrotated Cauchy stress at step N+1

  // multiple Cauchy stresses will be provided over the datamanager --> Peridigm_ElasticLinearCorrespondence

  computeCauchyStress(dt, numOwnedPoints, dataManager);

  // rotate back to the Eulerian frame
  double *unrotatedCauchyStressNP1, *cauchyStressNP1;

  if (nonLin == true) {
      dataManager.getData(m_unrotatedCauchyStressFieldId, PeridigmField::STEP_NP1)->ExtractView(&unrotatedCauchyStressNP1);
      dataManager.getData(m_cauchyStressFieldId, PeridigmField::STEP_NP1)->ExtractView(&cauchyStressNP1);

      CORRESPONDENCE::rotateCauchyStress(rotationTensorNP1,
                                         unrotatedCauchyStressNP1,
                                         cauchyStressNP1,
                                         numOwnedPoints);
                                         // Cauchy stress is now updated and in the rotated state.
  }
  else
  {
    dataManager.getData(m_cauchyStressFieldId, PeridigmField::STEP_NP1)->ExtractView(&cauchyStressNP1);

  }

  // Proceed with conversion to Piola-Kirchoff and force-vector states.
//--------------------------------------------------------------------------
 double *forceDensity;
 dataManager.getData(m_forceDensityFieldId, PeridigmField::STEP_NP1)->ExtractView(&forceDensity);
 dataManager.getData(m_forceDensityFieldId, PeridigmField::STEP_NP1)->PutScalar(0.0);
 double *partialStress;
 dataManager.getData(m_partialStressFieldId, PeridigmField::STEP_NP1)->ExtractView(&partialStress);
 dataManager.getData(m_partialStressFieldId, PeridigmField::STEP_NP1)->PutScalar(0.0);

 double *tempStressX, *tempStressY, *tempStressZ;
 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_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);

 double *hourglassStiff;

 dataManager.getData(m_hourglassStiffId, PeridigmField::STEP_NONE)->ExtractView(&hourglassStiff);

 CORRESPONDENCE::computeForcesAndStresses(
                                       numOwnedPoints,
                                       neighborhoodList,
                                       volume,
                                       horizon,
                                       modelCoordinates,
                                       coordinatesNP1,
                                       deformationGradient,
                                       cauchyStressNP1,
                                       shapeTensorInverse,
                                       bondDamage,
                                       C,
                                       pointAngles,
                                       forceDensity,
                                       partialStress,
                                       tempStressX,
                                       tempStressY,
                                       tempStressZ,
                                       hourglassStiff,
                                       m_hourglassCoefficient,
                                       m_stabilizationType,
                                       m_plane,
                                       m_tension,
                                       detachedNodes);

 //     std::cout<<numOwnedPoints<< " "<< *(deformationGradient)<<" "<<*(partialStress)<<std::endl;

  if (m_incremental == false){
      if (m_stabilizationType == 1){
      CORRESPONDENCE::computeHourglassForce(volume,
                                            horizon,
                                            modelCoordinates,
                                            coordinates,
                                            deformationGradient,
                                            forceDensity,
                                            neighborhoodList,
                                            numOwnedPoints,
                                            m_bulkModulus,
                                            m_hourglassCoefficient,
                                            bondDamage
                                            );
      }

      if (m_stabilizationType == 2){

      CORRESPONDENCE::computeCorrespondenceStabilityForce(volume,
                                            horizon,
                                            modelCoordinates,
                                            coordinates,
                                            deformationGradient,
                                            forceDensity,
                                            neighborhoodList,
                                            numOwnedPoints,
                                            m_bulkModulus,
                                            m_hourglassCoefficient,
                                            bondDamage
                                            );
      }
  }
}

void
PeridigmNS::CorrespondenceMaterial::computeJacobian(const double dt,
                                             const int numOwnedPoints,
                                             const int* ownedIDs,
                                             const int* neighborhoodList,
                                             PeridigmNS::DataManager& dataManager,
                                             PeridigmNS::SerialMatrix& jacobian,
                                             PeridigmNS::Material::JacobianType jacobianType) const
{

  if(m_applyAutomaticDifferentiationJacobian){
    // Compute the Jacobian via automatic differentiation
    computeAutomaticDifferentiationJacobian(dt, numOwnedPoints, ownedIDs, neighborhoodList, dataManager, jacobian, jacobianType);
  }
  else{
  //  // Call the base class function, which computes the Jacobian by finite difference
    computeJacobianFiniteDifference(dt, numOwnedPoints, ownedIDs, neighborhoodList, dataManager, jacobian, CENTRAL_DIFFERENCE, jacobianType);
  }
}

void
PeridigmNS::CorrespondenceMaterial::computeAutomaticDifferentiationJacobian(const double dt,
                                                                     const int numOwnedPoints,
                                                                     const int* ownedIDs,
                                                                     const int* neighborhoodList,
                                                                     PeridigmNS::DataManager& dataManager,
                                                                     PeridigmNS::SerialMatrix& jacobian,
                                                                     PeridigmNS::Material::JacobianType jacobianType) const
{
  // Compute contributions to the tangent matrix on an element-by-element basis

  // To reduce memory re-allocation, use static variable to store Fad types for
  // current coordinates (independent variables).

  static vector<Sacado::Fad::DFad<double> > coordinatesNP1_AD;
  static vector<Sacado::Fad::DFad<double> > coordinates_AD;

  static Sacado::Fad::DFad<double> C_AD[6][6];

  // Loop over all points.
  int neighborhoodListIndex = 0;
  for(int iID=0 ; iID<numOwnedPoints ; ++iID){

    // Create a temporary neighborhood consisting of a single point and its neighbors.
    int numNeighbors = neighborhoodList[neighborhoodListIndex++];
    int numEntries = numNeighbors+1;
    int numDof = 3*numEntries;
    vector<int> tempMyGlobalIDs(numEntries);
    // Put the node at the center of the neighborhood at the beginning of the list.
    tempMyGlobalIDs[0] = dataManager.getOwnedScalarPointMap()->GID(iID);
    vector<int> tempNeighborhoodList(numEntries);
    tempNeighborhoodList[0] = numNeighbors;

    for(int iNID=0 ; iNID<numNeighbors ; ++iNID){
      int neighborID = neighborhoodList[neighborhoodListIndex++];
      tempMyGlobalIDs[iNID+1] = dataManager.getOverlapScalarPointMap()->GID(neighborID);
      tempNeighborhoodList[iNID+1] = iNID+1;
    }

    Epetra_SerialComm serialComm;
    Teuchos::RCP<Epetra_BlockMap> tempOneDimensionalMap = Teuchos::rcp(new Epetra_BlockMap(numEntries, numEntries, &tempMyGlobalIDs[0], 1, 0, serialComm));
    Teuchos::RCP<Epetra_BlockMap> tempThreeDimensionalMap = Teuchos::rcp(new Epetra_BlockMap(numEntries, numEntries, &tempMyGlobalIDs[0], 3, 0, serialComm));
    Teuchos::RCP<Epetra_BlockMap> tempBondMap = Teuchos::rcp(new Epetra_BlockMap(1, 1, &tempMyGlobalIDs[0], numNeighbors, 0, serialComm));

    // Create a temporary DataManager containing data for this point and its neighborhood.
    PeridigmNS::DataManager tempDataManager;
    tempDataManager.setMaps(Teuchos::RCP<const Epetra_BlockMap>(),
                            tempOneDimensionalMap,
                            Teuchos::RCP<const Epetra_BlockMap>(),
                            tempThreeDimensionalMap,
                            tempBondMap);

    // The temporary data manager will have the same field specs and data as the real data manager.
    vector<int> fieldIds = dataManager.getFieldIds();
    tempDataManager.allocateData(fieldIds);
    tempDataManager.copyLocallyOwnedDataFromDataManager(dataManager);

    // Set up numOwnedPoints and ownedIDs.
    // There is only one owned ID, and it has local ID zero in the tempDataManager.
    int tempNumOwnedPoints = 1;
    vector<int> tempOwnedIDs(tempNumOwnedPoints);
    tempOwnedIDs[0] = 0;

    // Use the scratchMatrix as sub-matrix for storing tangent values prior to loading them into the global tangent matrix.
    // Resize scratchMatrix if necessary
    if(scratchMatrix.Dimension() < numDof)
      scratchMatrix.Resize(numDof);

    // Create a list of global indices for the rows/columns in the scratch matrix.
    vector<int> globalIndices(numDof);
    for(int i=0 ; i<numEntries ; ++i){
      int globalID = tempOneDimensionalMap->GID(i);
      for(int j=0 ; j<3 ; ++j)
        globalIndices[3*i+j] = 3*globalID+j;
    }

    // Extract pointers to the underlying data in the constitutive data array.

    double *modelCoordinates, *coordinates, *volume, *coordinatesNP1, *angles, *detachedNodes;
    tempDataManager.getData(m_modelAnglesId,           PeridigmField::STEP_NONE)->ExtractView(&angles);
    tempDataManager.getData(m_volumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&volume);
    tempDataManager.getData(m_modelCoordinatesFieldId, PeridigmField::STEP_NONE)->ExtractView(&modelCoordinates);
    tempDataManager.getData(m_coordinatesFieldId, PeridigmField::STEP_N)->ExtractView(&coordinates);
    tempDataManager.getData(m_coordinatesFieldId, PeridigmField::STEP_NP1)->ExtractView(&coordinatesNP1);
    dataManager.getData(m_detachedNodesFieldId, PeridigmField::STEP_NP1)->ExtractView(&detachedNodes);
    double *horizon, *bondDamage;
    tempDataManager.getData(m_horizonFieldId, PeridigmField::STEP_NONE)->ExtractView(&horizon);
    tempDataManager.getData(m_bondDamageFieldId, PeridigmField::STEP_NP1)->ExtractView(&bondDamage);

    // Create arrays of Fad objects for the current coordinates, dilatation, and force density
    // Modify the existing vector of Fad objects for the current coordinates
    if((int)coordinatesNP1_AD.size() < numDof) {
      coordinates_AD.resize(numDof);
      coordinatesNP1_AD.resize(numDof);
    }
    for(int i=0 ; i<numDof ; ++i){
      coordinates_AD[i].diff(i, numDof);
      coordinates_AD[i].val() = coordinates[i];
      coordinatesNP1_AD[i].diff(i, numDof);
      coordinatesNP1_AD[i].val() = coordinatesNP1[i];
    }

    vector<Sacado::Fad::DFad<double> > shapeTensorInverse_AD;
    vector<Sacado::Fad::DFad<double> > deformationGradient_AD;
    vector<Sacado::Fad::DFad<double> > cauchyStress_AD;
    vector<Sacado::Fad::DFad<double> > cauchyStressNP1_AD;
    vector<Sacado::Fad::DFad<double> > partialStress_AD;
    vector<Sacado::Fad::DFad<double> > tempStressX_AD;
    vector<Sacado::Fad::DFad<double> > tempStressY_AD;
    vector<Sacado::Fad::DFad<double> > tempStressZ_AD;
    vector<Sacado::Fad::DFad<double> > hourglassStiff_AD;

    partialStress_AD.resize(numDof*numDof);
    shapeTensorInverse_AD.resize(numDof*numDof);
    deformationGradient_AD.resize(numDof*numDof);
    cauchyStress_AD.resize(numDof*numDof);
    cauchyStressNP1_AD.resize(numDof*numDof);
    tempStressX_AD.resize(numDof*numDof);
    tempStressY_AD.resize(numDof*numDof);
    tempStressZ_AD.resize(numDof*numDof);
    hourglassStiff_AD.resize(numDof*numDof);

    for(int i=0 ; i<6 ; ++i){
        for(int j=0 ; j<6 ; ++j){
            C_AD[i][j].val() = C[i][j];
    }}

    int shapeTensorReturnCode = 0;
      shapeTensorReturnCode =
                CORRESPONDENCE::computeShapeTensorInverseAndApproximateDeformationGradient(volume,
                                                                                horizon,
                                                                                modelCoordinates,
                                                                                &coordinates_AD[0],
                                                                                &coordinatesNP1_AD[0],
                                                                                &shapeTensorInverse_AD[0],
                                                                                &deformationGradient_AD[0],
                                                                                bondDamage,
                                                                                &tempNeighborhoodList[0],
                                                                                tempNumOwnedPoints,
                                                                                m_plane,
                                                                                detachedNodes);
    double *cauchyStress;
    double *cauchyStressNP1;

    tempDataManager.getData(m_cauchyStressFieldId, PeridigmField::STEP_N)->ExtractView(&cauchyStress);
    tempDataManager.getData(m_cauchyStressFieldId, PeridigmField::STEP_N)->ExtractView(&cauchyStressNP1);
    CORRESPONDENCE::updateElasticCauchyStressSmallDef(&deformationGradient_AD[0],
                                          &cauchyStress_AD[0],
                                          &cauchyStressNP1_AD[0],
                                          tempNumOwnedPoints,
                                          &C_AD[0],
                                          angles,
                                          m_type,
                                          dt,
                                          m_incremental);

    // define the sacadoFAD force vector
    vector<Sacado::Fad::DFad<double> > force_AD(numDof);
    //std::cout<<m_stabilizationType<<std::endl;
    CORRESPONDENCE::computeForcesAndStresses(
                                        tempNumOwnedPoints,
                                        &tempNeighborhoodList[0],
                                        volume,
                                        horizon,
                                        modelCoordinates,
                                        &coordinatesNP1_AD[0],
                                        &deformationGradient_AD[0],
                                        &cauchyStressNP1_AD[0],
                                        &shapeTensorInverse_AD[0],
                                        bondDamage,
                                        &C_AD[0],
                                        angles,
                                        &force_AD[0],
                                        &partialStress_AD[0],
                                        &tempStressX_AD[0],
                                        &tempStressY_AD[0],
                                        &tempStressZ_AD[0],
                                        &hourglassStiff_AD[0],
                                        m_hourglassCoefficient,
                                        m_stabilizationType,
                                        m_plane,
                                        m_tension,
                                        detachedNodes);

    // Load derivative values into scratch matrix
    // Multiply by volume along the way to convert force density to force
    double value;
    for(int row=0 ; row<numDof ; ++row){
      for(int col=0 ; col<numDof ; ++col){
	value = force_AD[row].dx(col) ; //--> I think this must be it, because forces are already provided
    //value = force_AD[row].dx(col) * volume[row/3]; // given by peridigm org
	TEUCHOS_TEST_FOR_EXCEPT_MSG(!boost::math::isfinite(value), "**** NaN detected in correspondence::computeAutomaticDifferentiationJacobian().\n");
        scratchMatrix(row, col) = value;
      }
    }

    // Sum the values into the global tangent matrix (this is expensive).
    if (jacobianType == PeridigmNS::Material::FULL_MATRIX)
      jacobian.addValues((int)globalIndices.size(), &globalIndices[0], scratchMatrix.Data());
    else if (jacobianType == PeridigmNS::Material::BLOCK_DIAGONAL) {
      jacobian.addBlockDiagonalValues((int)globalIndices.size(), &globalIndices[0], scratchMatrix.Data());
    }
    else // unknown jacobian type
      TEUCHOS_TEST_FOR_EXCEPT_MSG(true, "**** Unknown Jacobian Type\n");
  }
}

void
PeridigmNS::CorrespondenceMaterial::computeJacobianFiniteDifference(const double dt,
                                                           const int numOwnedPoints,
                                                           const int* ownedIDs,
                                                           const int* neighborhoodList,
                                                           PeridigmNS::DataManager& dataManager,
                                                           PeridigmNS::SerialMatrix& jacobian,
                                                           FiniteDifferenceScheme finiteDifferenceScheme,
                                                           PeridigmNS::Material::JacobianType jacobianType) const
{

  // The mechanics Jacobian is of the form:
  //
  // dF_0x/dx_0  dF_0x/dy_0  dF_0x/dz_0  dF_0x/dx_1  dF_0x/dy_1  dF_0x/dz_1  ...  dF_0x/dx_n  dF_0x/dy_n  dF_0x/dz_n
  // dF_0y/dx_0  dF_0y/dy_0  dF_0y/dz_0  dF_0y/dx_1  dF_0y/dy_1  dF_0y/dz_1  ...  dF_0y/dx_n  dF_0y/dy_n  dF_0y/dz_n
  // dF_0z/dx_0  dF_0z/dy_0  dF_0z/dz_0  dF_0z/dx_1  dF_0z/dy_1  dF_0z/dz_1  ...  dF_0z/dx_n  dF_0z/dy_n  dF_0z/dz_n
  // dF_1x/dx_0  dF_1x/dy_0  dF_1x/dz_0  dF_1x/dx_1  dF_1x/dy_1  dF_1x/dz_1  ...  dF_1x/dx_n  dF_1x/dy_n  dF_1x/dz_n
  // dF_1y/dx_0  dF_1y/dy_0  dF_1y/dz_0  dF_1y/dx_1  dF_1y/dy_1  dF_1y/dz_1  ...  dF_1y/dx_n  dF_1y/dy_n  dF_1y/dz_n
  // dF_1z/dx_0  dF_1z/dy_0  dF_1z/dz_0  dF_1z/dx_1  dF_1z/dy_1  dF_1z/dz_1  ...  dF_1z/dx_n  dF_1z/dy_n  dF_1z/dz_n
  //     .           .           .           .           .           .                .           .           .
  //     .           .           .           .           .           .                .           .           .
  //     .           .           .           .           .           .                .           .           .
  // dF_nx/dx_0  dF_nx/dy_0  dF_nx/dz_0  dF_nx/dx_1  dF_nx/dy_1  dF_nx/dz_1  ...  dF_nx/dx_n  dF_nx/dy_n  dF_nx/dz_n
  // dF_ny/dx_0  dF_ny/dy_0  dF_ny/dz_0  dF_ny/dx_1  dF_ny/dy_1  dF_ny/dz_1  ...  dF_ny/dx_n  dF_ny/dy_n  dF_ny/dz_n
  // dF_nz/dx_0  dF_nz/dy_0  dF_nz/dz_0  dF_nz/dx_1  dF_nz/dy_1  dF_nz/dz_1  ...  dF_nz/dx_n  dF_nz/dy_n  dF_nz/dz_n

  // Each entry is computed by finite difference:
  //
  // Forward difference:
  // dF_0x/dx_0 = ( F_0x(perturbed x_0) - F_0x(unperturbed) ) / epsilon
  //
  // Central difference:
  // dF_0x/dx_0 = ( F_0x(positive perturbed x_0) - F_0x(negative perturbed x_0) ) / ( 2.0*epsilon )

  TEUCHOS_TEST_FOR_EXCEPT_MSG(m_finiteDifferenceProbeLength == DBL_MAX, "**** Finite-difference Jacobian requires that the \"Finite Difference Probe Length\" parameter be set.\n");
  double epsilon = m_finiteDifferenceProbeLength;

  PeridigmNS::DegreesOfFreedomManager& dofManager = PeridigmNS::DegreesOfFreedomManager::self();
  bool solveForDisplacement = dofManager.displacementTreatedAsUnknown();
  //bool solveForTemperature = dofManager.temperatureTreatedAsUnknown();
  int numDof = dofManager.totalNumberOfDegreesOfFreedom();
  int numDisplacementDof = dofManager.numberOfDisplacementDegreesOfFreedom();
  //int numTemperatureDof = dofManager.numberOfTemperatureDegreesOfFreedom();
  int displacementDofOffset = dofManager.displacementDofOffset();
  //int temperatureDofOffset = dofManager.temperatureDofOffset();

  // Get field ids for all relevant data
  PeridigmNS::FieldManager& fieldManager = PeridigmNS::FieldManager::self();
  int volumeFId(-1), coordinatesFId(-1), velocityFId(-1), forceDensityFId(-1); //, temperatureFId(-1), fluxDivergenceFId(-1);
  volumeFId = fieldManager.getFieldId("Volume");
  if (solveForDisplacement) {
    coordinatesFId = fieldManager.getFieldId("Coordinates");
    velocityFId = fieldManager.getFieldId("Velocity");
    forceDensityFId = fieldManager.getFieldId("Force_Density");
  }
  //if (solveForTemperature) {
  //  temperatureFId = fieldManager.getFieldId("Temperature");
  //  fluxDivergenceFId = fieldManager.getFieldId("Flux_Divergence");
  //}

  int neighborhoodListIndex = 0;
  for(int iID=0 ; iID<numOwnedPoints ; ++iID){

    // Create a temporary neighborhood consisting of a single point and its neighbors.
    int numNeighbors = neighborhoodList[neighborhoodListIndex++];
    vector<int> tempMyGlobalIDs(numNeighbors+1);
    // Put the node at the center of the neighborhood at the beginning of the list.
    tempMyGlobalIDs[0] = dataManager.getOwnedScalarPointMap()->GID(iID);
    vector<int> tempNeighborhoodList(numNeighbors+1);
    tempNeighborhoodList[0] = numNeighbors;
    for(int iNID=0 ; iNID<numNeighbors ; ++iNID){
      int neighborID = neighborhoodList[neighborhoodListIndex++];
      tempMyGlobalIDs[iNID+1] = dataManager.getOverlapScalarPointMap()->GID(neighborID);
      tempNeighborhoodList[iNID+1] = iNID+1;
    }

    Epetra_SerialComm serialComm;
    Teuchos::RCP<Epetra_BlockMap> tempOneDimensionalMap = Teuchos::rcp(new Epetra_BlockMap(numNeighbors+1, numNeighbors+1, &tempMyGlobalIDs[0], 1, 0, serialComm));
    Teuchos::RCP<Epetra_BlockMap> tempThreeDimensionalMap = Teuchos::rcp(new Epetra_BlockMap(numNeighbors+1, numNeighbors+1, &tempMyGlobalIDs[0], 3, 0, serialComm));
    Teuchos::RCP<Epetra_BlockMap> tempBondMap = Teuchos::rcp(new Epetra_BlockMap(1, 1, &tempMyGlobalIDs[0], numNeighbors, 0, serialComm));

    // Create a temporary DataManager containing data for this point and its neighborhood.
    PeridigmNS::DataManager tempDataManager;
    tempDataManager.setMaps(Teuchos::RCP<const Epetra_BlockMap>(),
                            tempOneDimensionalMap,
                            Teuchos::RCP<const Epetra_BlockMap>(),
                            tempThreeDimensionalMap,
                            tempBondMap);

    // The temporary data manager will have the same fields and data as the real data manager.
    vector<int> fieldIds = dataManager.getFieldIds();
    tempDataManager.allocateData(fieldIds);
    tempDataManager.copyLocallyOwnedDataFromDataManager(dataManager);

    // Set up numOwnedPoints and ownedIDs.
    // There is only one owned ID, and it has local ID zero in the tempDataManager.
    int tempNumOwnedPoints = 1;
    vector<int> tempOwnedIDs(1);
    tempOwnedIDs[0] = 0;

    // Extract pointers to the underlying data.
    double *volume, *y, *v, *force;//, *temperature, *fluxDivergence;
    tempDataManager.getData(volumeFId, PeridigmField::STEP_NONE)->ExtractView(&volume);
    if (solveForDisplacement) {
      tempDataManager.getData(coordinatesFId, PeridigmField::STEP_NP1)->ExtractView(&y);
      tempDataManager.getData(velocityFId, PeridigmField::STEP_NP1)->ExtractView(&v);
      tempDataManager.getData(forceDensityFId, PeridigmField::STEP_NP1)->ExtractView(&force);
    }
    //if (solveForTemperature) {
    //  tempDataManager.getData(temperatureFId, PeridigmField::STEP_NP1)->ExtractView(&temperature);
    //  tempDataManager.getData(fluxDivergenceFId, PeridigmField::STEP_NP1)->ExtractView(&fluxDivergence);
    //}

    // Create a temporary vector for storing force and/or flux divergence.
    Teuchos::RCP<Epetra_Vector> forceVector, tempForceVector, fluxDivergenceVector, tempFluxDivergenceVector;
    double *tempForce, *tempFluxDivergence;
    if (solveForDisplacement) {
      forceVector = tempDataManager.getData(forceDensityFId, PeridigmField::STEP_NP1);
      tempForceVector = Teuchos::rcp(new Epetra_Vector(*forceVector));
      tempForceVector->ExtractView(&tempForce);
    }
    //if (solveForTemperature) {
    //  fluxDivergenceVector = tempDataManager.getData(fluxDivergenceFId, PeridigmField::STEP_NP1);
    //  tempFluxDivergenceVector = Teuchos::rcp(new Epetra_Vector(*fluxDivergenceVector));
    //  tempFluxDivergenceVector->ExtractView(&tempFluxDivergence);
    //}

    // Use the scratchMatrix as sub-matrix for storing tangent values prior to loading them into the global tangent matrix.
    // Resize scratchMatrix if necessary
    if(scratchMatrix.Dimension() < numDof*(numNeighbors+1))
      scratchMatrix.Resize(numDof*(numNeighbors+1));

    // Create a list of global indices for the rows/columns in the scratch matrix.
    vector<int> globalIndices(numDof*(numNeighbors+1));
    for(int i=0 ; i<numNeighbors+1 ; ++i){
      int globalID = tempOneDimensionalMap->GID(i);
      for(int j=0 ; j<numDof ; ++j){
        globalIndices[numDof*i+j] = numDof*globalID+j;
      }
    }

    if(finiteDifferenceScheme == FORWARD_DIFFERENCE){
      if (solveForDisplacement) {
        // Compute and store the unperturbed force.
        computeForce(dt, tempNumOwnedPoints, &tempOwnedIDs[0], &tempNeighborhoodList[0], tempDataManager);

        for(int i=0 ; i<forceVector->MyLength() ; ++i)
          tempForce[i] = force[i];
      }
      //if (solveForTemperature) {
      //  // Compute and store the unperturbed flux divergence.
      //  computeFluxDivergence(dt, tempNumOwnedPoints, &tempOwnedIDs[0], &tempNeighborhoodList[0], tempDataManager);
      //  for(int i=0 ; i<fluxDivergenceVector->MyLength() ; ++i)
      //    tempFluxDivergence[i] = fluxDivergence[i];
      //}
    }

    // Perturb one dof in the neighborhood at a time and compute the force and/or flux divergence.
    // The point itself plus each of its neighbors must be perturbed.
    for(int iNID=0 ; iNID<numNeighbors+1 ; ++iNID){

      int perturbID;
      if(iNID < numNeighbors)
        perturbID = tempNeighborhoodList[iNID+1];
      else
        perturbID = 0;

      // Displacement degrees of freedom
      for(int dof=0 ; dof<numDisplacementDof ; ++dof){

        // Perturb a dof and compute the forces.
        double oldY = y[numDof*perturbID+dof];
        double oldV = v[numDof*perturbID+dof];

        if(finiteDifferenceScheme == CENTRAL_DIFFERENCE){
          // Compute and store the negatively perturbed force.
          y[numDof*perturbID+dof] -= epsilon;
          v[numDof*perturbID+dof] -= epsilon/dt;
          computeForce(dt, tempNumOwnedPoints, &tempOwnedIDs[0], &tempNeighborhoodList[0], tempDataManager);
          y[numDof*perturbID+dof] = oldY;
          v[numDof*perturbID+dof] = oldV;
          for(int i=0 ; i<forceVector->MyLength() ; ++i)
            tempForce[i] = force[i];
        }

        // Compute the purturbed force.
        y[numDof*perturbID+dof] += epsilon;
        v[numDof*perturbID+dof] += epsilon/dt;
        computeForce(dt, tempNumOwnedPoints, &tempOwnedIDs[0], &tempNeighborhoodList[0], tempDataManager);
        y[numDof*perturbID+dof] = oldY;
        v[numDof*perturbID+dof] = oldV;

        for(int i=0 ; i<numNeighbors+1 ; ++i){
          int forceID;
          if(i < numNeighbors)
            forceID = tempNeighborhoodList[i+1];
          else
            forceID = 0;

          for(int d=0 ; d<numDof ; ++d){
            double value = ( force[numDof*forceID+d] - tempForce[numDof*forceID+d] ) / epsilon;
            if(finiteDifferenceScheme == CENTRAL_DIFFERENCE)
              value *= 0.5;
            scratchMatrix(numDof*forceID + displacementDofOffset + d, numDof*perturbID + displacementDofOffset + dof) = value;
          }
        }
      }

      // Temperature degrees of freedom
    //if(solveForTemperature){
    //
    //  // Perturb a temperature value and compute the flux divergence.
    //  double oldTemperature = temperature[perturbID];
    //
    //  if(finiteDifferenceScheme == CENTRAL_DIFFERENCE){
    //    // Compute and store the negatively perturbed flux divergence.
    //    temperature[perturbID] -= epsilon;
    //    computeFluxDivergence(dt, tempNumOwnedPoints, &tempOwnedIDs[0], &tempNeighborhoodList[0], tempDataManager);
    //    temperature[perturbID] = oldTemperature;
    //    for(int i=0 ; i<fluxDivergenceVector->MyLength() ; ++i)
    //      tempFluxDivergence[i] = fluxDivergence[i];
    //  }
    //
    //// Compute the purturbed flux divergence.
    //  temperature[perturbID] += epsilon;
    //  computeFluxDivergence(dt, tempNumOwnedPoints, &tempOwnedIDs[0], &tempNeighborhoodList[0], tempDataManager);
    //  temperature[perturbID] = oldTemperature;
    //
    //  for(int i=0 ; i<numNeighbors+1 ; ++i){
    //    int fluxDivergenceID;
    //    if(i < numNeighbors)
    //      fluxDivergenceID = tempNeighborhoodList[i+1];
    //    else
    //      fluxDivergenceID = 0;
    //
    //    double value = ( fluxDivergence[fluxDivergenceID] - tempFluxDivergence[fluxDivergenceID] ) / epsilon;
    //    if(finiteDifferenceScheme == CENTRAL_DIFFERENCE)
    //      value *= 0.5;
    //    scratchMatrix(numDof*fluxDivergenceID + temperatureDofOffset, numDof*perturbID + temperatureDofOffset) = value;
    //}
    //}
    }

    // Multiply by nodal volume
    for(unsigned int row=0 ; row<globalIndices.size() ; ++row){
      for(unsigned int col=0 ; col<globalIndices.size() ; ++col){
        scratchMatrix(row, col) *= volume[row/numDof];
      }
    }

    // Check for NaNs
    for(unsigned int row=0 ; row<globalIndices.size() ; ++row){
      for(unsigned int col=0 ; col<globalIndices.size() ; ++col){
        TEUCHOS_TEST_FOR_EXCEPT_MSG(!std::isfinite(scratchMatrix(row, col)), "**** NaN detected in finite-difference Jacobian.\n");
      }
    }

    // Sum the values into the global tangent matrix (this is expensive).
    if (jacobianType == PeridigmNS::Material::FULL_MATRIX)
      jacobian.addValues((int)globalIndices.size(), &globalIndices[0], scratchMatrix.Data());
    else if (jacobianType == PeridigmNS::Material::BLOCK_DIAGONAL) {
      jacobian.addBlockDiagonalValues((int)globalIndices.size(), &globalIndices[0], scratchMatrix.Data());
    }
    else // unknown jacobian type
      TEUCHOS_TEST_FOR_EXCEPT_MSG(true, "**** Unknown Jacobian Type\n");
  }
}


#include <iostream>
#include <boost/python.hpp>
#include <map>
#include <string>
#include <exception>
#include <stdexcept>
#include "pyutil.hpp"

using namespace std;
using namespace boost::python;

struct Vec4d
{
  double values[4];

  Vec4d()
  {
    for(int iter = 0; iter < 4; ++iter)
      values[iter] = 0;
  }

  double dist(const Vec4d&o) const
  {
    double ssd = 0;
    for(int iter = 0; iter < 4; ++iter)
    {
      double diff = (values[iter] - o.values[iter]);
      ssd += diff * diff;
    }
    return ssd;
  }
};

std::ostream& operator<<(ostream&os, const Vec4d&o)
{
  os << "[" << o.values[0] << ", " << o.values[1] << ", " << o.values[2] << ", " << o.values[3] << "] ";
  return os;
}

#include "eval_ik.cpp"

bool strhas(const string&str,const string&has)
{
  return str.find(has) != std::string::npos;
}

class IK
{
public:
  map<string,double> null_solution;
  map<string,double> regularizers;
  float step_size;

  IK()
  {
  }

  double objective_cost(map<string,Vec4d>&target_positions,map<string,double>&solution)
  {
    // compute target component
    double ssd = 0;
    for(auto && targ : target_positions)
    {
      Vec4d cur = joint_position(targ.first,solution);
      ssd += cur.dist(targ.second);
    }

    // compute the reguarizer cost.
    for(auto && param : solution)
    {
      auto null_value = null_solution.find(param.first);
      if(null_value != null_solution.end())
      {
	double C = regularizers[param.first];
	double diff = null_value->second - param.second;
	ssd += C * diff * diff;
      }
    }

    return ssd;
  }

  map<string,double> perturb_solution_random(const map<string,double>&solution)
  {
    map<string,double> sol_prime = solution;
    static std::default_random_engine generator;
    static std::normal_distribution<double> distribution(0,1.0);

    int param_id = rand() % sol_prime.size();
    int iter =0;
    for(auto && param : sol_prime)
    {
      if(iter == param_id)
	param.second += distribution(generator);
      ++iter;
    }

    return sol_prime;
  }

  map<string,double> perturb_solution_gradient(map<string,Vec4d>&targets,map<string,double>&solution)
  {
    map<string,double> sp;

    for(auto && param : solution)
    {
      // data gradient
      double grad = 0;
      for(auto && target : targets)
      {
	Vec4d cur_pos = joint_position(target.first,solution);
	for(int i = 0; i < 4; ++i)
	{
	  grad += 2 * (cur_pos.values[i] - target.second.values[i]) * diff(target.first,param.first,i,solution);
	}
      }

      // regularizer gradient
      auto null_value = null_solution.at(param.first);
      double C = regularizers.at(param.first);
      double diff = null_value - param.second;
      grad += C * -2 * diff;

      // update
      sp[param.first] = param.second - step_size*grad;
    }

    return sp;
  }

  map<string,double> perturb_solution(map<string,Vec4d>&target,map<string,double>&solution)
  {
    static long ctr = 0;
    ctr++;
    if(ctr % 500 == 0)
      return perturb_solution_gradient(target,solution);
    else
      return perturb_solution_random(solution);
  }

  void print_match(map<string,Vec4d>&target_positions,map<string,double>&solution)
  {
    for(auto && targ : target_positions)
    {
      Vec4d cur = joint_position(targ.first,solution);
      cout << targ.first << targ.second << " vs " << cur << endl;
    }
  }

  map<string,double> extract_solution(boost::python::dict pySol)
  {
    boost::python::list sol_keys = pySol.keys();
    map<string,double> sol;
    for(int iter = 0; iter < len(sol_keys); ++iter)
    {
      boost::python::extract<std::string> key(sol_keys[iter]);
      boost::python::extract<double> value(pySol[sol_keys[iter]]);
      sol[key] = value;
    }
    return sol;
  }

  map<string,Vec4d> extract_targets(boost::python::dict target_positions)
  {
    boost::python::list target_keys = target_positions.keys();
    map<string,Vec4d> target;
    for( int iter = 0; iter < len(target_keys); ++iter)
    {
      boost::python::extract<std::string> key(target_keys[iter]);
      boost::python::list value = boost::python::extract<boost::python::list>(target_positions[target_keys[iter]]);
      Vec4d v;
      assert(len(value) == 4);
      for(int jter = 0; jter < len(value); ++jter)
	v.values[jter] = boost::python::extract<double>(value[jter]);
      target[key] = v;
    }
    return target;
  }

  void set_null_solution(boost::python::dict null_solution)
  {
    // convert init solution
    this->null_solution = extract_solution(null_solution);
  }

  void set_regularizers(boost::python::dict py_reguarizers)
  {
    // convert init solution
    this->regularizers = extract_solution(py_reguarizers);
  }

  void optimize(map<string,Vec4d>&target,map<string,double>&sol)
  {
    ScopedGILRelease unlock;

    double start_cost = objective_cost(target,sol);
    double best_cost = start_cost;
    cout << "init cost = " << best_cost << endl;
    int max_iter = 1000;
    for(int iter = 0; iter < max_iter; ++iter)
    {
      auto sol_prime = perturb_solution(target,sol);
      double cost_prime = objective_cost(target,sol_prime);
      if(cost_prime < best_cost)
      {
	cout << "ACCEPT: " << best_cost << " => " << cost_prime << endl;
	sol = sol_prime;
	best_cost = cost_prime;
      }
      else if(iter % 100 == 0)
      {
	cout << "REJECT: " << best_cost << " => " << cost_prime << endl;
      }

      if(iter % 1000 == 0)
      {
	cout << "iter " << iter << " of " << 50000 << endl;
      }
    }

    print_match(target,sol);
    cout << "init cost = " << start_cost << endl;
    cout << "final cost = " << objective_cost(target,sol) << endl;
  }

  boost::python::dict solve(boost::python::dict target_positions,boost::python::dict init_solution)
  {
    // convert arguments
    map<string,Vec4d> target = extract_targets(target_positions);

    // convert init solution
    map<string,double> sol = extract_solution(init_solution);

    // OPTIMIZE THE DAMN THING!!!
    {
      optimize(target,sol);
    }

    // convert to output
    boost::python::dict dictionary;
    for(auto && param : sol)
    {
      dictionary[param.first] = param.second;
    }
    return dictionary;
  }

  void set_grad_step_size(float s)
  {
    step_size = s;
  }
};

BOOST_PYTHON_MODULE(iksolver)
{
  class_<IK>("iksolver", init<>())
    .def(init<>())
    .def("solve", &IK::solve)
    .def("set_null_solution",&IK::set_null_solution)
    .def("set_regularizers",&IK::set_regularizers)
    .def("set_grad_step_size",&IK::set_grad_step_size);
}


//==============================================================================
//
//   (c) Copyright, 2010 University Corporation for Atmospheric Research (UCAR).
//       All rights reserved.
//       Do not copy or distribute without authorization.
//
//       File: $RCSfile: ructest.cc,v $
//       Version: $Revision: 1.1 $  Dated: $Date: 2010-05-20 21:21:04 $
//
//==============================================================================

/**
 * @file ruc2wrfgrid.cc
 *
 * Convert RUC 130 conus lat-longs to WRF grid coordinates
 *
 * @date 3/25/10
 */

// Include files
#include <stdio.h>
#include <string>
#include <vector>
#include <projects.h>
#include <boost/format.hpp>
#include "Proj4Wrap.hh"

using boost::format;
using std::string;
using std::vector;

// Functions

int main(int argc, char **argv)
{
  // Uses latitude where lambert conformal projection is true and
  // median aligned with cartesian y-axis
  double ruclov = -95;
  double rucla1 = 45.2931;
  double ruclo1 = -97.588;
  double ruclatin1 = 25.0;
  double ruclatin2 = 25.0;
  double rucdx = 13545.087;
  double rucdy = 13545.087;
  int rucnx = 5;
  int rucny = 4;

  string rucParamString = str(format("+proj=lcc +R=6371200 +lon_0=%1% +lat_0=%2% +lat_1=%3% +lat_2=%4%") % ruclov % ruclatin1 % ruclatin1 % ruclatin2);

  p4w::Proj4Wrap rucLambertProj(rucParamString, p4w::Proj4Wrap::LON_LAT_TYPE, ruclo1, rucla1, rucdx, rucdy);
  printf("ruc false easting, false northing: %g %g\n", rucLambertProj.getFalseEasting(), rucLambertProj.getFalseNorthing());
  double xc;
  double yc;
  double lon;
  double lat;

  rucLambertProj.ll2xy(ruclo1, rucla1, &xc, &yc);
  printf("xc, yc: %g %g\n", xc, yc);

  double la2 = 45.6415;
  double lo2 = -96.9482;

  rucLambertProj.ll2xy(lo2, la2, &xc, &yc);
  printf("xc, yc for lo2, la2: %g %g\n", xc, yc);
  rucLambertProj.xy2ll(0, 0, &lon, &lat);
  printf("lon, lat for origin: %g %g\n", lon, lat);

  rucLambertProj.xy2ll(4, 3, &lon, &lat);
  printf("lon, lat for 4,3: %g %g\n", lon, lat);

  double la3 = 45.5245;
  double lo3 = -97.27;
  rucLambertProj.ll2xy(lo3, la3, &xc, &yc);
  printf("xc, yc for lo3, la3: %g %g\n", xc, yc);
}


/*
 * Copyright (c) 2010-2013 Steffen Kieß
 *
 * 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 MATH_VECTOR2_HPP_INCLUDED
#define MATH_VECTOR2_HPP_INCLUDED

#include <Core/Util.hpp>
#include <Core/Assert.hpp>

#include <Math/Forward.hpp>
#include <Math/Abs.hpp>

#include <complex>

#include <boost/type_traits/is_convertible.hpp>
#include <boost/type_traits/alignment_of.hpp>
#include <boost/utility/enable_if.hpp>

//#define VECTOR2_USE_ARRAY

namespace Math {
  template <typename T> class
  Vector2 {
#ifdef VECTOR2_USE_ARRAY
    T data_[2];
#else
    T x_, y_;
#endif

    class PrivateType {
      friend class Vector2;
      NVCC_HOST_DEVICE PrivateType () {}
    };

  public:
    NVCC_HOST_DEVICE Vector2 () {}
#if !defined (__CUDACC__)
#if defined (__clang__) || GCC_VERSION_IS_ATLEAST(4, 6)
#pragma GCC diagnostic push
#endif
#pragma GCC diagnostic ignored "-Wconversion"
#pragma GCC diagnostic ignored "-Wfloat-conversion"
#endif
#ifdef VECTOR2_USE_ARRAY
    //NVCC_HOST_DEVICE explicit Vector2 (T v) { data_[0] = v; data_[1] = v }
    NVCC_HOST_DEVICE Vector2 (T x, T y) { data_[0] = x; data_[1] = y; }
    template <typename U> NVCC_HOST_DEVICE Vector2 (Vector2<U> v, UNUSED typename boost::enable_if<boost::is_convertible<U, T>, PrivateType>::type dummy = PrivateType ()) { data_[0] = v.x (); data_[1] = v.y (); }
    template <typename U> NVCC_HOST_DEVICE explicit Vector2 (Vector2<U> v, UNUSED typename boost::disable_if<boost::is_convertible<U, T>, PrivateType>::type dummy = PrivateType ()) { data_[0] = (T) v.x (); data_[1] = (T) v.y (); }
#else
    //NVCC_HOST_DEVICE explicit Vector2 (T v) : x_ (v), y_ (v) {}
    NVCC_HOST_DEVICE Vector2 (T x, T y) : x_ (x), y_ (y) {}
    template <typename U> NVCC_HOST_DEVICE Vector2 (Vector2<U> v, UNUSED typename boost::enable_if<boost::is_convertible<U, T>, PrivateType>::type dummy = PrivateType ()) : x_ (v.x ()), y_ (v.y ()) {}
    template <typename U> NVCC_HOST_DEVICE explicit Vector2 (Vector2<U> v, UNUSED typename boost::disable_if<boost::is_convertible<U, T>, PrivateType>::type dummy = PrivateType ()) : x_ (v.x ()), y_ (v.y ()) {}
#endif
#if !defined (__CUDACC__)
#if defined (__clang__) || GCC_VERSION_IS_ATLEAST(4, 6)
#pragma GCC diagnostic pop
#endif
#endif

    NVCC_HOST_DEVICE const T& x () const {
#ifdef VECTOR2_USE_ARRAY
      return data_[0];
#else
      return x_;
#endif
    }

    NVCC_HOST_DEVICE const T& y () const {
#ifdef VECTOR2_USE_ARRAY
      return data_[1];
#else
      return y_;
#endif
    }

    NVCC_HOST_DEVICE T& x () {
#ifdef VECTOR2_USE_ARRAY
      return data_[0];
#else
      return x_;
#endif
    }

    NVCC_HOST_DEVICE T& y () {
#ifdef VECTOR2_USE_ARRAY
      return data_[1];
#else
      return y_;
#endif
    }

    NVCC_HOST_DEVICE const T& operator[] (size_t i) const {
#ifdef VECTOR2_USE_ARRAY
      ASSERT (i >= 0 && i < 2);
      return data_[i];
#else
      if (i == 0)
        return x_;
      else if (i == 1)
        return y_;
      else
        ABORT ();
#endif
    }

    NVCC_HOST_DEVICE T& operator[] (size_t i) {
#ifdef VECTOR2_USE_ARRAY
      ASSERT (i >= 0 && i < 2);
      return data_[i];
#else
      if (i == 0)
        return x_;
      else if (i == 1)
        return y_;
      else
        ABORT ();
#endif
    }
  };

  // Operations on Vector2

#define RTS(op) DECLTYPE ((*(T*)NULL) op (*(U*)NULL))
#define RT(op) Vector2<RTS(op)>
#ifdef __CUDACC__ // Workaround BugPlayground/nvcc-5.0-templates-1.cu
  template <typename T> static ERROR_ATTRIBUTE ("should not be called") Vector2<T> helperGetVector2Type (T t);
#define RT2(op) DECLTYPE (helperGetVector2Type ((*(T*)NULL) op (*(U*)NULL)))
#define RETURN_RT(op) typedef RT2(op) Ty; return Ty
#else
#define RETURN_RT(op) return RT(op)
#endif
  template <typename T, typename U> NVCC_HOST_DEVICE inline RT(+) operator+ (Vector2<T> v1, Vector2<U> v2) {
    RETURN_RT(+) (v1.x () + v2.x (), v1.y () + v2.y ());
  }

  template <typename T, typename U> NVCC_HOST_DEVICE inline Vector2<T>& operator+= (Vector2<T>& v1, Vector2<U> v2) {
    v1.x () += v2.x (); v1.y () += v2.y ();
    return v1;
  }

  template <typename T, typename U> NVCC_HOST_DEVICE inline RT(-) operator- (Vector2<T> v1, Vector2<U> v2) {
    RETURN_RT(-) (v1.x () - v2.x (), v1.y () - v2.y ());
  }

  template <typename T, typename U> NVCC_HOST_DEVICE inline Vector2<T>& operator-= (Vector2<T>& v1, Vector2<U> v2) {
    v1.x () -= v2.x (); v1.y () -= v2.y ();
    return v1;
  }

  template <typename T, typename U> NVCC_HOST_DEVICE inline RT(*) operator* (Vector2<T> v, U scalar) {
    RETURN_RT(*) (v.x () * scalar, v.y () * scalar);
  }

  template <typename T, typename U> NVCC_HOST_DEVICE inline RT(*) operator* (T scalar, Vector2<U> v) {
    RETURN_RT(*) (scalar * v.x (), scalar * v.y ());
  }

  template <typename T, typename U> NVCC_HOST_DEVICE inline Vector2<T>& operator*= (Vector2<T>& v, U scalar) {
    v.x () *= scalar; v.y () *= scalar;
    return v;
  }

  template <typename T, typename U> NVCC_HOST_DEVICE inline RT(/) operator/ (Vector2<T> v, U scalar) {
    RETURN_RT(/) (v.x () / scalar, v.y () / scalar);
  }

  template <typename T, typename U> NVCC_HOST_DEVICE inline Vector2<T>& operator/= (Vector2<T>& v, U scalar) {
    v.x () /= scalar; v.y () /= scalar;
    return v;
  }

  template <typename T> NVCC_HOST_DEVICE inline bool operator== (Vector2<T> v1, Vector2<T> v2) {
    return v1.x () == v2.x () && v1.y () == v2.y ();
  }
  template <typename T> NVCC_HOST_DEVICE inline bool operator!= (Vector2<T> v1, Vector2<T> v2) {
    return !(v1 == v2);
  }

  template <typename T> struct Abs2Impl<Vector2<T> > {
    static DECLTYPE(Math::abs2 (*(T*)0)) apply (Vector2<T> v) {
      return Math::abs2 (v.x ()) + Math::abs2 (v.y ());
    }
  };

  // dot product
  template <typename T, typename U> NVCC_HOST_DEVICE inline RTS(*) operator* (Vector2<T> v1, Vector2<U> v2) {
    return v1.x () * v2.x () + v1.y () * v2.y ();
  }

#undef RTS
#undef RT
#ifdef __CUDACC__ // Workaround BugPlayground/nvcc-5.0-templates-1.cu
#undef RT2
#undef RETURN_RT
#endif

  // Unary +/-
  template <typename T> NVCC_HOST_DEVICE inline Vector2<T> operator+ (Vector2<T> v) {
    return Vector2<T> (+v.x (), +v.y ());
  }
  template <typename T> NVCC_HOST_DEVICE inline Vector2<T> operator- (Vector2<T> v) {
    return Vector2<T> (-v.x (), -v.y ());
  }

  template <typename F> Vector2<F> inline real (Vector2<std::complex<F> > v) {
    return Vector2<F> (real (v.x ()), real (v.y ()));
  }

  template <typename F> Vector2<F> inline imag (Vector2<std::complex<F> > v) {
    return Vector2<F> (imag (v.x ()), imag (v.y ()));
  }

  template <typename F> Vector2<std::complex<F> > inline conj (Vector2<std::complex<F> > v) {
    return Vector2<std::complex<F> > (conj (v.x ()), conj (v.y ()));
  }
}

#endif // !MATH_VECTOR2_HPP_INCLUDED


#include "VelocityFilter.h"
#include <iostream>
#include <fstream>
#include <vector>
#include "Distance.h"
#include <Eigen/Core>
#include "CTCD.h"

using namespace std;
using namespace Eigen;

static void writeMesh(const char *filename, const VectorXd &verts, const Matrix3Xi &faces)
{
	ofstream ofs(filename);

	for(int i=0; i<verts.size()/3; i++)
	{
		ofs << "v ";
		for(int j=0; j<3; j++)
			ofs << verts[3*i+j] << " ";
		ofs << endl;
	}

	for(int i=0; i<faces.cols(); i++)
	{
		ofs << "f ";
		for(int j=0; j<3; j++)
			ofs << faces.coeff(j, i)+1 << " ";
		ofs << endl;
	}
}

void loadMesh(const char *filename, VectorXd &verts, Matrix3Xi &faces)
{
	ifstream ifs(filename);
	if(!ifs)
		return;

	vector<double> pos;
	vector<int> faceidx;

	while(true)
	{
		char c;
		ifs >> c;
		if(!ifs)
			break;
		if(c == 'v')
		{
			for(int i=0; i<3; i++)
			{
				double p;
				ifs >> p;
				pos.push_back(p);
			}
		}
		if(c == 'f')
		{
			for(int i=0; i<3; i++)
			{
				int vert;
				ifs >> vert;
				faceidx.push_back(vert);
			}
		}
	}

	int nverts = pos.size()/3;
	int nfaces = faceidx.size()/3;

	verts.resize(3*nverts);
	for(int i=0; i<3*nverts; i++)
		verts[i] = pos[i];

	faces.resize(3, nfaces);
	for(int i=0; i<nfaces; i++)
		for(int j=0; j<3; j++)
			faces.coeffRef(j, i) = faceidx[3*i+j]-1;
}

double displaceInward(const VectorXd &q, const Matrix3Xi &f, const Matrix3Xd &vertNormals, double maxDisplacement, VectorXd &displacedq)
{
	int nverts = q.size()/3;
	int nfaces = f.cols();
	displacedq = q;
	double displacement = maxDisplacement;

	for(int i=0; i<nverts; i++)
	{
		Vector3d pos = q.segment<3>(3*i);
		Vector3d normal = vertNormals.col(i);
		Vector3d endpos = pos - maxDisplacement*normal;

		// vertex-face

		for(int j=0; j<nfaces; j++)
		{
			bool skip = false;
			for(int k=0; k<3; k++)
				if(f.col(j)[k] == i)
					skip = true;

			if(skip)
				continue;

			double t;
			if(CTCD::vertexFaceCTCD(pos,
					q.segment<3>(3*f.col(j)[0]),
					q.segment<3>(3*f.col(j)[1]),
					q.segment<3>(3*f.col(j)[2]),
					endpos,
					q.segment<3>(3*f.col(j)[0]),
					q.segment<3>(3*f.col(j)[1]),
					q.segment<3>(3*f.col(j)[2]),
					1e-8, t))
			{
				displacement = min(displacement, maxDisplacement*t/2);
			}
		}
		// edge-edge

		for(int j=0; j<nfaces; j++)
		{
			for(int k=0; k<3; k++)
			{
				if(f.col(j)[k] == i)
				{
					int vert2 = f.col(j)[(k+1)%3];
					Vector3d pos2 = q.segment<3>(3*vert2);
					Vector3d endpos2 = pos2 - maxDisplacement*vertNormals.col(vert2);
					for(int j2=0; j2<nfaces; j2++)
					{
						for(int k2=0; k2<3; k2++)
						{
							if(f.col(j2)[k2] == i || f.col(j2)[(k2+1)%3] == i)
								continue;
							if(f.col(j2)[k2] == vert2 || f.col(j2)[(k2+1)%3] == vert2)
								continue;

							double t;
							if(CTCD::edgeEdgeCTCD(pos, pos2,
								q.segment<3>(3*f.col(j2)[k2]),
								q.segment<3>(3*f.col(j2)[(k2+1)%3]),
								endpos, endpos2,
								q.segment<3>(3*f.col(j2)[k2]),
								q.segment<3>(3*f.col(j2)[(k2+1)%3]),
								1e-8, t))
							{
								displacement = min(displacement, maxDisplacement*t/2);
							}
						}
					}
				}
			}
		}
	}

	for(int i=0; i<nverts; i++)
	{
		displacedq.segment<3>(3*i) -= displacement*vertNormals.col(i);
	}

	return displacement;
}

int main(int argc, char *argv[])
{
	if(argc != 4)
	{
		std::cerr << "Usage: displaceInward (inputMesh) (outputMesh) (maxDisplacement)" << std::endl;
		return -1;
	}
	VectorXd q;
	Matrix3Xi f;
	loadMesh(argv[1], q, f);
	int numverts = q.size()/3;
	std::cout << "Loaded mesh with " << numverts << " vertices and " << f.cols() << " faces" << std::endl;

	double maxdisp = strtod(argv[3], NULL);

	Matrix3Xd normals(3, numverts);
	normals.setZero();
	for(int face=0; face<f.cols(); face++)
	{
		Vector3i verts = f.col(face);
		Vector3d n = ( q.segment<3>(3*verts[1]) - q.segment<3>(3*verts[0]) ).cross( q.segment<3>(3*verts[2]) - q.segment<3>(3*verts[0]) );
		n /= n.norm();

		for(int i=0; i<3; i++)
		{
			double sinangle = ( q.segment<3>(3*verts[(i+1)%3]) - q.segment<3>(3*verts[i]) ).cross( q.segment<3>(3*verts[(i+2)%3]) - q.segment<3>(3*verts[i]) ).norm();
			double cosangle = ( q.segment<3>(3*verts[(i+1)%3]) - q.segment<3>(3*verts[i]) ).dot( q.segment<3>(3*verts[(i+2)%3]) - q.segment<3>(3*verts[i]) );
			double angle = atan2(sinangle, cosangle);
			normals.col(verts[i]) += angle*n;
		}
	}
	for(int i=0; i<numverts; i++)
		normals.col(i) /= normals.col(i).norm();

	VectorXd newq;
	double mindisp = displaceInward(q, f, normals, maxdisp, newq);
	std::cout << "Actual displacement was " << mindisp << std::endl;
	writeMesh(argv[2], newq, f);
}


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

#include <string>
#include <iomanip>
#include <boost/math/distributions/binomial.hpp>
#include <boost/function.hpp>
#include <Eigen/Core>
#include "genfile/Error.hpp"
#include "components/HaplotypeFrequencyComponent/HaplotypeFrequencyComponent.hpp"

// #define DEBUG_HAPLOTYPE_FREQUENCY_LL 1
HaplotypeFrequencyLogLikelihood::HaplotypeFrequencyLogLikelihood(
	Matrix const& genotype_table,
	Matrix const& haplotype_table
):
	m_genotype_table( genotype_table ),
	m_haplotype_table( haplotype_table ),
	m_D_ll( 3 ),
	m_DDt_ll( 3, 3 )
{
	if( m_genotype_table.array().maxCoeff() == 0 && m_haplotype_table.array().maxCoeff() == 0 ) {
		throw genfile::BadArgumentError( "HaplotypeFrequencyLogLikelihood::HaplotypeFrequencyLogLikelihood()", "genotype_table, haplotype_table" ) ;
	}
	// store derivatives of elements of the parameters pi with respect to pi.
	m_dpi.push_back( std::vector< RowVector >( 2, RowVector::Zero( 3 ) )) ;
	m_dpi.push_back( std::vector< RowVector >( 2, RowVector::Zero( 3 ) )) ;
	m_dpi[0][0] = RowVector::Constant( 3, -1 ) ;
	m_dpi[0][1]( 0 ) = 1 ;
	m_dpi[1][0]( 1 ) = 1 ;
	m_dpi[1][1]( 2 ) = 1 ;
}

HaplotypeFrequencyLogLikelihood::Vector HaplotypeFrequencyLogLikelihood::get_MLE_by_EM() const {
	//
	// This method treats the number of individuals X in the G(1,1) cell
	// that have AB/ab genotypes as binomially distributed with some unknown
	// parameter p that must be estimated.  We pick a value of p and sum over
	// possible values of X to get a new estimate of the four parameters
	// pi00, pi01, pi10, pi11.  Then p is re-computed from the parameter values.
	//
	// We stop when the parameter estimate does not change, up to some tolerance.
	//
	Matrix const& G = m_genotype_table ;
	Vector pi = estimate_parameters( 0.5 ) ;
#if DEBUG_HAPLOTYPE_FREQUENCY_LL
	std::cerr << std::resetiosflags( std::ios::floatfield ) << "Maximising likelihood for genotypes:\n"
		<< m_genotype_table
		<< "\nand haplotypes:\n"
		<< m_haplotype_table
		<< "\n" ;
	std::cerr << "pi = " << pi(0) << " " << pi(1) << " " << pi(2) << " " << pi(3) << " " << ", p = 0.5.\n" ;
#endif
	if( G(1,1) != 0.0 ) {
		Vector old_pi ;
		std::size_t count = 0 ;
		std::size_t const max_count = 100000 ;
		double const tolerance = 0.0000001 ;
		do {
			old_pi = pi ;

			double p = ( pi(0) * pi( 3 ) ) ;
			if( p != 0 ) {
				p = p / ( p + ( pi( 1 ) * pi( 2 )) ) ;
			}

			pi = estimate_parameters( p ) ;
#if DEBUG_HAPLOTYPE_FREQUENCY_LL
			std::cerr << "pi = " << pi(0) << " " << pi(1) << " " << pi(2) << " " << pi(3) << " " << ", p = " << p << ".\n" ;
#endif
		}
		while( ( pi - old_pi ).array().abs().maxCoeff() > tolerance && ++count < max_count ) ;
		if( count == max_count ) {
			throw genfile::OperationFailedError(
				"HaplotypeFrequencyLogLikelihood::maximise_by_EM()",
				"object of type HaplotypeFrequencyLogLikelihood",
				"convergence"
			) ;
		}
	}
	return pi.tail( 3 ) ;
}

HaplotypeFrequencyLogLikelihood::Vector HaplotypeFrequencyLogLikelihood::estimate_parameters(
	double const p
) const {
	Matrix const& G = m_genotype_table ;
	Matrix const& H = m_haplotype_table ;

	// p is proportion of het / het genotypes
	// that are due to 00+11 haplotype combinations.
	double expected_00_11 = G( 1, 1 ) * p ;
	double expected_01_10 = G( 1, 1 ) - expected_00_11;

	// params are pi00 pi01 pi10 pi11
	Vector result( 4 ) ;
	result <<
		H(0,0) + G( 0, 1 ) + 2 * G( 0, 0 ) + G( 1, 0 ) + expected_00_11,
		H(0,1) + G( 0, 1 ) + 2 * G( 0, 2 ) + G( 1, 2 ) + expected_01_10,
		H(1,0) + G( 2, 1 ) + 2 * G( 2, 0 ) + G( 1, 0 ) + expected_01_10,
		H(1,1) + G( 1, 2 ) + 2 * G( 2, 2 ) + G( 2, 1 ) + expected_00_11
	;
	result /= result.sum() ;
	return result ;
}

void HaplotypeFrequencyLogLikelihood::evaluate_at( Vector const& pi ) {
	double const pi00 = 1.0 - pi.sum() ;
	double const& pi01 = pi(0) ;
	double const& pi10 = pi(1) ;
	double const& pi11 = pi(2) ;

	using std::log ;
	double const lpi00 = log( pi00 ) ;
	double const lpi01 = log( pi01 ) ;
	double const lpi10 = log( pi10 ) ;
	double const lpi11 = log( pi11 ) ;

	double het_probability = pi00 * pi11 + pi01 * pi10 ;

	Matrix const& G = m_genotype_table ;
	Matrix const& H = m_haplotype_table ;
	Matrix VG( 3, 3 ) ;
	VG <<
		2.0 * lpi00,		lpi00 + lpi01,				2.0 * lpi01,
		lpi00 + lpi10,		log( het_probability ),		lpi01 + lpi11,
		2.0 * lpi10,		lpi10 + lpi11,				2.0 * lpi11
	;

	Matrix VH( 2, 2 ) ;
	VG <<
		lpi00,		lpi01,
		lpi10,		lpi11
	;

	// cells with count 0 contribute 0 to the loglikelihood/
	// Since log(0) = -inf we have to handle this specially.
	// We do this by replacing these parameter entries with 0.
	VG = VG.array() * ( G.array() > 0.0 ).cast< double >() ;
	VH = VH.array() * ( H.array() > 0.0 ).cast< double >() ;

	m_ll = (
		(G.array() * VG.array())
		+ (H.array() * VH.array())
	).sum() ;
}
double HaplotypeFrequencyLogLikelihood::get_value_of_function() const {
	return m_ll ;
}

HaplotypeFrequencyLogLikelihood::Vector HaplotypeFrequencyLogLikelihood::get_value_of_first_derivative() {
	assert(0) ;
	return m_D_ll ;
}

HaplotypeFrequencyLogLikelihood::Matrix HaplotypeFrequencyLogLikelihood::get_value_of_second_derivative() {
	assert(0) ;
	return m_DDt_ll ;
}


/* Copyright 2017 The sfcpp Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

#pragma once

#include <Eigen/Dense>

#include <geo/ConvexPolytope.hpp>
#include <math/AffineSubspace.hpp>
#include <math/NatSet.hpp>

#include <forward_list>
#include <iostream>
#include <queue>
#include <stdexcept>
#include <unordered_map>
#include <unordered_set>
#include <vector>

namespace sfcpp {
namespace geo {

template <typename... Args>
std::function<void(Args...)> getDummyFunction() {
  return std::function<void(Args...)>([](Args...) {});
}

enum class FaceStatus { REMOVE, EXTEND, CONNECT, KEEP };

struct FaceInfo;

typedef FaceInfo *FaceInfoPtr;

static const double QH_EPSILON =
    1e-9;  // TODO: make relative instead of absolute

struct FaceInfo {
  math::NatSet vertexSet;

  std::unordered_set<FaceInfoPtr> parents;
  std::unordered_set<FaceInfoPtr> children;
  std::unordered_set<FaceInfoPtr> facesToConnect;

  // number of facets this face is belonging to
  uint32_t numAncestorFacets = 0;

  // number of ancestor facets where the currently processed point is on the
  // other side of the facet's hyperplane compared to the current polytope
  uint32_t p = 0;

  // number of ancestor facets where the currently processed point is inside the
  // facet's hyperplane
  uint32_t z = 0;

  bool shouldRemove = false;

  // for generation of Polytope
  size_t polytopeIndex = 0;  // corresponding index in polytope.faces[d] vector

  bool visited = false;

  virtual ~FaceInfo() {}

  FaceStatus getStatus() const {
    uint32_t n = numAncestorFacets - p - z;
    if (n > 0) {
      if (p > 0) {
        return FaceStatus::CONNECT;
      } else {
        return FaceStatus::KEEP;
      }
    } else {
      if (p > 0) {
        return FaceStatus::REMOVE;
      } else {
        return FaceStatus::EXTEND;
      }
    }
  }

  static std::function<void(FaceInfoPtr)> dummyFunction;

  bool isVertex() const { return vertexSet.size() == 1; }
};

struct FacetInfo : public FaceInfo {
  Eigen::VectorXd outerNormalVector;

  // dot product of any point of the face with outerNormalVector
  double offset = 0.0;

  std::vector<uint32_t> outsideSet;

  virtual ~FacetInfo() {}

  double signedDistance(Eigen::VectorXd v) const {
    return v.dot(outerNormalVector) - offset;
  }

  int distanceSign(Eigen::VectorXd v) {
    double signedDist = signedDistance(v);
    return signedDist > QH_EPSILON ? 1 : (signedDist < -QH_EPSILON ? -1 : 0);
  }
};

template <typename Set, typename T>
void removeFromSet(Set &set, T t) {
  auto it = set.find(t);
  if (it != set.end()) {
    set.erase(it);
  }
}

typedef FacetInfo *FacetInfoPtr;

/**
 * A custom implementation of the QuickHull algorithm that computes all faces of
 * the given polytope and also works if the facets are non-simplicial (which is
 * important for cube-based SFC, for example). This algorithm works in arbitrary
 * dimensions >= 2.
 */
class QuickHullAlgorithm {
  std::vector<Eigen::VectorXd> vertices;

  // an inner point of the polytope used compute the direction of facet normal
  // vectors
  Eigen::VectorXd innerPoint;
  size_t dim;

  std::unordered_set<FaceInfoPtr> vertexFaces;
  std::unordered_set<FacetInfoPtr> facets;
  std::vector<FacetInfoPtr> aboveFacets, insideFacets, newFacets;

  std::queue<FacetInfoPtr> unprocessedFacets;
  std::unordered_set<FaceInfoPtr> connectFaces;
  std::unordered_set<FaceInfoPtr> newConnectFaces;

  std::shared_ptr<ConvexPolytope> polytope;

  void visitChildrenWithFlag(
      FaceInfoPtr face, bool visitedValue,
      std::function<void(FaceInfoPtr)> const &func = FaceInfo::dummyFunction);
  void visitChildren(FaceInfoPtr face,
                     std::function<void(FaceInfoPtr)> const &func);

  void visitParentsWithFlag(
      FaceInfoPtr face, bool visitedValue,
      std::function<void(FaceInfoPtr)> const &func = FaceInfo::dummyFunction);

  template <typename Collection>
  void visitChildrenForEach(Collection const &collection,
                            std::function<void(FaceInfoPtr)> const &func) {
    for (auto elem : collection) {
      visitChildrenWithFlag(elem, true, func);
      visitChildrenWithFlag(elem, false);
    }
  }

  template <typename Collection>
  void visitChildrenOnce(Collection const &collection,
                         std::function<void(FaceInfoPtr)> const &func) {
    for (auto elem : collection) {
      visitChildrenWithFlag(elem, true, func);
    }
    for (auto elem : collection) {
      visitChildrenWithFlag(elem, false);
    }
  }

  void initializeSimplex();

  void computeNeighbors(uint32_t pointIndex, FacetInfoPtr facet);

  void prepareConnect(FaceInfoPtr face,
                      std::unordered_set<FaceInfoPtr> const &otherFaces);

  void deleteFaceRecursively(FaceInfoPtr face);
  void deleteChildrenRecursively(FaceInfoPtr face);

  void addPoint(uint32_t pointIndex, FacetInfoPtr facet);

  void compute();

  void addToPolytopeRecursively(FaceInfoPtr face, size_t d = 0);

  template <typename T>
  void setFacetNormal(FacetInfoPtr facet, T const &vertexIndices) {
    if (vertexIndices.size() == 0) {
      throw std::runtime_error(
          "QuickHullAlgorithm::setFacetNormal(): vertexIndices.size() == 0");
    }
    auto it = vertexIndices.begin();
    auto firstPoint = this->vertices[*it];
    math::AffineSubspace affSpace(firstPoint);

    ++it;
    for (; it != vertexIndices.end(); ++it) {
      affSpace.addVector(this->vertices[*it]);
    }

    auto vec = affSpace.orthogonalVector().normalized();

    if (vec.dot(firstPoint - innerPoint) < 0) {
      vec *= -1;
    }

    facet->outerNormalVector = vec;
    facet->offset = vec.dot(firstPoint);
  }

 public:
  QuickHullAlgorithm(Eigen::MatrixXd points);

  ~QuickHullAlgorithm();

  std::shared_ptr<ConvexPolytope> createPolytope();
};

} /* namespace geo */
} /* namespace sfcpp */


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_COMMON_DIVROUND_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_COMMON_DIVROUND_HPP_INCLUDED

#include <boost/simd/arithmetic/functions/divround.hpp>
#include <boost/simd/include/functions/simd/iround.hpp>
#include <boost/simd/include/functions/simd/iround.hpp>
#include <boost/simd/include/functions/simd/tofloat.hpp>
#include <boost/simd/include/functions/simd/divides.hpp>
#include <boost/simd/include/functions/rdivide.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/functions/simd/group.hpp>
#include <boost/simd/include/functions/simd/split.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/constants/two.hpp>
#include <boost/simd/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/meta/upgrade.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divround_, tag::cpu_
                                   , (A0)(X)
                                   , ((simd_<arithmetic_<A0>,X>))((simd_<arithmetic_<A0>,X>))
                                   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    { return boost::simd::iround(boost::simd::tofloat(a0)/boost::simd::tofloat(a1)); }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divround_, tag::cpu_
                                   , (A0)(X)
                                   , ((simd_<unsigned_<A0>,X>))((simd_<unsigned_<A0>,X>))
                                   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    { return boost::simd::rdivide(a0+a1/boost::simd::Two<A0>(), a1); }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divround_, tag::cpu_
                                   , (A0)(X)
                                   , ((simd_<int16_<A0>,X>))((simd_<int16_<A0>,X>))
                                   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename meta::scalar_of<A0>::type           stype;
      typedef typename dispatch::meta::upgrade<stype>::type itype;
      typedef simd::native<itype,X>                 ivtype;
      ivtype a0l, a0h, a1l, a1h;
      boost::simd::split(a0, a0l, a0h);
      boost::simd::split(a1, a1l, a1h);
      return simd::bitwise_cast<A0>(boost::simd::group(boost::simd::divround(a0l, a1l),
                                               boost::simd::divround(a0h, a1h)));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divround_, tag::cpu_
                                   , (A0)(X)
                                   , ((simd_<int8_<A0>,X>))((simd_<int8_<A0>,X>))
                                   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename meta::scalar_of<A0>::type           stype;
      typedef typename dispatch::meta::upgrade<stype>::type itype;
      typedef simd::native<itype, X>                 ivtype;
      ivtype a0l, a0h, a1l, a1h;
      boost::simd::split(a0, a0l, a0h);
      boost::simd::split(a1, a1l, a1h);
      return simd::bitwise_cast<A0>(group(divround(a0l, a1l),
                                          divround(a0h, a1h) ));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divround_, tag::cpu_
                                   , (A0)(X)
                                   , ((simd_<floating_<A0>,X>))((simd_<floating_<A0>,X>))
                                   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    { return boost::simd::round(a0/a1); }
  };
} } }

#endif