/* ============================================================================
 * Copyright (c) 2009-2020 BlueQuartz Software, LLC
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 *
 * 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.
 *
 * Neither the name of BlueQuartz Software, the US Air Force, nor the names of its
 * contributors may be used to endorse or promote products derived from this software
 * without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR 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.
 *
 * The code contained herein was partially funded by the following contracts:
 *    United States Air Force Prime Contract FA8650-07-D-5800
 *    United States Air Force Prime Contract FA8650-10-D-5210
 *    United States Prime Contract Navy N00173-07-C-2068
 *
 * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
#include "ApplyTransformationToGeometry.h"

#include <Eigen/Dense>

#include <QtCore/QTextStream>

#include "SIMPLib/Common/Constants.h"
#include "SIMPLib/Common/SIMPLRange.h"
#include "SIMPLib/DataContainers/DataContainerArray.h"
#include "SIMPLib/FilterParameters/AbstractFilterParametersReader.h"
#include "SIMPLib/FilterParameters/DataArraySelectionFilterParameter.h"
#include "SIMPLib/FilterParameters/DataContainerSelectionFilterParameter.h"
#include "SIMPLib/FilterParameters/DynamicTableFilterParameter.h"
#include "SIMPLib/FilterParameters/FloatFilterParameter.h"
#include "SIMPLib/FilterParameters/LinkedChoicesFilterParameter.h"
#include "SIMPLib/Geometry/EdgeGeom.h"
#include "SIMPLib/Geometry/IGeometry2D.h"
#include "SIMPLib/Geometry/IGeometry3D.h"
#include "SIMPLib/Geometry/VertexGeom.h"
#include "SIMPLib/Math/SIMPLibMath.h"
#include "SIMPLib/Utilities/ParallelDataAlgorithm.h"

#include "EbsdLib/Core/Orientation.hpp"
#include "EbsdLib/Core/OrientationTransformation.hpp"

#include "DREAM3DReview/DREAM3DReviewConstants.h"
#include "DREAM3DReview/DREAM3DReviewVersion.h"

namespace ApplyTransformationProgress
{
static size_t s_InstanceIndex = 0;
static std::map<size_t, int64_t> s_ProgressValues;
static std::map<size_t, int64_t> s_LastProgressInt;
} // namespace ApplyTransformationProgress

class ApplyTransformationToGeometryImpl
{

public:
  ApplyTransformationToGeometryImpl(ApplyTransformationToGeometry& filter, float* transformationMatrix, const SharedVertexList::Pointer verticesPtr)
  : m_Filter(filter)
  , m_TransformationMatrix(transformationMatrix)
  , m_Vertices(verticesPtr)
  {
  }
  ~ApplyTransformationToGeometryImpl() = default;

  void convert(size_t start, size_t end) const
  {
    using ProjectiveMatrix = Eigen::Matrix<float, 4, 4, Eigen::RowMajor>;
    Eigen::Map<ProjectiveMatrix> transformation(m_TransformationMatrix);

    int64_t progCounter = 0;
    int64_t totalElements = (end - start);
    int64_t progIncrement = static_cast<int64_t>(totalElements / 100);

    SharedVertexList& vertices = *(m_Vertices.get());
    for(size_t i = start; i < end; i++)
    {
      if(m_Filter.getCancel())
      {
        return;
      }
      Eigen::Vector4f position(vertices[3 * i + 0], vertices[3 * i + 1], vertices[3 * i + 2], 1);
      Eigen::Vector4f transformedPosition = transformation * position;
      vertices.setTuple(i, transformedPosition.data());

      if(progCounter > progIncrement)
      {
        m_Filter.sendThreadSafeProgressMessage(progCounter);
        progCounter = 0;
      }
      progCounter++;
    }
  }

  void operator()(const SIMPLRange& range) const
  {
    convert(range.min(), range.max());
  }

private:
  ApplyTransformationToGeometry& m_Filter;
  float* m_TransformationMatrix = nullptr;
  SharedVertexList::Pointer m_Vertices;
};

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
ApplyTransformationToGeometry::ApplyTransformationToGeometry()
{
  m_RotationAngle = 0.0f;
  m_RotationAxis[0] = 0.0f;
  m_RotationAxis[1] = 0.0f;
  m_RotationAxis[2] = 1.0f;

  m_Translation[0] = 0.0f;
  m_Translation[1] = 0.0f;
  m_Translation[2] = 0.0f;

  m_Scale[0] = 0.0f;
  m_Scale[1] = 0.0f;
  m_Scale[2] = 0.0f;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
ApplyTransformationToGeometry::~ApplyTransformationToGeometry() = default;

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void ApplyTransformationToGeometry::setupFilterParameters()
{
  FilterParameterVectorType parameters;
  {
    LinkedChoicesFilterParameter::Pointer parameter = LinkedChoicesFilterParameter::New();
    parameter->setHumanLabel("Transformation Type");
    parameter->setPropertyName("TransformationMatrixType");
    parameter->setSetterCallback(SIMPL_BIND_SETTER(ApplyTransformationToGeometry, this, TransformationMatrixType));
    parameter->setGetterCallback(SIMPL_BIND_GETTER(ApplyTransformationToGeometry, this, TransformationMatrixType));
    std::vector<QString> choices;
    choices.push_back("No Transformation");
    choices.push_back("Pre-Computed Transformation Matrix");
    choices.push_back("Manual Transformation Matrix");
    choices.push_back("Rotation");
    choices.push_back("Translation");
    choices.push_back("Scale");
    parameter->setChoices(choices);
    std::vector<QString> linkedProps = {"ComputedTransformationMatrix", "ManualTransformationMatrix", "RotationAngle", "RotationAxis", "Translation", "Scale"};
    parameter->setLinkedProperties(linkedProps);
    parameter->setEditable(false);
    parameter->setCategory(FilterParameter::Category::Parameter);
    parameters.push_back(parameter);
  }
  {
    QStringList rHeaders, cHeaders;
    std::vector<std::vector<double>> defaultTable;
    for(size_t i = 0; i < 4; i++)
    {
      std::vector<double> row(4, 0);
      row[i] = 1.0;
      defaultTable.push_back(row);
    }
    m_ManualTransformationMatrix.setTableData(defaultTable);
    parameters.push_back(SIMPL_NEW_DYN_TABLE_FP("Transformation Matrix", ManualTransformationMatrix, FilterParameter::Category::Parameter, ApplyTransformationToGeometry, 2));
  }
  parameters.push_back(SIMPL_NEW_FLOAT_FP("Rotation Angle (Degrees)", RotationAngle, FilterParameter::Category::Parameter, ApplyTransformationToGeometry, 3));
  parameters.push_back(SIMPL_NEW_FLOAT_VEC3_FP("Rotation Axis (ijk)", RotationAxis, FilterParameter::Category::Parameter, ApplyTransformationToGeometry, 3));
  parameters.push_back(SIMPL_NEW_FLOAT_VEC3_FP("Translation", Translation, FilterParameter::Category::Parameter, ApplyTransformationToGeometry, 4));
  parameters.push_back(SIMPL_NEW_FLOAT_VEC3_FP("Scale", Scale, FilterParameter::Category::Parameter, ApplyTransformationToGeometry, 5));
  DataContainerSelectionFilterParameter::RequirementType dcReq;
  IGeometry::Types geomTypes = {IGeometry::Type::Vertex, IGeometry::Type::Edge, IGeometry::Type::Triangle, IGeometry::Type::Quad, IGeometry::Type::Tetrahedral};
  dcReq.dcGeometryTypes = geomTypes;
  parameters.push_back(SIMPL_NEW_DC_SELECTION_FP("Geometry to Transform", GeometryToTransform, FilterParameter::Category::RequiredArray, ApplyTransformationToGeometry, dcReq));
  {
    DataArraySelectionFilterParameter::RequirementType dasReq =
        DataArraySelectionFilterParameter::CreateRequirement(SIMPL::TypeNames::Float, SIMPL::Defaults::AnyComponentSize, AttributeMatrix::Type::Generic, IGeometry::Type::Any);
    parameters.push_back(SIMPL_NEW_DA_SELECTION_FP("Transformation Matrix", ComputedTransformationMatrix, FilterParameter::Category::RequiredArray, ApplyTransformationToGeometry, dasReq, 1));
  }
  setFilterParameters(parameters);
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void ApplyTransformationToGeometry::readFilterParameters(AbstractFilterParametersReader* reader, int index)
{
  reader->openFilterGroup(this, index);
  setManualTransformationMatrix(reader->readDynamicTableData("ManualTransformationMatrix", getManualTransformationMatrix()));
  setComputedTransformationMatrix(reader->readDataArrayPath("ComputedTransformationMatrix", getComputedTransformationMatrix()));
  setGeometryToTransform(reader->readDataArrayPath("GeometryToTransform", getGeometryToTransform()));
  setTransformationMatrixType(reader->readValue("TransformationMatrixType", getTransformationMatrixType()));
  setRotationAxis(reader->readFloatVec3("RotationAxis", getRotationAxis()));
  setRotationAngle(reader->readValue("RotationAngle", getRotationAngle()));
  setTranslation(reader->readFloatVec3("Translation", getTranslation()));
  setScale(reader->readFloatVec3("Scale", getScale()));
  reader->closeFilterGroup();
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void ApplyTransformationToGeometry::dataCheck()
{
  clearErrorCode();
  clearWarningCode();

  IGeometry::Pointer igeom = getDataContainerArray()->getPrereqGeometryFromDataContainer<IGeometry>(this, getGeometryToTransform());

  if(getErrorCode() < 0)
  {
    return;
  }

  if(!std::dynamic_pointer_cast<IGeometry2D>(igeom) && !std::dynamic_pointer_cast<IGeometry3D>(igeom) && !std::dynamic_pointer_cast<VertexGeom>(igeom) && !std::dynamic_pointer_cast<EdgeGeom>(igeom))
  {
    QString ss =
        QObject::tr("Geometry to transform must be an unstructured geometry (Vertex, Edge, Triangle, Quadrilateral, or Tetrahedral), but the type is %1").arg(igeom->getGeometryTypeAsString());
    setErrorCondition(-702, ss);
  }

  std::vector<size_t> cDims = {4, 4};

  switch(getTransformationMatrixType())
  {
  case 0: // No-Op
  {
    QString ss = QObject::tr("No transformation has been selected, so this filter will perform no operations");
    setWarningCondition(-701, ss);
  }
  case 1: // Transformation matrix from array
  {
    m_TransformationMatrixPtr = getDataContainerArray()->getPrereqArrayFromPath<DataArray<float>>(this, getComputedTransformationMatrix(), cDims);
    if(m_TransformationMatrixPtr.lock())
    {
      m_TransformationMatrix = m_TransformationMatrixPtr.lock()->getPointer(0);
    }
    break;
  }
  case 2: // Manual transformation matrix
  {
    if(getManualTransformationMatrix().getNumRows() != 4)
    {
      QString ss = QObject::tr("Manually entered transformation matrix must have exactly 4 rows");
      setErrorCondition(-702, ss);
      return;
    }
    if(getManualTransformationMatrix().getNumCols() != 4)
    {
      QString ss = QObject::tr("Manually entered transformation matrix must have exactly 4 columns");
      setErrorCondition(-703, ss);
      return;
    }
    std::vector<std::vector<double>> tableData = getManualTransformationMatrix().getTableData();
    m_TransformationReference = FloatArrayType::CreateArray(1, cDims, "_INTERNAL_USE_ONLY_ManualTransformationMatrix", true);
    m_TransformationReference->initializeWithZeros();
    m_TransformationMatrixPtr = m_TransformationReference;
    if(m_TransformationMatrixPtr.lock())
    {
      m_TransformationMatrix = m_TransformationMatrixPtr.lock()->getPointer(0);
      for(size_t i = 0; i < tableData.size(); i++)
      {
        std::vector<double> row = tableData[i];
        for(size_t j = 0; j < row.size(); j++)
        {
          m_TransformationMatrix[4 * i + j] = static_cast<float>(row[j]);
        }
      }
    }
    break;
  }
  case 3: // Rotation via axis-angle
  {
    float rotAngle = m_RotationAngle * SIMPLib::Constants::k_PiOver180D;
    OrientationF om = OrientationTransformation::ax2om<OrientationF, OrientationF>(OrientationF(m_RotationAxis[0], m_RotationAxis[1], m_RotationAxis[2], rotAngle));

    m_TransformationReference = FloatArrayType::CreateArray(1, cDims, "_INTERNAL_USE_ONLY_ManualTransformationMatrix", true);
    m_TransformationReference->initializeWithZeros();
    m_TransformationMatrixPtr = m_TransformationReference;
    if(m_TransformationMatrixPtr.lock())
    {
      m_TransformationMatrix = m_TransformationMatrixPtr.lock()->getPointer(0);
      for(size_t i = 0; i < 3; i++)
      {
        m_TransformationMatrix[4 * i + 0] = om[3 * i + 0];
        m_TransformationMatrix[4 * i + 1] = om[3 * i + 1];
        m_TransformationMatrix[4 * i + 2] = om[3 * i + 2];
        m_TransformationMatrix[4 * i + 3] = 0.0f;
      }
      m_TransformationMatrix[4 * 3 + 3] = 1.0f;
    }
    break;
  }
  case 4: // Translation
  {
    m_TransformationReference = FloatArrayType::CreateArray(1, cDims, "_INTERNAL_USE_ONLY_ManualTransformationMatrix", true);
    m_TransformationReference->initializeWithZeros();
    m_TransformationMatrixPtr = m_TransformationReference;
    if(m_TransformationMatrixPtr.lock())
    {
      m_TransformationMatrix = m_TransformationMatrixPtr.lock()->getPointer(0);
      m_TransformationMatrix[4 * 0 + 0] = 1.0f;
      m_TransformationMatrix[4 * 1 + 1] = 1.0f;
      m_TransformationMatrix[4 * 2 + 2] = 1.0f;
      m_TransformationMatrix[4 * 0 + 3] = m_Translation[0];
      m_TransformationMatrix[4 * 1 + 3] = m_Translation[1];
      m_TransformationMatrix[4 * 2 + 3] = m_Translation[2];
      m_TransformationMatrix[4 * 3 + 3] = 1.0f;
    }
    break;
  }
  case 5: // Scale
  {
    m_TransformationReference = FloatArrayType::CreateArray(1, cDims, "_INTERNAL_USE_ONLY_ManualTransformationMatrix", true);
    m_TransformationReference->initializeWithZeros();
    m_TransformationMatrixPtr = m_TransformationReference;
    if(m_TransformationMatrixPtr.lock())
    {
      m_TransformationMatrix = m_TransformationMatrixPtr.lock()->getPointer(0);
      m_TransformationMatrix[4 * 0 + 0] = m_Scale[0];
      m_TransformationMatrix[4 * 1 + 1] = m_Scale[1];
      m_TransformationMatrix[4 * 2 + 2] = m_Scale[2];
      m_TransformationMatrix[4 * 3 + 3] = 1.0f;
    }
    break;
  }
  default: {
    QString ss = QObject::tr("Invalid selection for transformation type");
    setErrorCondition(-701, ss);
    break;
  }
  }
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void ApplyTransformationToGeometry::applyTransformation()
{

  IGeometry::Pointer igeom = getDataContainerArray()->getDataContainer(m_GeometryToTransform)->getGeometry();

  SharedVertexList::Pointer vertexList;

  if(IGeometry2D::Pointer igeom2D = std::dynamic_pointer_cast<IGeometry2D>(igeom))
  {
    vertexList = igeom2D->getVertices();
  }
  else if(IGeometry3D::Pointer igeom3D = std::dynamic_pointer_cast<IGeometry3D>(igeom))
  {
    vertexList = igeom3D->getVertices();
  }
  else if(VertexGeom::Pointer vertex = std::dynamic_pointer_cast<VertexGeom>(igeom))
  {
    vertexList = vertex->getVertices();
  }
  else if(EdgeGeom::Pointer edge = std::dynamic_pointer_cast<EdgeGeom>(igeom))
  {
    vertexList = edge->getVertices();
  }
  else
  {
    return;
  }

  using ProjectiveMatrix = Eigen::Matrix<float, 4, 4, Eigen::RowMajor>;
  Eigen::Map<ProjectiveMatrix> transformation(m_TransformationMatrix);
  m_TotalElements = vertexList->getNumberOfTuples();
  // Allow data-based parallelization
#if 1
  ParallelDataAlgorithm dataAlg;
  dataAlg.setRange(0, m_TotalElements);
  dataAlg.execute(ApplyTransformationToGeometryImpl(*this, m_TransformationMatrix, vertexList));
#else
  // THis chunk of code will FORCE single threaded mode. Don't do this unless you really mean it.
  ApplyTransformationToGeometryImpl doThis(*this, m_TransformationMatrix, vertexList);
  doThis({0, vertexList->getNumberOfTuples()});
#endif
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void ApplyTransformationToGeometry::execute()
{
  dataCheck();
  if(getErrorCode() < 0)
  {
    return;
  }
  if(getWarningCode() < 0)
  {
    return;
  }

  if(m_TransformationMatrixType == 0)
  {
    return;
  }
  // Needed for Threaded Progress Messages
  m_InstanceIndex = ++ApplyTransformationProgress::s_InstanceIndex;
  ApplyTransformationProgress::s_ProgressValues[m_InstanceIndex] = 0;
  ApplyTransformationProgress::s_LastProgressInt[m_InstanceIndex] = 0;

  applyTransformation();
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void ApplyTransformationToGeometry::sendThreadSafeProgressMessage(int64_t counter)
{
  std::lock_guard<std::mutex> guard(m_ProgressMessage_Mutex);

  int64_t& progCounter = ApplyTransformationProgress::s_ProgressValues[m_InstanceIndex];
  progCounter += counter;
  int64_t progressInt = static_cast<int64_t>((static_cast<float>(progCounter) / m_TotalElements) * 100.0f);

  int64_t progIncrement = m_TotalElements / 100;
  int64_t prog = 1;

  int64_t& lastProgressInt = ApplyTransformationProgress::s_LastProgressInt[m_InstanceIndex];

  if(progCounter > prog && lastProgressInt != progressInt)
  {
    QString ss = QObject::tr("Transforming || %1% Completed").arg(progressInt);
    notifyStatusMessage(ss);
    prog += progIncrement;
  }

  lastProgressInt = progressInt;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
AbstractFilter::Pointer ApplyTransformationToGeometry::newFilterInstance(bool copyFilterParameters) const
{
  ApplyTransformationToGeometry::Pointer filter = ApplyTransformationToGeometry::New();
  if(copyFilterParameters)
  {
    copyFilterParameterInstanceVariables(filter.get());
  }
  return filter;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString ApplyTransformationToGeometry::getCompiledLibraryName() const
{
  return DREAM3DReviewConstants::DREAM3DReviewBaseName;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString ApplyTransformationToGeometry::getBrandingString() const
{
  return "DREAM3DReview";
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString ApplyTransformationToGeometry::getFilterVersion() const
{
  QString version;
  QTextStream vStream(&version);
  vStream << DREAM3DReview::Version::Major() << "." << DREAM3DReview::Version::Minor() << "." << DREAM3DReview::Version::Patch();
  return version;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString ApplyTransformationToGeometry::getGroupName() const
{
  return DREAM3DReviewConstants::FilterGroups::DREAM3DReviewFilters;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QUuid ApplyTransformationToGeometry::getUuid() const
{
  return QUuid("{c681caf4-22f2-5885-bbc9-a0476abc72eb}");
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString ApplyTransformationToGeometry::getSubGroupName() const
{
  return DREAM3DReviewConstants::FilterSubGroups::RotationTransformationFilters;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString ApplyTransformationToGeometry::getHumanLabel() const
{
  return "Apply Transformation to Geometry";
}

// -----------------------------------------------------------------------------
ApplyTransformationToGeometry::Pointer ApplyTransformationToGeometry::NullPointer()
{
  return Pointer(static_cast<Self*>(nullptr));
}

// -----------------------------------------------------------------------------
std::shared_ptr<ApplyTransformationToGeometry> ApplyTransformationToGeometry::New()
{
  struct make_shared_enabler : public ApplyTransformationToGeometry
  {
  };
  std::shared_ptr<make_shared_enabler> val = std::make_shared<make_shared_enabler>();
  val->setupFilterParameters();
  return val;
}

// -----------------------------------------------------------------------------
QString ApplyTransformationToGeometry::getNameOfClass() const
{
  return QString("ApplyTransformationToGeometry");
}

// -----------------------------------------------------------------------------
QString ApplyTransformationToGeometry::ClassName()
{
  return QString("ApplyTransformationToGeometry");
}

// -----------------------------------------------------------------------------
void ApplyTransformationToGeometry::setManualTransformationMatrix(const DynamicTableData& value)
{
  m_ManualTransformationMatrix = value;
}

// -----------------------------------------------------------------------------
DynamicTableData ApplyTransformationToGeometry::getManualTransformationMatrix() const
{
  return m_ManualTransformationMatrix;
}

// -----------------------------------------------------------------------------
void ApplyTransformationToGeometry::setComputedTransformationMatrix(const DataArrayPath& value)
{
  m_ComputedTransformationMatrix = value;
}

// -----------------------------------------------------------------------------
DataArrayPath ApplyTransformationToGeometry::getComputedTransformationMatrix() const
{
  return m_ComputedTransformationMatrix;
}

// -----------------------------------------------------------------------------
void ApplyTransformationToGeometry::setGeometryToTransform(const DataArrayPath& value)
{
  m_GeometryToTransform = value;
}

// -----------------------------------------------------------------------------
DataArrayPath ApplyTransformationToGeometry::getGeometryToTransform() const
{
  return m_GeometryToTransform;
}

// -----------------------------------------------------------------------------
void ApplyTransformationToGeometry::setTransformationMatrixType(int value)
{
  m_TransformationMatrixType = value;
}

// -----------------------------------------------------------------------------
int ApplyTransformationToGeometry::getTransformationMatrixType() const
{
  return m_TransformationMatrixType;
}

// -----------------------------------------------------------------------------
void ApplyTransformationToGeometry::setRotationAxis(const FloatVec3Type& value)
{
  m_RotationAxis = value;
}

// -----------------------------------------------------------------------------
FloatVec3Type ApplyTransformationToGeometry::getRotationAxis() const
{
  return m_RotationAxis;
}

// -----------------------------------------------------------------------------
void ApplyTransformationToGeometry::setRotationAngle(float value)
{
  m_RotationAngle = value;
}

// -----------------------------------------------------------------------------
float ApplyTransformationToGeometry::getRotationAngle() const
{
  return m_RotationAngle;
}

// -----------------------------------------------------------------------------
void ApplyTransformationToGeometry::setTranslation(const FloatVec3Type& value)
{
  m_Translation = value;
}

// -----------------------------------------------------------------------------
FloatVec3Type ApplyTransformationToGeometry::getTranslation() const
{
  return m_Translation;
}

// -----------------------------------------------------------------------------
void ApplyTransformationToGeometry::setScale(const FloatVec3Type& value)
{
  m_Scale = value;
}

// -----------------------------------------------------------------------------
FloatVec3Type ApplyTransformationToGeometry::getScale() const
{
  return m_Scale;
}


/*******************************************************************************
*
*  Filename    : PlotLimit.cc
*  Description : Macros For plotting confidence level limits
*  Author      : Yi-Mu "Enoch" Chen [ ensc@hep1.phys.ntu.edu.tw ]
*
*******************************************************************************/
#include "ManagerUtils/Common/interface/ConfigReader.hpp"
#include "ManagerUtils/Common/interface/STLUtils.hpp"
#include "ManagerUtils/Maths/interface/Intersect.hpp"
#include "ManagerUtils/Maths/interface/Parameter.hpp"
#include "ManagerUtils/PlotUtils/interface/Common.hpp"
#include "ManagerUtils/SampleMgr/interface/SampleMgrLoader.hpp"

