/*! \file Peridigm_ElasticMaterial.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_ElasticMaterial.hpp"
#include "Peridigm_Field.hpp"
#include "elastic.h"
#ifdef PERIDIGM_KOKKOS
  #include "elastic_kokkos.h"
#endif
#include "material_utilities.h"
#include <Teuchos_Assert.hpp>
#include <Epetra_SerialComm.h>
#include <Sacado.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

using namespace std;

PeridigmNS::ElasticMaterial::ElasticMaterial(const Teuchos::ParameterList& params)
  : Material(params),
    m_bulkModulus(0.0), m_shearModulus(0.0), m_density(0.0), m_alpha(0.0), m_horizon(0.0),
    m_applyAutomaticDifferentiationJacobian(true),
    m_applyThermalStrains(false),
    m_computePartialStress(false),
    m_OMEGA(PeridigmNS::InfluenceFunction::self().getInfluenceFunction()),
    m_volumeFieldId(-1), m_damageFieldId(-1), m_weightedVolumeFieldId(-1), m_dilatationFieldId(-1), m_modelCoordinatesFieldId(-1),
    m_coordinatesFieldId(-1), m_forceDensityFieldId(-1), m_partialStressFieldId(-1), m_bondDamageFieldId(-1),
    m_deltaTemperatureFieldId(-1), m_planeStrain(false), m_planeStress(false), m_damageModelFieldId(-1)
{
  //! \todo Add meaningful asserts on material properties.
  m_bulkModulus = calculateBulkModulus(params);
  m_shearModulus = calculateShearModulus(params);
  m_density = params.get<double>("Density");
  m_horizon = params.get<double>("Horizon");
  if(params.isParameter("Apply Automatic Differentiation Jacobian"))
    m_applyAutomaticDifferentiationJacobian = params.get<bool>("Apply Automatic Differentiation Jacobian");
  if(params.isParameter("Plane Stress"))
    m_planeStress = params.get<bool>("Plane Stress");
  if(params.isParameter("Plane Strain"))
    m_planeStrain = params.get<bool>("Plane Strain");

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

  if(params.isParameter("Compute Partial Stress"))
    m_computePartialStress = params.get<bool>("Compute Partial Stress");

  PeridigmNS::FieldManager& fieldManager = PeridigmNS::FieldManager::self();
  m_volumeFieldId                  = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::SCALAR,      PeridigmField::CONSTANT, "Volume");
  m_damageFieldId                  = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::SCALAR,      PeridigmField::TWO_STEP, "Damage");
  m_weightedVolumeFieldId          = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::SCALAR,      PeridigmField::CONSTANT, "Weighted_Volume");
  m_dilatationFieldId              = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::SCALAR,      PeridigmField::TWO_STEP, "Dilatation");
  m_modelCoordinatesFieldId        = fieldManager.getFieldId(PeridigmField::NODE,    PeridigmField::VECTOR,      PeridigmField::CONSTANT, "Model_Coordinates");
  m_coordinatesFieldId             = fieldManager.getFieldId(PeridigmField::NODE,    PeridigmField::VECTOR,      PeridigmField::TWO_STEP, "Coordinates");
  m_forceDensityFieldId            = fieldManager.getFieldId(PeridigmField::NODE,    PeridigmField::VECTOR,      PeridigmField::TWO_STEP, "Force_Density");
  m_bondDamageFieldId              = fieldManager.getFieldId(PeridigmField::BOND,    PeridigmField::SCALAR,      PeridigmField::TWO_STEP, "Bond_Damage");
  int m_horizonFieldId             = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::SCALAR, 	 PeridigmField::CONSTANT, "Horizon");
  m_damageModelFieldId 			   = fieldManager.getFieldId(PeridigmField::NODE,    PeridigmField::VECTOR, 	 PeridigmField::TWO_STEP, "Damage_Model_Data");

  if(m_applyThermalStrains)
    m_deltaTemperatureFieldId      = fieldManager.getFieldId(PeridigmField::NODE,    PeridigmField::SCALAR,      PeridigmField::TWO_STEP, "Temperature_Change");
  if(m_computePartialStress)
    m_partialStressFieldId         = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::FULL_TENSOR, PeridigmField::TWO_STEP, "Partial_Stress");

  m_fieldIds.push_back(m_volumeFieldId);
  m_fieldIds.push_back(m_damageFieldId);
  m_fieldIds.push_back(m_weightedVolumeFieldId);
  m_fieldIds.push_back(m_dilatationFieldId);
  m_fieldIds.push_back(m_modelCoordinatesFieldId);
  m_fieldIds.push_back(m_coordinatesFieldId);
  m_fieldIds.push_back(m_forceDensityFieldId);
  m_fieldIds.push_back(m_bondDamageFieldId);
  m_fieldIds.push_back(m_horizonFieldId);
  m_fieldIds.push_back(m_damageModelFieldId);
  if(m_applyThermalStrains)
    m_fieldIds.push_back(m_deltaTemperatureFieldId);
  if(m_computePartialStress)
    m_fieldIds.push_back(m_partialStressFieldId);
}

PeridigmNS::ElasticMaterial::~ElasticMaterial()
{
}

void
PeridigmNS::ElasticMaterial::initialize(const double dt,
                                        const int numOwnedPoints,
                                        const int* ownedIDs,
                                        const int* neighborhoodList,
                                        PeridigmNS::DataManager& dataManager)
{
  // Extract pointers to the underlying data
  double *xOverlap,  *cellVolumeOverlap, *weightedVolume;
  dataManager.getData(m_modelCoordinatesFieldId, PeridigmField::STEP_NONE)->ExtractView(&xOverlap);
  dataManager.getData(m_volumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&cellVolumeOverlap);
  dataManager.getData(m_weightedVolumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&weightedVolume);
  // have to be initialized here, to have no probelm with damage - no daamge model mix
  dataManager.getData(m_damageModelFieldId, PeridigmField::STEP_NP1)->PutScalar(0.0);
  MATERIAL_EVALUATION::computeWeightedVolume(xOverlap,cellVolumeOverlap,weightedVolume,numOwnedPoints,neighborhoodList,m_horizon);
  dataManager.getData(m_dilatationFieldId, PeridigmField::STEP_NP1)->PutScalar(0.0);

}

void
PeridigmNS::ElasticMaterial::computeForce(const double dt,
                                          const int numOwnedPoints,
                                          const int* ownedIDs,
                                          const int* neighborhoodList,
                                          PeridigmNS::DataManager& dataManager) const
{
  // Zero out the forces
  dataManager.getData(m_forceDensityFieldId, PeridigmField::STEP_NP1)->PutScalar(0.0);
  if(m_computePartialStress)
    dataManager.getData(m_partialStressFieldId, PeridigmField::STEP_NP1)->PutScalar(0.0);

  // Extract pointers to the underlying data
  double *x, *y, *cellVolume, *weightedVolume, *dilatation, *bondDamage, *force, *deltaTemperature, *partialStress;

  dataManager.getData(m_modelCoordinatesFieldId, PeridigmField::STEP_NONE)->ExtractView(&x);
  dataManager.getData(m_coordinatesFieldId, PeridigmField::STEP_NP1)->ExtractView(&y);
  dataManager.getData(m_volumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&cellVolume);
  dataManager.getData(m_weightedVolumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&weightedVolume);
  dataManager.getData(m_dilatationFieldId, PeridigmField::STEP_NP1)->ExtractView(&dilatation);
  dataManager.getData(m_dilatationFieldId, PeridigmField::STEP_NP1)->PutScalar(0.0);
  dataManager.getData(m_bondDamageFieldId, PeridigmField::STEP_NP1)->ExtractView(&bondDamage);
  dataManager.getData(m_forceDensityFieldId, PeridigmField::STEP_NP1)->ExtractView(&force);

  deltaTemperature = NULL;
  if(m_applyThermalStrains)
    dataManager.getData(m_deltaTemperatureFieldId, PeridigmField::STEP_NP1)->ExtractView(&deltaTemperature);
  partialStress = NULL;
  if(m_computePartialStress)
    dataManager.getData(m_partialStressFieldId, PeridigmField::STEP_NP1)->ExtractView(&partialStress);

  MATERIAL_EVALUATION::computeDilatation(x,y,weightedVolume,cellVolume,bondDamage,dilatation,neighborhoodList,numOwnedPoints,m_horizon,m_OMEGA,m_alpha,deltaTemperature);

#ifdef PERIDIGM_KOKKOS
  MATERIAL_EVALUATION::computeInternalForceLinearElasticKokkos(x,y,weightedVolume,cellVolume,dilatation,bondDamage,scf,force,neighborhoodList,numOwnedPoints,m_bulkModulus,m_shearModulus,m_horizon,m_alpha,deltaTemperature);
#else
  MATERIAL_EVALUATION::computeInternalForceLinearElastic(x,y,weightedVolume,cellVolume,dilatation,bondDamage,force,partialStress,neighborhoodList,numOwnedPoints,m_bulkModulus,m_shearModulus,m_horizon,m_alpha,deltaTemperature, m_planeStrain, m_planeStress);
#endif
}

void
PeridigmNS::ElasticMaterial::computeStoredElasticEnergyDensity(const double dt,
                                                               const int numOwnedPoints,
                                                               const int* ownedIDs,
                                                               const int* neighborhoodList,
                                                               PeridigmNS::DataManager& dataManager) const
{
  // This function is intended to be called from a compute class.
  // The compute class should have already created the Stored_Elastic_Energy_Density field id.
  //int storedElasticEnergyDensityFieldId = PeridigmNS::FieldManager::self().getFieldId("Stored_Elastic_Energy_Density");

  double *x, *y, *cellVolume, *weightedVolume, *dilatation, *bondDamage;
  dataManager.getData(m_modelCoordinatesFieldId, PeridigmField::STEP_NONE)->ExtractView(&x);
  dataManager.getData(m_coordinatesFieldId, PeridigmField::STEP_NP1)->ExtractView(&y);
  dataManager.getData(m_volumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&cellVolume);
  dataManager.getData(m_weightedVolumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&weightedVolume);
  dataManager.getData(m_dilatationFieldId, PeridigmField::STEP_NP1)->ExtractView(&dilatation);
  dataManager.getData(m_bondDamageFieldId, PeridigmField::STEP_NP1)->ExtractView(&bondDamage);
  //double *storedElasticEnergyDensity;
  //dataManager.getData(storedElasticEnergyDensityFieldId, PeridigmField::STEP_NONE)->ExtractView(&storedElasticEnergyDensity);

  double *deltaTemperature = NULL;
  if(m_applyThermalStrains)
    dataManager.getData(m_deltaTemperatureFieldId, PeridigmField::STEP_NP1)->ExtractView(&deltaTemperature);

  double *damageModel;
  dataManager.getData(m_damageModelFieldId, PeridigmField::STEP_NP1)->ExtractView(&damageModel);
  int iID, iNID, numNeighbors, nodeId, neighborId;
  double omega, nodeInitialX[3], nodeCurrentX[3];
  double initialDistance, currentDistance, deviatoricExtension, neighborBondDamage;
  double nodeDilatation, alpha, temp;
//tbd plane strain/stress
  int neighborhoodListIndex(0), bondIndex(0);
  MATERIAL_EVALUATION::computeDilatation(x,y,weightedVolume,cellVolume,bondDamage,dilatation,neighborhoodList,numOwnedPoints,m_horizon,m_OMEGA,m_alpha,deltaTemperature);

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

    nodeId = ownedIDs[iID];
    nodeInitialX[0] = x[nodeId*3];
    nodeInitialX[1] = x[nodeId*3+1];
    nodeInitialX[2] = x[nodeId*3+2];
    nodeCurrentX[0] = y[nodeId*3];
    nodeCurrentX[1] = y[nodeId*3+1];
    nodeCurrentX[2] = y[nodeId*3+2];
    nodeDilatation = dilatation[nodeId];
    alpha = 15.0*m_shearModulus/weightedVolume[nodeId];

    temp = 0.0;

    numNeighbors = neighborhoodList[neighborhoodListIndex++];
    for(iNID=0 ; iNID<numNeighbors ; ++iNID){
      neighborId = neighborhoodList[neighborhoodListIndex++];
      neighborBondDamage = bondDamage[bondIndex++];
      initialDistance =
        distance(nodeInitialX[0], nodeInitialX[1], nodeInitialX[2],
                 x[neighborId*3], x[neighborId*3+1], x[neighborId*3+2]);
      currentDistance =
        distance(nodeCurrentX[0], nodeCurrentX[1], nodeCurrentX[2],
                 y[neighborId*3], y[neighborId*3+1], y[neighborId*3+2]);
      if(m_applyThermalStrains)
      currentDistance -= m_alpha*deltaTemperature[nodeId]*initialDistance;
      deviatoricExtension = (currentDistance - initialDistance) - nodeDilatation*initialDistance/3.0;
      omega=m_OMEGA(initialDistance,m_horizon);
      temp += (1.0-neighborBondDamage)*omega*deviatoricExtension*deviatoricExtension*cellVolume[neighborId];
    }
    //storedElasticEnergyDensity[nodeId] = 0.5*m_bulkModulus*nodeDilatation*nodeDilatation + 0.5*alpha*temp;
    damageModel[3*iID] = 0.5*m_bulkModulus*nodeDilatation*nodeDilatation + 0.5*alpha*temp;
  }
}

void
PeridigmNS::ElasticMaterial::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
    PeridigmNS::Material::computeJacobian(dt, numOwnedPoints, ownedIDs, neighborhoodList, dataManager, jacobian, jacobianType);
  }
}

void
PeridigmNS::ElasticMaterial::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> > y_AD;

  // 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 constitutiveData array.
    double *x, *y, *cellVolume, *weightedVolume, *damage, *bondDamage, *deltaTemperature;
    tempDataManager.getData(m_modelCoordinatesFieldId, PeridigmField::STEP_NONE)->ExtractView(&x);
    tempDataManager.getData(m_coordinatesFieldId, PeridigmField::STEP_NP1)->ExtractView(&y);
    tempDataManager.getData(m_volumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&cellVolume);
    tempDataManager.getData(m_weightedVolumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&weightedVolume);
    tempDataManager.getData(m_damageFieldId, PeridigmField::STEP_NP1)->ExtractView(&damage);
    tempDataManager.getData(m_bondDamageFieldId, PeridigmField::STEP_NP1)->ExtractView(&bondDamage);
    deltaTemperature = NULL;
    if(m_applyThermalStrains)
      tempDataManager.getData(m_deltaTemperatureFieldId, PeridigmField::STEP_NP1)->ExtractView(&deltaTemperature);
    // 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)y_AD.size() < numDof)
      y_AD.resize(numDof);
    for(int i=0 ; i<numDof ; ++i){
      y_AD[i].diff(i, numDof);
      y_AD[i].val() = y[i];
    }
    // Create vectors of empty AD types for the dependent variables
    vector<Sacado::Fad::DFad<double> > dilatation_AD(numEntries);
    vector<Sacado::Fad::DFad<double> > force_AD(numDof);

    vector<Sacado::Fad::DFad<double> > partialStress_AD;
    Sacado::Fad::DFad<double> *partialStress_AD_Ptr = NULL;
    if(m_computePartialStress){
      partialStress_AD.resize(numDof*numDof);
      partialStress_AD_Ptr = &partialStress_AD[0];
    }

    // Evaluate the constitutive model using the AD types
    MATERIAL_EVALUATION::computeDilatation(x,&y_AD[0],weightedVolume,cellVolume,bondDamage,&dilatation_AD[0],&tempNeighborhoodList[0],tempNumOwnedPoints,m_horizon,m_OMEGA,m_alpha,deltaTemperature);
    MATERIAL_EVALUATION::computeInternalForceLinearElastic(x,&y_AD[0],weightedVolume,cellVolume,&dilatation_AD[0],bondDamage,&force_AD[0],partialStress_AD_Ptr,&tempNeighborhoodList[0],tempNumOwnedPoints,m_bulkModulus,m_shearModulus,m_horizon,m_alpha,deltaTemperature);

    // 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) * cellVolume[row/3];
	TEUCHOS_TEST_FOR_EXCEPT_MSG(!boost::math::isfinite(value), "**** NaN detected in ElasticMaterial::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::ElasticMaterial::evalDilatation(const double dt,
                                                               const int numOwnedPoints,
                                                               const int* ownedIDs,
                                                               const int* neighborhoodList,
                                                               PeridigmNS::DataManager& dataManager) const
{
  double *x, *y, *cellVolume, *weightedVolume, *dilatation, *bondDamage, *damageModel, *deltaTemperature;
  dataManager.getData(m_damageModelFieldId, PeridigmField::STEP_NP1)->ExtractView(&damageModel);
  dataManager.getData(m_modelCoordinatesFieldId, PeridigmField::STEP_NONE)->ExtractView(&x);
  dataManager.getData(m_coordinatesFieldId, PeridigmField::STEP_NP1)->ExtractView(&y);
  dataManager.getData(m_volumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&cellVolume);
  dataManager.getData(m_weightedVolumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&weightedVolume);
  dataManager.getData(m_dilatationFieldId, PeridigmField::STEP_NP1)->ExtractView(&dilatation);
  dataManager.getData(m_dilatationFieldId, PeridigmField::STEP_NP1)->PutScalar(0.0);
  dataManager.getData(m_bondDamageFieldId, PeridigmField::STEP_NP1)->ExtractView(&bondDamage);
  const double *m = weightedVolume;
  int iID;
  deltaTemperature = NULL;
  if(m_applyThermalStrains)
    dataManager.getData(m_deltaTemperatureFieldId, PeridigmField::STEP_NP1)->ExtractView(&deltaTemperature);

  MATERIAL_EVALUATION::computeDilatation(x,y,weightedVolume,cellVolume,bondDamage,dilatation,neighborhoodList,numOwnedPoints,m_horizon,m_OMEGA,m_alpha,deltaTemperature);
  //std::cout<<m_shearModulus / (*m)<<" "<<m_bulkModulus  / (*m)<< " " << <<std::endl;
  for(iID=0 ; iID<numOwnedPoints ; ++iID, dilatation++, m++){

        damageModel[3*iID]   = *dilatation;
        damageModel[3*iID+1] = m_bulkModulus  / (*m);
        damageModel[3*iID+2] = m_shearModulus / (*m);

  }

}


#include <boost/random/shuffle_output.hpp>


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#ifndef DYNAMIC_SAMPLER_HH
#define DYNAMIC_SAMPLER_HH

#include "random.hh"
#include <functional>
#include <boost/mpl/if.hpp>

namespace graph_tool
{
using namespace std;
using namespace boost;

template <class Value>
class DynamicSampler
{
public:
    DynamicSampler() : _back(0), _n_items(0) {}

    DynamicSampler(const vector<Value>& items,
                   const vector<double>& probs)
        : _back(0), _n_items(0)
    {
        for (size_t i = 0; i < items.size(); ++i)
            insert(items[i], probs[i]);
    }

    typedef Value value_type;

    size_t get_left(size_t i)   const { return 2 * i + 1;               }
    size_t get_right(size_t i)  const { return 2 * i + 2;               }
    size_t get_parent(size_t i) const { return i > 0 ? (i - 1) / 2 : 0; }

    template <class RNG>
    const Value& sample(RNG& rng) const
    {
        uniform_real_distribution<> sample(0, _tree[0]);
        double u = sample(rng), c = 0;

        size_t pos = 0;
        while (_idx[pos] == numeric_limits<size_t>::max())
        {
            size_t l = get_left(pos);
            double a = _tree[l];
            if (u < a + c)
            {
                pos = l;
            }
            else
            {
                pos = get_right(pos);
                c += a;
            }
        }
        size_t i = _idx[pos];
        return _items[i];
    }

    size_t insert(const Value& v, double w)
    {
        size_t pos;
        if (_free.empty())
        {
            if (_back > 0)
            {
                // move parent to left leaf
                pos = get_parent(_back);
                size_t l = get_left(pos);
                _idx[l] = _idx[pos];
                _ipos[_idx[l]] = l;
                _tree[l] = _tree[pos];
                _idx[pos] = numeric_limits<size_t>::max();

                // position new item to the right
                _back = get_right(pos);
            }

            pos = _back;
            check_size(pos);

            _idx[pos] = _items.size();
            _items.push_back(v);
            _valid.push_back(true);
            _ipos.push_back(pos);
            _tree[pos] = w;
            _back++;
            check_size(_back);
        }
        else
        {
            pos = _free.back();
            auto i = _idx[pos];
            _items[i] = v;
            _valid[i] = true;
            _tree[pos] = w;
            _free.pop_back();
        }

        insert_leaf_prob(pos);
        _n_items++;
        return _idx[pos];
    }

    void remove(size_t i)
    {
        size_t pos = _ipos[i];
        remove_leaf_prob(pos);
        _free.push_back(pos);
        _items[i] = Value();
        _valid[i] = false;
        _n_items--;
    }

    void clear(bool shrink)
    {
        _items.clear();
        _ipos.clear();
        _tree.clear();
        _idx.clear();
        _free.clear();
        _valid.clear();
        if (shrink)
        {
            _items.shrink_to_fit();
            _ipos.shrink_to_fit();
            _tree.shrink_to_fit();
            _idx.shrink_to_fit();
            _free.shrink_to_fit();
            _valid.shrink_to_fit();
        }
        _back = 0;
        _n_items = 0;
    }

    void rebuild()
    {
        vector<Value> items;
        vector<double> probs;

        for (size_t i = 0; i < _tree.size(); ++i)
        {
            if (_idx[i] == numeric_limits<size_t>::max())
                continue;
            size_t j = _idx[i];
            if (!_valid[j])
                continue;
            items.push_back(_items[j]);
            probs.push_back(_tree[i]);
        }

        clear(true);

        for (size_t i = 0; i < items.size(); ++i)
            insert(items[i], probs[i]);
    }

    const Value& operator[](size_t i) const
    {
        return _items[i];
    }

    bool is_valid(size_t i) const
    {
        return ((i < _items.size()) && _valid[i]);
    }

    const auto& items() const
    {
        return _items;
    }

    auto begin() const
    {
        return _items.begin();
    }

    auto end() const
    {
        return _items.end();
    }

    size_t size() const
    {
        return _items.size();
    }

    bool empty() const
    {
        return _n_items == 0;
    }

private:

    void check_size(size_t i)
    {
        if (i >= _tree.size())
        {
            _idx.resize(i + 1, numeric_limits<size_t>::max());
            _tree.resize(i + 1, 0);
        }
    }

    void remove_leaf_prob(size_t i)
    {
        size_t parent = i;
        double w = _tree[i];
        while (parent > 0)
        {
            parent = get_parent(parent);
            _tree[parent] -= w;
        }
        _tree[i] = 0;
    }

    void insert_leaf_prob(size_t i)
    {
        size_t parent = i;
        double w = _tree[i];

        while (parent > 0)
        {
            parent = get_parent(parent);
            _tree[parent] += w;
        }
    }

    vector<Value>  _items;
    vector<size_t> _ipos;  // position of the item in the tree

    vector<double> _tree;  // tree nodes with weight sums
    vector<size_t> _idx;   // index in _items
    int _back;             // last item in tree

    vector<size_t> _free;  // empty leafs
    vector<bool> _valid;   // non-removed items
    size_t _n_items;
};

} // namespace graph_tool

#endif // DYNAMIC_SAMPLER_HH


#include "dynet/nodes.h"
#include "dynet/dynet.h"
#include "dynet/training.h"
#include "dynet/timing.h"
#include "dynet/rnn.h"
#include "dynet/gru.h"
#include "dynet/lstm.h"
#include "dynet/dict.h"
#include "dynet/expr.h"
#include "dynet/globals.h"
//#include "../utils/getpid.h"

#include <iostream>
#include <fstream>
#include <sstream>

#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>

using namespace std;
using namespace dynet;

float pdrop = 0.5;
unsigned LAYERS = 1;
//unsigned INPUT_DIM = 128;
unsigned INPUT_DIM = 40;
unsigned HIDDEN_DIM = 128;
unsigned MLP_HIDDEN_DIM = 32;

unsigned TAG_HIDDEN_DIM = 32;
unsigned TAG_DIM = 32;
unsigned TAG_SIZE = 0;

unsigned VOCAB_SIZE = 0;

bool eval = false;

int MAX_EPOCH = 10;

dynet::Dict d;
// use one dictionary
std::map<int, vector<float>> embeddings_all;

dynet::Dict td;
int kNONE;
int kSOS;
int kEOS;

template <class Builder>
struct DeepCRF3
{
  LookupParameter p_w;

  Builder l2rbuilder;
  Builder r2lbuilder;

  //deep information
  Parameter p_W_lstm_fwd;
  Parameter p_W_lstm_rev;
  Parameter p_bias;

  //crf part
  Parameter p_C;
  Parameter p_A_transition;

  //unsupervised part
  Parameter p_decoder;

  enum
  {
    Labelled = 0,
    Unlabelled = 1
  };

  explicit DeepCRF3(Model &model) : l2rbuilder(LAYERS, INPUT_DIM, HIDDEN_DIM, model),
                                    r2lbuilder(LAYERS, INPUT_DIM, HIDDEN_DIM, model)
  {
    p_w = model.add_lookup_parameters(VOCAB_SIZE, {INPUT_DIM});

    for (std::map<int, vector<float>>::iterator iter = embeddings_all.begin(); iter != embeddings_all.end(); iter++)
    {
      p_w.initialize(iter->first, iter->second);
    }

    //p_l2th = model.add_parameters({TAG_HIDDEN_DIM, HIDDEN_DIM});
    //p_r2th = model.add_parameters({TAG_HIDDEN_DIM, HIDDEN_DIM});
    //p_thbias = model.add_parameters({TAG_HIDDEN_DIM});

    //p_th2t = model.add_parameters({TAG_SIZE, TAG_HIDDEN_DIM});
    //p_tbias = model.add_parameters({TAG_SIZE});

    //crf
    p_W_lstm_fwd = model.add_parameters({MLP_HIDDEN_DIM, HIDDEN_DIM});
    p_W_lstm_rev = model.add_parameters({MLP_HIDDEN_DIM, HIDDEN_DIM});

    p_C = model.add_parameters({TAG_SIZE, MLP_HIDDEN_DIM});
    p_bias = model.add_parameters({MLP_HIDDEN_DIM});

    p_decoder = model.add_parameters({TAG_SIZE, VOCAB_SIZE});
    //p_decoder = model.add_parameters({VOCAB_SIZE, TAG_SIZE});
    p_A_transition = model.add_parameters({TAG_SIZE, TAG_SIZE});
  }

  void add_pretained_embeddings(LookupParameter &p_w, std::map<int, vector<float>> &embeddings)
  {
    // NOW SET THEIR VALUES!
    // iterate over map<int, floats> for english
    // copy floats into p_we[i] for each word index i in map
  }

  // return Expression of total loss
  Expression BuildTaggingGraph(const vector<int> &sent, const vector<int> &tags, ComputationGraph &cg, int labelling_type)
  {
    const unsigned slen = sent.size();
    l2rbuilder.new_graph(cg); // reset RNN builder for new graph
    l2rbuilder.start_new_sequence();
    r2lbuilder.new_graph(cg); // reset RNN builder for new graph
    r2lbuilder.start_new_sequence();

    Expression i_W_lstm_fwd = parameter(cg, p_W_lstm_fwd);
    Expression i_W_lstm_rev = parameter(cg, p_W_lstm_rev);
    Expression i_C = parameter(cg, p_C);
    Expression i_bias = parameter(cg, p_bias);

    Expression i_A_transition = parameter(cg, p_A_transition);

    vector<Expression> i_words(slen);
    vector<Expression> fwds(slen);
    vector<Expression> revs(slen);

    vector<Expression> fs(slen);
    vector<Expression> Gs(slen);

    vector<vector<Expression>> alpha_matrix(slen);
    vector<Expression> numerator_vector;

    Expression i_decoder = parameter(cg, p_decoder);

    vector<vector<dynet::real>> delta_matrix(slen);
    vector<vector<unsigned>> delta_matrix_bp(slen);

    // deep architecture
    // read sequence from left to right
    l2rbuilder.add_input(lookup(cg, p_w, kSOS));
    for (unsigned t = 0; t < slen; ++t)
    {
      i_words[t] = const_lookup(cg, p_w, sent[t]);
      if (!eval)
      {
        i_words[t] = noise(i_words[t], 0.1);
      }
      fwds[t] = l2rbuilder.add_input(i_words[t]);
    }

    // read sequence from right to left
    r2lbuilder.add_input(const_lookup(cg, p_w, kEOS));
    for (unsigned t = 0; t < slen; ++t)
      revs[slen - t - 1] = r2lbuilder.add_input(i_words[slen - t - 1]);

    // crf architecture
    for (unsigned cur_step = 0; cur_step < slen; cur_step++)
    {
      fs[cur_step] = transpose(i_C * tanh(i_W_lstm_fwd * fwds[cur_step] + i_W_lstm_rev * revs[cur_step] + i_bias));
      Gs[cur_step] = i_A_transition;
    }

    //FLAG for the labelling status
    int FLAG = labelling_type;
    //cerr << "\nFLAG = " << FLAG;
    switch (FLAG)
    {
    case Labelled:
    {
      //cerr << "\nProcess the labelled data";
      vector<Expression> alphas(slen);
      //		transpose(colwise_add(transpose(Gs[cur_step]), sum_cols(transpose(fs[0]))));
      alphas[0] = reshape(fs[0], {TAG_SIZE, 1});
      numerator_vector.push_back(pick(alphas[0], tags[0]));
      //cerr << "TAG_SIZE" << TAG_SIZE;
      for (unsigned cur_step = 1; cur_step < slen; cur_step++)
      {
        // phi matrix [y' (previous tag)][y (current tag)]
        //Expression phi_matrix = Gs[cur_step] + f_broadcast;
        Expression f = sum_cols(transpose(fs[cur_step]));
        Expression phi_matrix = transpose(colwise_add(transpose(Gs[cur_step]), f));

        numerator_vector.push_back(pick(select_cols(phi_matrix, {(unsigned)tags[cur_step]}), (unsigned)tags[cur_step - 1]));
        //LOLCAT(numerator_vector.back());

        //Expression alpha_matrix_before_lse = alpha_prev_broadcast + phi_matrix;
        Expression alpha_matrix_before_lse = colwise_add(phi_matrix, sum_cols(alphas[cur_step - 1]));

        vector<Expression> alpha_ys_after_lse_vector;

        // TODO: optimize
        for (unsigned cur_tag = 0; cur_tag < TAG_SIZE; cur_tag++)
        {
          Expression col = select_cols(alpha_matrix_before_lse, {cur_tag});
          //Expression col_phi = select_cols(phi_matrix, {cur_tag});

          //vector<Expression> alpha_y_before_lse_vector;

          //LOLCAT(col);
          //alpha_ys_after_lse_vector.push_back(logsumexp(alpha_y_before_lse_vector));
          alpha_ys_after_lse_vector.push_back(logsumexp_vectorized(col));
        }
        alphas[cur_step] = reshape(concatenate(alpha_ys_after_lse_vector), {TAG_SIZE, 1});
        //LOLCAT(alphas[cur_step]);
      }

      Expression logZ = logsumexp_vectorized(alphas[slen - 1]);
      Expression numerator = sum(numerator_vector);
      Expression loss = -(numerator - logZ);
      return loss;
    }
    case Unlabelled:
    {
      //cerr <<"\nProcess the unlabelled data";
      vector<Expression> alphas(slen);
      vector<Expression> numberator_uns(slen);
      //transpose(colwise_add(transpose(Gs[cur_step]), sum_cols(transpose(fs[0]))));
      alphas[0] = reshape(fs[0], {TAG_SIZE, 1});
      // the unsupervised part
      numberator_uns[0] = reshape(fs[0], {TAG_SIZE, 1});

      for (unsigned cur_step = 1; cur_step < slen; cur_step++)
      {
        //phi matrix [y' (previous tag)][y (current tag)]
        //Expression phi_matrix = Gs[cur_step] + f_broadcast;
        Expression f = sum_cols(transpose(fs[cur_step])); // TAG_SIZE, 1
        Expression phi_matrix = transpose(colwise_add(transpose(Gs[cur_step]), f));

        Expression alpha_matrix_before_lse = colwise_add(phi_matrix, sum_cols(alphas[cur_step - 1]));

        Expression theta = select_cols(i_decoder, {(unsigned)sent[cur_step]}); //TAG_SIZE,1
        Expression theta_dist = log_softmax(theta);

        Expression numberator_uns_matrix_before_lse = colwise_add(alpha_matrix_before_lse, theta_dist);

        vector<Expression> alpha_ys_after_lse_vector;
        vector<Expression> numberator_uns_ys_after_lse_vector;

        //TODO: optimize
        for (unsigned cur_tag = 0; cur_tag < TAG_SIZE; cur_tag++)
        {
          Expression col = select_cols(alpha_matrix_before_lse, {cur_tag});
          Expression col_numberator_uns = select_cols(numberator_uns_matrix_before_lse, {cur_tag});

          alpha_ys_after_lse_vector.push_back(logsumexp_vectorized(col));
          numberator_uns_ys_after_lse_vector.push_back(logsumexp_vectorized(col_numberator_uns));
        }
        alphas[cur_step] = reshape(concatenate(alpha_ys_after_lse_vector), {TAG_SIZE, 1});
        numberator_uns[cur_step] = reshape(concatenate(numberator_uns_ys_after_lse_vector), {TAG_SIZE, 1});
        //LOLCAT(alphas[cur_step]);
      }

      Expression logZ = logsumexp_vectorized(alphas[slen - 1]);
      Expression logNumberator_uns = logsumexp_vectorized(numberator_uns[slen - 1]);
      Expression loss = -(logNumberator_uns - logZ);
      return loss;
    }
    }
  }

  // return Expression of total loss
  Expression Evaluate(const vector<int> &sent, const vector<int> &tags, ComputationGraph &cg, double *cor = 0, unsigned *ntagged = 0)
  {
    //cerr <<"\nStart evaluation";
    const unsigned slen = sent.size();
    l2rbuilder.new_graph(cg); // reset RNN builder for new graph
    l2rbuilder.start_new_sequence();
    r2lbuilder.new_graph(cg); // reset RNN builder for new graph
    r2lbuilder.start_new_sequence();

    Expression i_W_lstm_fwd = parameter(cg, p_W_lstm_fwd);
    Expression i_W_lstm_rev = parameter(cg, p_W_lstm_rev);
    Expression i_C = parameter(cg, p_C);
    Expression i_bias = parameter(cg, p_bias);

    Expression i_A_transition = parameter(cg, p_A_transition);

    vector<Expression> i_words(slen);
    vector<Expression> fwds(slen);
    vector<Expression> revs(slen);

    vector<Expression> fs(slen);
    vector<Expression> Gs(slen);

    vector<vector<Expression>> alpha_matrix(slen);
    vector<Expression> numerator_vector;
    Expression i_decoder = parameter(cg, p_decoder);

    vector<vector<dynet::real>> delta_matrix(slen);
    vector<vector<unsigned>> delta_matrix_bp(slen);

    // deep architecture
    // read sequence from left to right
    l2rbuilder.add_input(lookup(cg, p_w, kSOS));
    for (unsigned t = 0; t < slen; ++t)
    {
      i_words[t] = const_lookup(cg, p_w, sent[t]);
      if (!eval)
      {
        i_words[t] = noise(i_words[t], 0.1);
      }
      fwds[t] = l2rbuilder.add_input(i_words[t]);
    }

    // read sequence from right to left
    r2lbuilder.add_input(const_lookup(cg, p_w, kEOS));
    for (unsigned t = 0; t < slen; ++t)
      revs[slen - t - 1] = r2lbuilder.add_input(i_words[slen - t - 1]);

    // crf architecture
    for (unsigned cur_step = 0; cur_step < slen; cur_step++)
    {
      fs[cur_step] = transpose(i_C * tanh(i_W_lstm_fwd * fwds[cur_step] + i_W_lstm_rev * revs[cur_step] + i_bias));
      Gs[cur_step] = i_A_transition;
    }

    vector<Expression> alphas(slen);
    //		transpose(colwise_add(transpose(Gs[cur_step]), sum_cols(transpose(fs[0]))));
    alphas[0] = reshape(fs[0], {TAG_SIZE, 1});
    numerator_vector.push_back(pick(alphas[0], tags[0]));

    //decoding
    delta_matrix[0].resize(TAG_SIZE);
    delta_matrix_bp[0].resize(TAG_SIZE);
    for (unsigned cur_tag = 0; cur_tag < TAG_SIZE; cur_tag++)
    {
      delta_matrix[0][cur_tag] = as_scalar(pick(alphas[0], cur_tag).value());
    }

    for (unsigned cur_step = 1; cur_step < slen; cur_step++)
    {
      // phi matrix [y' (previous tag)][y (current tag)]
      //Expression phi_matrix = Gs[cur_step] + f_broadcast;
      Expression f = sum_cols(transpose(fs[cur_step]));
      Expression phi_matrix = transpose(colwise_add(transpose(Gs[cur_step]), f));

      numerator_vector.push_back(pick(select_cols(phi_matrix, {(unsigned)tags[cur_step]}), (unsigned)tags[cur_step - 1]));
      //LOLCAT(numerator_vector.back());

      //Expression alpha_matrix_before_lse = alpha_prev_broadcast + phi_matrix;
      Expression alpha_matrix_before_lse = colwise_add(phi_matrix, sum_cols(alphas[cur_step - 1]));

      vector<Expression> alpha_ys_after_lse_vector;

      //decoding
      delta_matrix[cur_step].resize(TAG_SIZE);
      delta_matrix_bp[cur_step].resize(TAG_SIZE);

      // TODO: optimize
      for (unsigned cur_tag = 0; cur_tag < TAG_SIZE; cur_tag++)
      {
        Expression col = select_cols(alpha_matrix_before_lse, {cur_tag});
        Expression col_phi = select_cols(phi_matrix, {cur_tag});

        //decoding
        delta_matrix[cur_step][cur_tag] = numeric_limits<float>::lowest();
        delta_matrix_bp[cur_step][cur_tag] = -1;
        for (unsigned prev_tag = 0; prev_tag < TAG_SIZE; prev_tag++)
        {
          Expression phi = pick(col_phi, prev_tag);
          dynet::real cur = as_scalar(phi.value()) + delta_matrix[cur_step - 1][prev_tag];
          if (delta_matrix[cur_step][cur_tag] < cur)
          {
            delta_matrix[cur_step][cur_tag] = cur;
            delta_matrix_bp[cur_step][cur_tag] = prev_tag;
          }
        }
        assert(delta_matrix_bp[cur_step][cur_tag] != -1);

        //LOLCAT(col);
        //alpha_ys_after_lse_vector.push_back(logsumexp(alpha_y_before_lse_vector));
        alpha_ys_after_lse_vector.push_back(logsumexp_vectorized(col));
      }
      alphas[cur_step] = reshape(concatenate(alpha_ys_after_lse_vector), {TAG_SIZE, 1});
      //LOLCAT(alphas[cur_step]);
    }

    //decoding
    vector<int> predict(slen);
    dynet::real cur_max = numeric_limits<float>::lowest();
    unsigned max_index = -1;
    for (unsigned i = 0; i < delta_matrix[slen - 1].size(); i++)
    {
      if (cur_max < delta_matrix[slen - 1][i])
      {
        cur_max = delta_matrix[slen - 1][i];
        max_index = i;
      }
    }
    bool display = false;
    if (cur_max != numeric_limits<float>::min() && max_index != -1)
    {
      //assert (cur_max != FLT_MIN);
      //assert (max_index != -1);
      predict[slen - 1] = max_index;
      for (unsigned t = slen - 1; t > 0; t--)
      {
        predict[t - 1] = delta_matrix_bp[t][predict[t]];
      }
      assert(predict.size() == slen);
      for (unsigned i = 0; i < slen; i++)
      {
        if (tags[i] == predict[i])
        {
          (*cor)++;
        }
        if (display)
        {
          cerr << d.convert(sent[i]) << "|" << td.convert(tags[i])
               << "|" << td.convert(predict[i]) << " ";
        }
      }
      if (display)
      {
        cerr << endl;
      }
    }

    if (ntagged)
    {
      (*ntagged) += slen;
    }

    Expression logZ = logsumexp_vectorized(alphas[slen - 1]);
    Expression numerator = sum(numerator_vector);
    Expression loss = -(numerator - logZ);
    return loss;
  }

  Expression logsumexp_vectorized(Expression col)
  {
    Expression col_max = pick(kmax_pooling(transpose(col), 1), (unsigned)0);
    Expression diff = colwise_add(transpose(col), -col_max);
    Expression lse = col_max + log(sum_cols(exp(diff)));
    return lse;
  }
};

vector<string> split(const string &s, char c)
{
  vector<string> parts;
  string::size_type i = 0;
  string::size_type j = s.find(c);

  while (j != string::npos)
  {
    parts.push_back(s.substr(i, j - i));
    i = ++j;
    j = s.find(c, j);

    if (j == string::npos)
      parts.push_back(s.substr(i, s.length()));
  }
  return parts;
}

// import pretrained word embeddings and construct a dictionary
std::map<int, vector<float>> importEmbeddings(string inputFile)
{
  std::map<int, vector<float>> w_embeddings;
  unsigned welc = 0;
  unsigned dim = 0;
  cerr << "Loading word embeddings " << inputFile << "...\n";

  string line;
  ifstream in(inputFile);
  assert(in);
  while (getline(in, line))
  {

    // word, (val1, val2, ...)
    // if use multiCCA, then the space is ' '
    vector<string> splitedline = split(line, ' ');

    string word = splitedline[0];
    auto w_id = d.convert(word);
    vector<float> w_embedding;

    for (unsigned i = 1; i < splitedline.size(); i++)
    {
      w_embedding.push_back(std::stof(splitedline[i]));
    }
    if (dim == 0)
      dim = splitedline.size() - 1;
    else
      assert(dim == splitedline.size() - 1);
    w_embeddings[w_id] = w_embedding;

    ++welc;
  }

  cerr << welc << " word embeddings, " << dim << " dimensions \n";
  return w_embeddings;
}

vector<pair<vector<int>, vector<int>>> loadCorpus(string inputFile)
{
  cerr << "Loading data " << inputFile << "...\n";
  vector<pair<vector<int>, vector<int>>> data;
  string line;
  int lc = 0;
  int toks = 0;
  ifstream in(inputFile);
  assert(in);
  while (getline(in, line))
  {
    ++lc;
    int nc = 0;
    vector<int> x, y;
    read_sentence_pair(line, x, d, y, td);
    assert(x.size() == y.size());
    if (x.size() == 0)
    {
      cerr << line << endl;
      abort();
    }
    data.push_back(make_pair(x, y));
    for (unsigned i = 0; i < y.size(); ++i)
    {
      if (y[i] != kNONE)
      {
        ++nc;
      }
    }
    if (nc == 0)
    {
      cerr << "No tagged tokens in line " << lc << endl;
      abort();
    }
    toks += x.size();
  }
  cerr << lc << " lines, " << toks << " tokens, " << d.size() << " types\n";
  cerr << "Tags: " << td.size() << endl;
  return data;
}

void trainLabelled(vector<pair<vector<int>, vector<int>>> &training, DeepCRF3<LSTMBuilder> &lm, Trainer *sgd)
{
  int report = 0;
  int report_every_i = 100;
  double loss = 0;
  int ttags = 0;
  unsigned budget = training.size();
  cerr << "\nTraining labelled data: size = " << budget << " sentences";
  for (unsigned i = 0; i < budget; ++i)
  {
    // build graph for this instance
    ComputationGraph cg;
    auto &sent = training[i];
    int l_type = 0;
    //cerr << "Compute loss\n";
    Expression loss_expr = lm.BuildTaggingGraph(sent.first, sent.second, cg, l_type);
    loss += as_scalar(cg.forward(loss_expr));
    cg.backward(loss_expr);
    sgd->update(1.0);
    ttags += sent.first.size();
    ++report;
    if (report % report_every_i == 0)
    {
      cerr << "\n***TRAIN:labelled E = " << (loss / ttags);
    }
  }
}

void trainUnlabelled(vector<pair<vector<int>, vector<int>>> &training, DeepCRF3<LSTMBuilder> &lm, Trainer *sgd)
{
  int report = 0;
  int report_every_i = 100;
  int ttoks = 0;
  double loss = 0;
  unsigned budget = training.size();
  cerr << "\nTraining unlabelled data: size = " << budget << " sentences";
  for (unsigned i = 0; i < budget; ++i)
  {
    // build graph for this instance
    ComputationGraph cg;
    auto &sent = training[i];
    int l_type = 1;
    Expression loss_expr = lm.BuildTaggingGraph(sent.first, sent.second, cg, l_type);
    loss += as_scalar(cg.forward(loss_expr));
    cg.backward(loss_expr);
    sgd->update(1.0);
    ttoks += sent.first.size();
    ++report;
    if (report % report_every_i == 0)
    {
      cerr << "\n***TRAIN:unlabelled E = " << (loss / ttoks);
    }
  }
}

// evaluate the dev and test data
void testData(vector<pair<vector<int>, vector<int>>> &test, vector<pair<vector<int>, vector<int>>> &dev, DeepCRF3<LSTMBuilder> &lm, double &best)
{
  //dev data
  double loss = 0;
  unsigned tags = 0;
  double corr = 0;
  eval = true;
  //lm.p_th2t->scale_parameters(pdrop);
  for (auto &sent : dev)
  {
    ComputationGraph cg;
    Expression loss_expr = lm.Evaluate(sent.first, sent.second, cg, &corr, &tags);
    loss += as_scalar(cg.forward(loss_expr));
  }
  //lm.p_th2t->scale_parameters(1/pdrop);
  eval = false;
  cerr << "\n***DEV  E = " << (loss / tags) << " ppl=" << exp(loss / tags) << " acc=" << (corr / tags) << ' ';
  // test data
  loss = 0;
  tags = 0;
  corr = 0;
  eval = true;
  //lm.p_th2t->scale_parameters(pdrop);
  for (auto &sent : test)
  {
    ComputationGraph cg;
    Expression loss_expr = lm.Evaluate(sent.first, sent.second, cg, &corr, &tags);
    loss += as_scalar(cg.forward(loss_expr));
  }
  //lm.p_th2t->scale_parameters(1/pdrop);
  eval = false;
  if (loss < best)
  {
    best = loss;
    /*ofstream out(fname);
    boost::archive::text_oarchive oa(out);
    oa << model;*/
  }
  cerr << "\n***TEST  E = " << (loss / tags) << " ppl=" << exp(loss / tags) << " acc=" << (corr / tags) << ' ';
}

