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


#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 (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 <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;
}


#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;
}


#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;
}


#include "solarsystem.h"
#include "planet.h"
#include <iostream>
#include <armadillo>
#include <stdio.h>
#include <iomanip>
using namespace arma;
using namespace std;

solarsystem::solarsystem()
{
}
void solarsystem::add(planet n){
     number_planets++;
     all_planets.push_back(n);
 }
void solarsystem::print_position(ofstream &output, ofstream &output2, vector<planet> vec){
    print_position(output, output2, vec, 3);
}

void solarsystem::print_position(ofstream &output, ofstream &output2, vector<planet> vec, int n){
    if(n>3 || n<=0) n=3;
    for(int i=0; i<vec.size(); i++){
        planet &this = vec[i];
        std::cout << std::scientific;
        for(int j=0; j<n;j++){
        std::cout << this.position[j] << "   ";
        output << std::scientific << this.position[j] << "   ";
        output2 << std::scientific << this.velocity[j] << "   ";
        }
        std::cout << "         ";
        output  << "         ";
        output2  << "         ";
       }
    std::cout << std::endl;
    output << endl;
    output2 << endl;
}

void solarsystem::insert_data(vector<planet> vec, mat &ma){
    for(int i=0; i<vec.size(); i++){
        planet &this = vec[i];
        ma(i,6)=this.mass;

        for(int k=0; k<3;k++){
            ma(i,k)=this.position[k];
            ma(i,k+3)=this.velocity[k];
        }
    }
}

void solarsystem::synctroniz(vector<planet> vec, mat &ma){
    int n = vec.size();

    for(int j=0; j<n;j++){
        planet &this = vec[j];
    for (int i = 0; i < 3; ++i){
       this.position[i] =  ma(j,i);
       this.velocity[i] = ma(j,i+3);
}
}

}

void solarsystem::solverRK4(vector<planet> vec, double h, double tmax){

    mat y_i(number_planets,7);
    mat y_i_temp(number_planets,7);
    mat k1(number_planets,7);
    mat k2(number_planets,7);
    mat k3(number_planets,7);
    mat k4(number_planets,7);

    insert_data(vec , y_i);
    double t=0;

    //for print file_________
    char *filename = new char[1000];
    char *filename2 = new char[1000];
        sprintf(filename, "Planet_position_RK4_%f.dat", h);
        sprintf(filename2, "Planet_velocity_RK4_%f.dat", h);

        ofstream output (filename);
        ofstream output2 (filename2);

        if (output.is_open()){
            output.precision(5);
            output2.precision(5);
    while(t<tmax){

        derivative(y_i, k1, number_planets);

        sum_matrix(y_i_temp, 1, y_i, 0.5*h, k1, number_planets);
        derivative(y_i_temp, k2, number_planets);

        sum_matrix( y_i_temp, 1,  y_i, 0.5*h,  k2, number_planets);

        derivative( y_i_temp,  k3, number_planets);

        sum_matrix( y_i_temp, 1,  y_i, h,  k3, number_planets);

        derivative( y_i_temp,  k4, number_planets);

        for(int j=1; j<number_planets; j++){

             for(int i=0; i<6; i++){
                 y_i(j,i) = y_i(j,i) + h*(k1(j,i) + 2*k2(j,i) + 2*k3(j,i) + k4(j,i))/6;
             }

             //Syncroniz position and velocity with the classes
             planet &this = vec[j];
             for(int i=0; i<3; i++){
             this.position[i] = y_i(j,i);
             this.velocity[i] = y_i(j,i+3);
             }

        }

print_position(output, output2, vec, 3);

t+=h;

}

output.close();
}
}