#include "TstarAnalysis/Common/interface/PlotStyle.hpp"
#include "TstarAnalysis/LimitCalc/interface/Common.hpp"
#include "TstarAnalysis/LimitCalc/interface/Limit.hpp"

#include <algorithm>
#include <boost/algorithm/string.hpp>
#include <boost/filesystem.hpp>
#include <boost/format.hpp>
#include <fstream>
#include <map>
#include <sstream>
#include <string>
#include <vector>

#include "TFile.h"
#include "TTree.h"

using namespace std;
using namespace mgr;

/*******************************************************************************
*   Defining plotting control flow
*******************************************************************************/
pair<Parameter, Parameter>
MakeLimitPlot( const std::string& additionaltag )
{
  const map<double, double> xsec = GetXsectionMap();

  TGraph* onesiggraph = MakeCalcGraph( xsec, additionaltag, explim, onesig_up, onesig_down );
  TGraph* twosiggraph = MakeCalcGraph( xsec, additionaltag, explim, twosig_up, twosig_down );
  TGraph* expgraph    = MakeCalcGraph( xsec, additionaltag, explim, skip, skip );
  TGraph* obsgraph    = MakeCalcGraph( xsec, additionaltag, obslim, skip, skip );
  TGraph* theorygraph = MakeTheoryGraph( xsec );

  const double y_max = mgr::GetYmax( twosiggraph, obsgraph );
  const double y_min = mgr::GetYmin( twosiggraph, obsgraph );
  const double x_max = theorygraph->GetX()[theorygraph->GetN()-1];
  const double x_min = theorygraph->GetX()[0];

  // ----- Setting Styles  --------------------------------------------------------
  tstar::SetOneSigmaStyle( onesiggraph );
  tstar::SetTwoSigmaStyle( twosiggraph );
  tstar::SetExpLimitStyle( expgraph );
  tstar::SetObsLimitStyle( obsgraph );
  tstar::SetTheoryStyle( theorygraph );

  // Making object for plotting
  TCanvas* c1 = mgr::NewCanvas();
  mgr::SetSinglePad( c1 );

  TMultiGraph* mg = new TMultiGraph();

  mg->Add( twosiggraph );
  mg->Add( onesiggraph );
  mg->Add( theorygraph );
  mg->Add( expgraph );
  if( limnamer.CheckInput( "drawdata" ) ){
    mg->Add( obsgraph );
  }

  mg->Draw( "AL3" );
  mgr::SetAxis( mg );
  mg->GetXaxis()->SetTitle( "t* Mass (GeV/c^{2})" );// MUST Be after draw!!
  mg->GetYaxis()->SetTitle( "#sigma(pp#rightarrow t*#bar{t}*) (pb)" );// https://root.cern.ch/root/roottalk/roottalk09/0078.html
  mg->GetXaxis()->SetLimits( x_min, x_max );
  mg->SetMaximum( y_max*300. );
  mg->SetMinimum( y_min*0.3 );

  // Making legend
  const double legend_x_min = 0.55;
  const double legend_y_min = 0.50;
  TLegend* l                = mgr::NewLegend( legend_x_min, legend_y_min );
  l->AddEntry( theorygraph,    "R-S Model",              "lf" );
  l->AddEntry( (TObject*)NULL, "",                       "" );
  l->AddEntry( (TObject*)NULL, "95% CL_{s} upper limit", "" );
  l->AddEntry( obsgraph,       "Observed",               "l" );
  l->AddEntry( expgraph,       "Medium expected",        "l" );
  l->AddEntry( onesiggraph,    "68% expected",           "f" );
  l->AddEntry( twosiggraph,    "95% expected",           "f" );
  l->Draw();

  // Additional titles settings
  mgr::DrawCMSLabel();
  mgr::DrawLuminosity( limnamer.GetExt<double>( "era", "Lumi" ) );

  // Writing additional text
  const string rootlabel = limnamer.GetChannelEXT( "Root Name" );
  // const string fitmethod = limnamer.GetExt<string>( "fitmethod", "Full Name" );
  // const string funcname  = limnamer.GetExt<string>( "fitfunc", "Full Name" );
  boost::format explimfmt( "Expected: %s GeV/c^{2}" );
  boost::format obslimfmt( "Observed: %s GeV/c^{2}" );

  mgr::LatexMgr latex;
  latex.SetOrigin( PLOT_X_MIN, PLOT_Y_MAX + TEXT_MARGIN/2, BOTTOM_LEFT )
  .WriteLine( rootlabel );
  // .WriteLine( fitmethod )
  // .WriteLine( funcname );

  // Cannot have Latex style spacing '\,'
  const Parameter explim_err = GetInterSect( theorygraph, onesiggraph );
  const Parameter obslim_err = GetInterSect( theorygraph, obsgraph );
  const string explim_str    = FloatingPoint( explim_err.CentralValue(), 0, false );
  const string obslim_str    = FloatingPoint( obslim_err.CentralValue(), 0, false );

  latex.SetOrigin( PLOT_X_TEXT_MAX, legend_y_min-TEXT_MARGIN, TOP_RIGHT );
  latex.WriteLine( boost::str( explimfmt % explim_str ) );
  if( limnamer.CheckInput( "drawdata" ) ){
    latex.WriteLine( boost::str( obslimfmt % obslim_str ) );
  }

  // ----- Saving and cleaning up  ------------------------------------------------
  c1->SetLogy( kTRUE );
  c1->Update();

  if( limnamer.CheckInput( "seed" ) ){
    limnamer.AddCutOptions( "seed" );
  }

  if( limnamer.CheckInput( "rMin" ) ){
    limnamer.AddCutOptions( "rMin" );
  }

  if( limnamer.CheckInput( "rMax" ) ){
    limnamer.AddCutOptions( "rMax" );
  }

  mgr::SaveToROOT( c1, limnamer.PlotRootFile(), limnamer.OptFileName( "", "limit", additionaltag ) );
  mgr::SaveToPDF( c1, limnamer.PlotFileName( "limit", additionaltag ) );
  delete onesiggraph;
  delete twosiggraph;
  delete obsgraph;
  delete expgraph;
  delete theorygraph;
  delete mg;
  delete l;
  delete c1;

  return make_pair( explim_err, obslim_err );
}