/*
$./train_nncrf3 embeddings_file labelled_file unlabelled_file dev_file test_file max_epoch
*/

int main(int argc, char **argv)
{
  dynet::initialize(argc, argv);

  vector<pair<vector<int>, vector<int>>> data_labelled, data_unlabelled;
  vector<pair<vector<int>, vector<int>>> dev, test;
  string line;
  int tlc = 0;
  int ttoks = 0;

  // load embeddings and build dictionaries
  // import one language
  kNONE = td.convert("*");
  kSOS = d.convert("<s>");
  kEOS = d.convert("</s>");

  string inputEmbeddingFile = argv[1];
  embeddings_all = importEmbeddings(inputEmbeddingFile);
  d.freeze(); // no new word types allowed
  d.set_unk("<UNK>");
  VOCAB_SIZE = d.size();
  cerr << "Size of Dictionary of all languages: " << VOCAB_SIZE << endl;

  string inputFile = argv[2];
  data_labelled = loadCorpus(inputFile);
  inputFile = argv[3];
  data_unlabelled = loadCorpus(inputFile);
  td.freeze();
  TAG_SIZE = td.size();

  //the dev and test data; they are from the target language
  inputFile = argv[4];
  dev = loadCorpus(inputFile);
  inputFile = argv[5];
  test = loadCorpus(inputFile);

  // initialise the model
  ostringstream os;
  os << "tagger"
     << '_' << LAYERS
     << '_' << INPUT_DIM
     << '_' << HIDDEN_DIM
     << "-pid" << getpid() << ".params";
  const string fname = os.str();
  cerr << "Parameters will be written to: " << fname << endl;
  double best = 9e+99;

  Model model;
  bool use_momentum = true;
  Trainer *sgd = nullptr;
  if (use_momentum)
    sgd = new MomentumSGDTrainer(model);
  else
    sgd = new SimpleSGDTrainer(model);

  DeepCRF3<LSTMBuilder> lm(model);
  if (argc == 7)
  {
    MAX_EPOCH = atoi(argv[6]);
  }
  /*if (argc == 4)
  {
    string fname = argv[3];
    ifstream in(fname);
    boost::archive::text_iarchive ia(in);
    ia >> model;
  }*/

  unsigned report_every_i = 50;
  unsigned dev_every_i_reports = 25;
  /*unsigned si = training.size(); // a combined training datasets
  vector<unsigned> order(training.size());
  for (unsigned i = 0; i < order.size(); ++i)
    order[i] = i;
  bool first = true;
  int report = 0;
  unsigned lines = 0;*/

  int epoch = 0;
  while (1)
  {
    Timer iteration("completed in");
    double loss = 0;
    //epoch++;
    cerr << "\n***Training [epoch=" << epoch << "]";
    // train labelled data and unlabelled data
    trainLabelled(data_labelled, lm, sgd);
    trainUnlabelled(data_unlabelled, lm, sgd);

    //test
    testData(test, dev, lm, best);

    cerr << "\n***Best = " << best;
    sgd->status();
    sgd->update_epoch();
    epoch++;
    if (epoch > MAX_EPOCH)
      break;
  }
  delete sgd;
}