void solarsystem::solverVERLET(vector<planet> vec, double h, double tmax){

    mat y_i(number_planets,7);
    mat r_i_dt(number_planets,7);
    mat a_dt(number_planets,7);
    mat v_dt(number_planets,7);
    mat v_dt_2(number_planets,7);

    insert_data(vec , y_i);

    double t=0,zz =1;

    //print file___________________
    char *filename = new char[1000];
    char *filename2 = new char[1000];
        sprintf(filename, "Planet_position_Verlet_%f.dat", h);
        sprintf(filename2, "Planet_velocity_Verlet_%f.dat", h);

            ofstream output (filename);
            ofstream output2 (filename2);

            if (output.is_open()){
            output.precision(5);
            output2.precision(5);
    // end for print

    while(t<tmax){

        derivative(y_i, a_dt, number_planets);

        for(int j=0; j<number_planets; j++){

             for(int i=0; i<3; i++){
                 y_i(j,i) = y_i(j,i) + h*y_i(j,i+3) + 0.5*h*h*a_dt(j,i+3);
                 v_dt_2(j,i+3) = y_i(j,i+3) + 0.5*h*a_dt(j,i+3);
             }
        }

        derivative(y_i, a_dt, number_planets);

        for(int j=0; j<number_planets; j++){

             for(int i=3; i<6; i++){

                 y_i(j,i) = v_dt_2(j,i) + 0.5*h*a_dt(j,i);

             }

             planet &this = vec[j];
             for(int i=0; i<3; i++){
             this.position[i] = y_i(j,i);
             this.velocity[i] = y_i(j,i+3);
             }
        }
print_position(output, output2, vec,3);

t+=h;

}
output.close();
}

}
void solarsystem::sum_matrix(mat &result, double coeff_one, mat &first,double coeff_two, mat &second, int n){
    for(int j=0; j<n; j++){
         for(int i=0; i<6; i++){
            result(j,i) = coeff_one*first(j,i) + coeff_two*second(j,i);
         }
         result(j,6) = first(j,6);

    }
}
void solarsystem::printmat(mat &ma, int n){
    cout << endl;
    for(int i=0; i<7; i++){

        for(int k=0; k<n;k++){
            cout <<  ma(k,i)<<" " ;
        } cout << endl;}
}
double solarsystem::force(double x, double y, double z, double Mothers){
    double G=  4*M_PI*M_PI;
    double force=0;
    double distance=0;

    distance = x*x + y*y + z*z;

    force = G*Mothers/pow(distance, 1.5);

    return force;
}
void solarsystem::derivative(mat &dat, mat &de, int n){

    double accelleration_x=0,accelleration_y=0,accelleration_z=0, mod_force;
    for(int i=0; i<n; i++){

        accelleration_x=0,accelleration_y=0,accelleration_z=0;
        for(int j=0; j<n; j++){
            if(i!=j){

mod_force = force(dat(j,0)-dat(i,0),dat(j,1)-dat(i,1) ,dat(j,2)-dat(i,2),  dat(j,6));

accelleration_x += mod_force*(dat(j,0)-dat(i,0));
accelleration_y += mod_force*(dat(j,1)-dat(i,1));
accelleration_z += mod_force*(dat(j,2)-dat(i,2));
}
}
        de(i,3) = accelleration_x;
        de(i,4) = accelleration_y;
        de(i,5) = accelleration_z;

    }

    for(int i=0; i<n; i++){
        de(i,0) = dat(i,3); //velx
        de(i,1) = dat(i,4); //vely
        de(i,2) = dat(i,5); //velz
    }
}


/*
 * StokesM2L.cpp
 *
 *  Created on: Oct 12, 2016
 *      Author: wyan
 */

#include "SVD_pvfmm.hpp"

#include <Eigen/Dense>

#include <iomanip>
#include <iostream>

#define DIRECTLAYER 2