/*******************************************************************************
*   Filling out map from master file
*******************************************************************************/
map<double, double>
GetXsectionMap()
{
  const string theoryfile = limnamer.SubPackageDir() / "data/excitedtoppair13TeV.dat";
  if( !boost::filesystem::exists( theoryfile ) ){
    cerr << "Error! File containing theoretical cross section doesn't exists!" << endl;
    cerr << "Check file: " << theoryfile << endl;
    throw std::runtime_error( "File not found" );
  }

  ifstream xsec_file( theoryfile );
  string line;
  map<double, double> ans;
  double energy, mass, xsec_value;

  while( getline( xsec_file, line ) ){
    istringstream linestream( line );
    linestream >> energy >> mass >> xsec_value;
    ans[mass] = xsec_value * 1000.;
  }

  xsec_file.close();
  return ans;
}

/*******************************************************************************
*   Object generation functions
*******************************************************************************/
TGraphAsymmErrors*
MakeTheoryGraph( const map<double, double>& xsec )
{
  // Making the PDF/QCD scale error Graph objects for linear interpolation
  const vector<string> siglist = limnamer.MasterConfig().GetStaticStringList( "Signal List" );
  TGraph* errorupgraph         = new TGraph( siglist.size() );
  TGraph* errordowngraph       = new TGraph( siglist.size() );

  for( size_t i = 0; i < siglist.size(); ++i ){
    const auto& sig   = siglist.at( i );
    const double mass = GetInt( sig );

    mgr::SampleGroup sigmgr( sig, limnamer.MasterConfig() );

    const mgr::Parameter pdferr   = sigmgr.Sample().PDFUncertainty();
    const mgr::Parameter scaleerr = sigmgr.Sample().QCDScaleUncertainty();
    const mgr::Parameter toterr   = pdferr * scaleerr;

    errorupgraph->SetPoint( i, mass, toterr.RelUpperError() );
    errordowngraph->SetPoint( i, mass, toterr.RelLowerError() );
  }

  // Making the main theoretical curve line
  TGraphAsymmErrors* graph = new TGraphAsymmErrors();

  for( const auto& point : xsec ){
    const double mass = point.first;
    if( mass < 650 || mass > 1650 ){ continue; }
    const double thisxsec = point.second;
    const double xsecup   = thisxsec * errorupgraph->Eval( mass );
    const double xsecdown = thisxsec * errordowngraph->Eval( mass );

    const int newbin = graph->GetN();
    graph->SetPoint( newbin, mass, thisxsec );
    graph->SetPointError(
      newbin,
      0, 0,// No X error for now
      xsecdown,
      xsecup
      );
  }

  delete errorupgraph;
  delete errordowngraph;

  return graph;
}

/******************************************************************************/
extern const int explim      = 2;
extern const int obslim      = 5;
extern const int onesig_up   = 3;
extern const int onesig_down = 1;
extern const int twosig_up   = 4;
extern const int twosig_down = 0;
extern const int skip        = -1;

extern TGraph*
MakeCalcGraph(
  const std::map<double, double>& xsec,
  const string& additionaltag,
  const int centralentry,
  const int uperrorentry,
  const int downerrorentry
  )
{
  const vector<string> siglist = limnamer.MasterConfig().GetStaticStringList( "Signal List" );
  TGraphAsymmErrors* graph     = new TGraphAsymmErrors( siglist.size() );

  unsigned bin = 0;

  for( const auto& sig : siglist ){
    const double mass     = GetInt( sig );
    const double expxsec  = xsec.at( mass );
    const string filename = limnamer.RootFileName( "combine", sig, additionaltag );

    // Geting contents of higgs combine output file
    TFile* file = TFile::Open( filename.c_str() );
    if( !file ){
      fprintf( stderr, "Cannot open file (%s), skipping sample for %s\n",
        filename.c_str(),
        sig.c_str() );
      continue;
    }

    double temp;
    TTree* tree = ( (TTree*)file->Get( "limit" ) );
    tree->SetBranchAddress( "limit", &temp );

    // Getting the entries
    tree->GetEntry( centralentry );
    const double central = temp * expxsec;

    if( uperrorentry != skip ){ tree->GetEntry( uperrorentry ); }
    const double errorup = ( uperrorentry != skip ) ?
                           temp*expxsec - central :  0;

    if( downerrorentry != skip ){ tree->GetEntry( downerrorentry ); }
    const double errordown = ( downerrorentry != skip ) ?
                             central - temp*expxsec :  0;

    // closing file
    file->Close();

    graph->SetPoint( bin, mass, central );
    graph->SetPointError( bin, 50, 50, errordown, errorup );
    cout << boost::format( "%u %4.0lf  %8.6lf[+%8.6lf/-%8.6lf]" )
      % bin % mass % central % errorup % errordown << endl;
    ++bin;

    delete file;
  }

  return graph;
}

/*******************************************************************************
*   Function for calculating intersect points
*******************************************************************************/

Parameter
GetInterSect( const TGraph* graph, const TGraph* errgraph )
{
  double y             = 0;
  double explimit      = errgraph->GetX()[0];
  double explimit_up   = errgraph->GetX()[0];
  double explimit_down = errgraph->GetX()[0];

  for( int i = 0; i < graph->GetN()-1; ++i ){
    for( int j = 0; j < errgraph->GetN()-1; ++j ){

      const double graphmass        = graph->GetX()[i];
      const double graphmassnext    = graph->GetX()[i+1];
      const double graphval         = graph->GetY()[i];
      const double graphvalnext     = graph->GetY()[i+1];
      const double graphvalup       = graph->GetErrorYhigh( i ) + graphval;
      const double graphvalupnext   = graph->GetErrorYhigh( i+1 ) + graphvalnext;
      const double graphvaldown     = graphval - graph->GetErrorYlow( i );
      const double graphvaldownnext = graphvalnext - graph->GetErrorYlow( i+1 );

      const double errgraphmass     = errgraph->GetX()[j];
      const double errgraphmassnext = errgraph->GetX()[j+1];
      const double errgraphval      = errgraph->GetY()[j];
      const double errgraphvalnext  = errgraph->GetY()[j+1];
      const double hierr            = errgraph->GetErrorYhigh( j ) + errgraphval;
      const double hierrnext        = errgraph->GetErrorYhigh( j+1 ) + errgraphvalnext;
      const double loerr            = errgraphval - errgraph->GetErrorYlow( j );
      const double loerrnext        = errgraphvalnext - errgraph->GetErrorYlow( j+1 );

      // Central value intersect
      Intersect(
        graphmass, graphval,
        graphmassnext, graphvalnext,
        errgraphmass, errgraphval,
        errgraphmassnext, errgraphvalnext,
        explimit, y
        );

      // Lower theretical band intersect with upper limit band
      Intersect(
        graphmass, graphvaldown,
        graphmassnext, graphvaldownnext,
        errgraphmass, hierr,
        errgraphmassnext, hierrnext,
        explimit_down, y
        );

      // Upper theoretical band intersec with lower limit band
      Intersect(
        graphmass, graphvalup,
        graphmassnext, graphvalupnext,
        errgraphmass, loerr,
        errgraphmassnext, loerrnext,
        explimit_up, y
        );
    }
  }

  return Parameter( explimit, explimit_up - explimit, explimit - explimit_down );
}


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file portfolio/builders/vanillaoption.hpp
    \brief Abstract engine builders for European and American Options
    \ingroup builders
*/

#pragma once

#include <boost/make_shared.hpp>
#include <ored/portfolio/builders/cachingenginebuilder.hpp>
#include <ored/portfolio/enginefactory.hpp>
#include <ored/utilities/indexparser.hpp>
#include <ored/utilities/log.hpp>
#include <ored/utilities/to_string.hpp>
#include <ql/pricingengines/vanilla/analyticeuropeanengine.hpp>
#include <ql/pricingengines/vanilla/fdblackscholesvanillaengine.hpp>
#include <ql/processes/blackscholesprocess.hpp>
#include <ql/version.hpp>
#include <qle/pricingengines/analyticcashsettledeuropeanengine.hpp>
#include <qle/pricingengines/baroneadesiwhaleyengine.hpp>
#include <qle/termstructures/blackmonotonevarvoltermstructure.hpp>
#include <qle/termstructures/pricetermstructureadapter.hpp>