/**
 * Orthanc - A Lightweight, RESTful DICOM Store
 * Copyright (C) 2012-2016 Sebastien Jodogne, Medical Physics
 * Department, University Hospital of Liege, Belgium
 * Copyright (C) 2017-2020 Osimis S.A., Belgium
 *
 * This program is free software: you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation, either version 3 of the
 * License, or (at your option) any later version.
 *
 * In addition, as a special exception, the copyright holders of this
 * program give permission to link the code of its release with the
 * OpenSSL project's "OpenSSL" library (or with modified versions of it
 * that use the same license as the "OpenSSL" library), and distribute
 * the linked executables. You must obey the GNU General Public License
 * in all respects for all of the code used other than "OpenSSL". If you
 * modify file(s) with this exception, you may extend this exception to
 * your version of the file(s), but you are not obligated to do so. If
 * you do not wish to do so, delete this exception statement from your
 * version. If you delete this exception statement from all source files
 * in the program, then also delete it here.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 **/

#include "PrecompiledHeadersServer.h"
#include "SliceOrdering.h"

#include "../../OrthancFramework/Sources/Logging.h"
#include "../../OrthancFramework/Sources/Toolbox.h"
#include "ServerEnumerations.h"
#include "ServerIndex.h"

#include <algorithm>
#include <boost/lexical_cast.hpp>
#include <boost/noncopyable.hpp>