namespace Stokes1D3D {

/**
 * \brief Returns the coordinates of points on the surface of a cube.
 * \param[in] p Number of points on an edge of the cube is (n+1)
 * \param[in] c Coordinates to the centre of the cube (3D array).
 * \param[in] alpha Scaling factor for the size of the cube.
 * \param[in] depth Depth of the cube in the octree.
 * \return Vector with coordinates of points on the surface of the cube in the
 * format [x0 y0 z0 x1 y1 z1 .... ].
 */
template <class Real_t>
std::vector<Real_t> surface(int p, Real_t *c, Real_t alpha, int depth) {
    size_t n_ = (6 * (p - 1) * (p - 1) + 2); // Total number of points.

    std::vector<Real_t> coord(n_ * 3);
    coord[0] = coord[1] = coord[2] = -1.0;
    size_t cnt = 1;
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = -1.0;
            coord[cnt * 3 + 1] = (2.0 * (i + 1) - p + 1) / (p - 1);
            coord[cnt * 3 + 2] = (2.0 * j - p + 1) / (p - 1);
            cnt++;
        }
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = (2.0 * i - p + 1) / (p - 1);
            coord[cnt * 3 + 1] = -1.0;
            coord[cnt * 3 + 2] = (2.0 * (j + 1) - p + 1) / (p - 1);
            cnt++;
        }
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = (2.0 * (i + 1) - p + 1) / (p - 1);
            coord[cnt * 3 + 1] = (2.0 * j - p + 1) / (p - 1);
            coord[cnt * 3 + 2] = -1.0;
            cnt++;
        }
    for (size_t i = 0; i < (n_ / 2) * 3; i++)
        coord[cnt * 3 + i] = -coord[i];

    Real_t r = 0.5 * pow(0.5, depth);
    Real_t b = alpha * r;
    for (size_t i = 0; i < n_; i++) {
        coord[i * 3 + 0] = (coord[i * 3 + 0] + 1.0) * b + c[0];
        coord[i * 3 + 1] = (coord[i * 3 + 1] + 1.0) * b + c[1];
        coord[i * 3 + 2] = (coord[i * 3 + 2] + 1.0) * b + c[2];
    }
    return coord;
}

inline void Gkernel(const Eigen::Vector3d &target,
                    const Eigen::Vector3d &source, Eigen::Matrix3d &answer) {
    auto rst = target - source;
    double rnorm = rst.norm();
    if (rnorm < 1e-13) {
        answer.setZero();
        return;
    }

    auto part2 = rst * rst.transpose() / (rnorm * rnorm * rnorm);
    auto part1 = Eigen::Matrix3d::Identity() / rnorm;
    answer = part1 + part2;
}