namespace ore {
namespace data {

template <class T, typename... Args> class CachingOptionEngineBuilder : public CachingPricingEngineBuilder<T, Args...> {
public:
    CachingOptionEngineBuilder(const string& model, const string& engine, const set<string>& tradeTypes,
                               const AssetClass& assetClass)
        : CachingPricingEngineBuilder<T, Args...>(model, engine, tradeTypes), assetClass_(assetClass) {}

protected:
    boost::shared_ptr<GeneralizedBlackScholesProcess> getBlackScholesProcess(const string& assetName,
                                                                             const Currency& ccy,
                                                                             const AssetClass& assetClassUnderlying,
                                                                             const std::vector<Time>& timePoints = {}) {

        using VVTS = QuantExt::BlackMonotoneVarVolTermStructure;
        string config = this->configuration(ore::data::MarketContext::pricing);

        if (assetClassUnderlying == AssetClass::EQ) {
            Handle<BlackVolTermStructure> vol = this->market_->equityVol(assetName, config);
            if (!timePoints.empty()) {
                vol = Handle<BlackVolTermStructure>(boost::make_shared<VVTS>(vol, timePoints));
                vol->enableExtrapolation();
            }
            return boost::make_shared<GeneralizedBlackScholesProcess>(
                this->market_->equitySpot(assetName, config), this->market_->equityDividendCurve(assetName, config),
                this->market_->equityForecastCurve(assetName, config), vol);

        } else if (assetClassUnderlying == AssetClass::FX) {
            const string& ccyPairCode = assetName + ccy.code();
            Handle<BlackVolTermStructure> vol = this->market_->fxVol(ccyPairCode, config);
            if (!timePoints.empty()) {
                vol = Handle<BlackVolTermStructure>(boost::make_shared<VVTS>(vol, timePoints));
                vol->enableExtrapolation();
            }
            return boost::make_shared<GeneralizedBlackScholesProcess>(
                this->market_->fxSpot(ccyPairCode, config), this->market_->discountCurve(assetName, config),
                this->market_->discountCurve(ccy.code(), config), vol);

        } else if (assetClassUnderlying == AssetClass::COM) {

            Handle<BlackVolTermStructure> vol = this->market_->commodityVolatility(assetName, config);
            if (!timePoints.empty()) {
                vol = Handle<BlackVolTermStructure>(boost::make_shared<VVTS>(vol, timePoints));
                vol->enableExtrapolation();
            }

            // Create the commodity convenience yield curve for the process
            Handle<QuantExt::PriceTermStructure> priceCurve = this->market_->commodityPriceCurve(assetName, config);
            Handle<Quote> commoditySpot(boost::make_shared<QuantExt::DerivedPriceQuote>(priceCurve));
            Handle<YieldTermStructure> discount = this->market_->discountCurve(ccy.code(), config);
            Handle<YieldTermStructure> yield(
                boost::make_shared<QuantExt::PriceTermStructureAdapter>(*priceCurve, *discount));
            yield->enableExtrapolation();

            return boost::make_shared<GeneralizedBlackScholesProcess>(commoditySpot, yield, discount, vol);

        } else {
            QL_FAIL("Asset class of " << (int)assetClassUnderlying << " not recognized.");
        }
    }
    AssetClass assetClass_;
};

//! Abstract Engine Builder for Vanilla Options
/*! Pricing engines are cached by asset/currency

    \ingroup builders
 */
class VanillaOptionEngineBuilder
    : public CachingOptionEngineBuilder<string, const string&, const Currency&, const AssetClass&, const Date&> {
public:
    VanillaOptionEngineBuilder(const string& model, const string& engine, const set<string>& tradeTypes,
                               const AssetClass& assetClass, const Date& expiryDate)
        : CachingOptionEngineBuilder(model, engine, tradeTypes, assetClass), expiryDate_(expiryDate) {}

    boost::shared_ptr<PricingEngine> engine(const string& assetName, const Currency& ccy, const Date& expiryDate) {
        return CachingPricingEngineBuilder<string, const string&, const Currency&, const AssetClass&,
                                           const Date&>::engine(assetName, ccy, assetClass_, expiryDate);
    }

    boost::shared_ptr<PricingEngine> engine(const Currency& ccy1, const Currency& ccy2, const Date& expiryDate) {
        return CachingPricingEngineBuilder<string, const string&, const Currency&, const AssetClass&,
                                           const Date&>::engine(ccy1.code(), ccy2, assetClass_, expiryDate);
    }

protected:
    virtual string keyImpl(const string& assetName, const Currency& ccy, const AssetClass& assetClassUnderlying,
                           const Date& expiryDate) override {
        return assetName + "/" + ccy.code() + "/" + to_string(expiryDate);
    }

    Date expiryDate_;
};

//! Abstract Engine Builder for European Vanilla Options
/*! Pricing engines are cached by asset/currency

    \ingroup builders
 */
class EuropeanOptionEngineBuilder : public VanillaOptionEngineBuilder {
public:
    EuropeanOptionEngineBuilder(const string& model, const set<string>& tradeTypes, const AssetClass& assetClass)
        : VanillaOptionEngineBuilder(model, "AnalyticEuropeanEngine", tradeTypes, assetClass, Date()) {}

protected:
    virtual boost::shared_ptr<PricingEngine> engineImpl(const string& assetName, const Currency& ccy,
                                                        const AssetClass& assetClassUnderlying,
                                                        const Date& expiryDate) override {
        string key = keyImpl(assetName, ccy, assetClassUnderlying, expiryDate);
        boost::shared_ptr<GeneralizedBlackScholesProcess> gbsp =
            getBlackScholesProcess(assetName, ccy, assetClassUnderlying);
        Handle<YieldTermStructure> discountCurve =
            market_->discountCurve(ccy.code(), configuration(MarketContext::pricing));
        return boost::make_shared<QuantLib::AnalyticEuropeanEngine>(gbsp, discountCurve);
    }
};

/*! European cash-settled option engine builder
    \ingroup builders
 */
class EuropeanCSOptionEngineBuilder : public VanillaOptionEngineBuilder {
public:
    EuropeanCSOptionEngineBuilder(const string& model, const set<string>& tradeTypes, const AssetClass& assetClass)
        : VanillaOptionEngineBuilder(model, "AnalyticCashSettledEuropeanEngine", tradeTypes, assetClass, Date()) {}

protected:
    virtual boost::shared_ptr<PricingEngine> engineImpl(const string& assetName, const Currency& ccy,
                                                        const AssetClass& assetClassUnderlying,
                                                        const Date& expiryDate) override {
        string key = keyImpl(assetName, ccy, assetClassUnderlying, expiryDate);
        boost::shared_ptr<GeneralizedBlackScholesProcess> gbsp =
            getBlackScholesProcess(assetName, ccy, assetClassUnderlying);
        Handle<YieldTermStructure> discountCurve =
            market_->discountCurve(ccy.code(), configuration(MarketContext::pricing));
        return boost::make_shared<QuantExt::AnalyticCashSettledEuropeanEngine>(gbsp, discountCurve);
    }
};

//! Abstract Engine Builder for American Vanilla Options
/*! Pricing engines are cached by asset/currency

    \ingroup builders
 */
class AmericanOptionEngineBuilder : public VanillaOptionEngineBuilder {
public:
    AmericanOptionEngineBuilder(const string& model, const string& engine, const set<string>& tradeTypes,
                                const AssetClass& assetClass, const Date& expiryDate)
        : VanillaOptionEngineBuilder(model, engine, tradeTypes, assetClass, expiryDate) {}
};

//! Abstract Engine Builder for American Vanilla Options using Finite Difference Method
/*! Pricing engines are cached by asset/currency

    \ingroup builders
 */
class AmericanOptionFDEngineBuilder : public AmericanOptionEngineBuilder {
public:
    AmericanOptionFDEngineBuilder(const string& model, const set<string>& tradeTypes, const AssetClass& assetClass,
                                  const Date& expiryDate)
        : AmericanOptionEngineBuilder(model, "FdBlackScholesVanillaEngine", tradeTypes, assetClass, expiryDate) {}

protected:
    virtual boost::shared_ptr<PricingEngine> engineImpl(const string& assetName, const Currency& ccy,
                                                        const AssetClass& assetClass, const Date& expiryDate) override {
        // We follow the way FdBlackScholesBarrierEngine determines maturity for time grid generation
        Handle<YieldTermStructure> riskFreeRate =
            market_->discountCurve(ccy.code(), configuration(ore::data::MarketContext::pricing));
        Time expiry = riskFreeRate->dayCounter().yearFraction(riskFreeRate->referenceDate(), expiryDate);
        QL_REQUIRE(expiry > 0.0, "FdBlackScholesVanillaEngine expects a positive time to expiry but got "
                                     << expiry << " for an expiry date " << io::iso_date(expiryDate) << ".");

        FdmSchemeDesc scheme = parseFdmSchemeDesc(engineParameter("Scheme"));
        Size tGrid = (Size)(ore::data::parseInteger(engineParameter("TimeGridPerYear")) * expiry);
        Size xGrid = ore::data::parseInteger(engineParameter("XGrid"));
        Size dampingSteps = ore::data::parseInteger(engineParameter("DampingSteps"));
        bool monotoneVar = ore::data::parseBool(engineParameter("EnforceMonotoneVariance", "", false, "true"));

        boost::shared_ptr<GeneralizedBlackScholesProcess> gbsp;

        if (monotoneVar) {
            // Replicate the construction of time grid in FiniteDifferenceModel::rollbackImpl
            // This time grid is required to build a BlackMonotoneVarVolTermStructure which
            // ensures monotonic variance along the time grid
            const Size totalSteps = tGrid + dampingSteps;
            std::vector<Time> timePoints(totalSteps + 1);
            Array timePointsArray(totalSteps, expiry, -expiry / totalSteps);
            timePoints[0] = 0.0;
            for (Size i = 0; i < totalSteps; i++)
                timePoints[timePoints.size() - i - 1] = timePointsArray[i];
            timePoints.insert(std::upper_bound(timePoints.begin(), timePoints.end(), 0.99 / 365), 0.99 / 365);
            gbsp = getBlackScholesProcess(assetName, ccy, assetClass, timePoints);
        } else {
            gbsp = getBlackScholesProcess(assetName, ccy, assetClass);
        }
        return boost::make_shared<FdBlackScholesVanillaEngine>(gbsp, tGrid, xGrid, dampingSteps, scheme);
    }
};

//! Abstract Engine Builder for American Vanilla Options using Barone Adesi Whaley Approximation
/*! Pricing engines are cached by asset/currency

    \ingroup builders
 */
class AmericanOptionBAWEngineBuilder : public AmericanOptionEngineBuilder {
public:
    AmericanOptionBAWEngineBuilder(const string& model, const set<string>& tradeTypes, const AssetClass& assetClass)
        : AmericanOptionEngineBuilder(model, "BaroneAdesiWhaleyApproximationEngine", tradeTypes, assetClass, Date()) {}

protected:
    virtual boost::shared_ptr<PricingEngine> engineImpl(const string& assetName, const Currency& ccy,
                                                        const AssetClass& assetClass, const Date& expiryDate) override {
        boost::shared_ptr<GeneralizedBlackScholesProcess> gbsp = getBlackScholesProcess(assetName, ccy, assetClass);
        return boost::make_shared<QuantExt::BaroneAdesiWhaleyApproximationEngine>(gbsp);
    }
};

} // namespace data
} // namespace ore


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Klothakis, A. and Nikolos, I., “Modeling of Rarefied Hypersonic Flows Using the Massively
 *        Parallel DSMC Kernel “SPARTA”,” in 8th GRACM International Congress on Computational Mechanics,
 *        Volos, Greece, July 2015.
 *      Dirkx, D. and Mooij, E., Conceptual Shape Optimization of Entry Vehicles. Springer, 2017.
 */

#include <boost/make_shared.hpp>
#include <boost/assign/list_of.hpp>

#include <Eigen/Geometry>

#include "Tudat/InputOutput/basicInputOutput.h"
#include "Tudat/InputOutput/matrixTextFileReader.h"
#include "Tudat/InputOutput/spartaDataReader.h"
#include "Tudat/InputOutput/spartaInputOutput.h"

#include "Tudat/Astrodynamics/Aerodynamics/rarefiedFlowAnalysis.h"