namespace Orthanc
{
  static bool TokenizeVector(std::vector<float>& result,
                             const std::string& value,
                             unsigned int expectedSize)
  {
    std::vector<std::string> tokens;
    Toolbox::TokenizeString(tokens, value, '\\');

    if (tokens.size() != expectedSize)
    {
      return false;
    }

    result.resize(tokens.size());

    for (size_t i = 0; i < tokens.size(); i++)
    {
      try
      {
        const std::string token = Toolbox::StripSpaces(tokens[i]);
        result[i] = boost::lexical_cast<float>(token);
      }
      catch (boost::bad_lexical_cast&)
      {
        return false;
      }
    }

    return true;
  }

  static bool TokenizeVector(std::vector<float>& result,
                             const DicomMap& map,
                             const DicomTag& tag,
                             unsigned int expectedSize)
  {
    const DicomValue* value = map.TestAndGetValue(tag);

    if (value == NULL ||
        value->IsNull() ||
        value->IsBinary())
    {
      return false;
    }
    else
    {
      return TokenizeVector(result, value->GetContent(), expectedSize);
    }
  }

  static bool IsCloseToZero(double x)
  {
    return fabs(x) < 10.0 * std::numeric_limits<float>::epsilon();
  }

  bool SliceOrdering::ComputeNormal(Vector& normal,
                                    const DicomMap& dicom)
  {
    std::vector<float> cosines;

    if (TokenizeVector(cosines, dicom, DICOM_TAG_IMAGE_ORIENTATION_PATIENT, 6))
    {
      assert(cosines.size() == 6);
      normal[0] = cosines[1] * cosines[5] - cosines[2] * cosines[4];
      normal[1] = cosines[2] * cosines[3] - cosines[0] * cosines[5];
      normal[2] = cosines[0] * cosines[4] - cosines[1] * cosines[3];
      return true;
    }
    else
    {
      return false;
    }
  }

