/* ============================================================================
 * Copyright (c) 2009-2016 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 followig 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 "PrincipalComponentAnalysis.h"

#include <Eigen/Core>
#include <Eigen/Eigen>

#include <QtCore/QTextStream>

#include "SIMPLib/Common/Constants.h"
#include "SIMPLib/Common/TemplateHelpers.h"
#include "SIMPLib/DataArrays/IDataArray.h"
#include "SIMPLib/DataContainers/DataContainer.h"
#include "SIMPLib/DataContainers/DataContainerArray.h"
#include "SIMPLib/FilterParameters/AbstractFilterParametersReader.h"
#include "SIMPLib/FilterParameters/BooleanFilterParameter.h"
#include "SIMPLib/FilterParameters/ChoiceFilterParameter.h"
#include "SIMPLib/FilterParameters/DataArrayCreationFilterParameter.h"
#include "SIMPLib/FilterParameters/IntFilterParameter.h"
#include "SIMPLib/FilterParameters/LinkedBooleanFilterParameter.h"
#include "SIMPLib/FilterParameters/LinkedPathCreationFilterParameter.h"
#include "SIMPLib/FilterParameters/MultiDataArraySelectionFilterParameter.h"
#include "SIMPLib/FilterParameters/SeparatorFilterParameter.h"
#include "SIMPLib/FilterParameters/StringFilterParameter.h"

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

/* Create Enumerations to allow the created Attribute Arrays to take part in renaming */
enum createdPathID : RenameDataPath::DataID_t
{
  AttributeMatrixID21 = 21,