namespace tudat
{

namespace aerodynamics
{

using namespace unit_conversions;
using namespace input_output;

//! Returns default values of altitude for use in RarefiedFlowAnalysis.
std::vector< double > getDefaultRarefiedFlowAltitudePoints(
        const std::string& targetPlanet )
{
    std::vector< double > altitudePoints;

    // Set default points for Earth.
    if ( targetPlanet == "Earth" )
    {
        altitudePoints.resize( 5 );
        altitudePoints[ 0 ] = 225.0e3;
        altitudePoints[ 1 ] = 250.0e3;
        altitudePoints[ 2 ] = 300.0e3;
        altitudePoints[ 3 ] = 400.0e3;
        altitudePoints[ 4 ] = 600.0;
    }
    // Set default points for Mars.
    else if ( targetPlanet == "Mars" )
    {
        altitudePoints.resize( 5 );
        altitudePoints[ 0 ] = 125.0e3;
        altitudePoints[ 1 ] = 150.0e3;
        altitudePoints[ 2 ] = 200.0e3;
        altitudePoints[ 3 ] = 300.0e3;
        altitudePoints[ 4 ] = 500.0e3;
    }
    // Give error otherwise.
    else
    {
        throw std::runtime_error( "Error in altitude range selection for SPARTA simulation. "
                                  "Planet not supported." );
    }
    return altitudePoints;
}

//! Returns default values of Mach number for use in RarefiedFlowAnalysis.
std::vector< double > getDefaultRarefiedFlowMachPoints(
        const std::string& machRegime )
{
    std::vector< double > machPoints;

    // Set default points for full hypersonic analysis.
    if ( machRegime == "Full" )
    {
        machPoints.resize( 6 );

        machPoints[ 0 ] = 3.0;
        machPoints[ 1 ] = 4.0;
        machPoints[ 2 ] = 5.0;
        machPoints[ 3 ] = 8.0;
        machPoints[ 4 ] = 10.0;
        machPoints[ 5 ] = 20.0;
    }
    // Set default points for low hypersonic analysis.
    else if ( machRegime == "Low" )
    {
        machPoints.resize( 5 );
        machPoints[ 0 ] = 3.0;
        machPoints[ 1 ] = 4.0;
        machPoints[ 2 ] = 5.0;
        machPoints[ 3 ] = 8.0;
        machPoints[ 4 ] = 10.0;
    }
    // Set default points for high hypersonic analysis.
    else if ( machRegime == "High" )
    {
        machPoints.resize( 4 );
        machPoints[ 0 ] = 5.0;
        machPoints[ 1 ] = 8.0;
        machPoints[ 2 ] = 10.0;
        machPoints[ 3 ] = 20.0;
    }
    return machPoints;
}

//! Returns default values of angle of attack for use in RarefiedFlowAnalysis.
std::vector< double > getDefaultRarefiedFlowAngleOfAttackPoints(
        const std::string& angleOfAttackRegime )
{
    std::vector< double > angleOfAttackPoints;

    // Set default angles of attack
    double a = - 35;
    while ( a <= 35 )
    {
        angleOfAttackPoints.push_back( convertDegreesToRadians( a ) );
        a += 5;
    }

    // Add extra points if required
    if ( angleOfAttackRegime == "Full" )
    {
        std::vector< double > frontExtension = { convertDegreesToRadians( -85.0 ),
                                                 convertDegreesToRadians( -70.0 ),
                                                 convertDegreesToRadians( -55.0 ),
                                                 convertDegreesToRadians( -40.0 ) };
        std::vector< double > rearExtension = { convertDegreesToRadians( 40.0 ),
                                                convertDegreesToRadians( 55.0 ),
                                                convertDegreesToRadians( 70.0 ),
                                                convertDegreesToRadians( 85.0 ) };
        angleOfAttackPoints.insert( angleOfAttackPoints.begin( ), frontExtension.begin( ), frontExtension.end( ) );
        angleOfAttackPoints.insert( angleOfAttackPoints.end( ), rearExtension.begin( ), rearExtension.end( ) );
    }
    return angleOfAttackPoints;
}

//! Function to sort the rows of a matrix, based on the specified column and specified order.
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > sortMatrixRows(
        const Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor >& matrixToBeSorted,
        const int referenceColumn, const bool descendingOrder )
{
    // Declare eventual output vector
    Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > sortedMatrix;
    sortedMatrix.resizeLike( matrixToBeSorted );

    // Loop over rows and assign to new matrix
    Eigen::Matrix< double, 1, Eigen::Dynamic, Eigen::RowMajor > currentRow;
    for ( unsigned int i = 0; i < matrixToBeSorted.rows( ); i++ )
    {
        // Retrieve each row in 'scrambled order' and assign it to the new matrix
        currentRow = matrixToBeSorted.row( i );

        // Based on requested order
        if ( descendingOrder )
        {
            sortedMatrix.row( matrixToBeSorted.rows( ) - currentRow[ referenceColumn ] ) = currentRow;
            // no (- 1)'s are needed since both are defined starting from 1
        }
        else
        {
            sortedMatrix.row( currentRow[ referenceColumn ] - 1 ) = currentRow;
        }
    }

    // Give output
    return sortedMatrix;
}

//! Default constructor.
RarefiedFlowAnalysis::RarefiedFlowAnalysis(
        const std::string& SPARTAExecutable,
        const std::vector< std::vector< double > >& dataPointsOfIndependentVariables,
        boost::shared_ptr< TabulatedAtmosphere > atmosphereModel,
        const std::string& simulationGases,
        const std::string& geometryFileUser,
        const double referenceArea,
        const double referenceLength,
        const int referenceAxis,
        const Eigen::Vector3d& momentReferencePoint,
        const double gridSpacing,
        const double simulatedParticlesPerCell,
        const double wallTemperature,
        const double accommodationCoefficient,
        const bool printProgressInCommandWindow,
        const std::string MPIExecutable,
        const unsigned int numberOfCores ) :
    AerodynamicCoefficientGenerator< 3, 6 >(
          dataPointsOfIndependentVariables, referenceLength, referenceArea, referenceLength,
          momentReferencePoint,
          boost::assign::list_of( altitude_dependent )( mach_number_dependent )( angle_of_attack_dependent ),
          true, true ),
      SPARTAExecutable_( SPARTAExecutable ),simulationGases_( simulationGases ), referenceAxis_( referenceAxis ),
      gridSpacing_( gridSpacing ), simulatedParticlesPerCell_( simulatedParticlesPerCell ),
      wallTemperature_( wallTemperature ), accommodationCoefficient_( accommodationCoefficient ),
      printProgressInCommandWindow_( printProgressInCommandWindow ), MPIExecutable_( MPIExecutable ),
      numberOfCores_( numberOfCores ), referenceDimension_( static_cast< unsigned int >( referenceAxis_ ) )
{
    // Check inputs
    if ( referenceDimension_ > 2 )
    {
        throw std::runtime_error( "Error in SPARTA rarefied flow analysis. Reference axis makes "
                                  "no sense for a universe with 3 spacial dimensions (i.e., our universe). "
                                  "Note that the first dimension is identified with 0." );
    }

    // Analyze vehicle geometry
    analyzeGeometryFile( geometryFileUser );

    // Find atmospheric conditions based on altitude
    for ( unsigned int h = 0; h < dataPointsOfIndependentVariables_.at( 0 ).size( ); h++ )
    {
        atmosphericConditions_[ density_index ].push_back(
                    atmosphereModel->getDensity( dataPointsOfIndependentVariables_.at( 0 ).at( h ) ) );
        atmosphericConditions_[ pressure_index ].push_back(
                    atmosphereModel->getPressure( dataPointsOfIndependentVariables_.at( 0 ).at( h ) ) );
        atmosphericConditions_[ temperature_index ].push_back(
                    atmosphereModel->getTemperature( dataPointsOfIndependentVariables_.at( 0 ).at( h ) ) );
        atmosphericConditions_[ speed_of_sound_index ].push_back(
                    atmosphereModel->getSpeedOfSound( dataPointsOfIndependentVariables_.at( 0 ).at( h ) ) );
        atmosphericConditions_[ number_density_index ].push_back(
                    tudat::physical_constants::AVOGADRO_CONSTANT * atmosphericConditions_[ density_index ].at( h ) /
                    atmosphereModel->getMolarMass( dataPointsOfIndependentVariables_.at( 0 ).at( h ) ) );
    }

    // Get simulation conditions
    getSimulationConditions( );

    // Read SPARTA input template
    inputTemplate_ = readSpartaInputFileTemplate( );

    // Copy input shape file to default name
    std::string commandString = "cp " + geometryFileUser + " " + getSpartaInternalGeometryFile( );
    std::system( commandString.c_str( ) );

    // Run SPARTA simulation
    generateCoefficients( );

    // Create interpolator object
    createInterpolator( );
}

//! Open and read geometry file for SPARTA simulation.
void RarefiedFlowAnalysis::analyzeGeometryFile( const std::string& geometryFileUser )
{
    // Extract information on vehicle geometry
    std::pair< Eigen::Matrix< double, Eigen::Dynamic, 3 >, Eigen::Matrix< int, Eigen::Dynamic, 3 > >
            geometryData = readSpartaGeometryFile( geometryFileUser );
    shapePoints_ = geometryData.first;
    shapeTriangles_ = geometryData.second;
    numberOfPoints_ = shapePoints_.rows( );
    numberOfTriangles_ = shapeTriangles_.rows( );

    // Get maximum and minimum values in each dimension
    maximumDimensions_ = shapePoints_.colwise( ).maxCoeff( );
    minimumDimensions_ = shapePoints_.colwise( ).minCoeff( );

    // Compute normal to surface elements, area of surface elements and moment arm values
    Eigen::Matrix3d currentVertices;
    Eigen::Vector3d currentNormal;
    Eigen::Vector3d currentCentroid;
    double currentNormalNorm;
    elementSurfaceNormal_.resize( 3, numberOfTriangles_ );
    elementSurfaceArea_.resize( 1, numberOfTriangles_ );
    elementMomentArm_.resize( 3, numberOfTriangles_ );
    for ( int i = 0; i < numberOfTriangles_; i++ )
    {
        // Compute properties of current surface element
        for ( unsigned int j = 0; j < 3; j++ )
        {
            currentVertices.row( j ) = shapePoints_.row( shapeTriangles_( i, j ) - 1 );
        }
        currentNormal = ( currentVertices.row( 1 ) - currentVertices.row( 0 ) ).cross(
                    currentVertices.row( 2 ) - currentVertices.row( 0 ) );
        currentNormalNorm = currentNormal.norm( );
        currentCentroid = currentVertices.colwise( ).sum( ) / 3.0;

        // Find normal, area and distance to reference point
        elementSurfaceNormal_.col( i ) = currentNormal / currentNormalNorm;
        elementSurfaceArea_( i ) = 0.5 * currentNormalNorm;
        elementMomentArm_.col( i ) = currentCentroid - momentReferencePoint_;
    }

    // Compute cross-sectional area
    for ( unsigned int i = 0; i < 3; i++ )
    {
        shapeCrossSectionalArea_( i ) =
                0.5 * elementSurfaceNormal_.row( i ).cwiseAbs( ).dot( elementSurfaceArea_ );
    }

    // Check consistency with input dimensions
    const double tolerance = 1e-5;
    if ( std::fabs( shapeCrossSectionalArea_( referenceDimension_ ) - referenceArea_ ) > tolerance )
    {
        throw std::runtime_error( "Error in SPARTA geometry file. Input reference area does not match the "
                                  "combination of reference axis and geometry. Note that the first dimension "
                                  "is identified with 0. Tolerance set to: " + std::to_string( tolerance ) );
    }
}

//! Retrieve simulation conditions based on input and geometry.
void RarefiedFlowAnalysis::getSimulationConditions( )
{
    // Simulation boundary and grid
    for ( unsigned int i = 0; i < 3; i++ )
    {
        simulationBoundaries_( 2 * i ) = minimumDimensions_( i ) + 0.5 * minimumDimensions_.minCoeff( ); // add extra space around shape
        simulationBoundaries_( 2 * i + 1 ) = maximumDimensions_( i ) + 0.5 * maximumDimensions_.maxCoeff( ); // add extra space around shape
        if ( i == referenceDimension_ )
        {
            simulationBoundaries_( 2 * i ) -= 1.0; // add extra space along axis of velocity
            simulationBoundaries_( 2 * i + 1 ) += 1.0; // add extra space along axis of velocity
        }
        simulationGrid_( i ) = simulationBoundaries_( 2 * i + 1 ) - simulationBoundaries_( 2 * i );
    }
    simulationGrid_ /= gridSpacing_;

    // Convert molecular speed ratio to stream velocity and compute simulation time step and ratio of real to simulated variables
    freeStreamVelocities_.resize( dataPointsOfIndependentVariables_.at( 0 ).size( ), dataPointsOfIndependentVariables_.at( 1 ).size( ) );
    simulationTimeStep_.resize( dataPointsOfIndependentVariables_.at( 0 ).size( ), dataPointsOfIndependentVariables_.at( 1 ).size( ) );
    ratioOfRealToSimulatedParticles_.resize( dataPointsOfIndependentVariables_.at( 0 ).size( ), 1 );
    double simulationBoxLengthAlongReferenceAxis = ( simulationBoundaries_( 2 * referenceDimension_ + 1 ) -
                                                     simulationBoundaries_( 2 * referenceDimension_ ) );
    for ( unsigned int h = 0; h < dataPointsOfIndependentVariables_.at( 0 ).size( ); h++ )
    {
        for ( unsigned int m = 0; m < dataPointsOfIndependentVariables_.at( 1 ).size( ); m++ )
        {
            freeStreamVelocities_( h, m ) = dataPointsOfIndependentVariables_.at( 1 ).at( m ) *
                    atmosphericConditions_[ speed_of_sound_index ].at( h );
            simulationTimeStep_( h, m ) = 0.1 * simulationBoxLengthAlongReferenceAxis /
                    freeStreamVelocities_( h, m );
            // time step is taken as time it takes for a particle to travel for 10 % of the box
        }
        ratioOfRealToSimulatedParticles_( h ) = atmosphericConditions_[ number_density_index ].at( h ) *
                std::pow( gridSpacing_, 3 ) / simulatedParticlesPerCell_;
    }
}

//! Generate aerodynamic database.
void RarefiedFlowAnalysis::generateCoefficients( )
{
    // Inform user on progress
    std::cout << "Initiating SPARTA simulation. This may take a while." << std::endl;

    // Generate command string for SPARTA
    std::string runSPARTACommandString = "cd " + getSpartaDataPath( ) + "; ";
    if ( MPIExecutable_ != "" )
    {
        if ( numberOfCores_ < 1 )
        {
            throw std::runtime_error( "Error in SPARTA rarefied flow analysis. Number of cores needs to be "
                                      "an integer value larger or equal to one." );
        }
        runSPARTACommandString = runSPARTACommandString + MPIExecutable_ +
                " -np " + std::to_string( numberOfCores_ ) + " ";
    }
    runSPARTACommandString = runSPARTACommandString + SPARTAExecutable_ + " -echo log ";
    if ( !printProgressInCommandWindow_ )
    {
        runSPARTACommandString = runSPARTACommandString + "-screen none ";
    }
    runSPARTACommandString = runSPARTACommandString + "-in " + getSpartaInputFile( );

    // Predefine variables
    Eigen::Vector3d velocityVector;
    std::string temporaryOutputFile = getSpartaOutputPath( ) + "/coeff";
    std::vector< std::string > outputFileExtensions = { ".1000" };//{ ".400", ".600", ".800", ".1000" };
    Eigen::Matrix< double, Eigen::Dynamic, 7, Eigen::RowMajor > outputMatrix;
    Eigen::Matrix< double, 3, Eigen::Dynamic > meanPressureValues;
    Eigen::Matrix< double, 3, Eigen::Dynamic > meanShearValues;

    // Loop over simulation parameters and run SPARTA
    int systemStatus;
    // Loop over altitude
    for ( unsigned int h = 0; h < dataPointsOfIndependentVariables_.at( 0 ).size( ); h++ )
    {
        // Inform user on progress
        std::cout << std::endl << "Altitude: "
                  << dataPointsOfIndependentVariables_.at( 0 ).at( h ) / 1e3
                  << " km" << std::endl;

        // Loop over Mach numbers
        for ( unsigned int m = 0; m < dataPointsOfIndependentVariables_.at( 1 ).size( ); m++ )
        {
            // Inform user on progress
            std::cout << "Mach number: "
                      << dataPointsOfIndependentVariables_.at( 1 ).at( m )
                      << std::endl;

            // Loop over angles of attack
            for ( unsigned int a = 0; a < dataPointsOfIndependentVariables_.at( 2 ).size( ); a++ )
            {
                // Inform user on progress
                std::cout << "Angle of attack: "
                          << convertRadiansToDegrees( dataPointsOfIndependentVariables_.at( 2 ).at( a ) )
                          << " deg" << std::endl;

                // Get velocity vector
                velocityVector = Eigen::Vector3d::Zero( );
                velocityVector( referenceDimension_ ) = ( ( referenceAxis_ >= 0 ) ? - 1.0 : 1.0 ) *
                        freeStreamVelocities_( h, m );

                // Print to file
                FILE * fileIdentifier = std::fopen( getSpartaInputFile( ).c_str( ), "w" );
                std::fprintf( fileIdentifier, inputTemplate_.c_str( ),
                              simulationBoundaries_( 0 ), simulationBoundaries_( 1 ), simulationBoundaries_( 2 ),
                              simulationBoundaries_( 3 ), simulationBoundaries_( 4 ), simulationBoundaries_( 5 ),
                              simulationGrid_( 0 ), simulationGrid_( 1 ), simulationGrid_( 2 ),
                              atmosphericConditions_[ number_density_index ].at( h ), ratioOfRealToSimulatedParticles_( h ),
                              simulationGases_.c_str( ),
                              simulationGases_.c_str( ), velocityVector( 0 ), velocityVector( 1 ), velocityVector( 2 ),
                              simulationGases_.c_str( ), atmosphericConditions_[ temperature_index ].at( h ),
                              convertRadiansToDegrees( dataPointsOfIndependentVariables_.at( 2 ).at( a ) ),
                              wallTemperature_, accommodationCoefficient_,
                              simulationTimeStep_( h, m ),
                              getSpartaOutputDirectory( ).c_str( ) );
                std::fclose( fileIdentifier );

                // Run SPARTA
                systemStatus = std::system( runSPARTACommandString.c_str( ) );
                if ( systemStatus != 0 )
                {
                    throw std::runtime_error( "Error in SPARTA rarefied flow analysis. "
                                              "SPARTA Simulation failed. See the log.sparta file in "
                                              "Tudat/External/SPARTA/ for more details." );
                }

                // Loop over angles of attack
                meanPressureValues.resize( 3, numberOfTriangles_ );
                meanShearValues.resize( 3, numberOfTriangles_ );

                // Read output files and compute mean pressure and shear force values
                meanPressureValues.setZero( );
                meanShearValues.setZero( );
                for ( unsigned int i = 0; i < outputFileExtensions.size( ); i++ )
                {
                    outputMatrix = readMatrixFromFile( temporaryOutputFile + outputFileExtensions.at( i ), "\t ;,", "%", 9 );
                    outputMatrix = sortMatrixRows( outputMatrix, 0 );
                    for ( unsigned int j = 0; j < 3; j++ )
                    {
                        meanPressureValues.row( j ) += outputMatrix.col( j + 1 ).transpose( );
                        meanShearValues.row( j ) += outputMatrix.col( j + 4 ).transpose( );
                    }
                }
                meanPressureValues /= outputFileExtensions.size( );
                meanShearValues /= outputFileExtensions.size( );

                // Convert pressure and shear forces to aerodynamic coefficients
                aerodynamicCoefficients_[ h ][ m ][ a ] = computeAerodynamicCoefficientsFromPressureShearForces(
                            meanPressureValues,
                            meanShearValues,
                            atmosphericConditions_[ density_index ].at( h ),
                            atmosphericConditions_[ pressure_index ].at( h ),
                            freeStreamVelocities_( h, m ),
                            elementSurfaceNormal_,
                            elementSurfaceArea_,
                            elementMomentArm_,
                            referenceArea_,
                            referenceLength_ );

                // Clean up results folder
                std::string commandString = "rm " + getSpartaOutputPath( ) + "/coeff.*";
                std::system( commandString.c_str( ) );
            }
        }
    }

    // Inform user on progress
    std::cout << std::endl << "SPARTA simulation complete." << std::endl << std::endl;
}

//! Get aerodynamic coefficients at specific conditions.
Eigen::Vector6d RarefiedFlowAnalysis::getAerodynamicCoefficientsDataPoint(
        const boost::array< int, 3 > independentVariables )
{
    // Return requested coefficients.
    return aerodynamicCoefficients_( independentVariables );
}

} // namespace aerodynamics

} // namespace tudat