  bool SliceOrdering::IsParallelOrOpposite(const Vector& u,
                                           const Vector& v)
  {
    // Check out "GeometryToolbox::IsParallelOrOpposite()" in Stone of
    // Orthanc for explanations
    const double u1 = u[0];
    const double u2 = u[1];
    const double u3 = u[2];
    const double normU = sqrt(u1 * u1 + u2 * u2 + u3 * u3);

    const double v1 = v[0];
    const double v2 = v[1];
    const double v3 = v[2];
    const double normV = sqrt(v1 * v1 + v2 * v2 + v3 * v3);

    if (IsCloseToZero(normU * normV))
    {
      return false;
    }
    else
    {
      const double cosAngle = (u1 * v1 + u2 * v2 + u3 * v3) / (normU * normV);

      return (IsCloseToZero(cosAngle - 1.0) ||      // Close to +1: Parallel, non-opposite
              IsCloseToZero(fabs(cosAngle) - 1.0)); // Close to -1: Parallel, opposite
    }
  }

  struct SliceOrdering::Instance : public boost::noncopyable
  {
  private:
    std::string   instanceId_;
    bool          hasPosition_;
    Vector        position_;
    bool          hasNormal_;
    Vector        normal_;
    bool          hasIndexInSeries_;
    size_t        indexInSeries_;
    unsigned int  framesCount_;

  public:
    Instance(ServerIndex& index,
             const std::string& instanceId) :
      instanceId_(instanceId),
      framesCount_(1)
    {
      DicomMap instance;
      if (!index.GetMainDicomTags(instance, instanceId, ResourceType_Instance, ResourceType_Instance))
      {
        throw OrthancException(ErrorCode_UnknownResource);
      }

      const DicomValue* frames = instance.TestAndGetValue(DICOM_TAG_NUMBER_OF_FRAMES);
      if (frames != NULL &&
          !frames->IsNull() &&
          !frames->IsBinary())
      {
        try
        {
          const std::string token = Toolbox::StripSpaces(frames->GetContent());
          framesCount_ = boost::lexical_cast<unsigned int>(token);
        }
        catch (boost::bad_lexical_cast&)
        {
        }
      }

      std::vector<float> tmp;
      hasPosition_ = TokenizeVector(tmp, instance, DICOM_TAG_IMAGE_POSITION_PATIENT, 3);

      if (hasPosition_)
      {
        position_[0] = tmp[0];
        position_[1] = tmp[1];
        position_[2] = tmp[2];
      }

      hasNormal_ = ComputeNormal(normal_, instance);

      std::string s;
      hasIndexInSeries_ = false;

      try
      {
        if (index.LookupMetadata(s, instanceId, MetadataType_Instance_IndexInSeries))
        {
          indexInSeries_ = boost::lexical_cast<size_t>(s);
          hasIndexInSeries_ = true;
        }
      }
      catch (boost::bad_lexical_cast&)
      {
      }
    }

    const std::string& GetIdentifier() const
    {
      return instanceId_;
    }

    bool HasPosition() const
    {
      return hasPosition_;
    }

    float ComputeRelativePosition(const Vector& normal) const
    {
      assert(HasPosition());
      return (normal[0] * position_[0] +
              normal[1] * position_[1] +
              normal[2] * position_[2]);
    }

    bool HasIndexInSeries() const
    {
      return hasIndexInSeries_;
    }

    size_t GetIndexInSeries() const
    {
      assert(HasIndexInSeries());
      return indexInSeries_;
    }

    unsigned int GetFramesCount() const
    {
      return framesCount_;
    }

    bool HasNormal() const
    {
      return hasNormal_;
    }

    const Vector& GetNormal() const
    {
      assert(hasNormal_);
      return normal_;
    }
  };

  class SliceOrdering::PositionComparator
  {
  private:
    const Vector&  normal_;

  public:
    explicit PositionComparator(const Vector& normal) : normal_(normal)
    {
    }

    int operator() (const Instance* a,
                    const Instance* b) const
    {
      return a->ComputeRelativePosition(normal_) < b->ComputeRelativePosition(normal_);
    }
  };

  bool SliceOrdering::IndexInSeriesComparator(const SliceOrdering::Instance* a,
                                              const SliceOrdering::Instance* b)
  {
    return a->GetIndexInSeries() < b->GetIndexInSeries();
  }

  void SliceOrdering::ComputeNormal()
  {
    DicomMap series;
    if (!index_.GetMainDicomTags(series, seriesId_, ResourceType_Series, ResourceType_Series))
    {
      throw OrthancException(ErrorCode_UnknownResource);
    }

    hasNormal_ = ComputeNormal(normal_, series);
  }

  void SliceOrdering::CreateInstances()
  {
    std::list<std::string> instancesId;
    index_.GetChildren(instancesId, seriesId_);

    instances_.reserve(instancesId.size());
    for (std::list<std::string>::const_iterator
           it = instancesId.begin(); it != instancesId.end(); ++it)
    {
      instances_.push_back(new Instance(index_, *it));
    }
  }

  bool SliceOrdering::SortUsingPositions()
  {
    if (instances_.size() <= 1)
    {
      // One single instance: It is sorted by default
      return true;
    }

    if (!hasNormal_)
    {
      return false;
    }

    for (size_t i = 0; i < instances_.size(); i++)
    {
      assert(instances_[i] != NULL);

      if (!instances_[i]->HasPosition() ||
          (instances_[i]->HasNormal() &&
           !IsParallelOrOpposite(instances_[i]->GetNormal(), normal_)))
      {
        return false;
      }
    }

    PositionComparator comparator(normal_);
    std::sort(instances_.begin(), instances_.end(), comparator);

    float a = instances_[0]->ComputeRelativePosition(normal_);
    for (size_t i = 1; i < instances_.size(); i++)
    {
      float b = instances_[i]->ComputeRelativePosition(normal_);

      if (std::fabs(b - a) <= 10.0f * std::numeric_limits<float>::epsilon())
      {
        // Not enough space between two slices along the normal of the volume
        return false;
      }

      a = b;
    }

    // This is a 3D volume
    isVolume_ = true;
    return true;
  }

  bool SliceOrdering::SortUsingIndexInSeries()
  {
    if (instances_.size() <= 1)
    {
      // One single instance: It is sorted by default
      return true;
    }

    for (size_t i = 0; i < instances_.size(); i++)
    {
      assert(instances_[i] != NULL);
      if (!instances_[i]->HasIndexInSeries())
      {
        return false;
      }
    }

    std::sort(instances_.begin(), instances_.end(), IndexInSeriesComparator);

    for (size_t i = 1; i < instances_.size(); i++)
    {
      if (instances_[i - 1]->GetIndexInSeries() == instances_[i]->GetIndexInSeries())
      {
        // The current "IndexInSeries" occurs 2 times: Not a proper ordering
        LOG(WARNING) << "This series contains 2 slices with the same index, trying to display it anyway";
        break;
      }
    }

    return true;
  }

  SliceOrdering::SliceOrdering(ServerIndex& index,
                               const std::string& seriesId) :
    index_(index),
    seriesId_(seriesId),
    isVolume_(false)
  {
    ComputeNormal();
    CreateInstances();

    if (!SortUsingPositions() &&
        !SortUsingIndexInSeries())
    {
      throw OrthancException(ErrorCode_CannotOrderSlices,
                             "Unable to order the slices of series " + seriesId);
    }
  }