  DataArrayID30 = 30,
  DataArrayID31 = 31,
  DataArrayID32 = 32,
  DataArrayID33 = 33,
};

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
PrincipalComponentAnalysis::PrincipalComponentAnalysis() = default;

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

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void PrincipalComponentAnalysis::setupFilterParameters()
{
  FilterParameterVectorType parameters;
  MultiDataArraySelectionFilterParameter::RequirementType mdaReq =
      MultiDataArraySelectionFilterParameter::CreateRequirement(SIMPL::Defaults::AnyPrimitive, 1, AttributeMatrix::Type::Any, IGeometry::Type::Any);
  parameters.push_back(
      SIMPL_NEW_MDA_SELECTION_FP("Attribute Arrays for Computing Principal Components", SelectedDataArrayPaths, FilterParameter::Category::RequiredArray, PrincipalComponentAnalysis, mdaReq));
  {
    ChoiceFilterParameter::Pointer choices = ChoiceFilterParameter::New();
    choices->setHumanLabel("Matrix Approach");
    choices->setPropertyName("MatrixApproach");
    choices->setSetterCallback(SIMPL_BIND_SETTER(PrincipalComponentAnalysis, this, MatrixApproach));
    choices->setGetterCallback(SIMPL_BIND_GETTER(PrincipalComponentAnalysis, this, MatrixApproach));
    QVector<QString> approaches = {"Correlation", "Covariance"};
    choices->setChoices(approaches);
    choices->setCategory(FilterParameter::Category::Parameter);
    parameters.push_back(choices);
  }
  QStringList linkedProps = {"NumberOfDimensionsForProjection", "ProjectedDataSpaceArrayPath"};
  parameters.push_back(SIMPL_NEW_LINKED_BOOL_FP("Project Data Space", ProjectDataSpace, FilterParameter::Category::Parameter, PrincipalComponentAnalysis, linkedProps));
  parameters.push_back(SIMPL_NEW_INTEGER_FP("Number of Dimensions for Projection", NumberOfDimensionsForProjection, FilterParameter::Category::Parameter, PrincipalComponentAnalysis));
  DataArrayCreationFilterParameter::RequirementType dacReq = DataArrayCreationFilterParameter::CreateRequirement(AttributeMatrix::Type::Any, IGeometry::Type::Any);
  parameters.push_back(SIMPL_NEW_DA_CREATION_FP("Projected Data Space", ProjectedDataSpaceArrayPath, FilterParameter::Category::CreatedArray, PrincipalComponentAnalysis, dacReq));
  parameters.push_back(SeparatorFilterParameter::Create("Principal Component Data", FilterParameter::Category::CreatedArray));
  parameters.push_back(SIMPL_NEW_STRING_FP("Principal Component Attribute Matrix", PCAttributeMatrixName, FilterParameter::Category::CreatedArray, PrincipalComponentAnalysis));
  parameters.push_back(SIMPL_NEW_STRING_FP("Principal Component Eigenvalues", PCEigenvaluesName, FilterParameter::Category::CreatedArray, PrincipalComponentAnalysis));
  parameters.push_back(SIMPL_NEW_STRING_FP("Principal Component Eigenvectors", PCEigenvectorsName, FilterParameter::Category::CreatedArray, PrincipalComponentAnalysis));
  setFilterParameters(parameters);
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void PrincipalComponentAnalysis::readFilterParameters(AbstractFilterParametersReader* reader, int index)
{
  reader->openFilterGroup(this, index);
  setSelectedDataArrayPaths(reader->readDataArrayPathVector("SelectedDataArrayPaths", getSelectedDataArrayPaths()));
  setPCAttributeMatrixName(reader->readString("PCAttributeMatrixName", getPCAttributeMatrixName()));
  setPCEigenvaluesName(reader->readString("PCEigenvaluesName", getPCEigenvaluesName()));
  setPCEigenvectorsName(reader->readString("PCEigenvectorsName", getPCEigenvectorsName()));
  setMatrixApproach(reader->readValue("MatrixApproach", getMatrixApproach()));
  setProjectDataSpace(reader->readValue("ProjectDataSpace", getProjectDataSpace()));
  setNumberOfDimensionsForProjection(reader->readValue("NumberOfDimensionsForProjection", getNumberOfDimensionsForProjection()));
  setProjectedDataSpaceArrayPath(reader->readDataArrayPath("ProjectedDataSpaceArrayPath", getProjectedDataSpaceArrayPath()));
  reader->closeFilterGroup();
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void PrincipalComponentAnalysis::initialize()
{
  m_SelectedWeakPtrVector.clear();
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void PrincipalComponentAnalysis::dataCheck()
{
  clearErrorCode();
  clearWarningCode();
  initialize();

  if(getSelectedDataArrayPaths().size() < 2)
  {
    QString ss = QObject::tr("At least two Attribute Array must be selected");
    setErrorCondition(-11001, ss);
    return;
  }

  QVector<DataArrayPath> paths = getSelectedDataArrayPaths();

  if(!DataArrayPath::ValidateVector(paths))
  {
    QString ss = QObject::tr("There are Attribute Arrays selected that are not contained in the same Attribute Matrix; all selected Attribute Arrays must belong to the same Attribute Matrix");
    setErrorCondition(-11002, ss);
  }

  for(auto&& path : paths)
  {
    IDataArray::WeakPointer ptr = getDataContainerArray()->getPrereqIDataArrayFromPath(this, path);
    if(ptr.lock())
    {
      m_SelectedWeakPtrVector.push_back(ptr);
      int32_t numComps = ptr.lock()->getNumberOfComponents();
      if(numComps != 1)
      {
        QString ss = QObject::tr("Attribute Arrays must be scalar arrays, but %1 has %2 total components").arg(ptr.lock()->getName()).arg(numComps);
        setErrorCondition(-11003, ss);
      }
    }
  }

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

  std::vector<size_t> tDims(1, paths.size());

  DataContainer::Pointer m = getDataContainerArray()->getDataContainer(getSelectedDataArrayPaths().at(0).getDataContainerName());
  m->createNonPrereqAttributeMatrix(this, getPCAttributeMatrixName(), tDims, AttributeMatrix::Type::Generic, AttributeMatrixID21);

  DataArrayPath tempPath;
  std::vector<size_t> cDims(1, 1);

  tempPath.update(getSelectedDataArrayPaths().at(0).getDataContainerName(), getPCAttributeMatrixName(), getPCEigenvaluesName());
  m_PCEigenvaluesPtr = getDataContainerArray()->createNonPrereqArrayFromPath<DataArray<double>>(this, tempPath, 0, cDims, "", DataArrayID31);
  if(m_PCEigenvaluesPtr.lock())
  {
    m_PCEigenvalues = m_PCEigenvaluesPtr.lock()->getPointer(0);
  }

  cDims[0] = paths.size();

  tempPath.update(getSelectedDataArrayPaths().at(0).getDataContainerName(), getPCAttributeMatrixName(), getPCEigenvectorsName());
  m_PCEigenvectorsPtr = getDataContainerArray()->createNonPrereqArrayFromPath<DataArray<double>>(this, tempPath, 0, cDims, "", DataArrayID32);
  if(m_PCEigenvectorsPtr.lock())
  {
    m_PCEigenvectors = m_PCEigenvectorsPtr.lock()->getPointer(0);
  }

  if(getProjectDataSpace())
  {
    if(getNumberOfDimensionsForProjection() <= 0)
    {
      QString ss = QObject::tr("Number of dimensions for the projected space (%1) must be greater than 0").arg(getNumberOfDimensionsForProjection());
      setErrorCondition(-11004, ss);
    }

    if(getNumberOfDimensionsForProjection() > paths.size())
    {
      QString ss = QObject::tr("Number of dimensions for the projected space (%1) must be less than or equal to the number of selected Attribute Arrays (%2)")
                       .arg(getNumberOfDimensionsForProjection())
                       .arg(paths.size());
      setErrorCondition(-11005, ss);
    }

    cDims[0] = getNumberOfDimensionsForProjection();

    m_ProjectedDataSpacePtr = getDataContainerArray()->createNonPrereqArrayFromPath<DataArray<double>>(this, getProjectedDataSpaceArrayPath(), 0, cDims, "", DataArrayID33);
    if(m_ProjectedDataSpacePtr.lock())
    {
      m_ProjectedDataSpace = m_ProjectedDataSpacePtr.lock()->getPointer(0);
    }
    if(getErrorCode() >= 0)
    {
      paths.push_back(getProjectedDataSpaceArrayPath());
    }
  }

  getDataContainerArray()->validateNumberOfTuples(this, paths);
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
template <typename T>
void copyDataArrays(IDataArray::Pointer dataPtr, std::vector<double>& copy, int32_t arrayIndex)
{
  typename DataArray<T>::Pointer inDataPtr = std::dynamic_pointer_cast<DataArray<T>>(dataPtr);
  T* dPtr = inDataPtr->getPointer(0);
  size_t numTuples = inDataPtr->getNumberOfTuples();

  for(size_t i = 0; i < numTuples; i++)
  {
    copy[numTuples * arrayIndex + i] = static_cast<double>(dPtr[i]);
  }
}

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

  if(m_SelectedDataArrayPaths.size() != m_SelectedWeakPtrVector.size())
  {
    QString ss = QObject::tr("The number of selected Attribute Arrays does not equal the number of internal weak pointers");
    setErrorCondition(-11008, ss);
    return;
  }

  // Set up our Eigen variables ; we use a dynamic double matrix since we don't know
  // the number of arrays or their tuples at compile time ; could have just used the
  // MatrixXd typedef instead of our own, but this is explicit about the ordering
  // being column major
  auto numArrays = m_SelectedWeakPtrVector.size();
  size_t numTuples = m_SelectedWeakPtrVector[0].lock()->getNumberOfTuples();

  typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> CovarianceMatrix;

  // Copy the incoming data into a contiguous chunk of memory, casting everything to doubles
  std::vector<double> inDataVector(numTuples * numArrays, 0);

  for(auto i = 0; i < numArrays; i++)
  {
    EXECUTE_FUNCTION_TEMPLATE(this, copyDataArrays, m_SelectedWeakPtrVector[i].lock(), m_SelectedWeakPtrVector[i].lock(), inDataVector, i);
  }

  // Interface our CovarianceMatrix typedef with our contiguous array in memory
  // Using a colum major matrix, so each array is a column, so the arrays should
  // be layed out end-to-end in memory ; initialization for sizes in Eigen is:
  // numRows x numColumns
  Eigen::Map<CovarianceMatrix> dataMat(inDataVector.data(), numTuples, numArrays);

  // If the correlation approach is being used, the data must be standardize to have
  // mean 0 and unit variance
  if(m_MatrixApproach == 0)
  {
    // Standardize data to have mean 0 and unit variance
    dataMat = (dataMat.rowwise() - dataMat.colwise().mean());
    Eigen::RowVectorXd stdDev = dataMat.colwise().squaredNorm();
    for(auto i = 0; i < stdDev.cols(); i++)
    {
      stdDev(i) /= dataMat.rows();
      stdDev(i) = sqrt(stdDev(i));
    }
    dataMat = dataMat.array().rowwise() / stdDev.array();
  }

  // Calculate the covariance matrix using the matrix formulation, checking if
  // tuples are 1 to avoid division by zero
  // If correlation was chosen, then this is technically the correlation matrix,
  // but since the means are zero in that case the centering operation is trivial
  Eigen::MatrixXd centeredMat = dataMat.rowwise() - dataMat.colwise().mean();
  Eigen::MatrixXd covMat;

  if(numTuples == 1)
  {
    covMat = (centeredMat.adjoint() * centeredMat) / (1.0);
  }
  else
  {
    covMat = (centeredMat.adjoint() * centeredMat) / (double(numTuples - 1.0));
  }

  // Perform the eigen decomposition to get the eigenvectors and eigenvalues
  // of the covariance matrix, then copy those values into the primary
  // DataArray pointers
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> pca(covMat);

  for(auto i = 0; i < pca.eigenvalues().size(); i++)
  {
    m_PCEigenvalues[i] = pca.eigenvalues()(i);
  }

  for(auto i = 0; i < pca.eigenvectors().cols(); i++)
  {
    for(auto j = 0; j < pca.eigenvectors().rows(); j++)
    {
      m_PCEigenvectors[pca.eigenvectors().rows() * i + j] = pca.eigenvectors()(j, i);
    }
  }

  if(m_ProjectDataSpace)
  {
    // Extract the projective transform
    // Eigen orders the eigenvalues/eigenvectors in ascending order, so just grab
    // the rightmost columns equal to the number of projective dimensions
    Eigen::MatrixXd transform = pca.eigenvectors().rightCols(m_NumberOfDimensionsForProjection);

    // Multiply each row in the centered data matrix by transform and copy the projected
    // points into the DataArray pointer
    for(auto i = 0; i < centeredMat.rows(); i++)
    {
      Eigen::VectorXd tmp = centeredMat.row(i) * transform;
      for(int32_t j = 0; j < m_NumberOfDimensionsForProjection; j++)
      {
        m_ProjectedDataSpace[m_NumberOfDimensionsForProjection * i + j] = tmp(j);
      }
    }
  }
}

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

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

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

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

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

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QUuid PrincipalComponentAnalysis::getUuid() const
{
  return QUuid("{ec163736-39c8-5c69-9a56-61940a337c07}");
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString PrincipalComponentAnalysis::getSubGroupName() const
{
  return DREAM3DReviewConstants::FilterSubGroups::DimensionalityReductionFilters;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString PrincipalComponentAnalysis::getHumanLabel() const
{
  return "Principal Component Analysis";
}

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

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

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

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

// -----------------------------------------------------------------------------
void PrincipalComponentAnalysis::setSelectedDataArrayPaths(const QVector<DataArrayPath>& value)
{
  m_SelectedDataArrayPaths = value;
}

// -----------------------------------------------------------------------------
QVector<DataArrayPath> PrincipalComponentAnalysis::getSelectedDataArrayPaths() const
{
  return m_SelectedDataArrayPaths;
}

// -----------------------------------------------------------------------------
void PrincipalComponentAnalysis::setPCAttributeMatrixName(const QString& value)
{
  m_PCAttributeMatrixName = value;
}

// -----------------------------------------------------------------------------
QString PrincipalComponentAnalysis::getPCAttributeMatrixName() const
{
  return m_PCAttributeMatrixName;
}

// -----------------------------------------------------------------------------
void PrincipalComponentAnalysis::setPCEigenvaluesName(const QString& value)
{
  m_PCEigenvaluesName = value;
}

// -----------------------------------------------------------------------------
QString PrincipalComponentAnalysis::getPCEigenvaluesName() const
{
  return m_PCEigenvaluesName;
}

// -----------------------------------------------------------------------------
void PrincipalComponentAnalysis::setPCEigenvectorsName(const QString& value)
{
  m_PCEigenvectorsName = value;
}

// -----------------------------------------------------------------------------
QString PrincipalComponentAnalysis::getPCEigenvectorsName() const
{
  return m_PCEigenvectorsName;
}

// -----------------------------------------------------------------------------
void PrincipalComponentAnalysis::setMatrixApproach(int value)
{
  m_MatrixApproach = value;
}

// -----------------------------------------------------------------------------
int PrincipalComponentAnalysis::getMatrixApproach() const
{
  return m_MatrixApproach;
}

// -----------------------------------------------------------------------------
void PrincipalComponentAnalysis::setProjectDataSpace(bool value)
{
  m_ProjectDataSpace = value;
}

// -----------------------------------------------------------------------------
bool PrincipalComponentAnalysis::getProjectDataSpace() const
{
  return m_ProjectDataSpace;
}

// -----------------------------------------------------------------------------
void PrincipalComponentAnalysis::setNumberOfDimensionsForProjection(int value)
{
  m_NumberOfDimensionsForProjection = value;
}

// -----------------------------------------------------------------------------
int PrincipalComponentAnalysis::getNumberOfDimensionsForProjection() const
{
  return m_NumberOfDimensionsForProjection;
}

// -----------------------------------------------------------------------------
void PrincipalComponentAnalysis::setProjectedDataSpaceArrayPath(const DataArrayPath& value)
{
  m_ProjectedDataSpaceArrayPath = value;
}

// -----------------------------------------------------------------------------
DataArrayPath PrincipalComponentAnalysis::getProjectedDataSpaceArrayPath() const
{
  return m_ProjectedDataSpaceArrayPath;
}


//==================================================================================================
/*!
  @file
  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_GAMMA_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_GAMMA_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!
  @ingroup group-euler
    Function object implementing gamma capabilities

   Computes the gamma function:
   \f$\displaystyle \int_0^{\infty} t^{x-1}e^{-t}\mbox{d}t\f$

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = gamma(x);
    @endcode

    @par Decorators

    std_ for floating entries provides access to @c std::tgamma

    @see gammaln

  **/
  Value gamma(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/gamma.hpp>
#include <boost/simd/function/simd/gamma.hpp>

#endif


/// Test file for solving a transformation

#include "bigssMath.h"
#include <cmath>

#include <Eigen/SVD>

// #define M_PI 3.14159

int main()
{
  int nPts = 4;
  Eigen::Matrix3Xd moving = 100 * Eigen::Matrix3Xd::Random(3,nPts);
  Eigen::Affine3d T, T2;
  T.setIdentity();
  T.translate(Eigen::Vector3d(10,20,30));
  T.rotate(Eigen::AngleAxisd(0.5*M_PI, Eigen::Vector3d::UnitY()));

  T2.setIdentity();

  Eigen::Matrix3Xd fixed = T * moving;

  BIGSS::computeTransform(moving, fixed, T2);

  Eigen::Matrix4d diff =  T.matrix() - T2.matrix();
  std::cout << diff.norm() << std::endl;

  std::cout << T2 * moving - fixed << std::endl << std::endl;

  Eigen::Matrix3Xd cmoving = moving.leftCols(nPts);
  Eigen::Matrix3Xd cfixed = fixed.leftCols(nPts);
  Eigen::Affine3d T3;
  Eigen::VectorXi ordering;

  cmoving.col(0) = moving.col(1);
  cmoving.col(1) = moving.col(3);
  cmoving.col(2) = moving.col(0);
  cmoving.col(3) = moving.col(2);

  std::cout << moving << std::endl << std::endl;
  std::cout << cmoving << std::endl << std::endl;
  BIGSS::computeCorrespondencelessTransform(cmoving, cfixed, T3, ordering);

  std::cout << "Transform (truth): " << std::endl << T.matrix() << std::endl << std::endl;
  std::cout << "Transform (with correspondence): " << std::endl << T2.matrix() << std::endl << std::endl;
  std::cout << "Transform (correspondenceless): " << std::endl << T3.matrix() << std::endl << std::endl;
  std::cout << ordering << std::endl << std::endl;
  std::cout << "original: " << std::endl << moving << std::endl << std::endl;
  std::cout << "permuted: " << std::endl << cmoving << std::endl << std::endl;
  std::cout << "Press any key to exit..." << std::endl;
  std::getchar();
}


#include <cstdlib>
#include <list>
#include <Eigen/Core>
#include <moihgp/moihgp_regression.h>
#include <moihgp/matern32ss.h>
#include <iostream>
#include <time.h>

int main()
{

    double dt = 0.1;
    size_t num_output = 2;
    size_t num_latent = 1;
    bool threading = true;
    Eigen::MatrixXd H(2, 2);
    H << 0.7, 0.3, -0.3, 0.7;
    std::vector<Eigen::VectorXd> data;
    double t = 0.0;
    while (t < 2 * M_PI)
    {
        Eigen::VectorXd x(num_latent);
        x << sin(t), sin(4*t);
        data.push_back(H * x + 0.1 * Eigen::VectorXd(num_output).setRandom());
        t += dt;
    }
    data.shrink_to_fit();
    size_t num_data = data.size();

    moihgp::MOIHGPRegression<moihgp::Matern32StateSpace> gp(dt, num_output, num_latent, num_data, threading);
    clock_t tic = clock();
    int niter = gp.fit(data);
    clock_t toc = clock();

    std::cout << "Iteration count: " << niter << std::endl;
    std::cout << "Elapsed time: " << double(toc - tic) / 1000.0 << "ms" << std::endl;

    return 0;

}


/*
 *  Copyright 2013 Néstor Morales Hernández <nestor@isaatc.ull.es>
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 */

#include <iostream>
#include <stdio.h>
#include <fstream>
#include <boost/concept_check.hpp>

#include "polarcalibration.h"

cv::Point2d PolarCalibration::image2World(const cv::Point2d & point, const cv::Size & imgDimensions) {
    return cv::Point(point.x * 0.5 + imgDimensions.width / 4.0, /*imgDimensions.height -*/ (point.y * 0.5  + imgDimensions.height / 4.0));
}

cv::Point2d PolarCalibration::getPointFromLineAndX(const double & x, const cv::Vec3f line) {
    return cv::Point2d(x, -(line[0] * x + line[2]) / line[1]);
}

void PolarCalibration::showNewEpiline(const cv::Point2d epipole, const cv::Vec3f & lineB, const cv::Vec3f & lineE,
                                      const cv::Vec3f & newLine, const cv::Point2d & pOld, const cv::Point2d & pNew,
                                      const cv::Size & imgDimensions, std::string windowName) {

    cv::Mat img = cv::Mat::zeros(imgDimensions.height, imgDimensions.width, CV_8UC3);

    // Image limits are drawn
    cv::line(img, image2World(cv::Point2d(0, 0), imgDimensions),
             image2World(cv::Point2d(imgDimensions.width - 1, 0), imgDimensions), cv::Scalar::all(255));
    cv::line(img, image2World(cv::Point2d(0, 0), imgDimensions),
             image2World(cv::Point2d(0, imgDimensions.height - 1), imgDimensions), cv::Scalar::all(255));
    cv::line(img, image2World(cv::Point2d(imgDimensions.width - 1, 0), imgDimensions),
             image2World(cv::Point2d(imgDimensions.width - 1, imgDimensions.height - 1), imgDimensions), cv::Scalar::all(255));
    cv::line(img, image2World(cv::Point2d(0, imgDimensions.height - 1), imgDimensions),
             image2World(cv::Point2d(imgDimensions.width - 1, imgDimensions.height - 1), imgDimensions), cv::Scalar::all(255));

    cv::line(img, image2World(epipole, imgDimensions),
             image2World(pNew, imgDimensions), cv::Scalar(0, 255, 255));
    cv::line(img, image2World(pOld, imgDimensions),
             image2World(pNew, imgDimensions), cv::Scalar(0, 255, 255));

    cv::circle(img, image2World(epipole, imgDimensions), 4, cv::Scalar(0, 255, 0), -1);

    cv::line(img, image2World(getPointFromLineAndX(-2 * imgDimensions.width, lineB), imgDimensions),
             image2World(getPointFromLineAndX(2 * imgDimensions.width, lineB), imgDimensions), cv::Scalar(255, 0, 255), 1);
    cv::line(img, image2World(getPointFromLineAndX(-2 * imgDimensions.width, lineE), imgDimensions),
             image2World(getPointFromLineAndX(2 * imgDimensions.width, lineE), imgDimensions), cv::Scalar(0, 255, 255), 1);

    cv::line(img, image2World(getPointFromLineAndX(-2 * imgDimensions.width, newLine), imgDimensions),
             image2World(getPointFromLineAndX(2 * imgDimensions.width, newLine), imgDimensions), cv::Scalar(0, 255, 0), 2);

    cv::circle(img, image2World(pOld, imgDimensions), 3, cv::Scalar(255, 0, 0), -1);
    cv::circle(img, image2World(pNew, imgDimensions), 3, cv::Scalar(0, 0, 255), -1);

    cv::flip(img, img, 0);

    cv::namedWindow(windowName.c_str());
    cv::imshow(windowName.c_str(), img);
}

void PolarCalibration::showCommonRegion(const cv::Point2d epipole, const cv::Vec3f & line11, const cv::Vec3f & line12,
                                        const cv::Vec3f & line13, const cv::Vec3f & line14,
                                        const cv::Vec3f & lineB, const cv::Vec3f & lineE,
                                        const cv::Point2d & b, const cv::Point2d & e, const cv::Size & imgDimensions,
                                        const vector<cv::Point2f> & externalPoints, std::string windowName) {

    cv::Mat img = cv::Mat::zeros(imgDimensions.height, imgDimensions.width, CV_8UC3);

    // Image limits are drawn
    cv::line(img, image2World(cv::Point2d(0, 0), imgDimensions),
             image2World(cv::Point2d(imgDimensions.width - 1, 0), imgDimensions), cv::Scalar::all(255));
    cv::line(img, image2World(cv::Point2d(0, 0), imgDimensions),
             image2World(cv::Point2d(0, imgDimensions.height - 1), imgDimensions), cv::Scalar::all(255));
    cv::line(img, image2World(cv::Point2d(imgDimensions.width - 1, 0), imgDimensions),
             image2World(cv::Point2d(imgDimensions.width - 1, imgDimensions.height - 1), imgDimensions), cv::Scalar::all(255));
    cv::line(img, image2World(cv::Point2d(0, imgDimensions.height - 1), imgDimensions),
             image2World(cv::Point2d(imgDimensions.width - 1, imgDimensions.height - 1), imgDimensions), cv::Scalar::all(255));

    cv::circle(img, image2World(epipole, imgDimensions), 4, cv::Scalar(0, 255, 0), 1);
//     cv::circle(img, image2World(externalPoints[0], imgDimensions), 3, cv::Scalar(255, 255, 0), -1);
//     cv::circle(img, image2World(externalPoints[1], imgDimensions), 3, cv::Scalar(255, 255, 0), -1);
//
//     cv::line(img, image2World(getPointFromLineAndX(epipole.x, line11), imgDimensions),
//              image2World(getPointFromLineAndX(0, line11), imgDimensions), cv::Scalar(255, 0, 0));

//     cv::line(img, image2World(getPointFromLineAndX(-2 * imgDimensions.width, lineB), imgDimensions),
//              image2World(getPointFromLineAndX(2 * imgDimensions.width, lineB), imgDimensions), cv::Scalar(255, 0, 255), 3);
//     cv::line(img, image2World(getPointFromLineAndX(-2 * imgDimensions.width, lineE), imgDimensions),
//              image2World(getPointFromLineAndX(2 * imgDimensions.width, lineE), imgDimensions), cv::Scalar(0, 255, 255), 3);

    cv::line(img, image2World(getPointFromLineAndX(-2 * imgDimensions.width, line11), imgDimensions),
             image2World(getPointFromLineAndX(2 * imgDimensions.width, line11), imgDimensions), cv::Scalar(255, 0, 0), 2);
    cv::line(img, image2World(getPointFromLineAndX(-2 * imgDimensions.width, line12), imgDimensions),
             image2World(getPointFromLineAndX(2 * imgDimensions.width, line12), imgDimensions), cv::Scalar(255, 0, 0));

//     cv::line(img, image2World(getPointFromLineAndX(-2 * imgDimensions.width, line13), imgDimensions),
//              image2World(getPointFromLineAndX(2 * imgDimensions.width, line13), imgDimensions), cv::Scalar(0, 0, 255), 2);
//     cv::line(img, image2World(getPointFromLineAndX(-2 * imgDimensions.width, line14), imgDimensions),
//              image2World(getPointFromLineAndX(2 * imgDimensions.width, line14), imgDimensions), cv::Scalar(0, 0, 255));

    cv::circle(img, image2World(b, imgDimensions), 10, cv::Scalar(128, 255, 128), 1);
    cv::circle(img, image2World(e, imgDimensions), 10, cv::Scalar(128, 255, 128), 3);

    cv::flip(img, img, 0);

    cv::namedWindow(windowName.c_str());
    cv::imshow(windowName.c_str(), img);

//     cv::waitKey(0);
//     exit(0);
}


#include <boost/compute/types/complex.hpp>


/*
Copyright (C) 2012 Mathias Eitz and Ronald Richter.
All rights reserved.

This file is part of the imdb library and is made available under
the terms of the BSD license (see the LICENSE file).
*/

#ifndef KMEANS_INIT_HPP
#define KMEANS_INIT_HPP

#include <vector>
#include <boost/random.hpp>

#include "distance.hpp"

template <class index_t, class collection_t>
void kmeans_init_random(std::vector<index_t>& centers, const collection_t& collection, std::size_t numclusters)
{
    assert(collection.size() >= numclusters);

    centers.resize(collection.size());
    for (std::size_t i = 0; i < centers.size(); i++) centers[i] = i;
    std::random_shuffle(centers.begin(), centers.end());
    centers.resize(numclusters);
}

template <class index_t, class collection_t, class dist_fn>
void kmeans_init_plusplus(std::vector<index_t>& result, const collection_t& collection, std::size_t numclusters, const dist_fn& distfn)
{
    assert(numclusters > 0);
    assert(collection.size() >= numclusters);

    typedef typename collection_t::value_type item_t;
    typedef boost::mt19937                    rng_t;
    typedef boost::uniform_real<double>       unirand_t;

    rng_t rng;

    boost::variate_generator<rng_t&, unirand_t> unirand(rng, unirand_t(0.0, 1.0));

    std::size_t numtrials = 2 + std::log(numclusters);

    // add first cluster, randomly chosen
    std::set<index_t> centers;
    index_t first = unirand() * collection.size();
    centers.insert(first);

    // compute distance between first cluster center and all others
    // and accumulate the distances that gives the current potential
    std::vector<double> dists(collection.size());
    double potential = 0.0;
    for (std::size_t i = 0; i < collection.size(); i++)
    {
        double d = distfn(collection[first], collection[i]);
        dists[i] = d*d;
        potential += dists[i];
    }
    std::cout << "kmeans++ init: numclusters=" << numclusters << " numtrials=" << numtrials << " collection.size=" << collection.size() << " init pot=" << potential << std::endl;

    // iteratively add centers
    for (std::size_t c = 1; c < numclusters; c++)
    {
        double min_potential = std::numeric_limits<double>::max();
        std::size_t best_index = 0;

        for (std::size_t i = 0; i < numtrials; i++)
        {
            std::size_t index;

            // get new center
            double r = unirand() * potential;
            for (index = 0; index < collection.size()-1 && r > dists[index]; index++)
            {
                r -= dists[index];
            }

            while (centers.count(index) > 0) index = (index + 1) % collection.size();

            // recompute potential
            double p = 0.0;
            for (std::size_t k = 0; k < collection.size(); k++)
            {
                double d = distfn(collection[index], collection[k]);
                p += std::min(dists[k], d*d);
            }

            if (p < min_potential)
            {
                min_potential = p;
                best_index = index;
            }
        }

        for (std::size_t i = 0; i < collection.size(); i++)
        {
            double d = distfn(collection[best_index], collection[i]);
            dists[i] = d*d;
        }

        potential = min_potential;

        centers.insert(best_index);

        std::cout << "new center " << c << ": potential=" << potential << " index=" << best_index << std::endl;
    }

    std::copy(centers.begin(), centers.end(), std::back_inserter(result));
}

enum KmeansInitAlgorithm
{
    KmeansInitRandom,
    KmeansInitPlusPlus
};

#endif // KMEANS_INIT_HPP


#include <boost/test/unit_test.hpp>
#include "libs/experiments/cm_results.h"
#include "libs/experiments/confusion_matric.h"
#include <tuple>
#include <vector>
#include <iostream>
#include <algorithm>
#include <numeric>

using input_data = std::tuple<exprs::value_type, exprs::value_type>;

inline constexpr exprs::value_type measured(const input_data& in) {
    return std::get<0>(in);
}

inline constexpr exprs::value_type predict(const input_data& in) {
    return std::get<1>(in);
}

std::ostream& operator << (std::ostream& os, const input_data& in) {
    return os<<"measure: "<<measured(in)<<", predicted: "<<predict(in);
}

using test_input_t = std::vector<input_data>;

BOOST_AUTO_TEST_CASE(test_all_true_cm)
{
    exprs::cm_results output;
    BOOST_TEST_REQUIRE(output.true_pos == 0u);
    BOOST_TEST_REQUIRE(output.true_neg == 0u);
    BOOST_TEST_REQUIRE(output.false_pos == 0u);
    BOOST_TEST_REQUIRE(output.false_neg == 0u);
    // test all true positive
    const test_input_t test_data{
        std::make_tuple(1.f, 1.f),
        std::make_tuple(1.f, 1.f),
        std::make_tuple(1.f, 1.f),
        std::make_tuple(1.f, 1.f)
    };

    output = std::accumulate(std::begin(test_data), std::end(test_data), output, [](const auto& t, const auto& v) {
                        return t + exprs::binclass::cm::calculate(measured(v), predict(v));
                    }
            );

    BOOST_TEST_REQUIRE(output.true_pos == test_data.size());
    BOOST_TEST_REQUIRE(output.true_neg == 0u);
    BOOST_TEST_REQUIRE(output.false_pos == 0u);
    BOOST_TEST_REQUIRE(output.false_neg == 0u);
}

BOOST_AUTO_TEST_CASE(test_all_true_negative)
{
    exprs::cm_results output;
    // test all true negative
    const test_input_t true_negatives{
        std::make_tuple(0.f, 0.f),
        std::make_tuple(0.f, 0.f),
        std::make_tuple(0.f, 0.f),
        std::make_tuple(0.f, 0.f)
    };
    output = std::accumulate(std::begin(true_negatives), std::end(true_negatives), output, [](const auto& t, const auto& v) {
                        return t + exprs::binclass::cm::calculate(measured(v), predict(v));
                    }
            );
    BOOST_TEST_REQUIRE(output.true_neg == true_negatives.size());
    BOOST_TEST_REQUIRE(output.true_pos == 0u);
    BOOST_TEST_REQUIRE(output.false_pos == 0u);
    BOOST_TEST_REQUIRE(output.false_neg == 0u);
}

BOOST_AUTO_TEST_CASE(test_all_false_negative)
{
    exprs::cm_results output;
    // test all true negative
    const test_input_t false_negatives{
        std::make_tuple(1.f, 0.f),
        std::make_tuple(1.f, 0.f),
        std::make_tuple(1.f, 0.f),
        std::make_tuple(1.f, 0.f)
    };
    output = std::accumulate(std::begin(false_negatives), std::end(false_negatives), output, [](const auto& t, const auto& v) {
                        return t + exprs::binclass::cm::calculate(measured(v), predict(v));
                    }
            );
    BOOST_TEST_REQUIRE(output.false_neg == false_negatives.size());
    BOOST_TEST_REQUIRE(output.true_pos == 0u);
    BOOST_TEST_REQUIRE(output.false_pos == 0u);
    BOOST_TEST_REQUIRE(output.true_neg == 0u);
}

BOOST_AUTO_TEST_CASE(test_all_false_positives)
{
    exprs::cm_results output;
    // test all true negative
    const test_input_t false_positives{
        std::make_tuple(0.f, 1.f),
        std::make_tuple(0.f, 1.f),
        std::make_tuple(0.f, 1.f),
        std::make_tuple(0.f, 1.f)
    };
    output = std::accumulate(std::begin(false_positives), std::end(false_positives), output, [](const auto& t, const auto& v) {
                        return t + exprs::binclass::cm::calculate(measured(v), predict(v));
                    }
            );
    BOOST_TEST_REQUIRE(output.false_pos == false_positives.size());
    BOOST_TEST_REQUIRE(output.true_pos == 0u);
    BOOST_TEST_REQUIRE(output.true_neg == 0u);
    BOOST_TEST_REQUIRE(output.false_neg == 0u);
}

BOOST_AUTO_TEST_CASE(test_5_tp_3_fp_2_tn_1_fn)
{
    exprs::cm_results output;
    // test all true negative
    const test_input_t mix_results{
        std::make_tuple(0.f, 1.f),  // FP
        std::make_tuple(1.f, 1.f),  // TP
        std::make_tuple(0.f, 0.f),  // TN
        std::make_tuple(1.f, 1.f),  // TP
        std::make_tuple(0.f, 1.f),  // FP
        std::make_tuple(0.f, 1.f),   // FP
        std::make_tuple(1.f, 1.f),  // TP
        std::make_tuple(1.f, 1.f),  // TP
        std::make_tuple(1.f, 0.f),  // FN
        std::make_tuple(1.f, 1.f),  // TP
        std::make_tuple(0.f, 0.f)  // TN

    };
    output = std::accumulate(std::begin(mix_results), std::end(mix_results), output, [](const auto& t, const auto& v) {
                        return t + exprs::binclass::cm::calculate(measured(v), predict(v));
                    }
            );
    BOOST_TEST_REQUIRE(output.false_pos == 3u);
    BOOST_TEST_REQUIRE(output.true_pos == 5u);
    BOOST_TEST_REQUIRE(output.true_neg == 2u);
    BOOST_TEST_REQUIRE(output.false_neg == 1u);
}


#include "wali/domains/RepresentativeString.hpp"

#include <utility>
#include <boost/functional/hash.hpp>

namespace wali {
  namespace domains {

    namespace {
      typedef boost::optional<std::string> OptString;

      OptString
      concat(OptString const & left,
             OptString const & right)
      {
        if (left && right) {
          return *left + *right;
        }
        else {
          // x * 0, 0 * x, or 0 * 0
          return OptString();
        }
      }

      OptString
      min(OptString const & left,
          OptString const & right)
      {
        if (left && right) {
          if (left->size() < right->size()) {
            return left;
          }
          if (left->size() == right->size()) {
            return std::min(*left, *right);
          }
          return right;
        }
        if (left) {
          return left;
        }
        return right;
      }
    }

    sem_elem_t RepresentativeString::one() const
    {
      static sem_elem_t elem = new RepresentativeString("");
      return elem;
    }

    sem_elem_t RepresentativeString::zero() const
    {
      static sem_elem_t z = new RepresentativeString();
      return z;
    }

    //---------------------------------
    // semiring operations
    //---------------------------------
    sem_elem_t RepresentativeString::extend( SemElem* rhs )
    {
      assert(dynamic_cast<RepresentativeString*>(rhs));
      RepresentativeString* that = static_cast< RepresentativeString* >(rhs);

      return new RepresentativeString(concat(this->_value, that->_value));
    }

    sem_elem_t RepresentativeString::combine( SemElem* rhs )
    {
      assert(dynamic_cast<RepresentativeString*>(rhs));
      RepresentativeString* that = static_cast< RepresentativeString* >(rhs);

      return new RepresentativeString(min(this->_value, that->_value));
    }

    bool RepresentativeString::equal(SemElem *rhs) const
    {
      assert(dynamic_cast<RepresentativeString*>(rhs));
      RepresentativeString* that = static_cast< RepresentativeString* >(rhs);

      if (!this->_value && !that->_value) {
        // both zero
        return true;
      }
      if (this->_value && that->_value) {
        // both nonzero
        return *this->_value == *that->_value;
      }
      return false;
    }

    bool RepresentativeString::containerLessThan(SemElem const *rhs) const
    {
      assert(dynamic_cast<RepresentativeString const *>(rhs));
      RepresentativeString const * that = static_cast< RepresentativeString const * >(rhs);

      return this->_value < that->_value;
    }

    size_t RepresentativeString::hash() const
    {
      if (_value) {
        boost::hash<std::string> hasher;
        return hasher(*_value);
      }
      // A random number. (Not kidding: I got it from random.org!)
      return 239023451;
    }

    //------------------------------------
    // output
    //------------------------------------
    std::ostream & RepresentativeString::print(std::ostream &out) const
    {
      if (_value) {
        out << "RepresentativeString(" << *_value << ")";
      }
      else {
        out << "RepresentativeString::zero";
      }
      return out;
    }

  }
}

// Yo, Emacs!
// Local Variables:
//   c-file-style: "ellemtel"
//   c-basic-offset: 2
// End:


// Copyright 2004 The Trustees of Indiana University.

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

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#include <boost/graph/sequential_vertex_coloring.hpp>
#include <boost/test/minimal.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <utility>

using namespace boost;

int test_main(int, char*[])
{
  typedef adjacency_list<listS, vecS, undirectedS> Graph;
  typedef graph_traits<Graph>::vertices_size_type vertices_size_type;
  typedef property_map<Graph, vertex_index_t>::const_type vertex_index_map;

  typedef std::pair<int, int> Edge;
  enum nodes {A, B, C, D, E, n};
  Edge edge_array[] = { Edge(A, C), Edge(B, B), Edge(B, D), Edge(B, E),
                        Edge(C, B), Edge(C, D), Edge(D, E), Edge(E, A),
                        Edge(E, B) };
  int m = sizeof(edge_array) / sizeof(Edge);
  Graph g(edge_array, edge_array + m, n);

  // Test with the normal order
  std::vector<vertices_size_type> color_vec(num_vertices(g));
  iterator_property_map<vertices_size_type*, vertex_index_map,
                        vertices_size_type, vertices_size_type&>
    color(&color_vec.front(), get(vertex_index, g));
  vertices_size_type num_colors = sequential_vertex_coloring(g, color);
  BOOST_CHECK(num_colors == 3);
  BOOST_CHECK(get(color, (vertices_size_type)A) == 0);
  BOOST_CHECK(get(color, (vertices_size_type)B) == 0);
  BOOST_CHECK(get(color, (vertices_size_type)C) == 1);
  BOOST_CHECK(get(color, (vertices_size_type)D) == 2);
  BOOST_CHECK(get(color, (vertices_size_type)E) == 1);
  return 0;
}


#include <boost/test/unit_test.hpp>

#include "../math/math.h"

USING_NAMESPACE(sway)

BOOST_AUTO_TEST_SUITE(TRectTestSuite)

/*!
 * Убеждаемся, что конструктор по умолчанию приводит все компоненты к нулю.
 */
BOOST_AUTO_TEST_CASE(TRectTestCase_DefaultConstructor) {
	math::TRect<s32> rect;

	BOOST_CHECK_EQUAL(rect.getL(), 0);
	BOOST_CHECK_EQUAL(rect.getT(), 0);
	BOOST_CHECK_EQUAL(rect.getR(), 0);
	BOOST_CHECK_EQUAL(rect.getB(), 0);
}

/*!
 * Убеждаемся, что конструктор устанавливает все значения компонентов в те,
 * которые были заданы.
 */
BOOST_AUTO_TEST_CASE(TRectTestCase_ComponentConstructor) {
	const s32 x = 8, y = 16, w = 32, h = 64;

	math::TRect<s32> rect = math::TRect<s32>(x, y, w, h);

	BOOST_CHECK_EQUAL(rect.getL(), x);
	BOOST_CHECK_EQUAL(rect.getT(), y);
	BOOST_CHECK_EQUAL(rect.getR(), x + w);
	BOOST_CHECK_EQUAL(rect.getB(), y + h);
	BOOST_CHECK_EQUAL(rect.getW(), w);
	BOOST_CHECK_EQUAL(rect.getH(), h);
}

BOOST_AUTO_TEST_CASE(TRectTestCase_Offset) {
	const s32 x = 8, y = 16, w = 32, h = 64;
	const s32 xoffset = 2, yoffset = 4;

	math::TRect<s32> rect = math::TRect<s32>(x, y, w, h);
	rect.offset(xoffset, yoffset);

	BOOST_CHECK_EQUAL(rect.getL(), x + xoffset);
	BOOST_CHECK_EQUAL(rect.getT(), y + yoffset);
	BOOST_CHECK_EQUAL(rect.getR(), x + xoffset + w);
	BOOST_CHECK_EQUAL(rect.getB(), y + yoffset + h);
	BOOST_CHECK_EQUAL(rect.getW(), w);
	BOOST_CHECK_EQUAL(rect.getH(), h);
}

/*!
 * Убеждаемся, что преобразование в TSize<type> проходит правильно.
 */
BOOST_AUTO_TEST_CASE(TRectTestCase_ConvertToSize) {
	const s32 x = 8, y = 16, w = 32, h = 64;

	math::TRect<s32> rect = math::TRect<s32>(x, y, w, h);
	math::TSize<s32> size = rect.toSize();

	BOOST_CHECK_EQUAL(size.getW(), w);
	BOOST_CHECK_EQUAL(size.getH(), h);
}

BOOST_AUTO_TEST_CASE(TRectTestCase_IsEmpty) {
	BOOST_CHECK(math::TRect<s32>(0, 0, 0, 32).isEmpty());
	BOOST_CHECK(math::TRect<s32>(0, 0, 32, 0).isEmpty());
}

BOOST_AUTO_TEST_SUITE_END()


#include <opencv2/opencv.hpp>
#include <Eigen/SparseCholesky>
#include <opencv2/core/eigen.hpp>
#include "Consts.h"
using namespace std;
using namespace cv;
using namespace Eigen;

Mat Get_Pixel_Indices_Mat(int rows, int cols) {//Fill the Pixels mat as indices of image pixels - by rows
	Mat Pixel_idx(rows, cols, DataType<int>::type, Scalar(0));
	int i, j, k = 0;
	for (i = 0; i < rows; i++) {
		for (j = 0; j < cols; j++) {
			Pixel_idx.at<int>(i,j) = k;
			k++;
		}
	}
	return Pixel_idx;
}

void Calc_Mean(Mat values, Mat * mean) {
	double result = 0;
	int i, j;
	for (i = 0; i < CHANNELS_NUM; i++) {
		for (j = 0; j < values.rows; j++) {
			result += values.at<double>(j, i);
		}
		(*mean).at<double>(0,i) = result/(double)values.rows;
		result = 0;
	}
	return;
}

Rect Get_Frame(int x, int y, int frame_size, int rows, int cols) {
	int width = frame_size, height = frame_size;
	int m = x - frame_size/2, n = y - frame_size/2;
	if (x > frame_size / 2 && x < cols - frame_size / 2 && y > frame_size / 2 && y < rows - frame_size / 2) {
		Rect R(m, n, width, height);
		return R;
	}
	else {
		if (m < 0) {
			width = frame_size + m;
			m = 0;
		}
		else if (cols - m - frame_size < 0) {
			width = cols - m;
		}
		if (n < 0) {
			height = frame_size + n;
			n = 0;
		}
		else if (rows - n - frame_size < 0) {
			height = rows - n;
		}
		Rect R(m, n, width, height);
		return R;
	}
}

Mat Laplacian_In_Patch(Mat Values, Mat Mean, Mat Covar, double regularization_parameter) {
	int i, j;
	Mat Image_Part_Mat_T(CHANNELS_NUM, Values.cols, DataType<double>::type);
	Mat Image_Part_Mat(Values.rows, CHANNELS_NUM, DataType<double>::type);
	Mat Covar_Part_Mat(CHANNELS_NUM, CHANNELS_NUM, DataType<double>::type);
	Mat Identity(CHANNELS_NUM, CHANNELS_NUM, DataType<double>::type);
	Mat Result;
	setIdentity(Identity, Scalar(1));
	Covar_Part_Mat = (regularization_parameter / (double)Values.rows)*Identity;
	Covar_Part_Mat = Covar + Covar_Part_Mat;
	Covar_Part_Mat = Covar_Part_Mat.inv();
	for (i = 0; i < CHANNELS_NUM; i++) {
		for (j = 0; j < Values.rows; j++) {
			Image_Part_Mat.at<double>(j, i) = Values.at<double>(j, i) - Mean.at<double>(0, i);
		}
	}
	transpose(Image_Part_Mat, Image_Part_Mat_T);
	Result = Image_Part_Mat*Covar_Part_Mat*Image_Part_Mat_T;
	for (i = 0; i < Result.rows; i++) {
		for (j = 0; j < Result.cols; j++) {
			Result.at<double>(i, j) = -Result.at<double>(i, j) - 1;
		}
	}
	Result = Result / Values.rows;
	return Result;
}

Mat Reshape_To_Channels_And_Normalize(Mat source){
	int i, j, k , Patch_reshaped_iterator = 0;
	Mat Patch_reshaped(source.rows*source.cols, CHANNELS_NUM, DataType<double>::type);
	for (j = 0; j < source.rows; j++) {
		for (i = 0; i < source.cols; i++) {
			for (k = 0; k < CHANNELS_NUM; k++) {
				Patch_reshaped.at<double>(Patch_reshaped_iterator, k) = (double)source.at<Vec3b>(j, i)[CHANNELS_NUM - k - 1] / (double)255;
			}
			Patch_reshaped_iterator++;
		}
	}
	return Patch_reshaped;
}

void Calc_Covar(Mat patch, Mat patch_idx, int patch_size, Mat Mean, Mat * Covar) {
	Mat Mean_Mul_Mean_T(CHANNELS_NUM, CHANNELS_NUM, DataType<double>::type);
	Mat Mean_T(CHANNELS_NUM, 1, DataType<double>::type);
	Mat Patch_T(CHANNELS_NUM, patch_size, DataType<double>::type);
	transpose(Mean, Mean_T);
	Mean_Mul_Mean_T = Mean_T*(Mean);
	transpose(patch, Patch_T);
	*Covar = Patch_T*patch / patch_size;
	*Covar = *Covar - Mean_Mul_Mean_T;
	return;
}

Mat Normalize_Mat(Mat Transmission) {
	int i, j;
	Mat normalized(Transmission.rows, Transmission.cols, DataType<double>::type);
	for (i = 0; i < Transmission.rows; i++) {
		for (j = 0; j < Transmission.cols; j++) {
			normalized.at<double>(i, j) = (double)Transmission.at<uchar>(i, j) / 255;
		}
	}
	return normalized;
}

void Assign_Lap_Values_To_Correct_Indices(Mat Values, Mat indices, vector<Triplet<double>> * tripletList) {
	int i, j, lap_index_x, lap_index_y;
	double res_value = 0;
	for (i = 0; i < Values.rows; i++) {
		for (j = 0; j < Values.cols; j++) {
			lap_index_x = indices.at<int>(0, i);
			lap_index_y = indices.at<int>(0, j);
			if (lap_index_y > lap_index_x) {
				continue;
			}
			res_value = Values.at<double>(i, j);
			if (i == j) {
				res_value += 1;
			}
			(*tripletList).push_back(Triplet<double>(lap_index_x, lap_index_y, res_value));
		}
	}
	return;
}

SparseMatrix<double> GetLaplacian(Mat image, int frame_size, double regularization_parameter) { //frame_size is supposably 3
	int rows = image.rows;
	int cols = image.cols;
	int size = rows*cols;
	int i, area, j = 0;
	int num_of_frames_for_single_pixel = frame_size*frame_size;
	int entries_per_column_estimation = frame_size * 2 - 1;
	int estimation_of_entries = size*entries_per_column_estimation;
	Mat mean(1, CHANNELS_NUM, DataType<double>::type, Scalar(0));
	Mat covar(CHANNELS_NUM, CHANNELS_NUM, DataType<double>::type, Scalar(0));
	Mat Patch, Patch_idx, Patch_Reshaped, Lap_In_Patch;
	Mat Pixel_idx = Get_Pixel_Indices_Mat(rows, cols);
	Mat Mean(1, CHANNELS_NUM, DataType<double>::type, Scalar(0));
	Mat Covar(CHANNELS_NUM, CHANNELS_NUM, DataType<double>::type, Scalar(0));
	vector<Triplet<double>> tripletList;
	SparseMatrix<double> result_LowerTriangle(size, size);
	tripletList.reserve(estimation_of_entries);
	result_LowerTriangle.reserve(VectorXi::Constant(size, entries_per_column_estimation));//CORRECT RESERVE FOR LOWER TRIANGLEVIEW
	for (i = 0; i < rows; i++) {
		for (j = 0; j < cols; j++) {
			Rect R = Get_Frame(j, i, frame_size, rows, cols);
			area = R.area();
			Patch = image(R);
			Patch_Reshaped = Reshape_To_Channels_And_Normalize(Patch);
			Calc_Mean(Patch_Reshaped, &Mean);
			Calc_Covar(Patch_Reshaped, Patch_idx, area, Mean, &Covar);
			Lap_In_Patch = Laplacian_In_Patch(Patch_Reshaped, Mean, Covar, regularization_parameter);
			Patch_idx = Pixel_idx(R);
			if (!Patch_idx.isContinuous())
			{
				Patch_idx = Patch_idx.clone();
			}
			Patch_idx = Patch_idx.reshape(0, 1);
			Assign_Lap_Values_To_Correct_Indices(Lap_In_Patch, Patch_idx, &tripletList);
		}
	}
	result_LowerTriangle.setFromTriplets(tripletList.begin(), tripletList.end());
	SparseMatrix<double> result = result_LowerTriangle.selfadjointView<Lower>();
	return result;
}

Mat SoftMatting(Mat Transmission, Mat image, int frame_size, double regularization_parameter, double lambda) {
	int i, j;
	int Laplacian_Size = image.rows*image.cols;
	Mat t;
	Mat normalized_Transmission = Normalize_Mat(Transmission);
	Mat b = normalized_Transmission.reshape(0, Laplacian_Size);
	Mat Transmission_new(Transmission.rows, Transmission.cols, DataType<uchar>::type);
	Matrix<double, Dynamic, Dynamic, ColMajor> b_Eigen(b.rows, b.cols);
	Map<VectorXd> b_Vector(b_Eigen.data(), b_Eigen.size());
	SparseMatrix<double> Laplacian = GetLaplacian(image, frame_size, regularization_parameter);
	SparseMatrix<double> Identity_Mat(Laplacian_Size, Laplacian_Size);
	SparseMatrix<double> A(Laplacian_Size, Laplacian_Size);
	SimplicialLLT<SparseMatrix<double>> solver;
	VectorXd x;
	b = lambda*b;
	cv2eigen(b, b_Eigen);
	Identity_Mat.setIdentity();
	A = lambda*Identity_Mat;
	A += Laplacian;
	x = solver.compute(A).solve(b_Vector);//Solve Ax = b for x
	b_Eigen.col(0) = x;
	eigen2cv(b_Eigen, t);
	t = t.reshape(0, Transmission.rows);
	for (i = 0; i < Transmission.rows; i++) {
		for (j = 0; j < Transmission.cols; j++) {
			Transmission_new.at<uchar>(i, j) = (uchar)min(255, (int)(t.at<double>(i, j) * 255));
		}
	}
	return Transmission_new;
}


/*
 * Copyright 2009-2020 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE espfit_test

// Third party includes
#include <boost/test/unit_test.hpp>

// Local VOTCA includes
#include "votca/tools/eigenio_matrixmarket.h"
#include "votca/xtp/espfit.h"
#include "votca/xtp/logger.h"
#include "votca/xtp/orbitals.h"
#include <libint2/initialize.h>
using namespace votca::xtp;
using namespace votca;

BOOST_AUTO_TEST_SUITE(espfit_test)

BOOST_AUTO_TEST_CASE(esp_charges) {
  libint2::initialize();
  Orbitals orbitals;
  orbitals.QMAtoms().LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) +
                                  "/espfit/molecule.xyz");
  orbitals.SetupDftBasis(std::string(XTP_TEST_DATA_FOLDER) +
                         "/espfit/3-21G.xml");
  orbitals.setNumberOfOccupiedLevels(5);

  Eigen::MatrixXd MOs = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/espfit/MOs.mm");
  orbitals.MOs().eigenvectors() = MOs;
  orbitals.MOs().eigenvalues() = Eigen::VectorXd::Ones(13);
  QMState gs = QMState("n");
  Logger log;
  Espfit esp = Espfit(log);
  esp.setUseSVD(1e-8);
  StaticSegment result = esp.Fit2Density(orbitals, gs, "xcoarse");
  Eigen::VectorXd pcharges = Eigen::VectorXd::Zero(orbitals.QMAtoms().size());
  Index index = 0;
  for (const auto& site : result) {
    pcharges(index) = site.getCharge();
    index++;
  }
  Eigen::VectorXd p_ref = Eigen::VectorXd::Zero(3);
  p_ref << -0.827774, 0.413985, 0.41379;

  bool check_esp_num = p_ref.isApprox(pcharges, 0.01);
  if (!check_esp_num) {
    std::cout << "ref" << std::endl;
    std::cout << p_ref << std::endl;
    std::cout << "calc" << std::endl;
    std::cout << pcharges << std::endl;
  }
  BOOST_CHECK_EQUAL(check_esp_num, 1);

  std::vector<std::pair<Index, Index> > pairconstraint;
  std::pair<Index, Index> p1;
  p1.first = 1;
  p1.second = 2;
  pairconstraint.push_back(p1);
  Espfit esp2 = Espfit(log);
  esp2.setUseSVD(1e-8);
  esp2.setPairConstraint(pairconstraint);
  StaticSegment result2 = esp2.Fit2Density(orbitals, gs, "xcoarse");
  Eigen::VectorXd pcharges_equal = Eigen::VectorXd::Zero(result2.size());
  index = 0;
  for (const auto& site : result2) {
    pcharges_equal(index) = site.getCharge();
    index++;
  }

  bool check_p1 = (std::abs(pcharges_equal(1) - pcharges_equal(2)) < 1e-6);
  BOOST_CHECK_EQUAL(check_p1, 1);

  std::vector<QMFragment<double> > regionconstraint;

  std::string indeces = "1:2";
  QMFragment<double> reg = QMFragment<double>(0, indeces);
  reg.value() = 1.0;
  regionconstraint.push_back(reg);
  Espfit esp3 = Espfit(log);
  esp3.setRegionConstraint(regionconstraint);
  esp3.setUseSVD(1e-8);
  StaticSegment result3 = esp3.Fit2Density(orbitals, gs, "xcoarse");
  Eigen::VectorXd pcharges_reg =
      Eigen::VectorXd::Zero(orbitals.QMAtoms().size());
  index = 0;

  for (const auto& site : result3) {
    pcharges_reg(index) = site.getCharge();
    index++;
  }

  bool check_reg = (std::abs(pcharges_reg.segment(1, 2).sum() - 1.0) < 1e-6);
  if (!check_reg) {
    std::cout << "All charges " << pcharges_reg << std::endl;
    std::cout << "Sum of charges 1,2,3 should equal 1:"
              << pcharges_reg.segment(1, 2).sum() << std::endl;
  }
  BOOST_CHECK_EQUAL(check_reg, 1);

  libint2::finalize();
}

BOOST_AUTO_TEST_SUITE_END()


#include "AnalysisSolver.h"

#include <sofa/core/ObjectFactory.h>

#include <Eigen/SVD>
#include <Eigen/Eigenvalues>

namespace sofa {
namespace component {
namespace linearsolver {

SOFA_DECL_CLASS(AnalysisSolver)
int AnalysisSolverClass = core::RegisterObject("Analysis solver: runs other KKTSolvers successively on a given problem and performs extra analysis on KKT system").add< AnalysisSolver >();

AnalysisSolver::AnalysisSolver()
    : condest(initData(&condest, false, "condest", "compute condition number with svd"))
    , eigenvaluesign(initData(&eigenvaluesign, false, "eigenvaluesign", "computing the sign of the eigenvalues (of the implicit matrix H)"))
    , dump_qp(initData(&dump_qp, "dump_qp", "dump qp to file if non-empty"))

{}

void AnalysisSolver::init() {

	solvers.clear();
	this->getContext()->get<KKTSolver>( &solvers, core::objectmodel::BaseContext::Local );

	if( solvers.size() < 2 ) {
		std::cerr << "warning: no other kkt solvers found" << std::endl;
	} else {
		std::cout << "AnalysisSolver: dynamics/correction will use "
				  << solvers.back()->getName()
				  << std::endl;
	}
}

void AnalysisSolver::factor(const system_type& system) {

	// start at 1 since 0 is current solver
    for( unsigned i = 1, n = solvers.size() ; i < n; ++i ) {
		solvers[i]->factor(system);
	}

    typedef system_type::dmat dmat;

    if( condest.getValue() ) {

        dmat kkt;

        kkt.setZero(system.size(), system.size());

        kkt <<
            dmat(system.H), dmat(-system.J.transpose()),
            dmat(-system.J), dmat(-system.C);

        system_type::vec eigen = kkt.jacobiSvd().singularValues();

        real min = eigen.tail<1>()(0);
        real max = eigen(0);

        if( min < std::numeric_limits<real>::epsilon() ) std::cout << "AnalysisSolver: singular KKT system"<<std::endl;
        else
        {
            const real cond = max/min;
            std::cout << "condition number (KKT system): " << cond << " ("<<max<<"/"<<min<<")"<<std::endl;
            std::cout << "required precision (KKT system):  "<<ceil(log(cond))<<" bits"<<std::endl;
        }

        eigen = dmat(system.H).jacobiSvd().singularValues();

        min = eigen.tail<1>()(0);
        max = eigen(0);

        if( min < std::numeric_limits<real>::epsilon() ) std::cout << "AnalysisSolver: singular implicit system"<<std::endl;
        else
        {
            const real cond = max/min;
            std::cout << "condition number (implicit system): " << cond << " ("<<max<<"/"<<min<<")"<<std::endl;
            std::cout << "required precision (implicit system):  "<<ceil(log(cond))<<" bits"<<std::endl;
        }
    }

     if( eigenvaluesign.getValue() ) {

        Eigen::EigenSolver<dmat> es( dmat(system.H) );
//        Eigen::MatrixXcd D = es.eigenvalues().asDiagonal();
        unsigned positive = 0, negative = 0;
        for( dmat::Index i =0 ; i< es.eigenvalues().rows() ; ++i )
            if( es.eigenvalues()[i].real() < 0 ) negative++;
            else positive++;

        std::cout << "eigenvalues H: neg="<<negative<<" pos="<<positive<<std::endl;
        if( negative )
        {
            getContext()->getRootContext()->setAnimate(false);

            std::cerr<<es.eigenvalues().array().abs().minCoeff()<<std::endl;
            std::cerr<<" $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$\n";
        }

    }

    // TODO add more as needed
}

static void write_qp(std::ofstream& out,
                     const AssembledSystem& sys,
                     const AssembledSystem::vec& rhs) {

    const char endl = '\n';

    out << sys.m << ' ' << sys.n << endl;

    out << sys.H << endl;
    out << -rhs.head(sys.m).transpose() << endl;
    out << sys.P << endl;

    if( sys.n ) {
        out << sys.J << endl;
        out << rhs.tail(sys.n).transpose() << endl;

        // TODO unilateral mask !
    }

}

// solution is that of the first solver
void AnalysisSolver::correct(vec& res,
                             const system_type& sys,
                             const vec& rhs,
                             real damping) const {
	assert( solvers.size() > 1 );
	solvers.back()->correct(res, sys, rhs, damping);

    if(!dump_qp.getValue().empty() ) {
        const std::string filename = dump_qp.getValue() + ".correction";
        std::ofstream out(filename.c_str());

        write_qp(out, sys, rhs);
    }
}

// solution is that of the last solver
void AnalysisSolver::solve(vec& res,
							const system_type& sys,
							const vec& rhs) const {

    vec backup = res; // backup initial solution

	// start at 1 since 0 is current solver
    for( unsigned i = 1, n = solvers.size(); i < n; ++i ) {
		res = backup;
		solvers[i]->solve(res, sys, rhs);
	}

    if(!dump_qp.getValue().empty() ) {
        const std::string filename = dump_qp.getValue() + ".correction";
        std::ofstream out(filename.c_str());

        write_qp(out, sys, rhs);
    }

}

}
}
}


#include <algorithm>
#include <boost/optional.hpp>
#include <climits>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <list>
#include <map>
#include <numeric>
#include <queue>
#include <set>
#include <sstream>
#include <string>
#include <tuple>
#include <type_traits>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#define REP(i, n) for(int i = 0, i##_MACRO = (n); i < i##_MACRO; i++)
#define RANGE(i, a, b) for(int i = (a), i##_MACRO = (b); i < i##_MACRO; i++)
#define EACH(e, a) for(auto&& e : a)
#define ALL(a) std::begin(a), std::end(a)
#define RALL(a) std::rbegin(a), std::rend(a)
#define FILL(a, n) memset((a), n, sizeof(a))
#define FILLZ(a) FILL(a, 0)
#define INT(x) (static_cast<int>(x))

using namespace std;

using ll = long long;
using VI = vector<int>;
using VI2D = vector<vector<int>>;

constexpr int INF = 2e9;
constexpr double EPS = 1e-10;
constexpr double PI = acos(-1.0);

constexpr int dx[] = {-1, 0, 1, 0};
constexpr int dy[] = {0, -1, 0, 1};

template <typename T>
constexpr int sign(T x) {
	return x < 0 ? -1 : x > 0 ? 1 : 0;
}

template <>
constexpr int sign(double x) {
	return x < -EPS ? -1 : x > EPS ? 1 : 0;
}

template <typename T, typename U>
constexpr void chmax(T& m, U x) {
	m = max(m, x);
}

template <typename T, typename U>
constexpr void chmin(T& m, U x) {
	m = min(m, x);
}

template <typename T>
constexpr T square(T x) {
	return x * x;
}

constexpr double deg_to_rad(double deg) {
	return PI * deg / 180.0;
}

constexpr double rad_to_deg(double rad) {
	return rad * 180 / PI;
}

// 二次元ベクトルクラス
class Vec2d {
	double _x;
	double _y;

	public:
	// 原点の位置ベクトル
	static constexpr Vec2d origin() { return Vec2d(0.0, 0.0); }

	// ゼロベクトル
	static constexpr Vec2d zero() { return Vec2d::origin(); }

	constexpr Vec2d(double a, double b) : _x(a), _y(b) {}

	constexpr double x() const { return this->_x; }
	constexpr double y() const { return this->_y; }

	// 外積
	constexpr double det(const Vec2d& rhs) const {
		return this->x() * rhs.y() - this->y() * rhs.x();
	}

	// 内積
	constexpr double dot(const Vec2d& rhs) const {
		return this->x() * rhs.x() + this->y() * rhs.y();
	}

	// 長さ
	constexpr double length() const { return this->distance(Vec2d::origin()); }

	// 2つの位置ベクトル間のユークリッド距離
	constexpr double distance(const Vec2d& rhs) const {
		return std::sqrt(square(this->x() - rhs.x()) +
						 square(this->y() - rhs.y()));
	}

	// 2つの位置ベクトル間のマンハッタン距離
	constexpr double manhattan_distance(const Vec2d& rhs) const {
		return std::abs(this->x() - rhs.x()) + std::abs(this->y() - rhs.y());
	}

	// ベクトルとx軸のなす角
	constexpr double argument() const {
		return std::atan2(this->y(), this->x());
	}

	// 反時計回りに回転したベクトル
	constexpr Vec2d rotate(double rad) const {
		double xx = this->x();
		double yy = this->y();
		return Vec2d(xx * cos(rad) - yy * sin(rad),
					 xx * sin(rad) + yy * cos(rad));
	}

	// 単位ベクトル
	constexpr Vec2d unit() const {
		double len = this->length();
		return Vec2d(this->x() / len, this->y() / len);
	}

	// 法線ベクトル
	constexpr Vec2d normal() const {
		double len = this->length();
		return Vec2d(this->y() / len, -this->x() / len);
	}

	// 平行判定
	constexpr bool is_parallel(const Vec2d& rhs) const { return false; }

	// ベクトル和
	constexpr Vec2d operator+() const { return *this; }
	constexpr Vec2d operator+(const Vec2d& rhs) const {
		return Vec2d(this->x() + rhs.x(), this->y() + rhs.y());
	}

	// ベクトル差
	constexpr Vec2d operator-() const { return Vec2d(-this->x(), -this->y()); }
	constexpr Vec2d operator-(const Vec2d& rhs) const {
		return Vec2d(this->x() - rhs.x(), this->y() - rhs.y());
	}

	// スカラー積
	constexpr Vec2d operator*(const double rhs) const {
		return Vec2d(this->x() * rhs, this->y() * rhs);
	}
	constexpr Vec2d operator/(const double rhs) const {
		return Vec2d(this->x() / rhs, this->y() / rhs);
	}

	constexpr bool operator<(const Vec2d& rhs) const {
		return sign(this->x()) ? this->y() < rhs.y() : this->x() < rhs.x();
	}

	private:
};

// 点の進行方向
constexpr int ccw(Vec2d a, Vec2d b, Vec2d c) {
	Vec2d ab = b - a;
	Vec2d ac = c - a;
	int det = ab.det(ac);
	if(det > 0) {
		return 1; // 反時計回り
	}
	if(det < 0) {
		return -1; // 時計回り
	}
	if(ab.dot(ac) < 0) {
		return 2; // c-a-b
	}
	if(ab.normal() < ac.normal()) {
		return -2; // a-b-c
	}
	return 0; // a-c-b
}

int main() {
	double a, b, h, m;
	cin >> a >> b >> h >> m;
	Vec2d hvec(0, a);
	Vec2d mvec(0, b);
	double harg = -deg_to_rad(h * 30 + (m / 2));
	double marg = -deg_to_rad(m * 6);
	Vec2d moved_hour_vec = hvec.rotate(harg);
	Vec2d moved_min_vec = mvec.rotate(marg);
	double dist = moved_min_vec.distance(moved_hour_vec);
	cout << setprecision(16) << dist << endl;
	return 0;
}


//============================================================================
// Name        : OctoROACH_Framework.cpp
// Author      : Luis Valbuena
// Version     :
// Copyright   : Marhes laboratory UNM
// Description : OctoROACH Framework.
//============================================================================

#include "vicon_pos.h"
#include "stdlib.h"
#include "Client.h"
#include "librerialuis.h"
#include "math.h"

#include <boost/python.hpp>
using namespace boost::python;

using namespace std;

//#define ROBOTIP  "localhost"
//#define ROBOTPORT 6665

struct cpluspluscode_Wrap4Python
{
	cpluspluscode_Wrap4Python()
	{
		a_11= -0.019709753875392;
		a_21= -0.015173074449847;
		a_31= -0.999690603843965;
		a_12= -0.861839978997550;
		a_22= -0.506579426897183;
		a_32=  0.024680657325591;
		a_13= -0.506797174620847;
		a_23=  0.862059778702301;
		a_33= -0.003092206342704;

		std::string Models[4];
		std::string temp;

		temp="OctoROACH";
		Models[0]=temp;

		temp="OctoROACH2";
		Models[1]=temp;

		temp="quadrotor_2b";
		Models[2]=temp;

		temp="quadrotor_3";
		Models[3]=temp;

		//v = new vicon_pos("OctoROACH","OctoROACH");
		v = new vicon_pos();
		v->update();
		indexx_OCTO=v->find_sub("OctoROACH");
		indexx_OCTO2=v->find_sub("OctoROACH2");
	 	indexx_Quad1=v->find_sub("quadrotor_2b");
	 	indexx_Quad2=v->find_sub("quadrotor_3");
		cout << "Index OctoROACH = " << indexx_OCTO <<", "<< "Index OctoROACH2 = "<< indexx_OCTO2 <<", "<< "Index quad 1 = "<< indexx_Quad1 <<", "<< "Index quad 2 = "<< indexx_Quad2 <<", "  <<"\n";
	}
	void retrievePositioning()
	{

		float s_euler_x,c_euler_x,s_euler_y,c_euler_y,s_euler_z,c_euler_z;
		float r_11,r_12,r_13,r_21,r_22,r_23,r_31,r_32,r_33;
		float euler_x,euler_y,euler_z;
		float tempx,tempy;

		//cout << "index = " << indexx << " \n";

		v->update();

///////////////////////OCTOROACH_ONE////////////////////
		v->get_subject_marker(indexx_OCTO,"Origin",tempx,tempy,pos_z_octo1);
		v->get_subject_euler_xyz(indexx_OCTO,euler_x,euler_y,euler_z);

		s_euler_x=sin(euler_x);
		c_euler_x=cos(euler_x);
		s_euler_y=sin(euler_y);
		c_euler_y=cos(euler_y);
		s_euler_z=sin(euler_z);
		c_euler_z=cos(euler_z);

		r_11=c_euler_y*c_euler_z;
		r_12=-c_euler_y*s_euler_z;
		r_13=s_euler_y;
		r_21=c_euler_x*s_euler_z + c_euler_z*s_euler_x*s_euler_y;
		r_22=c_euler_x*c_euler_z - s_euler_x*s_euler_y*s_euler_z;
		r_23=-c_euler_y*s_euler_x;
		r_31=s_euler_x*s_euler_z - c_euler_x*c_euler_z*s_euler_y;
		r_32=c_euler_z*s_euler_x + c_euler_x*s_euler_y*s_euler_z;
		r_33=c_euler_x*c_euler_y;

		alpha_octo1=-atan2Mia(a_13*r_21 + a_23*r_22 + a_33*r_23,a_13*r_31 + a_23*r_32 + a_33*r_33);
		betha_octo1= asin( a_13*r_11 + a_23*r_12 + a_33*r_13 );
		theta_octo1=-atan2Mia(a_12*r_11 + a_22*r_12 + a_32*r_13,a_11*r_11 + a_21*r_12 + a_31*r_13);

		//Fixing origin offset
		pos_x_octo1 = (76.2*cos(theta_octo1)) + (20.3*sin(theta_octo1)) + tempx;
		pos_y_octo1 = (76.2*sin(theta_octo1)) + (-20.3*cos(theta_octo1)) + tempy;

	//cout << "position Octo 1= [" << pos_x_octo1<<" "<<pos_y_octo1<<" "<<pos_z_octo1 <<"]"<< "   orientation = ["<<(180/PI)*alpha_octo1<<" "<< (180/PI)*betha_octo1<<" "<< (180/PI)*theta_octo1 <<"]"  <<"\n";
///////////////////////OCTOROACH_ONE////////////////////

///////////////////////OCTOROACH_TWO////////////////////
		v->get_subject_marker(indexx_OCTO2,"Origin",tempx,tempy,pos_z_octo2);
		v->get_subject_euler_xyz(indexx_OCTO2,euler_x,euler_y,euler_z);

		s_euler_x=sin(euler_x);
		c_euler_x=cos(euler_x);
		s_euler_y=sin(euler_y);
		c_euler_y=cos(euler_y);
		s_euler_z=sin(euler_z);
		c_euler_z=cos(euler_z);

		r_11=c_euler_y*c_euler_z;
		r_12=-c_euler_y*s_euler_z;
		r_13=s_euler_y;
		r_21=c_euler_x*s_euler_z + c_euler_z*s_euler_x*s_euler_y;
		r_22=c_euler_x*c_euler_z - s_euler_x*s_euler_y*s_euler_z;
		r_23=-c_euler_y*s_euler_x;
		r_31=s_euler_x*s_euler_z - c_euler_x*c_euler_z*s_euler_y;
		r_32=c_euler_z*s_euler_x + c_euler_x*s_euler_y*s_euler_z;
		r_33=c_euler_x*c_euler_y;

		alpha_octo2=-atan2Mia(a_13*r_21 + a_23*r_22 + a_33*r_23,a_13*r_31 + a_23*r_32 + a_33*r_33);
		betha_octo2= asin( a_13*r_11 + a_23*r_12 + a_33*r_13 );
		theta_octo2=-atan2Mia(a_12*r_11 + a_22*r_12 + a_32*r_13,a_11*r_11 + a_21*r_12 + a_31*r_13);

		//Fixing origin offset
		pos_x_octo2 = (76.2*cos(theta_octo2)) + (20.3*sin(theta_octo2)) + tempx;
		pos_y_octo2 = (76.2*sin(theta_octo2)) + (-20.3*cos(theta_octo2)) + tempy;

	//cout << "position Octo 2= [" << pos_x_octo2<<" "<<pos_y_octo2<<" "<<pos_z_octo2 <<"]"<< "   orientation = ["<<(180/PI)*alpha_octo2<<" "<< (180/PI)*betha_octo2<<" "<< (180/PI)*theta_octo2 <<"]"  <<"\n";
///////////////////////OCTOROACH_TWO////////////////////

///////////////////////QUAD_ONE////////////////////
		v->get_subject_marker(indexx_Quad1,"CoG",pos_x_quad1,pos_y_quad1,pos_z_quad1);
		v->get_subject_euler_xyz(indexx_Quad1,alpha_quad1,betha_quad1,theta_quad1);

	//cout << "position Quad 1= [" << pos_x_quad1<<" "<<pos_y_quad1<<" "<<pos_z_quad1<<"]"<< "   orientation = ["<<(180/PI)*alpha_quad1<<" "<< (180/PI)*betha_quad1<<" "<< (180/PI)*theta_quad1<<"]"  <<"\n";
///////////////////////QUAD_ONE////////////////////

///////////////////////QUAD_TWO////////////////////
		v->get_subject_marker(indexx_Quad2,"CoG",pos_x_quad2,pos_y_quad2,pos_z_quad2);
		v->get_subject_euler_xyz(indexx_Quad2,alpha_quad2,betha_quad2,theta_quad2);

	//cout << "position Quad 2= [" << pos_x_quad2<<" "<<pos_y_quad2<<" "<<pos_z_quad2<<"]"<< "   orientation = ["<<(180/PI)*alpha_quad2<<" "<< (180/PI)*betha_quad2<<" "<< (180/PI)*theta_quad2<<"]"  <<"\n";
///////////////////////QUAD_TWO////////////////////
	}
	double octo1_return_pos_x(){return double(pos_x_octo1);}
	double octo1_return_pos_y(){return double(pos_y_octo1);}
	double octo1_return_pos_z(){return double(pos_z_octo1);}
	double octo1_return_orientation_alpha(){return double(alpha_octo1);}
	double octo1_return_orientation_betha(){return double(betha_octo1);}
	double octo1_return_orientation_theta(){return theta_octo1;}

	double octo2_return_pos_x(){return double(pos_x_octo2);}
	double octo2_return_pos_y(){return double(pos_y_octo2);}
	double octo2_return_pos_z(){return double(pos_z_octo2);}
	double octo2_return_orientation_alpha(){return double(alpha_octo2);}
	double octo2_return_orientation_betha(){return double(betha_octo2);}
	double octo2_return_orientation_theta(){return theta_octo2;}

	double quad1_return_pos_x(){return double(pos_x_quad1);}
	double quad1_return_pos_y(){return double(pos_y_quad1);}
	double quad1_return_pos_z(){return double(pos_z_quad1);}
	double quad1_return_orientation_alpha(){return double(alpha_quad1);}
	double quad1_return_orientation_betha(){return double(betha_quad1);}
	double quad1_return_orientation_theta(){return theta_quad1;}

	double quad2_return_pos_x(){return double(pos_x_quad2);}
	double quad2_return_pos_y(){return double(pos_y_quad2);}
	double quad2_return_pos_z(){return double(pos_z_quad2);}
	double quad2_return_orientation_alpha(){return double(alpha_quad2);}
	double quad2_return_orientation_betha(){return double(betha_quad2);}
	double quad2_return_orientation_theta(){return theta_quad2;}

	float a_11,a_12,a_13,a_21,a_22,a_23,a_31,a_32,a_33;
	float pos_x_octo1,pos_y_octo1,pos_z_octo1;
	float alpha_octo1,betha_octo1,theta_octo1;

	float pos_x_octo2,pos_y_octo2,pos_z_octo2;
	float alpha_octo2,betha_octo2,theta_octo2;

	float pos_x_quad1,pos_y_quad1,pos_z_quad1;
	float alpha_quad1,betha_quad1,theta_quad1;

	float pos_x_quad2,pos_y_quad2,pos_z_quad2;
	float alpha_quad2,betha_quad2,theta_quad2;

	int indexx_OCTO;
	int indexx_OCTO2;
	int indexx_Quad1;
	int indexx_Quad2;

	vicon_pos* v;
};

BOOST_PYTHON_MODULE(cpluspluscode_Wrap4Python_ext)
{
	class_<cpluspluscode_Wrap4Python>("cpluspluscode_Wrap4Python", init<>())
			.def("retrievePositioning", &cpluspluscode_Wrap4Python::retrievePositioning)
			.def("octo1_return_pos_x",&cpluspluscode_Wrap4Python::octo1_return_pos_x)
			.def("octo1_return_pos_y",&cpluspluscode_Wrap4Python::octo1_return_pos_y)
			.def("octo1_return_pos_z",&cpluspluscode_Wrap4Python::octo1_return_pos_z)
			.def("octo1_return_orientation_alpha",&cpluspluscode_Wrap4Python::octo1_return_orientation_alpha)
			.def("octo1_return_orientation_betha",&cpluspluscode_Wrap4Python::octo1_return_orientation_betha)
			.def("octo1_return_orientation_theta",&cpluspluscode_Wrap4Python::octo1_return_orientation_theta)
			.def("octo2_return_pos_x",&cpluspluscode_Wrap4Python::octo2_return_pos_x)
			.def("octo2_return_pos_y",&cpluspluscode_Wrap4Python::octo2_return_pos_y)
			.def("octo2_return_pos_z",&cpluspluscode_Wrap4Python::octo2_return_pos_z)
			.def("octo2_return_orientation_alpha",&cpluspluscode_Wrap4Python::octo2_return_orientation_alpha)
			.def("octo2_return_orientation_betha",&cpluspluscode_Wrap4Python::octo2_return_orientation_betha)
			.def("octo2_return_orientation_theta",&cpluspluscode_Wrap4Python::octo2_return_orientation_theta)
			.def("quad1_return_pos_x",&cpluspluscode_Wrap4Python::quad1_return_pos_x)
			.def("quad1_return_pos_y",&cpluspluscode_Wrap4Python::quad1_return_pos_y)
			.def("quad1_return_pos_z",&cpluspluscode_Wrap4Python::quad1_return_pos_z)
			.def("quad1_return_orientation_alpha",&cpluspluscode_Wrap4Python::quad1_return_orientation_alpha)
			.def("quad1_return_orientation_betha",&cpluspluscode_Wrap4Python::quad1_return_orientation_betha)
			.def("quad1_return_orientation_theta",&cpluspluscode_Wrap4Python::quad1_return_orientation_theta)
			.def("quad2_return_pos_x",&cpluspluscode_Wrap4Python::quad2_return_pos_x)
			.def("quad2_return_pos_y",&cpluspluscode_Wrap4Python::quad2_return_pos_y)
			.def("quad2_return_pos_z",&cpluspluscode_Wrap4Python::quad2_return_pos_z)
			.def("quad2_return_orientation_alpha",&cpluspluscode_Wrap4Python::quad2_return_orientation_alpha)
			.def("quad2_return_orientation_betha",&cpluspluscode_Wrap4Python::quad2_return_orientation_betha)
			.def("quad2_return_orientation_theta",&cpluspluscode_Wrap4Python::quad2_return_orientation_theta)
	;
}
/*
int main(){

	float s_euler_x,c_euler_x,s_euler_y,c_euler_y,s_euler_z,c_euler_z;
	float a_11,a_12,a_13,a_21,a_22,a_23,a_31,a_32,a_33;
	float r_11,r_12,r_13,r_21,r_22,r_23,r_31,r_32,r_33;

	float alpha,betha,theta;

	a_11= -0.019709753875392;
	a_21= -0.015173074449847;
	a_31= -0.999690603843965;
	a_12= -0.861839978997550;
	a_22= -0.506579426897183;
	a_32=  0.024680657325591;
	a_13= -0.506797174620847;
	a_23=  0.862059778702301;
	a_33= -0.003092206342704;

	int a=0, indexx;

	float pos_x,pos_y,pos_z,euler_x,euler_y,euler_z;

	vicon_pos* v = new vicon_pos("OctoROACH","OctoROACH");
	v->update();

	indexx=v->find_sub("OctoROACH");
	cout << "index = " << indexx << " \n";

	for(a=0;a<500;a++)
	{
		v->update();

		//v->get_subject_coord(indexx,pos_x,pos_y,pos_z);

		v->get_subject_marker(indexx,"Origin",pos_x,pos_y,pos_z);
		v->get_subject_euler_xyz(indexx,euler_x,euler_y,euler_z);

		s_euler_x=sin(euler_x);
		c_euler_x=cos(euler_x);
		s_euler_y=sin(euler_y);
		c_euler_y=cos(euler_y);
		s_euler_z=sin(euler_z);
		c_euler_z=cos(euler_z);

		r_11=c_euler_y*c_euler_z;
		r_12=-c_euler_y*s_euler_z;
		r_13=s_euler_y;
		r_21=c_euler_x*s_euler_z + c_euler_z*s_euler_x*s_euler_y;
		r_22=c_euler_x*c_euler_z - s_euler_x*s_euler_y*s_euler_z;
		r_23=-c_euler_y*s_euler_x;
		r_31=s_euler_x*s_euler_z - c_euler_x*c_euler_z*s_euler_y;
		r_32=c_euler_z*s_euler_x + c_euler_x*s_euler_y*s_euler_z;
		r_33=c_euler_x*c_euler_y;

		alpha=-atan2Mia(a_13*r_21 + a_23*r_22 + a_33*r_23,a_13*r_31 + a_23*r_32 + a_33*r_33);
		betha= asin( a_13*r_11 + a_23*r_12 + a_33*r_13 );
		theta=-atan2Mia(a_12*r_11 + a_22*r_12 + a_32*r_13,a_11*r_11 + a_21*r_12 + a_31*r_13);

		cout << "position = [" << pos_x<<" "<<pos_y<<" "<<pos_z <<"]"<< "   orientation("<< 1+a<<",:) = ["<<(180/PI)*alpha<<" "<< (180/PI)*betha<<" "<< (180/PI)*theta <<"]"  <<"\n";
	//	cout <<"orientation("<< 1+a<<",:) = ["<< euler_x<<" "<<euler_y<<" "<<euler_z <<"]"  <<"\n";
	}

	return 0;
}
*/


#include <iostream>
#include <algorithm>
#include <iterator>
#include <cmath>
#include <fstream>
#include <valarray>
#include <sstream>
#include <string>
#include <list>
#include <vector>
#include <tuple>
#include <functional>
#include <utility>
#include <iomanip>

using namespace std::string_literals;

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/math/constants/constants.hpp>

#include "vlasovpp/field.h"
#include "vlasovpp/complex_field.h"
#include "vlasovpp/weno.h"
#include "vlasovpp/fft.h"
#include "vlasovpp/array_view.h"
#include "vlasovpp/poisson.h"
#include "vlasovpp/rk.h"
#include "vlasovpp/config.h"
#include "vlasovpp/signal_handler.h"
#include "vlasovpp/iteration.h"
#include "vlasovpp/physic.h"
#include "vlasovpp/tool.h"

namespace math = boost::math::constants;
const std::complex<double> & I = std::complex<double>(0.,1.);

#define SQ(X) ((X)*(X))
#define Zi(i) (i*f.step.dz+f.range.z_min)
#define Vkx(k) (k*f.step.dvx+f.range.vx_min)
#define Vky(k) (k*f.step.dvy+f.range.vy_min)
#define Vkz(k) (k*f.step.dvz+f.range.vz_min)

{% for i in range(0,expLt_mat|count) %}
  {% for j in range(0,expLt_mat[i]|count) %}
std::complex<double>
matrixExpr{{ i }}{{ j }} ( double t , double k ) {
  return {{ expLt_mat[i][j] }};
}
  {% endfor %}
{% endfor %}

int
main ( int argc , char const * argv[] )
{
  // load configuration
  auto c = config(argc,argv,"{{ simu_name }}");
  c.create_output_directory();
  save_config(c);

/* ---------------------------------------------------------------- */
  // physics variables initilization

  field3d<double> f(boost::extents[c.Nvx][c.Nvy][c.Nvz][c.Nz]);
  complex_field<double,3> hf(boost::extents[c.Nvx][c.Nvy][c.Nvz][c.Nz]);

  ublas::vector<double> jcx(c.Nz,0.) , jcy(c.Nz,0.);
  ublas::vector<double> Ex(c.Nz,0.)  , Ey(c.Nz,0.);
  ublas::vector<double> Bx(c.Nz,0.)  , By(c.Nz,0.);

  // range in velocity are done by jinja parameter
  f.range.vx_min = {{ frange.vx_min }}; f.range.vx_max = {{ frange.vx_max }};
  f.range.vy_min = {{ frange.vy_min }}; f.range.vy_max = {{ frange.vy_max }};
  f.range.vz_min = {{ frange.vz_min }}; f.range.vz_max = {{ frange.vz_max }};
  // range in space are done from config (K parameter)
  f.range.z_min =  0.;  f.range.z_max = 2.*math::pi<double>()/c.K;

  f.compute_steps();

  const double v_par  = c.v_par;
  const double v_perp = c.v_perp;
  const double nh = c.nh;
  const double B0 = c.B0;

  ublas::vector<double> Kz(c.Nz);
  {
    const double L = f.range.len_z();
    for ( auto i=0u ; i<c.Nz/2 ; ++i ) { Kz[i]      = 2.*math::pi<double>()*i/L; }
    for ( auto i=-c.Nz/2 ; i<0 ; ++i ) { Kz[c.Nz+i] = 2.*math::pi<double>()*i/L; }
  }

  // initial condition
  auto M1 = maxwellian(nh,{0.,0.,0.},{v_perp,v_perp,v_par});
  for ( auto k_x=0u ; k_x<c.Nvx ; ++k_x ) {
    for ( auto k_y=0u ; k_y<c.Nvy ; ++k_y ) {
      for ( auto k_z=0u ; k_z<c.Nvz ; ++k_z ) {
        for (std::size_t i=0u ; i<f.size_x() ; ++i ) {
          const double vx = k_x*f.step.dvx + f.range.vx_min;
          const double vy = k_y*f.step.dvy + f.range.vy_min;
          const double vz = k_z*f.step.dvz + f.range.vz_min;
          const double z  = i*f.step.dz + f.range.z_min;
          f[k_x][k_y][k_z][i] = M1( z,vx,vy,vz );
        }
        fft::fft(f[k_x][k_y][k_z].begin(),f[k_x][k_y][k_z].end(),hf[k_x][k_y][k_z].begin());
      }
    }
  }
  for ( auto i=0u ; i<c.Nz ; ++i ) {
    double z = f.range.z_min + f.step.dz*i;
    Bx[i] = c.alpha * std::sin(c.K*z);
  }

  // end physics variables initialization
/* ---------------------------------------------------------------- */
  // monitorring initialization

  std::vector<double> times;           times.reserve(100);
  std::vector<double> electric_energy; electric_energy.reserve(100);
  std::vector<double> kinetic_energy;  kinetic_energy.reserve(100);
  std::vector<double> magnetic_energy; magnetic_energy.reserve(100);
  std::vector<double> cold_energy;     cold_energy.reserve(100);
  std::vector<double> mass;            mass.reserve(100);
  std::vector<double> Bxmax;           Bxmax.reserve(100);
  std::vector<double> Bymax;           Bymax.reserve(100);
  std::vector<double> Exmax;           Exmax.reserve(100);
  std::vector<double> Eymax;           Eymax.reserve(100);

  ublas::vector<double> fdvxdvydz(c.Nvz,0.);
  ublas::vector<double> vxfdv(c.Nz,0.), vyfdv(c.Nz,0.), vzfdv(c.Nz,0.);
  ublas::vector<double> ec_perp(c.Nz,0.), ec_vz(c.Nz,0.);
  ublas::vector<double> rho_h(c.Nz,0.);
  field<double,1> fdvxdvy(boost::extents[c.Nvz][c.Nz]);

  // init all computer functor for each monitoring value
  auto compute_vperp_integral       = computer::vperp_integral<double>( f );
  auto compute_z_vperp_integral     = computer::z_vperp_integral<double>( f );
  auto compute_z_vz_integral        = computer::z_vz_integral<double>( f );
  auto compute_local_kinetic_energy = computer::local_kinetic_energy<double>( f );
  auto compute_electric_energy      = computer::space_energy( f.step.dz );
  auto compute_magnetic_energy      = computer::space_energy( f.step.dz );
  auto compute_cold_energy          = computer::space_energy( f.step.dz );
  auto compute_hot_mass_energy      = computer::hot_mass_energy<double>( f );

  auto printer_z_data  = factory::printer__x_data( f.range.z_min  , f.step.dz  );
  auto printer_vz_data = factory::printer__x_data( f.range.vz_min , f.step.dvz );

  electric_energy.push_back( compute_electric_energy(Ex,Ey) );
  magnetic_energy.push_back( compute_magnetic_energy(Bx,By) );
  cold_energy.push_back( compute_cold_energy(jcx,jcy) );
  compute_hot_mass_energy(hf);
  kinetic_energy.push_back( compute_hot_mass_energy.he );
  mass.push_back( compute_hot_mass_energy.mass );
  Bxmax.push_back( max_abs(Bx) );
  Bymax.push_back( max_abs(By) );
  Exmax.push_back( max_abs(Ex) );
  Eymax.push_back( max_abs(Ey) );

  monitoring::reactive_monitoring<std::vector<double>> moni(
    c.output_dir/("energy_"s + c.name + ".dat"s) ,
    times ,
    {&electric_energy,&magnetic_energy,&cold_energy,&kinetic_energy,&mass,&Exmax,&Eymax,&Bxmax,&Bymax}
  );

  // end monitorring initialization
/* ---------------------------------------------------------------- */
  // substage initialization

  //{# declaration of each substage variables #}
  ublas::vector<std::complex<double>> {%- for (lhs,_) in schemes %} {{ lhs.jcx }}(c.Nz,0.) {{ "," if not loop.last else "" }}{% endfor %};
  ublas::vector<std::complex<double>> {%- for (lhs,_) in schemes %} {{ lhs.jcy }}(c.Nz,0.) {{ "," if not loop.last else "" }}{% endfor %};
  ublas::vector<std::complex<double>> {%- for (lhs,_) in schemes %} {{ lhs.Bx  }}(c.Nz,0.) {{ "," if not loop.last else "" }}{% endfor %};
  ublas::vector<std::complex<double>> {%- for (lhs,_) in schemes %} {{ lhs.By  }}(c.Nz,0.) {{ "," if not loop.last else "" }}{% endfor %};
  ublas::vector<std::complex<double>> {%- for (lhs,_) in schemes %} {{ lhs.Ex  }}(c.Nz,0.) {{ "," if not loop.last else "" }}{% endfor %};
  ublas::vector<std::complex<double>> {%- for (lhs,_) in schemes %} {{ lhs.Ey  }}(c.Nz,0.) {{ "," if not loop.last else "" }}{% endfor %};

  complex_field<double,3> {%- for (lhs,_) in schemes[:-1] %} {{ lhs.fh }}(boost::extents[c.Nvx][c.Nvy][c.Nvz][c.Nz]) {{ ", " if not loop.last else "" }}{% endfor %};
  field3d<double> dvf(boost::extents[c.Nvx][c.Nvy][c.Nvz][c.Nz]);

  // init Fourier variables (just for first loop and first stage)
  fft::fft( jcx.begin() , jcx.end() , {{ schemes[-1][0].jcx }}.begin() );
  fft::fft( jcy.begin() , jcy.end() , {{ schemes[-1][0].jcy }}.begin() );
  fft::fft(  Ex.begin() ,  Ex.end() , {{ schemes[-1][0].Ex  }}.begin() );
  fft::fft(  Ey.begin() ,  Ey.end() , {{ schemes[-1][0].Ey  }}.begin() );
  fft::fft(  Bx.begin() ,  Bx.end() , {{ schemes[-1][0].Bx  }}.begin() );
  fft::fft(  By.begin() ,  By.end() , {{ schemes[-1][0].By  }}.begin() );

  {% macro compute_hjfx( hf ) -%}
    ( w_1*c_ - w_2*s_ )*{{ hf }}[k_x][k_y][k_z][i]*f.volumeV()
  {%- endmacro %}
  {% macro compute_hjfy( hf ) -%}
    ( w_1*s_ + w_2*c_ )*{{ hf }}[k_x][k_y][k_z][i]*f.volumeV()
  {%- endmacro %}

  {% macro compute_velocity_vx( Ex , Ey , Bx , By ) -%}
    -( {{ Ex }}[i]*c_ + {{ Ey }}[i]*s_ + v_z*{{ Bx }}[i]*s_ - v_z*{{ By }}[i]*c_)
  {%- endmacro %}
  {% macro compute_velocity_vy( Ex , Ey , Bx , By ) -%}
    -(-{{ Ex }}[i]*s_ + {{ Ey }}[i]*c_ + v_z*{{ Bx }}[i]*c_ + v_z*{{ By }}[i]*s_)
  {%- endmacro %}
  {% macro compute_velocity_vz( Ex , Ey , Bx , By ) -%}
    -(-{{ Bx }}[i]*( w_1*s_ + w_2*c_ ) + {{ By }}[i]*( w_1*c_ - w_2*s_ ))
  {%- endmacro %}

  auto next_snapshot = c.snaptimes.begin();

  iteration_4d::iteration<double> iter;
  iter.dt = c.dt0;
  times.push_back(iter.current_time);
  const double dt_cfl_maxwell = 2.0*std::sqrt(2.0)/c.Nz;

  while ( iter.current_time<c.Tf )
  {
    const double current_t = iter.current_time;
    const double dt = iter.dt;
    std::cout << "\r" << iteration_4d::time(iter) << std::flush;

    // {# write here only one stage and loop with jinja2 #}
    {% for (lhs,rhs) in schemes %}
    //////////////////////////////////////////////////////////////////
    { // begin stage {{ loop.index }}
      double c_ = std::cos(B0*( current_t + {{ lhs.dt }}*dt )) ,
             s_ = std::sin(B0*( current_t + {{ lhs.dt }}*dt )) ;

      // compute $\int v_x {{ schemes[loop.index0-1][0].fh }}\,\mathrm{d}v$ and $\int v_y {{ schemes[loop.index0-1][0].fh }}\,\mathrm{d}v$
      ublas::vector<std::complex<double>> hjhx(c.Nz,0.) , hjhy(c.Nz,0.);
      for ( auto k_x=0u ; k_x<c.Nvx ; ++k_x ) {
        const double w_1 = k_x*f.step.dvx + f.range.vx_min;
        for ( auto k_y=0u ; k_y<c.Nvy ; ++k_y ) {
          const double w_2 = k_y*f.step.dvy + f.range.vy_min;
          for ( auto k_z=0u ; k_z<c.Nvz ; ++k_z ) {
            for ( auto i=1u ; i<c.Nz ; ++i ) {
              hjhx[i] += {{ compute_hjfx(schemes[loop.index0-1][0].fh) }};
              hjhy[i] += {{ compute_hjfy(schemes[loop.index0-1][0].fh) }};
            }
          }
        }
      }
      // keep zero mean
      hjhx[0] = 0.0;
      hjhy[0] = 0.0;

      // --> compute {{ lhs.jcx }}, {{ lhs.jcy }}, {{ lhs.Bx }}, {{ lhs.By }}, {{ lhs.Ex }}, {{ lhs.Ey }} (all spatial values)
      for ( auto i=1u ; i<c.Nz ; ++i ) {
        {% if not loop.last %}
          {{ lhs.jcx }}[i] = {{ rhs.jcx }};
          // ---
          {{ lhs.jcy }}[i] = {{ rhs.jcy }};
          // ---
          {{ lhs.Bx  }}[i] = {{ rhs.Bx  }};
          // ---
          {{ lhs.By  }}[i] = {{ rhs.By  }};
          // ---
          {{ lhs.Ex  }}[i] = {{ rhs.Ex  }};
          // ---
          {{ lhs.Ey  }}[i] = {{ rhs.Ey  }};
        {% else %}
          auto {{ lhs.jcx }}_tmp = {{ rhs.jcx }};
          // ---
          auto {{ lhs.jcy }}_tmp = {{ rhs.jcy }};
          // ---
          auto {{ lhs.Bx  }}_tmp = {{ rhs.Bx  }};
          // ---
          auto {{ lhs.By  }}_tmp = {{ rhs.By  }};
          // ---
          auto {{ lhs.Ex  }}_tmp = {{ rhs.Ex  }};
          // ---
          auto {{ lhs.Ey  }}_tmp = {{ rhs.Ey  }};

          {{ lhs.jcx }}[i] = {{ lhs.jcx }}_tmp;
          {{ lhs.jcy }}[i] = {{ lhs.jcy }}_tmp;
          {{ lhs.Bx  }}[i] = {{ lhs.Bx  }}_tmp;
          {{ lhs.By  }}[i] = {{ lhs.By  }}_tmp;
          {{ lhs.Ex  }}[i] = {{ lhs.Ex  }}_tmp;
          {{ lhs.Ey  }}[i] = {{ lhs.Ey  }}_tmp;
        {% endif %}
      }
      // keep zero mean
      {{ lhs.jcx }}[0] = 0.0;
      {{ lhs.jcy }}[0] = 0.0;
      {{ lhs.Bx  }}[0] = 0.0;
      {{ lhs.By  }}[0] = 0.0;
      {{ lhs.Ex  }}[0] = 0.0;
      {{ lhs.Ey  }}[0] = 0.0;

      // --> compute {{ lhs.fh }}
      // iFFT of hf
      for ( auto k_x=0u ; k_x<c.Nvx ; ++k_x ) {
        for ( auto k_y=0u ; k_y<c.Nvy ; ++k_y ) {
          for ( auto k_z=0u ; k_z<c.Nvz ; ++k_z ) {
            fft::ifft( {{ schemes[loop.index0-1][0].fh }}[k_x][k_y][k_z].begin() ,
                       {{ schemes[loop.index0-1][0].fh }}[k_x][k_y][k_z].end()   ,
                       f[k_x][k_y][k_z].begin()
                    );
          }
        }
      }
      // iFFT of hEx, hEy, hBx and hBy
      fft::ifft({{ schemes[loop.index0-1][0].Ex }}.begin(),{{ schemes[loop.index0-1][0].Ex }}.end(),Ex.begin());
      fft::ifft({{ schemes[loop.index0-1][0].Ey }}.begin(),{{ schemes[loop.index0-1][0].Ey }}.end(),Ey.begin());
      fft::ifft({{ schemes[loop.index0-1][0].Bx }}.begin(),{{ schemes[loop.index0-1][0].Bx }}.end(),Bx.begin());
      fft::ifft({{ schemes[loop.index0-1][0].By }}.begin(),{{ schemes[loop.index0-1][0].By }}.end(),By.begin());

      // compute approximation of (E×vB)∂ᵥf
      for ( auto k_x=0u ; k_x<c.Nvx ; ++k_x ) {
        for ( auto k_y=0u ; k_y<c.Nvy ; ++k_y ) {
          for ( auto k_z=0u ; k_z<c.Nvz ; ++k_z ) {
            for ( auto i=0u ; i<c.Nz ; ++i ) {
              const double w_1 = k_x*f.step.dvx + f.range.vx_min;
              const double w_2 = k_y*f.step.dvy + f.range.vy_min;
              const double v_z = k_z*f.step.dvz + f.range.vz_min;

              const double velocity_vx = {{ compute_velocity_vx('Ex','Ey','Bx','By') }};
              const double velocity_vy = {{ compute_velocity_vy('Ex','Ey','Bx','By') }};
              const double velocity_vz = {{ compute_velocity_vz('Ex','Ey','Bx','By') }};

              dvf[k_x][k_y][k_z][i] = + weno3d::d_vx(velocity_vx,f,k_x,k_y,k_z,i)
                                      + weno3d::d_vy(velocity_vy,f,k_x,k_y,k_z,i)
                                      + weno3d::d_vz(velocity_vz,f,k_x,k_y,k_z,i);
            }
          }
        }
      }

      // update hf
      for ( auto k_x=0u ; k_x<c.Nvx ; ++k_x ) {
        for ( auto k_y=0u ; k_y<c.Nvy ; ++k_y ) {
          for ( auto k_z=0u ; k_z<c.Nvz ; ++k_z ) {
            const double v_z = k_z*f.step.dvz + f.range.vz_min;
            fft::spectrum_ hfvxvyvz(c.Nz);
            hfvxvyvz.fft(dvf[k_x][k_y][k_z].begin());
            for ( auto i=0u ; i<c.Nz ; ++i ) {
              {{ lhs.fh }}[k_x][k_y][k_z][i] = {{ rhs.fh }};
            }
          }
        }
      }

    } // end stage {{ loop.index }}
    //////////////////////////////////////////////////////////////////
    {% endfor %}

    fft::ifft(hEx.begin(),hEx.end(),Ex.begin());
    fft::ifft(hEy.begin(),hEy.end(),Ey.begin());
    fft::ifft(hBx.begin(),hBx.end(),Bx.begin());
    fft::ifft(hBy.begin(),hBy.end(),By.begin());
    fft::ifft(hjcx.begin(),hjcx.end(),jcx.begin());
    fft::ifft(hjcy.begin(),hjcy.end(),jcy.begin());

    electric_energy.push_back(compute_electric_energy(Ex,Ey));
    magnetic_energy.push_back(compute_magnetic_energy(Bx,By));
    cold_energy.push_back(compute_cold_energy(jcx,jcy));
    compute_hot_mass_energy(hf);
    kinetic_energy.push_back(compute_hot_mass_energy.he);
    mass.push_back(compute_hot_mass_energy.mass);
    Exmax.push_back( max_abs(Ex) );
    Eymax.push_back( max_abs(Ey) );
    Bxmax.push_back( max_abs(Bx) );
    Bymax.push_back( max_abs(By) );

    ++iter.iter;
    iter.current_time += iter.dt;
    times.push_back(current_t);
    moni.push();

    if ( iter.iter % 1000 == 0 ) {
      std::stringstream filename;

      filename.str("");
      compute_vperp_integral( hf );
      filename << "fdvxdvy_" << c.name << "_" << iter.iter << ".dat";
      compute_vperp_integral.fdvxdvy.write( c.output_dir / filename.str() );

      filename.str("");
      compute_z_vz_integral( hf );
      filename << "fdzdvz_" << c.name << "_" << iter.iter << ".dat";
      compute_z_vz_integral.fdzdvz.write( c.output_dir / filename.str() );

      filename.str("");
      compute_z_vperp_integral( hf );
      filename << "fdvxdvydz_" << c.name << "_" << iter.iter << ".dat";
      c << monitoring::make_data( filename.str() , compute_z_vperp_integral.fdvxdvydz , printer_vz_data );
    } // end monitoring %1000

  } // end time loop
  std::cout << "\r" << iteration_4d::time(iter) << std::endl;

  auto writer_t_y = [&,count=0] (auto const& y) mutable {
    std::stringstream ss; ss<<times[count++]<<" "<<y;
    return ss.str();
  };

  // this data are already in energy_XXX.dat
  c << monitoring::make_data( "ee"s + c.name + ".dat"s , electric_energy , writer_t_y );
  c << monitoring::make_data( "eb"s + c.name + ".dat"s , magnetic_energy , writer_t_y );
  c << monitoring::make_data( "ec"s + c.name + ".dat"s , cold_energy     , writer_t_y );
  c << monitoring::make_data( "ek"s + c.name + ".dat"s , kinetic_energy  , writer_t_y );
  c << monitoring::make_data( "m"s  + c.name + ".dat"s , mass            , writer_t_y );

  {
    std::stringstream filename;

    filename.str("");
    compute_vperp_integral( hf );
    filename << "fdvxdvy_" << c.name << "_Tf.dat";
    compute_vperp_integral.fdvxdvy.write( c.output_dir / filename.str() );

    filename.str("");
    compute_z_vz_integral( hf );
    filename << "fdzdvz_" << c.name << "_Tf.dat";
    compute_z_vz_integral.fdzdvz.write( c.output_dir / filename.str() );

    filename.str("");
    compute_z_vperp_integral( hf );
    filename << "fdvxdvydz_" << c.name << "_Tf.dat";
    c << monitoring::make_data( filename.str() , compute_z_vperp_integral.fdvxdvydz , printer_vz_data );
  }

return 0;
}