#define NOMINMAX
#include "LinearHexahedralPusoFormulation.hpp"
#include <assert.h>
#include <boost/detail/limits.hpp>
#include "LinearHexahedralPusoFormulationHelper.hpp"
#include "domain/elements/FiniteElement.hpp"
#include "domain/elements/ElementGeometry.hpp"
#include "domain/elements/MatrixOption.hpp"
#include "domain/elements/Node.hpp"
#include "domain/materials/MaterialModel.hpp"
#include "domain/physics/InfinitesimalState.hpp"
#include "domain/physics/UpdatedPhysicalState.hpp"
#include "foundation/NotImplementedException.hpp"
#include "foundation/NotSupportedException.hpp"
#include "Foundation/BLAS/ColumnVector.hpp"
#include "domain/analyses/AnalysisTimeline.hpp"
#include "foundation/memory/pointer.hpp"

namespace adf = axis::domain::formulations;
namespace ada = axis::domain::analyses;
namespace ade = axis::domain::elements;
namespace adm = axis::domain::materials;
namespace adp = axis::domain::physics;
namespace af = axis::foundation;
namespace afb = axis::foundation::blas;
namespace afm = axis::foundation::memory;
namespace afu = axis::foundation::uuids;

namespace {
  typedef adf::LinearHexahedralPusoFormulation hexa;
}

hexa::LinearHexahedralPusoFormulation(void)
{
  lumpedMass_ = NULLPTR;
  Bmatrix_ = NULLPTR;
  nodeCoordinates_ = NULLPTR;
  jacobian_ = NULLPTR;
  for (int i = 0; i < 4; i++)
  {
    hourglassForces_[i] = NULLPTR;
  }
  volume_ = 0;
  hourglassEnergy_ = 0;
}

hexa::~LinearHexahedralPusoFormulation(void)
{
  ClearMemory();
  if (nodeCoordinates_ != NULLPTR)
  {
    absref<afb::DenseMatrix>(nodeCoordinates_).Destroy();
    System::ModelMemory().Deallocate(nodeCoordinates_);
  }
  if (Bmatrix_ != NULLPTR)
  {
    absref<afb::DenseMatrix>(Bmatrix_).Destroy();
    System::ModelMemory().Deallocate(Bmatrix_);
  }
  if (jacobian_ != NULLPTR)
  {
    absref<afb::DenseMatrix>(jacobian_).Destroy();
    System::ModelMemory().Deallocate(jacobian_);
  }
  for (int i = 0; i < 4; i++)
  {
    if (hourglassForces_[i] != NULLPTR)
    {
      absref<afb::ColumnVector>(hourglassForces_[i]).Destroy();
      System::ModelMemory().Deallocate(hourglassForces_[i]);
      hourglassForces_[i] = NULLPTR;
    }
  }
  jacobian_ = NULLPTR;
  Bmatrix_ = NULLPTR;
  nodeCoordinates_ = NULLPTR;
}

void hexa::Destroy( void ) const
{
  delete this;
}

const afb::SymmetricMatrix& hexa::GetStiffness( void ) const
{
  // TODO : implement stiffness matrix according to Puso (2000).
  throw af::NotImplementedException(
    _T("This element formulation does not implement stiffness matrix."));
}

const afb::SymmetricMatrix& hexa::GetConsistentMass( void ) const
{
  throw af::NotImplementedException(
    _T("This element formulation does not implement consistent mass."));
}

const afb::ColumnVector& hexa::GetLumpedMass( void ) const
{
  return absref<afb::ColumnVector>(lumpedMass_);
}

void hexa::AllocateMemory( void )
{
  // initialize matrices
  nodeCoordinates_ = afb::DenseMatrix::Create(8, 3);
  Bmatrix_ = afb::DenseMatrix::Create(3, 8);
  jacobian_ = afb::DenseMatrix::Create(3, 3);
  for (int i = 0; i < 4; i++)
  {
    hourglassForces_[i] = afb::ColumnVector::Create(3);
  }
}

void hexa::CalculateInitialState( void )
{
  // get initial nodal coordinates
  ade::ElementGeometry& geometry = Element().Geometry();
  auto& Bmatrix = absref<afb::DenseMatrix>(Bmatrix_);
  auto& nodeCoordinates = absref<afb::DenseMatrix>(nodeCoordinates_);
  auto& jacobian = absref<afb::DenseMatrix>(jacobian_);
  CalculateNodeCoordinate(nodeCoordinates, geometry);
  CalculateBMatrix(Bmatrix, volume_, nodeCoordinates);
  CalculateJacobian(jacobian, nodeCoordinates);

  // clear hourglass force vectors
  for (int i = 0; i < 4; i++)
  {
    absref<afb::ColumnVector>(hourglassForces_[i]).ClearAll();
  }
}

void hexa::UpdateStrain( const afb::ColumnVector& elementDisplacementIncrement )
{
  const afb::ColumnVector& du = elementDisplacementIncrement; // shorthand
  const afb::DenseMatrix& B = absref<afb::DenseMatrix>(Bmatrix_);
  afb::ColumnVector& strain = Element().PhysicalState().Strain();
  afb::ColumnVector& strainIncrement = Element().PhysicalState().LastStrainIncrement();

  // our B-matrix is somewhat "different", so we rearrange the matrix-vector
  // computation normally done
  strainIncrement.ClearAll();
  for (int i = 0; i < 8; i++)
  {
    real e1 = B(0,i)*du(3*i);                                 // du1/dx
    real e2 = B(1,i)*du(3*i + 1);                             // du2/dy
    real e3 = B(2,i)*du(3*i + 2);                             // du3/dz
    real e4 = B(2,i)*du(3*i + 1) + B(1,i)*du(3*i + 2);        // du2/dz + du3/dy
    real e5 = B(2,i)*du(3*i) + B(0,i)*du(3*i + 2);            // du1/dz + du3/dx
    real e6 = B(1,i)*du(3*i) + B(0,i)*du(3*i + 1);            // du1/dy + du2/dx
    strainIncrement(0) += e1;
    strainIncrement(1) += e2;
    strainIncrement(2) += e3;
    strainIncrement(3) += e4;
    strainIncrement(4) += e5;
    strainIncrement(5) += e6;
  }
  afb::VectorSum(strain, 1.0, strain, 1.0, strainIncrement);
}

void hexa::UpdateInternalForce( afb::ColumnVector& internalForce,
  const afb::ColumnVector& elementDisplacementIncrement,
  const afb::ColumnVector& elementVelocity,
  const ada::AnalysisTimeline& timeInfo )
{
  internalForce.ClearAll();
  CalculateCentroidalInternalForce(internalForce);
  StabilizeInternalForce(internalForce, elementDisplacementIncrement);
  internalForce.Scale(-1.0);
}

void hexa::UpdateMatrices( const ade::MatrixOption& whichMatrices,
  const afb::ColumnVector& elementDisplacement,
  const afb::ColumnVector& elementVelocity )
{
  if (whichMatrices.DoesRequestConsistentMassMatrix())
  {
    throw af::NotSupportedException(
      _T("This element formulation does not support consistent mass."));
  }
  else if (whichMatrices.DoesRequestLumpedMassMatrix())
  {
    UpdateLumpedMassMatrix();
  }
  else if (whichMatrices.DoesRequestStiffnessMatrix())
  {
    throw af::NotSupportedException(
      _T("This element formulation does not support stiffness matrix."));
  }
  else
  {
    assert(!_T("An unpredictable matrix option was requested!"));
  }
}

void hexa::ClearMemory( void )
{
  if (lumpedMass_ != NULLPTR)
  {
    absref<afb::ColumnVector>(lumpedMass_).Destroy();
    System::ModelMemory().Deallocate(lumpedMass_);
  }
  lumpedMass_ = NULLPTR;
}

real hexa::GetCriticalTimestep(const afb::ColumnVector& modelDisplacement) const
{
  /* According to Puso (2000), the calculation below is empirically faster and
     works for most of the problems. Also, other more conservative equations,
     is generally coupled to material model and does not account hourglass
     stiffness, which generally presents two significant drawbacks.
  **/
  const ade::ElementGeometry& geometry = Element().Geometry();
  const adm::MaterialModel& material = Element().Material();
  const real speed = material.GetWavePropagationSpeed();

  // get the shortest distance between nodes
  real length = std::numeric_limits<real>::max();
  for (int i = 0; i < 7; i++)
  {
    const ade::Node& nodeX = geometry[i];
    for (int j = i + 1; j < 8; j++)
    {
      const ade::Node& nodeY = geometry[j];
      real dx = nodeX.X() - nodeY.X();
      real dy = nodeX.Y() - nodeY.Y();
      real dz = nodeX.Z() - nodeY.Z();
      real dist = sqrt(dx*dx + dy*dy + dz*dz);
      if (dist < length)
      {
        length = dist;
      }
    }
  }

  // this is a pessimistic approach: the fastest speed and the shortest distance
  real solidLongitudinalWaveSpeed = sqrt(speed / length);
  return solidLongitudinalWaveSpeed;
}