  SliceOrdering::~SliceOrdering()
  {
    for (std::vector<Instance*>::iterator
           it = instances_.begin(); it != instances_.end(); ++it)
    {
      if (*it != NULL)
      {
        delete *it;
      }
    }
  }

  const std::string& SliceOrdering::GetInstanceId(size_t index) const
  {
    if (index >= instances_.size())
    {
      throw OrthancException(ErrorCode_ParameterOutOfRange);
    }
    else
    {
      return instances_[index]->GetIdentifier();
    }
  }

  unsigned int SliceOrdering::GetFramesCount(size_t index) const
  {
    if (index >= instances_.size())
    {
      throw OrthancException(ErrorCode_ParameterOutOfRange);
    }
    else
    {
      return instances_[index]->GetFramesCount();
    }
  }

  void SliceOrdering::Format(Json::Value& result) const
  {
    result = Json::objectValue;
    result["Type"] = (isVolume_ ? "Volume" : "Sequence");

    Json::Value tmp = Json::arrayValue;
    for (size_t i = 0; i < GetInstancesCount(); i++)
    {
      tmp.append(GetBasePath(ResourceType_Instance, GetInstanceId(i)) + "/file");
    }

    result["Dicom"] = tmp;

    Json::Value slicesShort = Json::arrayValue;

    tmp.clear();
    for (size_t i = 0; i < GetInstancesCount(); i++)
    {
      std::string base = GetBasePath(ResourceType_Instance, GetInstanceId(i));
      for (size_t j = 0; j < GetFramesCount(i); j++)
      {
        tmp.append(base + "/frames/" + boost::lexical_cast<std::string>(j));
      }

      Json::Value tmp2 = Json::arrayValue;
      tmp2.append(GetInstanceId(i));
      tmp2.append(0);
      tmp2.append(GetFramesCount(i));

      slicesShort.append(tmp2);
    }

    result["Slices"] = tmp;
    result["SlicesShort"] = slicesShort;
  }
}


// It works if the --p2p option is given!??
#include <iostream>
#include <boost/program_options.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/random.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>

#include <ndt_registration/ndt_matcher_d2d.h>
#include <ndt_registration/ndt_matcher_d2d_feature.h>
#include <ndt_map/ndt_map.h>
#include <ndt_map/cell_vector.h>
#include <ndt_map/lazy_grid.h>
#include <pointcloud_vrml/pointcloud_utils.h>

using namespace std;
using namespace perception_oru;
namespace po = boost::program_options;

int main(int argc, char** argv)
{
    cout << "--------------------------------------------------" << endl;
    cout << "Small test program of CellVector + F2F matcher " << endl;
    cout << "--------------------------------------------------" << endl;

    po::options_description desc("Allowed options");
    Eigen::Matrix<double,6,1> pose_increment_v;
    string static_file_name, moving_file_name;
    int nb_clusters, nb_points;
    double std_dev, min, max;
    desc.add_options()
    ("help", "produce help message")
    ("x", po::value<double>(&pose_increment_v(0))->default_value(1.), "x pos gt offset")
    ("y", po::value<double>(&pose_increment_v(1))->default_value(1.), "y pos gt offset")
    ("z", po::value<double>(&pose_increment_v(2))->default_value(1.), "z pos gt offset")
    ("X", po::value<double>(&pose_increment_v(3))->default_value(0.1), "x axis rot gt offset")
    ("Y", po::value<double>(&pose_increment_v(4))->default_value(0.1), "y axis rot gt offset")
    ("Z", po::value<double>(&pose_increment_v(5))->default_value(0.1), "z axis rot gt offset")
    ("nb_clusters", po::value<int>(&nb_clusters)->default_value(20), "number of clusters")
    ("nb_points", po::value<int>(&nb_points)->default_value(10), "number of points per clusters")
    ("std_dev", po::value<double>(&std_dev)->default_value(0.1), "standard deviation of the points drawn from a normal distribution (here independent on the axes")
    ("min", po::value<double>(&min)->default_value(-10), "minimum center point")
    ("max", po::value<double>(&max)->default_value(10), "maximum center point")
    ("lazzy", "use lazzygrid")
    ("p2preg", "calculate NDTMatchF2F using two pointclouds")
    ("irregular_grid", "use irregular grid in the p2p registration")
    ("nfeaturecorr", "feature_f2f should NOT be evaluated (NDTMatcherFeatureF2F)")
    ("singleres", "use single resolution in the 'p2p' matching")
    ("nf2f", "if f2f should NOT be evaluated")
    ("usegt", "if the ground truth should be used as initial estimate")
    ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help"))
    {
        cout << desc << "\n";
        return 1;
    }
    bool use_lazzy = vm.count("lazzy");
    bool use_p2preg = vm.count("p2preg");
    bool use_irregular_grid = vm.count("irregular_grid");
    bool use_featurecorr = !vm.count("nfeaturecorr");
    bool use_singleres = vm.count("singleres");
    bool use_f2f = !vm.count("nf2f");
    bool usegt = vm.count("usegt");
    pcl::PointCloud<pcl::PointXYZ> static_pc, moving_pc, tmp_pc;
    Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> gt_transform;

    // Generate the static point cloud + indices...
    boost::mt19937 rng;
    boost::uniform_real<> ud(min,max);
    boost::normal_distribution<> nd(0.0, std_dev);
    boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor(rng, nd);
    boost::variate_generator<boost::mt19937&, boost::uniform_real<> > var_uni(rng, ud);
    std::vector<std::pair<int,int> > corresp;

    size_t index = 0;
    std::vector<std::vector<size_t> > all_indices;
    for (int i = 0; i < nb_clusters; i++)
    {
        std::vector<size_t> indices;
        double c_x = var_uni();
        double c_y = var_uni();
        double c_z = var_uni();
        for (int j = 0; j < nb_points; j++)
        {
            static_pc.push_back(pcl::PointXYZ(c_x+var_nor(), c_y+var_nor(), c_z+var_nor()));
            indices.push_back(index);
            index++;
        }
        all_indices.push_back(indices);
        corresp.push_back(std::pair<int,int>(i,nb_clusters-1-i)); // nb_clusters-1-i -> To check the da functions is working.
    }
    std::vector<std::vector<size_t> > all_indices_moving(all_indices.size());  // Reverse the correspondances (To check the da functions)
    std::reverse_copy(all_indices.begin(), all_indices.end(), all_indices_moving.begin());

    for (int i = 0; i < 1/*nb_clusters*/; i++)
    {
        tmp_pc.push_back(pcl::PointXYZ(i,i,i));
    }

    // Specify some offset...
    gt_transform = Eigen::Translation<double,3>(pose_increment_v(0),pose_increment_v(1),pose_increment_v(2))*
                   Eigen::AngleAxis<double>(pose_increment_v(3),Eigen::Vector3d::UnitX()) *
                   Eigen::AngleAxis<double>(pose_increment_v(4),Eigen::Vector3d::UnitY()) *
                   Eigen::AngleAxis<double>(pose_increment_v(5),Eigen::Vector3d::UnitZ()) ;

    std::vector<double> resolutions;
    if (use_singleres)
        resolutions.push_back(1.);
    NDTMatcherD2D<pcl::PointXYZ,pcl::PointXYZ> matcher(use_irregular_grid,!use_singleres,resolutions);
    Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> T_f2f,T_p2p, T_feat;
    T_f2f.setIdentity();
    T_p2p.setIdentity();
    T_feat.setIdentity();
    if (usegt)
    {
        T_f2f = gt_transform;
        T_p2p = gt_transform;
        T_feat = gt_transform;
    }

    moving_pc = transformPointCloud(gt_transform, static_pc);

    if (use_p2preg)
    {
        matcher.match(moving_pc, static_pc, T_p2p);
    }
    {
        double current_resolution = 1;

        SpatialIndex<pcl::PointXYZ>* index = NULL;
        if (use_lazzy)
        {
            index = new LazyGrid<pcl::PointXYZ>(current_resolution);
        }
        else
        {
            index = new CellVector<pcl::PointXYZ>();
        }

        NDTMap<pcl::PointXYZ> ndt(index);
        if (!use_lazzy)
            ndt.loadPointCloud( static_pc, all_indices );
        else
            ndt.loadPointCloud (static_pc );
        ndt.computeNDTCells();

        NDTMap<pcl::PointXYZ> mov(index);
        if (!use_lazzy)
            mov.loadPointCloud( moving_pc, all_indices_moving );
        else
            mov.loadPointCloud( moving_pc );
        mov.computeNDTCells();

        if (use_f2f)
        {
            matcher.match( mov, ndt, T_f2f );
        }
        if (use_featurecorr)
        {
            NDTMatcherFeatureD2D<pcl::PointXYZ,pcl::PointXYZ> matcher_feat(corresp);
            matcher_feat.match( mov, ndt, T_feat );
        }
        delete index;
    }

    std::cout<<"GT translation "<<gt_transform.translation().transpose()
             <<" (norm) "<<gt_transform.translation().norm()<<std::endl;
    std::cout<<"GT rotation "<<gt_transform.rotation().eulerAngles(0,1,2).transpose()
             <<" (norm) "<<gt_transform.rotation().eulerAngles(0,1,2).norm()<<std::endl;

    if (use_f2f)
    {
        std::cout<<"f2f translation "<<T_f2f.translation().transpose()
                 <<" (norm) "<<T_f2f.translation().norm()<<std::endl;
        std::cout<<"f2f rotation "<<T_f2f.rotation().eulerAngles(0,1,2).transpose()
                 <<" (norm) "<<T_f2f.rotation().eulerAngles(0,1,2).norm()<<std::endl;
    }

    if (use_featurecorr)
    {
        std::cout<<"feat translation "<<T_feat.translation().transpose()
                 <<" (norm) "<<T_feat.translation().norm()<<std::endl;
        std::cout<<"feat rotation "<<T_feat.rotation().eulerAngles(0,1,2).transpose()
                 <<" (norm) "<<T_feat.rotation().eulerAngles(0,1,2).norm()<<std::endl;
    }

    if (use_p2preg)
    {
        std::cout<<"p2preg translation "<<T_p2p.translation().transpose()
                 <<" (norm) "<<T_p2p.translation().norm()<<std::endl;
        std::cout<<"p2preg rotation "<<T_p2p.rotation().eulerAngles(0,1,2).transpose()
                 <<" (norm) "<<T_p2p.rotation().eulerAngles(0,1,2).norm()<<std::endl;
    }
    cout << "done." << endl;
}


#include <iostream>

#include "mtao/opengl/Window.h"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wint-in-bool-context"
#include <Eigen/Geometry>
#pragma GCC diagnostic pop
#include <Corrade/Containers/Array.h>
#include <Corrade/Containers/ArrayView.h>
#include <Corrade/Utility/Arguments.h>
#include <Magnum/EigenIntegration/Integration.h>
#include <Magnum/GL/Renderer.h>
#include <Magnum/Shaders/VertexColor.h>
#include <mtao/eigen/stack.h>
#include <mtao/eigen_utils.h>
#include <mtao/geometry/grid/grid.h>
#include <mtao/opengl/objects/grid.h>
#include <mtao/types.h>

#include <Eigen/IterativeLinearSolvers>
#include <algorithm>
#include <memory>
#include <mtao/opengl/shaders/vector_field.hpp>

#include "imgui.h"
#include "mtao/geometry/bounding_box.hpp"
#include "mtao/geometry/mesh/boundary_facets.h"
#include "mtao/geometry/mesh/read_obj.hpp"
#include "mtao/opengl/drawables.h"

using namespace Magnum::Math::Literals;