// calculate the M2L matrix of images from 2 to 1000
int main(int argc, char **argv) {
    Eigen::initParallel();
    Eigen::setNbThreads(1);

    const int pEquiv = atoi(argv[1]); // (8-1)^2*6 + 2 points
    const int pCheck = atoi(argv[1]);
    const double scaleEquiv = 1.05;
    const double scaleCheck = 2.95;
    const double pCenterEquiv[3] = {
        -(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2};
    const double pCenterCheck[3] = {
        -(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2};
    auto pointMEquiv = surface(
        pEquiv, (double *)&(pCenterEquiv[0]), scaleEquiv,
        0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0
    auto pointMCheck = surface(
        pCheck, (double *)&(pCenterCheck[0]), scaleCheck,
        0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0

    auto pointLEquiv = surface(
        pEquiv, (double *)&(pCenterCheck[0]), scaleCheck,
        0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0
    auto pointLCheck = surface(
        pCheck, (double *)&(pCenterEquiv[0]), scaleEquiv,
        0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0

    //	for (int i = 0; i < pointLEquiv.size() / 3; i++) {
    //		std::cout << pointLEquiv[3 * i] << " " << pointLEquiv[3 * i + 1]
    //<< " " << pointLEquiv[3 * i + 2] << " "
    //				<< std::endl;
    //	}
    //
    //	for (int i = 0; i < pointLCheck.size() / 3; i++) {
    //		std::cout << pointLCheck[3 * i] << " " << pointLCheck[3 * i + 1]
    //<< " " << pointLCheck[3 * i + 2] << " "
    //				<< std::endl;
    //	}

    const int imageN = 500000; // images to sum
    // calculate the operator M2L with least square
    const int equivN = pointMEquiv.size() / 3;
    const int checkN = pointLCheck.size() / 3;
    Eigen::MatrixXd A(3 * checkN, 3 * equivN);
#pragma omp parallel for
    for (int k = 0; k < checkN; k++) {
        Eigen::Matrix3d G = Eigen::Matrix3d::Zero();
        Eigen::Vector3d Cpoint(pointLCheck[3 * k], pointLCheck[3 * k + 1],
                               pointLCheck[3 * k + 2]);
        for (int l = 0; l < equivN; l++) {
            const Eigen::Vector3d Lpoint(pointLEquiv[3 * l],
                                         pointLEquiv[3 * l + 1],
                                         pointLEquiv[3 * l + 2]);
            Gkernel(Cpoint, Lpoint, G);
            A.block<3, 3>(3 * k, 3 * l) = G;
        }
    }
    Eigen::MatrixXd ApinvU(A.cols(), A.rows());
    Eigen::MatrixXd ApinvVT(A.cols(), A.rows());
    pinv(A, ApinvU, ApinvVT);

    Eigen::MatrixXd M2L(3 * equivN, 3 * equivN);
#pragma omp parallel for
    for (int i = 0; i < equivN; i++) {
        const Eigen::Vector3d Mpoint(pointMEquiv[3 * i], pointMEquiv[3 * i + 1],
                                     pointMEquiv[3 * i + 2]);
        Eigen::MatrixXd f(3 * checkN, 3);
        //		std::cout<<"debug:"<<Mpoint<<std::endl;
        for (int k = 0; k < checkN; k++) {
            Eigen::Matrix3d temp = Eigen::Matrix3d::Zero();
            Eigen::Vector3d Cpoint(pointLCheck[3 * k], pointLCheck[3 * k + 1],
                                   pointLCheck[3 * k + 2]);
            //			std::cout<<"debug:"<<k<<std::endl;
            // sum the images
            for (int per = DIRECTLAYER + 1; per < imageN; per++) {
                Eigen::Vector3d perVec(1.0 * per, 0, 0);
                Eigen::Matrix3d G1 = Eigen::Matrix3d::Zero();
                Eigen::Matrix3d G2 = Eigen::Matrix3d::Zero();
                Gkernel(Cpoint, Mpoint + perVec, G1);
                Gkernel(Cpoint, Mpoint - perVec, G2);
                temp = temp + (G1 + G2);
            }
            //			std::cout << temp << std::endl;
            f.block<3, 3>(3 * k, 0) = temp;
        }
        M2L.block(0, 3 * i, 3 * equivN, 3) =
            (ApinvU.transpose() * (ApinvVT.transpose() * f));
    }

    // dump M2L
    for (int i = 0; i < 3 * equivN; i++) {
        for (int j = 0; j < 3 * equivN; j++) {
            std::cout << i << " " << j << " " << std::scientific
                      << std::setprecision(18) << M2L(i, j) << std::endl;
        }
    }

    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>
        forcePoint(3);
    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>
        forceValue(3);
    forcePoint[0] = Eigen::Vector3d(0.5, 0.55, 0.2);
    forceValue[0] = Eigen::Vector3d(-0.2, 0, 0);
    forcePoint[1] = Eigen::Vector3d(0.5, 0.5, 0.5);
    forceValue[1] = Eigen::Vector3d(1, 0, 0);
    forcePoint[2] = Eigen::Vector3d(0.7, 0.7, 0.7);
    forceValue[2] = Eigen::Vector3d(-0.8, 0, 0);

    // solve M
    A.resize(3 * checkN, 3 * equivN);
    ApinvU.resize(A.cols(), A.rows());
    ApinvVT.resize(A.cols(), A.rows());
    Eigen::VectorXd f(3 * checkN);
#pragma omp parallel for
    for (int k = 0; k < checkN; k++) {
        Eigen::Vector3d temp = Eigen::Vector3d::Zero();
        Eigen::Matrix3d G = Eigen::Matrix3d::Zero();
        Eigen::Vector3d Cpoint(pointMCheck[3 * k], pointMCheck[3 * k + 1],
                               pointMCheck[3 * k + 2]);
        for (int p = 0; p < 3; p++) {
            Gkernel(Cpoint, forcePoint[p], G);
            temp = temp + G * (forceValue[p]);
        }
        f.block<3, 1>(3 * k, 0) = temp;
        for (int l = 0; l < equivN; l++) {
            Eigen::Vector3d Mpoint(pointMEquiv[3 * l], pointMEquiv[3 * l + 1],
                                   pointMEquiv[3 * l + 2]);
            Gkernel(Cpoint, Mpoint, G);
            A.block<3, 3>(3 * k, 3 * l) = G;
        }
    }
    pinv(A, ApinvU, ApinvVT);
    Eigen::VectorXd Msource = (ApinvU.transpose() * (ApinvVT.transpose() * f));
    // impose zero sum
    double fx = 0, fy = 0, fz = 0;
    for (int i = 0; i < equivN; i++) {
        fx += Msource[3 * i];
        fy += Msource[3 * i + 1];
        fz += Msource[3 * i + 2];
    }
    std::cout << "fx svd before correction: " << fx << std::endl;
    std::cout << "fy svd before correction: " << fy << std::endl;
    std::cout << "fz svd before correction: " << fz << std::endl;

    fx /= equivN;
    fy /= equivN;
    fz /= equivN;
    for (int i = 0; i < equivN; i++) {
        Msource[3 * i] -= fx;
        Msource[3 * i + 1] -= fy;
        Msource[3 * i + 2] -= fz;
    }

    std::cout << "Msource: " << Msource << std::endl;

    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>
        forcePointExt(0);
    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>
        forceValueExt(0);
    for (size_t p = 0; p < forcePoint.size(); p++) {
        for (int k = -DIRECTLAYER; k < DIRECTLAYER + 1; k++) {
            forcePointExt.push_back(Eigen::Vector3d(k, 0, 0) + forcePoint[p]);
            forceValueExt.push_back(forceValue[p]);
        }
    }

    Eigen::VectorXd M2Lsource = M2L * (Msource);
    Eigen::Vector3d samplePoint(0.6, 0.5, 0.5);
    Eigen::Vector3d Usample(0, 0, 0);
    Eigen::Vector3d UsampleSP(0, 0, 0);
    Eigen::Matrix3d G;
    for (size_t p = 0; p < forcePointExt.size(); p++) {
        Gkernel(samplePoint, forcePointExt[p], G);
        Usample += G * (forceValueExt[p]);
    }
    std::cout << "Usample Direct:" << Usample << std::endl;

    for (int p = 0; p < equivN; p++) {
        Eigen::Vector3d Lpoint(pointLEquiv[3 * p], pointLEquiv[3 * p + 1],
                               pointLEquiv[3 * p + 2]);
        Eigen::Vector3d Fpoint(M2Lsource[3 * p], M2Lsource[3 * p + 1],
                               M2Lsource[3 * p + 2]);
        Gkernel(samplePoint, Lpoint, G);
        UsampleSP = UsampleSP + G * (Fpoint);
    }

    std::cout << "Usample M2L:" << UsampleSP << std::endl;
    std::cout << "Usample M2L total:" << UsampleSP + Usample << std::endl;

    Eigen::Vector3d UsampleN(0, 0, 0);
    for (size_t p = 0; p < forcePoint.size(); p++) {
        Eigen::Matrix3d G1 = Eigen::Matrix3d::Zero();
        Eigen::Matrix3d G2 = Eigen::Matrix3d::Zero();
        for (int per = DIRECTLAYER + 1; per < imageN; per++) {
            Eigen::Vector3d perVec(1.0 * per, 0, 0);
            Gkernel(samplePoint, forcePoint[p] + perVec, G1);
            Gkernel(samplePoint, forcePoint[p] - perVec, G2);
            UsampleN += (G1 + G2) * (forceValue[p]);
        }
        Gkernel(samplePoint, forcePoint[p], G2);
    }

    std::cout << "Usample N Images:" << UsampleN + Usample << std::endl;
    std::cout << "error" << UsampleSP - UsampleN << std::endl;

    return 0;
}

} // namespace Stokes1D3D

#undef DIRECTLAYER


#include <boost/multiprecision/cpp_int.hpp>

using int_type = boost::multiprecision::cpp_int;

#include <iostream>
#include <vector>

#include "structured.h"
#include "config.h"
#include "gadget.h"
#include "optimizer.h"
#include "orbit.h"
#include "rational.h"

int main() {
  std::cout << "Analyzing orbits of the symmetry group..." << std::endl;
  OrbitInfo orbitInfo;

  std::cout << "Finding all structured sets" << std::endl;
  std::vector<bool> structuredSets = getAllStructuredSets();
  long long i = 0;
  for (long long ind = 0; ind < n_nodes; ++ind)
    i += structuredSets[ind];
  std::cout << "Found " << i << " nodes to consider" << std::endl;

  EdgeOrbitInfo edgeOrbitInfo(orbitInfo, structuredSets);
  Optimizer<int_type> optimizer;
  Evaluator<int_type> evaluator(orbitInfo, edgeOrbitInfo);

  std::cout << "Optimizing gadget..." << std::endl;

  auto gadget = optimizer.gadgetSearch(orbitInfo, edgeOrbitInfo);

  std::cout << "Finished optimizing gadget" << std::endl;

  std::cout << gadget;

  auto randomCost = evaluator.relaxedRandomCost(gadget);

  int_type totalWeight = 0;
  for (auto edgeOrbit : edgeOrbitInfo.edgeOrbits)
    totalWeight += edgeOrbit.size() * gadget.getWeight(edgeOrbit[0]);
  auto dictCost = Rational<int_type>(totalWeight, dimension);

  auto inapproximabilityFactor = randomCost / dictCost;

  std::cout << "The found gadget has inapproximability factor "
            << inapproximabilityFactor.a << "/" << inapproximabilityFactor.b
            << std::endl;
}


// Petter Strandmark 2012.

#include <cstdio>
#include <iomanip>
#include <iostream>
#include <limits>
#include <stdexcept>

#include <Eigen/Dense>

#include <spii/spii.h>
#include <spii/solver.h>

namespace spii {

void LBFGSSolver::solve(const Function& function,
                        SolverResults* results) const
{
	double global_start_time = wall_time();

	// Dimension of problem.
	size_t n = function.get_number_of_scalars();

	if (n == 0) {
		results->exit_condition = SolverResults::FUNCTION_TOLERANCE;
		return;
	}

	// Current point, gradient and Hessian.
	double fval   = std::numeric_limits<double>::quiet_NaN();
	double fprev  = std::numeric_limits<double>::quiet_NaN();
	double normg0 = std::numeric_limits<double>::quiet_NaN();
	double normg  = std::numeric_limits<double>::quiet_NaN();
	double normdx = std::numeric_limits<double>::quiet_NaN();

	Eigen::VectorXd x, g;

	// Copy the user state to the current point.
	function.copy_user_to_global(&x);
	Eigen::VectorXd x2(n);

	// L-BFGS history.
	std::vector<Eigen::VectorXd>  s_data(this->lbfgs_history_size),
	                              y_data(this->lbfgs_history_size);
	std::vector<Eigen::VectorXd*> s(this->lbfgs_history_size),
	                              y(this->lbfgs_history_size);
	for (int h = 0; h < this->lbfgs_history_size; ++h) {
		s_data[h].resize(function.get_number_of_scalars());
		s_data[h].setZero();
		y_data[h].resize(function.get_number_of_scalars());
		y_data[h].setZero();
		s[h] = &s_data[h];
		y[h] = &y_data[h];
	}

	Eigen::VectorXd rho(this->lbfgs_history_size);
	rho.setZero();

	Eigen::VectorXd alpha(this->lbfgs_history_size);
	alpha.setZero();
	Eigen::VectorXd q(n);
	Eigen::VectorXd r(n);

	// Needed from the previous iteration.
	Eigen::VectorXd x_prev(n), s_tmp(n), y_tmp(n);

	CheckExitConditionsCache exit_condition_cache;

	//
	// START MAIN ITERATION
	//
	results->startup_time   += wall_time() - global_start_time;
	results->exit_condition = SolverResults::INTERNAL_ERROR;
	int iter = 0;
	bool last_iteration_successful = true;
	int number_of_line_search_failures = 0;
	int number_of_restarts = 0;
	while (true) {

		//
		// Evaluate function and derivatives.
		//
		double start_time = wall_time();
		// y[0] should contain the difference between the gradient
		// in this iteration and the gradient from the previous.
		// Therefore, update y before and after evaluating the
		// function.
		if (iter > 0) {
			y_tmp = -g;
		}
		fval = function.evaluate(x, &g);

		normg = std::max(g.maxCoeff(), -g.minCoeff());
		if (iter == 0) {
			normg0 = normg;
		}
		results->function_evaluation_time += wall_time() - start_time;

		//
		// Update history
		//
		start_time = wall_time();

		if (iter > 0 && last_iteration_successful) {
			s_tmp = x - x_prev;
			y_tmp += g;

			double sTy = s_tmp.dot(y_tmp);
			if (sTy > 1e-16) {
				// Shift all pointers one step back, discarding the oldest one.
				Eigen::VectorXd* sh = s[this->lbfgs_history_size - 1];
				Eigen::VectorXd* yh = y[this->lbfgs_history_size - 1];
				for (int h = this->lbfgs_history_size - 1; h >= 1; --h) {
					s[h]   = s[h - 1];
					y[h]   = y[h - 1];
					rho[h] = rho[h - 1];
				}
				// Reuse the storage of the discarded data for the new data.
				s[0] = sh;
				y[0] = yh;

				*y[0] = y_tmp;
				*s[0] = s_tmp;
				rho[0] = 1.0 / sTy;
			}
		}

		results->lbfgs_update_time += wall_time() - start_time;

		//
		// Test stopping criteriea
		//
		start_time = wall_time();
		if (iter > 1 && this->check_exit_conditions(fval, fprev, normg,
		                                            normg0, x.norm(), normdx,
		                                            last_iteration_successful,
		                                            &exit_condition_cache, results)) {
			break;
		}
		if (iter >= this->maximum_iterations) {
			results->exit_condition = SolverResults::NO_CONVERGENCE;
			break;
		}

		if (this->callback_function) {
			CallbackInformation information;
			information.objective_value = fval;
			information.x = &x;
			information.g = &g;

			if (!callback_function(information)) {
				results->exit_condition = SolverResults::USER_ABORT;
				break;
			}
		}

		results->stopping_criteria_time += wall_time() - start_time;

		//
		// Compute search direction via L-BGFS two-loop recursion.
		//
		start_time = wall_time();
		bool should_restart = false;

		double H0 = 1.0;
		if (iter > 0) {
			// If the gradient is identical two iterations in a row,
			// y will be the zero vector and H0 will be NaN. In this
			// case the line search will fail and L-BFGS will be restarted
			// with a steepest descent step.
			H0 = s[0]->dot(*y[0]) / y[0]->dot(*y[0]);

			// If isinf(H0) || isnan(H0)
			if (H0 ==  std::numeric_limits<double>::infinity() ||
			    H0 == -std::numeric_limits<double>::infinity() ||
			    H0 != H0) {
				should_restart = true;
			}
		}

		q = -g;

		for (int h = 0; h < this->lbfgs_history_size; ++h) {
			alpha[h] = rho[h] * s[h]->dot(q);
			q = q - alpha[h] * (*y[h]);
		}

		r = H0 * q;

		for (int h = this->lbfgs_history_size - 1; h >= 0; --h) {
			double beta = rho[h] * y[h]->dot(r);
			r = r + (*s[h]) * (alpha[h] - beta);
		}

		// If the function improves very little, the approximated Hessian
		// might be very bad. If this is the case, it is better to discard
		// the history once in a while. This allows the solver to correctly
		// solve some badly scaled problems.
		double restart_test = std::fabs(fval - fprev) /
		                      (std::fabs(fval) + std::fabs(fprev));
		if (iter > 0 && iter % 100 == 0 && restart_test
		                                   < this->lbfgs_restart_tolerance) {
			should_restart = true;
		}
		if (! last_iteration_successful) {
			should_restart = true;
		}

		if (should_restart) {
			if (this->log_function) {
				char str[1024];
				if (number_of_restarts <= 10) {
					std::sprintf(str, "Restarting: fval = %.3e, deltaf = %.3e, max|g_i| = %.3e, test = %.3e",
								 fval, std::fabs(fval - fprev), normg, restart_test);
					this->log_function(str);
				}
				if (number_of_restarts == 10) {
					this->log_function("NOTE: No more restarts will be reported.");
				}
				number_of_restarts++;
			}
			r = -g;
			for (int h = 0; h < this->lbfgs_history_size; ++h) {
				(*s[h]).setZero();
				(*y[h]).setZero();
			}
			rho.setZero();
			alpha.setZero();
			// H0 is not used, but its value will be printed.
			H0 = std::numeric_limits<double>::quiet_NaN();
		}

		results->lbfgs_update_time += wall_time() - start_time;

		//
		// Perform line search.
		//
		start_time = wall_time();
		double start_alpha = 1.0;
		// In the first iteration, start with a much smaller step
		// length. (heuristic used by e.g. minFunc)
		if (iter == 0) {
			double sumabsg = 0.0;
			for (size_t i = 0; i < n; ++i) {
				sumabsg += std::fabs(g[i]);
			}
			start_alpha = std::min(1.0, 1.0 / sumabsg);
		}
		double alpha_step = this->perform_linesearch(function, x, fval, g,
		                                             r, &x2, start_alpha);

		if (alpha_step <= 0) {
			if (this->log_function) {
				this->log_function("Line search failed.");
				char str[1024];
				std::sprintf(str, "%4d %+.3e %9.3e %.3e %.3e %.3e %.3e",
					iter, fval, std::fabs(fval - fprev), normg, alpha_step, H0, rho[0]);
				this->log_function(str);
			}
			if (! last_iteration_successful || number_of_line_search_failures++ > 10) {
				// This happens quite seldom. Every time it has happened, the function
				// was actually converged to a solution.
				results->exit_condition = SolverResults::GRADIENT_TOLERANCE;
				break;
			}

			last_iteration_successful = false;
		}
		else {
			// Record length of this step.
			normdx = alpha_step * r.norm();
			// Compute new point.
			x_prev = x;
			x = x + alpha_step * r;

			last_iteration_successful = true;
		}

		results->backtracking_time += wall_time() - start_time;

		//
		// Log the results of this iteration.
		//
		start_time = wall_time();

		int log_interval = 1;
		if (iter > 30) {
			log_interval = 10;
		}
		if (iter > 200) {
			log_interval = 100;
		}
		if (iter > 2000) {
			log_interval = 1000;
		}
		if (this->log_function && iter % log_interval == 0) {
			if (iter == 0) {
				this->log_function("Itr       f       deltaf   max|g_i|   alpha      H0       rho");
			}

			this->log_function(
				to_string(
					std::setw(4), iter, " ",
					std::setw(10), std::setprecision(3), std::scientific, std::showpos, fval, std::noshowpos, " ",
					std::setw(9),  std::setprecision(3), std::scientific, std::fabs(fval - fprev), " ",
					std::setw(9),  std::setprecision(3), std::setprecision(3), std::scientific, normg, " ",
					std::setw(9),  std::setprecision(3), std::scientific, alpha_step, " ",
					std::setw(9),  std::setprecision(3), std::scientific, H0, " ",
					std::setw(9),  std::setprecision(3), std::scientific, rho[0]
				)
			);
		}
		results->log_time += wall_time() - start_time;

		fprev = fval;
		iter++;
	}

	function.copy_global_to_user(x);
	results->total_time += wall_time() - global_start_time;

	if (this->log_function) {
		char str[1024];
		std::sprintf(str, " end %+.3e           %.3e", fval, normg);
		this->log_function(str);
	}
}

}  // namespace spii