void hexa::UpdateLumpedMassMatrix( void )
{
  if (lumpedMass_ == NULLPTR)
  {
    lumpedMass_ = axis::foundation::blas::ColumnVector::Create(24);
  }
  adm::MaterialModel& material = Element().Material();
  real massPerNode = volume_ * material.Density() / 8;
  absref<afb::ColumnVector>(lumpedMass_).SetAll(massPerNode);
}

void hexa::CalculateCentroidalInternalForce( afb::ColumnVector& internalForce )
{
  const afb::DenseMatrix& B = absref<afb::DenseMatrix>(Bmatrix_);
  const afb::ColumnVector& stress = Element().PhysicalState().Stress();
  afb::ColumnVector& fint = internalForce;
  for (int i = 0; i < 8; i++)
  {
    real f1 = B(0, i)*stress(0) + B(2, i)*stress(4) + B(1, i)*stress(5);
    real f2 = B(1, i)*stress(1) + B(2, i)*stress(3) + B(0, i)*stress(5);
    real f3 = B(2, i)*stress(2) + B(1, i)*stress(3) + B(0, i)*stress(4);

    fint(3*i  ) = f1;
    fint(3*i+1) = f2;
    fint(3*i+2) = f3;
  }
  fint.Scale(volume_);
}

real adf::LinearHexahedralPusoFormulation::GetTotalArtificialEnergy( void ) const
{
  return hourglassEnergy_;
}

void hexa::StabilizeInternalForce( axis::foundation::blas::ColumnVector& internalForce,
                                   const axis::foundation::blas::ColumnVector& displacementIncrement )
{
  id_type id = Element().GetInternalId();

  // These are some shorthands that we are going to use
  const afb::DenseMatrix& J = absref<afb::DenseMatrix>(jacobian_);
  const afb::DenseMatrix& B = absref<afb::DenseMatrix>(Bmatrix_);
  const afb::ColumnVector& du = displacementIncrement;
  afb::DenseMatrix& X = absref<afb::DenseMatrix>(nodeCoordinates_);
  const afb::DenseMatrix& C = Element().Material().GetMaterialTensor();

  afb::ColumnVector sv1(8), sv2(8), sv3(8), sv4(8);
  afb::ColumnVector *stabilizationVector[4] = {&sv1, &sv2, &sv3, &sv4};
  for (int i = 0; i < 4; i++) // iterate through vectors
  {
    // gamma_i = h_i
    for (int idx = 0; idx < 8; idx++)
    {
      (*stabilizationVector[i])(idx) = hourglassVector_[i][idx];
    }

    // sum [j = 1 : 3]
    for (int j = 0; j < 3; j++)
    {
      // dot product h_i * x_j
      real dotProduct = 0;
      for (int m = 0; m < 8; m++)
      {
        dotProduct += hourglassVector_[i][m] * X(m, j);
      }

      for (int k = 0; k < 8; k++) // iterate through vector indices
      {
        (*stabilizationVector[i])(k) -= dotProduct * B(j, k);
      }
    }

    stabilizationVector[i]->Scale(0.125);
  }

  // Step 2: Calculate hourglass displacements (already in isoparametric
  // domain, Eq. 8)
  afb::ColumnVector v1(8), v2(8), v3(8), v4(8);
  afb::ColumnVector *hourglassDisplacement[4] = {&v1, &v2, &v3, &v4};
  for (int i = 0; i < 4; i++)
  {
    hourglassDisplacement[i]->ClearAll();
    real v_1 = 0, v_2 = 0, v_3 = 0;
    for (int nodeIdx = 0; nodeIdx < 8; nodeIdx++)
    {
      // in generalized coordinates...
      v_1 += (*stabilizationVector[i])(nodeIdx) * du(3*nodeIdx    );
      v_2 += (*stabilizationVector[i])(nodeIdx) * du(3*nodeIdx + 1);
      v_3 += (*stabilizationVector[i])(nodeIdx) * du(3*nodeIdx + 2);
    }

    // ...now in isoparametric coordinates
    for (int k = 0; k < 3; k++)
    {
      (*hourglassDisplacement[i])(k) = J(0, k)*v_1 + J(1, k)*v_2 + J(2, k)*v_3;
    }
  }

  // Step 3: Calculate hat(J)_0^(-1), which maps strains to isoparametric
  // coordinates (Eq. 3.61 pg. 3.14, LS-DYNA Theory Manual)
  afb::ColumnVector strainJacobian(6);
  real jSquaredNorm[3];
  for (int i = 0; i < 3; i++)
  {
    real squaredNorm = J(0,i)*J(0,i) + J(1,i)*J(1,i) + J(2,i)*J(2,i);
    jSquaredNorm[i] = squaredNorm;
  }
  strainJacobian(0) = 1.0 / jSquaredNorm[0];
  strainJacobian(1) = 1.0 / jSquaredNorm[1];
  strainJacobian(2) = 1.0 / jSquaredNorm[2];
  strainJacobian(3) = 1.0 / sqrt(jSquaredNorm[0] * jSquaredNorm[1]);
  strainJacobian(4) = 1.0 / sqrt(jSquaredNorm[2] * jSquaredNorm[1]);
  strainJacobian(5) = 1.0 / sqrt(jSquaredNorm[0] * jSquaredNorm[2]);

  // Step 4: Map material stiffness matrix to isoparametric space.
  afb::DenseMatrix Ciso(6, 6);
  for (int i = 0; i < 6; i++)
  {
    for (int j = 0; j < 6; j++)
    {
      real c_ij = strainJacobian(i) * C(i, j) * strainJacobian(j);
      Ciso(i, j) = c_ij;
    }
  }

  // Step 5: Calculate internal degrees of freedom for enhanced strain.
  real H = Ciso(0,0) + Ciso(1,1) + Ciso(2,2) + 2.0*(Ciso(0,1) +
    Ciso(1,2) + Ciso(0,2));
  real da1 = 1.0/Ciso(0,0) * (Ciso(0,1)*v1(1) + Ciso(0,4)*v2(1) +
    Ciso(0,2)*v2(2) + Ciso(0,4)*v1(2));
  real da2 = 1.0/Ciso(1,1) * (Ciso(1,0)*v1(0) + Ciso(1,5)*v3(0) +
    Ciso(1,2)*v3(2));
  real da3 = 1.0/Ciso(2,2) * (Ciso(2,0)*v2(0) + Ciso(2,3)*v3(0) +
    Ciso(2,1)*v3(1) + Ciso(2,3)*v2(1));
  real da4 = v4(2)/H * (Ciso(0,2) + Ciso(1,2) + Ciso(2,2));
  real da5 = v4(0)/H * (Ciso(0,0) + Ciso(1,0) + Ciso(2,0));
  real da6 = v4(1)/H * (Ciso(0,1) + Ciso(1,1) + Ciso(2,1));

  // Step 6: Calculate hourglass force increment
  afb::ColumnVector& df1 = absref<afb::ColumnVector>(hourglassForces_[0]);
  afb::ColumnVector& df2 = absref<afb::ColumnVector>(hourglassForces_[1]);
  afb::ColumnVector& df3 = absref<afb::ColumnVector>(hourglassForces_[2]);
  afb::ColumnVector& df4 = absref<afb::ColumnVector>(hourglassForces_[3]);
  real beta1 = 8.0/3.0 * volume_;
  real beta2 = 8.0/9.0 * volume_;
  real df11 = beta1 * (Ciso(0,0)*v1(0) + Ciso(0,2)*v3(2) + Ciso(0,5)*(v3(0) +
    v1(2)) - Ciso(1,0)*da2);
  real df12 = beta1 * (Ciso(1,1)*v1(1) + Ciso(1,2)*v2(2) + Ciso(1,4)*(v2(1) +
    v1(2)) - Ciso(0,1)*da1);
  real df13 = beta1 * (Ciso(4,4)*(v2(1) + v1(2)) + Ciso(5,5)*(v3(0) + v1(2)) +
    Ciso(5,0)*v1(0) + Ciso(4,1)*v1(1)
    + Ciso(4,2)*v2(2) + Ciso(5,2)*v3(2) - Ciso(0,4)*da1);
  real df21 = beta1 * (Ciso(0,0)*v2(0) + Ciso(0,1)*v3(1) + Ciso(0,3)*(v3(0) +
    v2(1)) - Ciso(2,0)*da3);
  real df22 = beta1 * (Ciso(3,3)*(v3(0) + v2(1)) + Ciso(4,4)*(v2(1) + v1(2)) +
    Ciso(3,0)*v2(0) + Ciso(3,1)*v3(1)
    + Ciso(4,1)*v1(1) + Ciso(4,2)*v2(2) - Ciso(0,4)*da1 - Ciso(2,3)*da3);
  real df23 = beta1 * (Ciso(2,1)*v1(1) + Ciso(2,2)*v2(2) + Ciso(2,4)*(v2(1) +
    v1(2)) - Ciso(0,2)*da1);
  real df31 = beta1 * (Ciso(3,3)*(v3(0) + v2(1)) + Ciso(5,5)*(v3(0) + v1(2)) +
    Ciso(3,0)*v2(0) + Ciso(3,1)*v3(1)
    + Ciso(5,0)*v1(0) + Ciso(5,2)*v3(2) - Ciso(1,5)*da2 - Ciso(2,3)*da3);
  real df32 = beta1 * (Ciso(1,0)*v2(0) + Ciso(1,1)*v3(1) + Ciso(1,3)*(v3(0) +
    v2(1)) - Ciso(2,1)*da3);
  real df33 = beta1 * (Ciso(2,0)*v1(0) + Ciso(2,2)*v3(2) + Ciso(2,5)*(v3(0) +
    v1(2)) - Ciso(1,2)*da2);
  real df41 = beta2 * (Ciso(0,0)*v4(0) - (Ciso(0,0) + Ciso(1,0) + Ciso(2,0))*da5);
  real df42 = beta2 * (Ciso(1,1)*v4(1) - (Ciso(0,1) + Ciso(1,1) + Ciso(2,1))*da6);
  real df43 = beta2 * (Ciso(2,2)*v4(2) - (Ciso(0,2) + Ciso(1,2) + Ciso(2,2))*da4);
  df1(0) += df11; df1(1) += df12; df1(2) += df13;
  df2(0) += df21; df2(1) += df22; df2(2) += df23;
  df3(0) += df31; df3(1) += df32; df3(2) += df33;
  df4(0) += df41; df4(1) += df42; df4(2) += df43;

  // Step 7: Calculate stabilization forces (fStab).
  afb::ColumnVector fStab(24);
  fStab.ClearAll();
  for (int i = 0; i < 4; i++)
  {
    const afb::ColumnVector& fi = absref<afb::ColumnVector>(hourglassForces_[i]);
    for (int nodeIdx = 0; nodeIdx < 8; nodeIdx++)
    {
      real gamma = (*stabilizationVector[i])(nodeIdx);
      real fs1 = gamma * (J(0,0)*fi(0) + J(0,1)*fi(1) + J(0,2)*fi(2));
      real fs2 = gamma * (J(1,0)*fi(0) + J(1,1)*fi(1) + J(1,2)*fi(2));
      real fs3 = gamma * (J(2,0)*fi(0) + J(2,1)*fi(1) + J(2,2)*fi(2));

      fStab(3*nodeIdx    ) += fs1;
      fStab(3*nodeIdx + 1) += fs2;
      fStab(3*nodeIdx + 2) += fs3;
    }
  }

  // Calculate hourglass energy increment
  real hourglassEnergyIncrement = 0;
  for (int nodeIdx = 0; nodeIdx < 8; nodeIdx++)
  {
    real gamma1 = (*stabilizationVector[0])(nodeIdx);
    real gamma2 = (*stabilizationVector[1])(nodeIdx);
    real gamma3 = (*stabilizationVector[2])(nodeIdx);
    real gamma4 = (*stabilizationVector[3])(nodeIdx);
    real dfs1 = gamma1 * (J(0,0)*df11 + J(0,1)*df12 + J(0,2)*df13)
              + gamma2 * (J(0,0)*df21 + J(0,1)*df22 + J(0,2)*df23)
              + gamma3 * (J(0,0)*df31 + J(0,1)*df32 + J(0,2)*df33)
              + gamma4 * (J(0,0)*df41 + J(0,1)*df42 + J(0,2)*df43);
    real dfs2 = gamma1 * (J(1,0)*df11 + J(1,1)*df12 + J(1,2)*df13)
              + gamma2 * (J(1,0)*df21 + J(1,1)*df22 + J(1,2)*df23)
              + gamma3 * (J(1,0)*df31 + J(1,1)*df32 + J(1,2)*df33)
              + gamma4 * (J(1,0)*df41 + J(1,1)*df42 + J(1,2)*df43);
    real dfs3 = gamma1 * (J(2,0)*df11 + J(2,1)*df12 + J(2,2)*df13)
              + gamma2 * (J(2,0)*df21 + J(2,1)*df22 + J(2,2)*df23)
              + gamma3 * (J(2,0)*df31 + J(2,1)*df32 + J(2,2)*df33)
              + gamma4 * (J(2,0)*df41 + J(2,1)*df42 + J(2,2)*df43);
    hourglassEnergyIncrement += dfs1*du(3*nodeIdx) + dfs2*du(3*nodeIdx + 1) +
      dfs3*du(3*nodeIdx + 2);
  }
  hourglassEnergy_ += hourglassEnergyIncrement;

  // Step 8: Stabilize internal force
  afb::VectorSum(internalForce, 1.0, internalForce, 1.0, fStab);
}