// using namespace mtao::opengl;
//
// std::vector<glm::vec2> pts;
//
// enum class Mode: int { Smoothing, LSReinitialization };
// Mode mode = Mode::LSReinitialization;
// mtao::MatrixX<float> VV;
//
// float permeability = 100.0;
// float timestep = 1000.0;
// float look_distance = 0.6;
// glm::mat4 mvp_it;
// float rotation_angle;
// glm::vec3 edge_color;
// bool animate = false;
// using Mat = mtao::MatrixX<GLfloat>;
// Mat data;
// Mat data_original;
// Mat dx;
// Mat dy;
// Mat signs;
// Eigen::SparseMatrix<float> L;
//
// int NI=200;
// int NJ=200;
// Eigen::SimplicialLDLT<Eigen::SparseMatrix<float>> solver;
//
// std::unique_ptr<renderers::MeshRenderer> renderer;
// std::unique_ptr<renderers::MeshRenderer> renderer3;
// std::unique_ptr<renderers::BBoxRenderer2> bbox_renderer;
// std::unique_ptr<renderers::AxisRenderer> axis_renderer;
//
// std::unique_ptr<ShaderProgram> edge_program;
// std::unique_ptr<Window> window;
//
//
// Camera2D cam;
// Camera3D cam3;
//
// void set_mvp(int w, int h) {
//    cam.set_shape(w,h);
//    auto&& m = cam.m();
//    m = glm::mat4();
//    m = glm::rotate(m,(float) rotation_angle,glm::vec3(0,0,1));
//
//    //cam.v() = glm::lookAt(glm::vec3(1,0,0), glm::vec3(0,0,0),
//    glm::vec3(0,1,0)); cam.pan(); cam.set_scale(look_distance); cam.update();
//
//    mvp_it = cam.mvp_inv_trans();
//    cam3.set_shape(w,h);
//
//    cam3.pan();
//    cam3.update();
//
//}
//
//
//
//
//
// void set_colors(const Mat& data) {
//    mtao::RowVectorX<GLfloat> col = Eigen::Map<const
//    mtao::RowVectorX<GLfloat>>(data.data(),data.size());
//
//    mtao::MatrixX<GLfloat> Col =
//    mtao::eigen::vstack((col.array()>0).select(col,0),-(col.array()<0).select(col,0),mtao::RowVectorX<GLfloat>::Zero(data.size()));
//
//    Col.array() = Col.array().pow(.5);
//    renderer->setColor(Col);
//
//    VV.row(2) = col;
//    renderer3->setVertices(VV);
//    renderer3->setColor(Col);
//}
//
// void prepare_mesh(int i, int j) {
//
//    renderer3 = std::make_unique<renderers::MeshRenderer>(3);
//    renderer = std::make_unique<renderers::MeshRenderer>(2);
//    bbox_renderer = std::make_unique<renderers::BBoxRenderer2>();
//    axis_renderer = std::make_unique<renderers::AxisRenderer>(2);
//
//;
//    mtao::geometry::grid::Grid2f g({{i,j}});
//    mtao::geometry::grid::GridTriangulator<decltype(g)> gt(g);
//    auto V = gt.vertices();
//    auto F = gt.faces();
//    V.array() -= .5;
//    VV.resize(3,V.cols());
//    VV.topRows(2) = V;
//
//    mtao::RowVectorX<GLfloat> col = V.colwise().norm().array() - GLfloat(.25);
//    data = Eigen::Map<Mat>(col.data(),i,j);
//
//
//
//    using BBox = renderers::BBoxRenderer2::BBox;
//    BBox bb;
//    for(int i = 0; i < V.cols(); ++i) {
//        bb.extend(V.col(i));
//    }
//    bbox_renderer->set(bb);
//
//    data = data.array().pow(3);
//
//    data = .01 * (data.array() >
//    0).select(Mat::Ones(data.rows(),data.cols()),-1);
//
//    //data.setZero();
//    //data(NI/2,NJ/2) = 1;
//    //data(NI/3,NJ/2) = -1;
//
//    renderer->setMesh(V,F,false);
//    renderer->set_face_style(renderers::MeshRenderer::FaceStyle::Color);
//    renderer->set_vertex_style();
//
//    renderer3->set_face_style(renderers::MeshRenderer::FaceStyle::Color);
//    renderer3->set_vertex_style();
//    renderer3->setFaces(F);
//    set_colors(data);
//    data_original = data;
//}
//
// void lsreconstruction_precompute() {
//    signs = data.array() / (data.array().pow(2) + 1e-5).sqrt();
//}
//
//
//
//
//
// ImVec4 clear_color = ImColor(114, 144, 154);
//
//
//
// void centered_difference() {
//
//    auto dx_ = (NI-1)*(data.rightCols(NJ-1) - data.leftCols(NJ-1));
//    auto dy_ = (NJ-1)*(data.topRows(NI-1) - data.bottomRows(NI-1));
//    dx.resizeLike(data);
//    dy.resizeLike(data);
//    dx.setZero();
//    dy.setZero();
//    dx.leftCols(NJ-1) = dx_;
//    dx.rightCols(NJ-1) += dx_;
//    dy.topRows(NI-1) = dy_;
//    dy.bottomRows(NI-1) += dy_;
//    dx.block(0,1,NI,NJ-2) /= 2;
//    dy.block(1,0,NI-2,NJ) /= 2;
//}
//
// glm::vec2 grid_space(const glm::vec2& p) {
//    return (p + glm::vec2(.5) ) * glm::vec2(NI,NJ);
//}
// glm::vec2 grid_mouse_pos() {
//    return grid_space(cam.mouse_pos(ImGui::GetIO().MousePos));
//}
//
// Mat levelset_reinit() {
//    centered_difference();
//    //return (dx.array().pow(2) + dy.array().pow(2)).sqrt() - 1;
//    auto dx_ = (NI-1)*(data.rightCols(NJ-1) - data.leftCols(NJ-1));
//    auto dy_ = (NJ-1)*(data.topRows(NI-1) - data.bottomRows(NI-1));
//
//    Mat a(data.rows(),data.cols());
//    Mat b(data.rows(),data.cols());
//    Mat c(data.rows(),data.cols());
//    Mat d(data.rows(),data.cols());
//    a.setZero();
//    b.setZero();
//    c.setZero();
//    d.setZero();
//
//    dx.resizeLike(data);
//    dy.resizeLike(data);
//    dx.setZero();
//    dy.setZero();
//    a.leftCols(NJ-1) = dx_;
//    b.rightCols(NJ-1) = dx_;
//    c.bottomRows(NI-1) = dy_;
//    d.topRows(NI-1) = dy_;
//
//    auto ap = a.array().max(0).pow(2);
//    auto am = a.array().min(0).pow(2);
//    auto bp = b.array().max(0).pow(2);
//    auto bm = b.array().min(0).pow(2);
//    auto cp = c.array().max(0).pow(2);
//    auto cm = c.array().min(0).pow(2);
//    auto dp = d.array().max(0).pow(2);
//    auto dm = d.array().min(0).pow(2);
//
//    Mat grad = mtao::eigen::finite((data_original.array() <= 0).select
//    (ap.max(bm) + cp.max(dm),
//    bp.max(am) + dp.max(cm)) - 1);
//    Mat D = signs.cwiseProduct(grad);
//    std::cout << "Mean error: " << D.mean() << std::endl;
//    D = data -  1.0 / (4 * std::max(NI,NJ)) * D;
//    return D;
//
//
//    //dy.block(1,0,NI-2,NJ) /= 2;
//}
//
// Eigen::SparseMatrix<float> grid_boundary(int ni, int nj, bool
// dirichlet_boundary) {
//    int system_size = ni*nj;
//    int usize = (ni+1) * nj;
//    int vsize = (nj+1) * ni;
//    int offset = usize;
//
//    auto u_ind = [ni,nj](int i, int j) {
//        return i + j*(ni + 1);
//    };
//
//    auto v_ind = [ni,nj,offset](int i, int j) {
//        return i + j*ni + offset;
//    };
//
//    std::vector< Eigen::Triplet<float> > triplets;
//
//    Eigen::SparseMatrix<float> D(usize + vsize, system_size);
//    for (int j = 0; j < nj ; ++j) {
//        for (int i = 0; i < ni ; ++i) {
//            int index = i + ni*j;
//            if(!(dirichlet_boundary && i == 0)) {
//                triplets.push_back(Eigen::Triplet<float>( u_ind(i,j),
//                index, 1.0));
//            }
//            if(!(dirichlet_boundary && i == ni-1)) {
//            triplets.push_back(Eigen::Triplet<float>( u_ind(i+1,j), index,
//            -1.0));
//            }
//
//
//            if(!(dirichlet_boundary && j == 0)) {
//            triplets.push_back(Eigen::Triplet<float>(  v_ind(i,j),
//            index, 1.0));
//            }
//            if(!(dirichlet_boundary && j == ni-1)) {
//            triplets.push_back(Eigen::Triplet<float>(  v_ind(i,j+1), index,
//            -1.0));
//            }
//
//        }
//    }
//    D.setFromTriplets(triplets.begin(), triplets.end());
//    return D;
//}
//
// Mat smoothing() {
//    Mat D = data;
//    Eigen::Map<Eigen::VectorXf> theta(D.data(),D.size());
//    theta = solver.solve(theta);
//    return D;
//}
// void smoothing_precompute() {
//    int size = NI*NJ;
//    auto B = grid_boundary(NI,NJ,true);
//    //size ~ dx * dy ~ dx^2
//    L = 1.0 / size * B.transpose() * B;
//    Eigen::SparseMatrix<float> A(size,size);
//
//    A.setIdentity();
//    A += timestep * L;
//    solver.compute(A);
//}
//
// void do_animation() {
//
//    switch(mode) {
//
//        case Mode::Smoothing:
//            data = smoothing(); break;
//        case Mode::LSReinitialization:
//            data = levelset_reinit(); break;
//    }
//
//
//    set_colors(data);
//}
//
// void render(int width, int height) {
//    if(animate) {
//        do_animation();
//    }
//    // Rendering
//    glViewport(0, 0, width, height);
//    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
//
//    glClearColor(clear_color.x, clear_color.y, clear_color.z, clear_color.w);
//
//    set_mvp(width,height);
//
//
//    renderer->set_mvp(cam.mvp());
//    renderer->set_mvp(cam.mv(),cam.p());
//    bbox_renderer->set_mvp(cam.mvp());
//    axis_renderer->set_mvp(cam.mvp());
//
//    renderer3->set_mvp(cam3.mvp());
//    renderer3->set_mvp(cam3.mv(),cam3.p());
//    renderer->set_mvp(cam3.mvp());
//    renderer->set_mvp(cam3.mv(),cam3.p());
//    bbox_renderer->set_mvp(cam3.mvp());
//    axis_renderer->set_mvp(cam3.mvp());
//    glEnable(GL_DEPTH_TEST);
//    glDepthFunc(GL_ALWAYS);
//    glEnable(GL_BLEND);
//    glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
//    glEnable(GL_DEPTH_TEST);
//    //renderer->render();
//
//    glDepthFunc(GL_LESS);
//    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
//    renderer3->render();
//    //bbox_renderer->render();
//    //axis_renderer->render();
//
//
//
//}
//
//
// void gui_func() {
//    {
//        float look_min=0.0001f, look_max=100.0f;
//        ImGui::Text("Hello, world!");
//
//        ImGui::SliderFloat("look_distance", &look_distance,
//        look_min,look_max,"%.3f"); ImGui::SliderFloat("angle",
//        &rotation_angle,0,M_PI,"%.3f"); auto&& io = ImGui::GetIO();
//        look_distance += .5 * io.MouseWheel;
//        look_distance = std::min(std::max(look_distance,look_min),look_max);
//
//        ImGui::Checkbox("animate", &animate);
//
//        {
//        static const char* items[] = {
//            "Smoothing",
//            "Levelset Reinitialization"
//        };
//            int m = static_cast<int>(mode);
//            ImGui::Combo("Mode", &m, items, IM_ARRAYSIZE(items));
//            mode = static_cast<Mode>(m);
//        }
//        {
//            float tmp = permeability;
//            float tmp2 = timestep;
//            ImGui::InputFloat("Permeability", &tmp,0,1e-5);
//            ImGui::InputFloat("Timestep", &tmp2,0,1e-5);
//            if(tmp != permeability || tmp2 != timestep) {
//                smoothing_precompute();
//                lsreconstruction_precompute();
//            }
//            permeability = tmp;
//            timestep = tmp2;
//        }
//
//        renderer->imgui_interface();
//
//
//        ImGui::ColorEdit3("clear color", (float*)&clear_color);
//        auto gpos = grid_mouse_pos();
//        glm::ivec2 gposi;
//        gposi.x = std::max<int>(0,std::min<int>(NI-1,gpos.x));
//        gposi.y = std::max<int>(0,std::min<int>(NJ-1,gpos.y));
//        ImGui::Text("Application average %.3f ms/frame (%.1f FPS)
//        [%.3f,%.3f]", 1000.0f / ImGui::GetIO().Framerate,
//        ImGui::GetIO().Framerate,gpos.x,gpos.y); ImGui::Text("[%d,%d]: %.5f",
//        gposi.x,gposi.y,data(gposi.x,gposi.y));
//
//
//
//
//
//
//
//    }
//    if(ImGui::Button("Reset  Translation?")) {
//        cam.reset();
//    }
//    if(ImGui::Button("Step")) {
//        do_animation();
//    }
//    if(ImGui::Button("norm")) {
//        Mat n= (dx.array().pow(2) + dy.array().pow(2));
//        set_colors(n);
//    }
//    if(ImGui::Button("dx")) {
//        set_colors(dx);
//    }
//    if(ImGui::Button("dy")) {
//        set_colors(dy);
//    }
//    if(ImGui::Button("Reset")) {
//        data = data_original;
//    }
//}
// void set_keys() {
//    //auto&& h= window->hotkeys();
//    /*
//
//    h.add([&]() {
//            std::scoped_lock l(render_mutex);
//            renderer->hide_meshes();
//            renderer->show_mesh_vertices();
//            },"hide meshes and show vertices", GLFW_KEY_K);
//    h.add([&]() {
//            std::scoped_lock l(render_mutex);
//            renderer->toggle_form_normalization();
//            simrender_dirty = true;
//            },"toggle form normalization", GLFW_KEY_O);
//
//    h.add([&]() {
//            std::scoped_lock l(render_mutex);
//            renderer->hide_all();
//            },"hide everything", GLFW_KEY_L);
//    h.add([&]() {
//            std::scoped_lock l(render_mutex);
//            rendermode = SimRenderMode::Pressure;
//            renderer->set2form_style();
//            simrender_dirty = true;
//            debug() << "Pressure";
//            },"visualize pressure", GLFW_KEY_3);
//    h.add([&]() {
//            std::scoped_lock l(render_mutex);
//            rendermode = SimRenderMode::Vorticity;
//            renderer->set0form_style();
//            simrender_dirty = true;
//            debug() << "Vorticity";
//            },"visualize vorticity", GLFW_KEY_1);
//    h.add([&]() {
//            std::scoped_lock l(render_mutex);
//            rendermode = SimRenderMode::Streamfunctions;
//            renderer->set0form_style();
//            simrender_dirty = true;
//            debug() << "Streamfunction";
//            },"visualize streamfunction", GLFW_KEY_4);
//    h.add([&]() {
//            std::scoped_lock l(render_mutex);
//            rendermode = SimRenderMode::Divergence;
//            renderer->set2form_style();
//            simrender_dirty = true;
//            debug() << "Divergence";
//            },"visualize divergence", GLFW_KEY_2);
//    h.add([&]() {
//            std::scoped_lock l(render_mutex);
//            rendermode = SimRenderMode::Velocity;
//            renderer->hide_all();
//            renderer->enable_vfields();
//            simrender_dirty = true;
//            },"visualize velocity", GLFW_KEY_5);
//    h.add([&]() {
//            std::scoped_lock l(render_mutex);
//            rendermode = SimRenderMode::Dirichlet;
//            renderer->hide_all();
//            renderer->enable_vfields();
//            simrender_dirty = true;
//            },"visualize dirichlet", GLFW_KEY_6);
//    h.add([&]() {
//            std::scoped_lock l(render_mutex);
//            velform = dirichlet_boundary;
//            update_form_objects();
//            simrender_dirty = true;
//            },"visualize dirichlet", GLFW_KEY_D);
//    h.add([&]() {
//            std::scoped_lock l(render_mutex);
//            velform = dirichlet_boundary;
//            std::tie(divergence,pressure) =
//            fluid_decomposition_compress<1>(*m_ptr,dirichlet_boundary);
//            velform = velform_compress<2>(*m_ptr,pressure);
//            update_form_objects();
//            simrender_dirty = true;
//            },"visualize dirichlet_pressure", GLFW_KEY_F);
//
//    h.add([&]() {
//            cur_edge--;
//            edge_clamp();
//    }, "Decrement current edge", GLFW_KEY_N);
//    h.add([&]() {
//            cur_edge++;
//            edge_clamp();
//    }, "Increment current edge", GLFW_KEY_M);
//    h.add([&]() {
//            velform = dirichlet_boundary;
//            set_form_states();
//            simrender_dirty = true;
//            },"use dirichlet boundary conditions", GLFW_KEY_D);
//    h.add([&]() {
//            cur_edge++;
//            edge_clamp();
//            }, "Update edge", GLFW_KEY_M);
//            */
//
//}
//
//
// int main(int argc, char * argv[]) {
//
//    set_opengl_version_hints(4,5);
//    window = std::make_unique<Window>();
//    set_keys();
//    window->set_gui_func(gui_func);
//    window->set_render_func(render);
//    window->makeCurrent();
//
//    prepare_mesh(NI,NJ);
//    smoothing_precompute();
//    lsreconstruction_precompute();
//    window->run();
//
//    exit(EXIT_SUCCESS);
//}