afu::Uuid adf::LinearHexahedralPusoFormulation::GetTypeId( void ) const
{
  // 993F06D4-A4B7-4B49-B5D3-42DA6FE86AE3
  int bytes[16] = {0x99, 0x3F, 0x06, 0xD4, 0xA4, 0xB7, 0x4B, 0x49,
                   0xB5, 0xD3, 0x42, 0xDA, 0x6F, 0xE8, 0x6A, 0xE3};
  return afu::Uuid(bytes);
}

// void hexa::StabilizeInternalForce_org( afb::Vector& internalForce,
//                                    const afb::Vector& displacementIncrement )
// {
//   id_type id = Element().GetInternalId();
//
//   // These are some shorthands that we are going to use
//   const afb::Matrix& J = absref<afb::Matrix>(jacobian_);
//   const afb::Matrix& B = absref<afb::Matrix>(Bmatrix_);
//   const afb::Vector& du = displacementIncrement;
//   afb::Matrix& X = absref<afb::Matrix>(nodeCoordinates_);
//   const afb::Matrix& C = Element().Material().GetMaterialTensor();
//
//   // Step 1: Calculate stabilization vectors gamma_i (Eq. 11)
//   real stabilizationVector[3][4][8];
//   for (int dofIdx = 0; dofIdx < 3; dofIdx++)
//   {
//     for (int modeIdx = 0; modeIdx < 4; modeIdx++) // iterate through vectors
//     {
//       real *gamma = stabilizationVector[dofIdx][modeIdx];
//       const real *h = hourglassVector_[modeIdx];
//       real dotProduct = 0;
//       for (int m = 0; m < 8; m++) // dot product h_i * x_j
//       {
//         dotProduct += h[m] * X(m, dofIdx);
//       }
//       for (int k = 0; k < 8; k++) // iterate through vector indices
//       {
//         gamma[k] = 0.125 * (h[k] - dotProduct*B(dofIdx, k));
//       }
//     }
//   }
//
//   // Step 2: Calculate hourglass displacements (already in isoparametric
//   // domain, Eq. 8)
//   real hourglassDisplacement[4][3];
//   for (int i = 0; i < 4; i++)
//   {
//     real v_1 = 0, v_2 = 0, v_3 = 0;
//     for (int nodeIdx = 0; nodeIdx < 8; nodeIdx++)
//     {
//       // in generalized coordinates...
//       v_1 += stabilizationVector[0][i][nodeIdx] * du(3*nodeIdx    );
//       v_2 += stabilizationVector[1][i][nodeIdx] * du(3*nodeIdx + 1);
//       v_3 += stabilizationVector[2][i][nodeIdx] * du(3*nodeIdx + 2);
//     }
//
//     // ...now in isoparametric coordinates
//     for (int k = 0; k < 3; k++)
//     {
//       hourglassDisplacement[k][i] = J(0, k)*v_1 + J(1, k)*v_2 + J(2, k)*v_3;
//     }
//   }
//
//   // Step 3: Calculate hat(J)_0^(-1), which maps strains to isoparametric
//   // coordinates (Eq. 3.61 pg. 3.14, LS-DYNA Theory Manual)
//   afb::ColumnVector strainJacobian(6);
//   real jSquaredNorm[3];
//   for (int i = 0; i < 3; i++)
//   {
//     real squaredNorm = J(0,i)*J(0,i) + J(1,i)*J(1,i) + J(2,i)*J(2,i);
//     jSquaredNorm[i] = squaredNorm;
//   }
//   strainJacobian(0) = 1.0 / jSquaredNorm[0];
//   strainJacobian(1) = 1.0 / jSquaredNorm[1];
//   strainJacobian(2) = 1.0 / jSquaredNorm[2];
//   strainJacobian(3) = 1.0 / sqrt(jSquaredNorm[0] * jSquaredNorm[1]);
//   strainJacobian(4) = 1.0 / sqrt(jSquaredNorm[2] * jSquaredNorm[1]);
//   strainJacobian(5) = 1.0 / sqrt(jSquaredNorm[0] * jSquaredNorm[2]);
//
//   // Step 4: Map material stiffness matrix to isoparametric space.
//   afb::DenseMatrix Ciso(6, 6);
//   for (int i = 0; i < 6; i++)
//   {
//     for (int j = 0; j < 6; j++)
//     {
//       real c_ij = strainJacobian(i) * C(i, j) * strainJacobian(j);
//       Ciso(i, j) = c_ij;
//     }
//   }
//
//   // Step 5: Calculate internal degrees of freedom for enhanced strain.
//   real *v1 = hourglassDisplacement[0];
//   real *v2 = hourglassDisplacement[1];
//   real *v3 = hourglassDisplacement[2];
//   real *v4 = hourglassDisplacement[3];
//   real H = Ciso(0,0) + Ciso(1,1) + Ciso(2,2) + 2.0*(Ciso(0,1) + Ciso(1,2) + Ciso(0,2));
//   real da1 = 1.0/Ciso(0,0) * (Ciso(0,1)*v1[1] + Ciso(0,4)*v2[1] + Ciso(0,2)*v2[2] + Ciso(0,4)*v1[2]);
//   real da2 = 1.0/Ciso(1,1) * (Ciso(1,0)*v1[0] + Ciso(1,5)*v3[0] + Ciso(1,2)*v3[2]);
//   real da3 = 1.0/Ciso(2,2) * (Ciso(2,0)*v2[0] + Ciso(2,3)*v3[0] + Ciso(2,1)*v3[1] + Ciso(2,3)*v2[1]);
//   real da4 = v4[2]/H * (Ciso(0,2) + Ciso(1,2) + Ciso(2,2));
//   real da5 = v4[0]/H * (Ciso(0,0) + Ciso(1,0) + Ciso(2,0));
//   real da6 = v4[1]/H * (Ciso(0,1) + Ciso(1,1) + Ciso(2,1));
//
//   // Step 6: Calculate hourglass force increment
//   afb::Vector& df1 = absref<afb::Vector>(hourglassForces_[0]);
//   afb::Vector& df2 = absref<afb::Vector>(hourglassForces_[1]);
//   afb::Vector& df3 = absref<afb::Vector>(hourglassForces_[2]);
//   afb::Vector& df4 = absref<afb::Vector>(hourglassForces_[3]);
//   real beta1 = 8.0/3.0 * volume_;
//   real beta2 = 8.0/9.0 * volume_;
//   real df11 = beta1 * (Ciso(0,0)*v1[0] + Ciso(0,2)*v3[2] + Ciso(0,5)*(v3[0] + v1[2]) - Ciso(1,0)*da2);
//   real df12 = beta1 * (Ciso(1,1)*v1[1] + Ciso(1,2)*v2[2] + Ciso(1,4)*(v2[1] + v1[2]) - Ciso(0,1)*da1);
//   real df13 = beta1 * (Ciso(4,4)*(v2[1] + v1[2]) + Ciso(5,5)*(v3[0] + v1[2]) + Ciso(5,0)*v1[0] + Ciso(4,1)*v1[1]
//               + Ciso(4,2)*v2[2] + Ciso(5,2)*v3[2] - Ciso(0,4)*da1);
//   real df21 = beta1 * (Ciso(0,0)*v2[0] + Ciso(0,1)*v3[1] + Ciso(0,3)*(v3[0] + v2[1]) - Ciso(2,0)*da3);
//   real df22 = beta1 * (Ciso(3,3)*(v3[0] + v2[1]) + Ciso(4,4)*(v2[1] + v1[2]) + Ciso(3,0)*v2[0] + Ciso(3,1)*v3[1]
//               + Ciso(4,1)*v1[1] + Ciso(4,2)*v2[2] - Ciso(0,4)*da1 - Ciso(2,3)*da3);
//   real df23 = beta1 * (Ciso(2,1)*v1[1] + Ciso(2,2)*v2[2] + Ciso(2,4)*(v2[1] + v1[2]) - Ciso(0,2)*da1);
//   real df31 = beta1 * (Ciso(3,3)*(v3[0] + v2[1]) + Ciso(5,5)*(v3[0] + v1[2]) + Ciso(3,0)*v2[0] + Ciso(3,1)*v3[1]
//               + Ciso(5,0)*v1[0] + Ciso(5,2)*v3[2] - Ciso(1,5)*da2 - Ciso(2,3)*da3);
//   real df32 = beta1 * (Ciso(1,0)*v2[0] + Ciso(1,1)*v3[1] + Ciso(1,3)*(v3[0] + v2[1]) - Ciso(2,1)*da3);
//   real df33 = beta1 * (Ciso(2,0)*v1[0] + Ciso(2,2)*v3[2] + Ciso(2,5)*(v3[0] + v1[2]) - Ciso(1,2)*da2);
//   real df41 = beta2 * (Ciso(0,0)*v4[0] - (Ciso(0,0) + Ciso(1,0) + Ciso(2,0))*da5);
//   real df42 = beta2 * (Ciso(1,1)*v4[1] - (Ciso(0,1) + Ciso(1,1) + Ciso(2,1))*da6);
//   real df43 = beta2 * (Ciso(2,2)*v4[2] - (Ciso(0,2) + Ciso(1,2) + Ciso(2,2))*da4);
//   df1(0) += df11; df1(1) += df12; df1(2) += df13;
//   df2(0) += df21; df2(1) += df22; df2(2) += df23;
//   df3(0) += df31; df3(1) += df32; df3(2) += df33;
//   df4(0) += df41; df4(1) += df42; df4(2) += df43;
//
//   // Step 7: Calculate stabilization forces (fStab).
//   afb::ColumnVector fStab(24);
//   afb::ColumnVector dFstab(24);
//   fStab.ClearAll();
//   for (int i = 0; i < 4; i++)
//   {
//     const afb::Vector& fi = absref<afb::Vector>(hourglassForces_[i]);
//     for (int nodeIdx = 0; nodeIdx < 8; nodeIdx++)
//     {
//       real gammaX = stabilizationVector[0][i][nodeIdx];
//       real gammaY = stabilizationVector[1][i][nodeIdx];
//       real gammaZ = stabilizationVector[2][i][nodeIdx];
//       real fs1 = gammaX * (J(0,0)*fi(0) + J(0,1)*fi(1) + J(0,2)*fi(2));
//       real fs2 = gammaY * (J(1,0)*fi(0) + J(1,1)*fi(1) + J(1,2)*fi(2));
//       real fs3 = gammaZ * (J(2,0)*fi(0) + J(2,1)*fi(1) + J(2,2)*fi(2));
//
//       fStab(3*nodeIdx    ) += fs1;
//       fStab(3*nodeIdx + 1) += fs2;
//       fStab(3*nodeIdx + 2) += fs3;
//     }
//   }
//
//   // Calculate hourglass energy increment
//   real hourglassEnergyIncrement = 0;
//   for (int nodeIdx = 0; nodeIdx < 8; nodeIdx++)
//   {
//     real gammaX1 = stabilizationVector[0][0][nodeIdx];
//     real gammaY1 = stabilizationVector[1][0][nodeIdx];
//     real gammaZ1 = stabilizationVector[2][0][nodeIdx];
//     real gammaX2 = stabilizationVector[0][1][nodeIdx];
//     real gammaY2 = stabilizationVector[1][1][nodeIdx];
//     real gammaZ2 = stabilizationVector[2][1][nodeIdx];
//     real gammaX3 = stabilizationVector[0][2][nodeIdx];
//     real gammaY3 = stabilizationVector[1][2][nodeIdx];
//     real gammaZ3 = stabilizationVector[2][2][nodeIdx];
//     real gammaX4 = stabilizationVector[0][3][nodeIdx];
//     real gammaY4 = stabilizationVector[1][3][nodeIdx];
//     real gammaZ4 = stabilizationVector[2][3][nodeIdx];
//
//     real dfs1 = gammaX1 * (J(0,0)*df11 + J(0,1)*df12 + J(0,2)*df13)
//               + gammaX2 * (J(0,0)*df21 + J(0,1)*df22 + J(0,2)*df23)
//               + gammaX3 * (J(0,0)*df31 + J(0,1)*df32 + J(0,2)*df33)
//               + gammaX4 * (J(0,0)*df41 + J(0,1)*df42 + J(0,2)*df43);
//     real dfs2 = gammaY1 * (J(1,0)*df11 + J(1,1)*df12 + J(1,2)*df13)
//               + gammaY2 * (J(1,0)*df21 + J(1,1)*df22 + J(1,2)*df23)
//               + gammaY3 * (J(1,0)*df31 + J(1,1)*df32 + J(1,2)*df33)
//               + gammaY4 * (J(1,0)*df41 + J(1,1)*df42 + J(1,2)*df43);
//     real dfs3 = gammaZ1 * (J(2,0)*df11 + J(2,1)*df12 + J(2,2)*df13)
//               + gammaZ2 * (J(2,0)*df21 + J(2,1)*df22 + J(2,2)*df23)
//               + gammaZ3 * (J(2,0)*df31 + J(2,1)*df32 + J(2,2)*df33)
//               + gammaZ4 * (J(2,0)*df41 + J(2,1)*df42 + J(2,2)*df43);
//
//     hourglassEnergyIncrement += dfs1*du(3*nodeIdx) + dfs2*du(3*nodeIdx + 1) + dfs3*du(3*nodeIdx + 2);
//   }
//   hourglassEnergy_ += hourglassEnergyIncrement;
//
//   // Step 8: Stabilize internal force
//   afb::VectorAlgebra::Sum(internalForce, 1.0, internalForce, 1.0, fStab);
// }