using namespace mtao::opengl;

class MeshViewer : public mtao::opengl::Window2 {
   public:
    enum class Mode : int { Smoothing, LSReinitialization };
    Mode mode = Mode::LSReinitialization;

    float permeability = 100.0;
    float timestep = 1000.0;
    bool animate = false;
    using Vec = mtao::VectorX<GLfloat>;
    using Vec2 = mtao::Vec2f;
    Vec data;
    Vec data_original;
    Vec dx;
    Vec dy;
    Vec signs;
    Eigen::AlignedBox<float, 2> bbox;
    Eigen::SparseMatrix<float> L;

    std::array<int, 2> N{{20, 20}};
    int& NI = N[0];
    int& NJ = N[1];
    Vector2 cursor;
    mtao::vector<mtao::Vec2f> points;

    Eigen::SimplicialLDLT<Eigen::SparseMatrix<float>> solver;

    MeshViewer(const Arguments& args) : Window2(args) {
        bbox.min().setConstant(-1);
        bbox.max().setConstant(1);

        edge_drawable = new mtao::opengl::Drawable<Magnum::Shaders::Flat2D>{
            grid, _flat_shader, drawables()};
        edge_drawable->activate_triangles({});
        edge_drawable->activate_edges();
        // face_drawable = new
        // mtao::opengl::Drawable<Magnum::Shaders::VertexColor2D>{grid,_vcolor_shader,
        // drawables()};
        grid.setParent(&root());
        cursor_mesh.setParent(&scene());
        cursor_drawable = new mtao::opengl::Drawable<Magnum::Shaders::Flat2D>{
            cursor_mesh, _flat_shader, drawables()};
        cursor_mesh.setVertexBuffer(mtao::Vec2f::Zero().eval());
        cursor_drawable->data().color = 0xffffff_rgbf;
        visible_grid.setParent(&scene());
        visible_drawable = new mtao::opengl::Drawable<Magnum::Shaders::Flat2D>{
            visible_grid, _flat_shader, drawables()};
        vfield_mesh.setParent(&scene());
        _vf_viewer =
            new mtao::opengl::Drawable<mtao::opengl::VectorFieldShader<2>>{
                vfield_mesh, _vf_shader, drawables()};
        update();
    }
    void update() {
        // mtao::geometry::grid::Grid2f g(std::array<int,2>{{NI,NJ,NK}});
        auto g = mtao::geometry::grid::Grid2f::from_bbox(
            bbox, std::array<int, 2>{{NI, NJ}});

        // auto V = g.vertices();
        // mtao::RowVectorX<float> C = V.colwise()
        grid.setColorBuffer(
            mtao::eigen::vstack(g.vertices().array().pow(.5)
                                //,mtao::RowVectorX<float>::Zero(g.size())
                                ,
                                1 - g.vertices().colwise().sum().array() / 2,
                                mtao::RowVectorX<float>::Ones(g.size())));
        grid.set(g);
        {
            vfield_mesh.setVertexBuffer(g.vertices());
            mtao::ColVecs2f V = g.vertices();
            // V.array() -= .5;
            auto x = V.row(0).eval();
            V.row(0) = -V.row(1);
            V.row(1) = x;
            vfield_mesh.setVFieldBuffer(V);
        }

        cursor_drawable->deactivate();
        cursor_drawable->activate_points();
        _vf_viewer->deactivate();
        _vf_viewer->activate_points();
        {
            auto g = mtao::geometry::grid::Grid2f(std::array<int, 2>{{2, 2}});

            // auto V = g.vertices();
            // mtao::RowVectorX<float> C = V.colwise()
            visible_grid.set(g);
            visible_drawable->deactivate();
            visible_drawable->activate_edges();
        }
    }
    void do_animation() {}
    float scale = 1.0;
    void gui() override {
        if (ImGui::InputInt2("N", &NI)) {
            update();
        }
        if (ImGui::SliderFloat2("min", bbox.min().data(), -2, 2)) {
            bbox.min() = (bbox.min().array() < bbox.max().array())
                             .select(bbox.min(), bbox.max());
            update();
        }
        if (ImGui::SliderFloat2("max", bbox.max().data(), -2, 2)) {
            bbox.max() = (bbox.min().array() > bbox.max().array())
                             .select(bbox.min(), bbox.max());
            update();
        }
        if (ImGui::SliderFloat("scale", &scale, 0, 2)) {
            _vf_shader.setScale(scale);
        }
        if (edge_drawable) {
            edge_drawable->gui();
        }
        if (face_drawable) {
            face_drawable->gui();
        }
        if (ImGui::Button("Step")) {
            do_animation();
        }
        ImGui::Text("Cursor Position: (%f,%f)", cursor.x(), cursor.y());
    }
    void draw() override {
        if (animate) {
            do_animation();
        }
        Magnum::GL::Renderer::disable(
            Magnum::GL::Renderer::Feature::FaceCulling);
        Magnum::GL::Renderer::setPointSize(10.);
        Window2::draw();
    }

    void mouseMoveEvent(MouseMoveEvent& event) override {
        Window2::mouseMoveEvent(event);
        cursor = localPosition(event.position());
    }
    void mousePressEvent(MouseEvent& event) override {
        Window2::mousePressEvent(event);
        if (!ImGui::GetIO().WantCaptureMouse) {
            if (event.button() == MouseEvent::Button::Left) {
                points.emplace_back(
                    EigenIntegration::cast<mtao::Vec2f>(cursor));
                cursor_mesh.setVertexBuffer(mtao::eigen::stl2eigen(points));
                if (points.size() > 1) {
                    mtao::ColVecs2i E(2, points.size());
                    E.row(0) = mtao::RowVecXi::LinSpaced(points.size(), 0,
                                                         points.size() - 1);
                    E.row(1).rightCols(E.cols() - 1) =
                        E.row(0).leftCols(E.cols() - 1);
                    E(1, 0) = points.size() - 1;
                    cursor_mesh.setEdgeBuffer(E.cast<unsigned int>());
                    cursor_drawable->activate_edges();
                }
            }
        }
    }

   private:
    Magnum::Shaders::Flat2D _flat_shader;
    Magnum::Shaders::VertexColor2D _vcolor_shader;
    mtao::opengl::VectorFieldShader<2> _vf_shader;
    mtao::opengl::objects::Grid<2> grid;
    mtao::opengl::objects::Mesh<2> cursor_mesh;
    mtao::opengl::objects::Grid<2> visible_grid;
    mtao::opengl::objects::Mesh<2> vfield_mesh;
    mtao::opengl::Drawable<Magnum::Shaders::Flat2D>* edge_drawable = nullptr;
    mtao::opengl::Drawable<Magnum::Shaders::VertexColor2D>* face_drawable =
        nullptr;
    mtao::opengl::Drawable<Magnum::Shaders::Flat2D>* cursor_drawable = nullptr;
    mtao::opengl::Drawable<Magnum::Shaders::Flat2D>* visible_drawable = nullptr;
    mtao::opengl::Drawable<mtao::opengl::VectorFieldShader<2>>* _vf_viewer =
        nullptr;
};

MAGNUM_APPLICATION_MAIN(MeshViewer)