/**
 * @file recurrent_attention.hpp
 * @author Marcus Edel
 *
 * Definition of the RecurrentAttention class.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#ifndef MLPACK_METHODS_ANN_LAYER_RECURRENT_ATTENTION_HPP
#define MLPACK_METHODS_ANN_LAYER_RECURRENT_ATTENTION_HPP

#include <mlpack/prereqs.hpp>
#include <boost/ptr_container/ptr_vector.hpp>

#include "../visitor/delta_visitor.hpp"
#include "../visitor/output_parameter_visitor.hpp"
#include "../visitor/reset_visitor.hpp"
#include "../visitor/weight_size_visitor.hpp"

#include "layer_types.hpp"
#include "add_merge.hpp"
#include "sequential.hpp"

namespace mlpack {
namespace ann /** Artificial Neural Network. */ {

/**
 * This class implements the Recurrent Model for Visual Attention, using a
 * variety of possible layer implementations.
 *
 * For more information, see the following paper.
 *
 * @code
 * @article{MnihHGK14,
 *   title={Recurrent Models of Visual Attention},
 *   author={Volodymyr Mnih, Nicolas Heess, Alex Graves, Koray Kavukcuoglu},
 *   journal={CoRR},
 *   volume={abs/1406.6247},
 *   year={2014}
 * }
 * @endcode
 *
 * @tparam InputDataType Type of the input data (arma::colvec, arma::mat,
 *         arma::sp_mat or arma::cube).
 * @tparam OutputDataType Type of the output data (arma::colvec, arma::mat,
 *         arma::sp_mat or arma::cube).
 */
template <
    typename InputDataType = arma::mat,
    typename OutputDataType = arma::mat
>
class RecurrentAttention
{
 public:
  /**
   * Default constructor: this will not give a usable RecurrentAttention object,
   * so be sure to set all the parameters before use.
   */
  RecurrentAttention();

  /**
   * Create the RecurrentAttention object using the specified modules.
   *
   * @param start The module output size.
   * @param start The recurrent neural network module.
   * @param start The action module.
   * @param rho Maximum number of steps to backpropagate through time (BPTT).
   */
  template<typename RNNModuleType, typename ActionModuleType>
  RecurrentAttention(const size_t outSize,
                     const RNNModuleType& rnn,
                     const ActionModuleType& action,
                     const size_t rho);

  /**
   * Ordinary feed forward pass of a neural network, evaluating the function
   * f(x) by propagating the activity forward through f.
   *
   * @param input Input data used for evaluating the specified function.
   * @param output Resulting output activation.
   */
  template<typename eT>
  void Forward(arma::Mat<eT>&& input, arma::Mat<eT>&& output);

  /**
   * Ordinary feed backward pass of a neural network, calculating the function
   * f(x) by propagating x backwards trough f. Using the results from the feed
   * forward pass.
   *
   * @param input The propagated input activation.
   * @param gy The backpropagated error.
   * @param g The calculated gradient.
   */
  template<typename eT>
  void Backward(const arma::Mat<eT>&& /* input */,
                arma::Mat<eT>&& gy,
                arma::Mat<eT>&& g);

  /*
   * Calculate the gradient using the output delta and the input activation.
   *
   * @param input The input parameter used for calculating the gradient.
   * @param error The calculated error.
   * @param gradient The calculated gradient.
   */
  template<typename eT>
  void Gradient(arma::Mat<eT>&& /* input */,
                arma::Mat<eT>&& /* error */,
                arma::Mat<eT>&& /* gradient */);

  //! Get the model modules.
  std::vector<LayerTypes<>>& Model() { return network; }

    //! The value of the deterministic parameter.
  bool Deterministic() const { return deterministic; }
  //! Modify the value of the deterministic parameter.
  bool& Deterministic() { return deterministic; }

  //! Get the parameters.
  OutputDataType const& Parameters() const { return parameters; }
  //! Modify the parameters.
  OutputDataType& Parameters() { return parameters; }

  //! Get the input parameter.
  InputDataType const& InputParameter() const { return inputParameter; }
  //! Modify the input parameter.
  InputDataType& InputParameter() { return inputParameter; }

  //! Get the output parameter.
  OutputDataType const& OutputParameter() const { return outputParameter; }
  //! Modify the output parameter.
  OutputDataType& OutputParameter() { return outputParameter; }

  //! Get the delta.
  OutputDataType const& Delta() const { return delta; }
  //! Modify the delta.
  OutputDataType& Delta() { return delta; }

  //! Get the gradient.
  OutputDataType const& Gradient() const { return gradient; }
  //! Modify the gradient.
  OutputDataType& Gradient() { return gradient; }

  /**
   * Serialize the layer
   */
  template<typename Archive>
  void serialize(Archive& ar, const unsigned int /* version */);

 private:
  //! Calculate the gradient of the attention module.
  void IntermediateGradient()
  {
    intermediateGradient.zeros();

    // Gradient of the action module.
    if (backwardStep == (rho - 1))
    {
      boost::apply_visitor(GradientVisitor(std::move(initialInput),
          std::move(actionError)), actionModule);
    }
    else
    {
      boost::apply_visitor(GradientVisitor(std::move(boost::apply_visitor(
          outputParameterVisitor, actionModule)), std::move(actionError)),
          actionModule);
    }

    // Gradient of the recurrent module.
    boost::apply_visitor(GradientVisitor(std::move(boost::apply_visitor(
        outputParameterVisitor, rnnModule)), std::move(recurrentError)),
        rnnModule);

    attentionGradient += intermediateGradient;
  }

  //! Locally-stored module output size.
  size_t outSize;

  //! Locally-stored start module.
  LayerTypes<> rnnModule;

  //! Locally-stored input module.
  LayerTypes<> actionModule;

  //! Number of steps to backpropagate through time (BPTT).
  size_t rho;

  //! Locally-stored number of forward steps.
  size_t forwardStep;

  //! Locally-stored number of backward steps.
  size_t backwardStep;

  //! If true dropout and scaling is disabled, see notes above.
  bool deterministic;

  //! Locally-stored weight object.
  OutputDataType parameters;

  //! Locally-stored model modules.
  std::vector<LayerTypes<>> network;

  //! Locally-stored weight size visitor.
  WeightSizeVisitor weightSizeVisitor;

  //! Locally-stored delta visitor.
  DeltaVisitor deltaVisitor;

  //! Locally-stored output parameter visitor.
  OutputParameterVisitor outputParameterVisitor;

  //! Locally-stored feedback output parameters.
  std::vector<arma::mat> feedbackOutputParameter;

  //! List of all module parameters for the backward pass (BBTT).
  std::vector<arma::mat> moduleOutputParameter;

  //! Locally-stored delta object.
  OutputDataType delta;

  //! Locally-stored gradient object.
  OutputDataType gradient;

  //! Locally-stored input parameter object.
  InputDataType inputParameter;

  //! Locally-stored output parameter object.
  OutputDataType outputParameter;

  //! Locally-stored recurrent error parameter.
  arma::mat recurrentError;

  //! Locally-stored action error parameter.
  arma::mat actionError;

  //! Locally-stored action delta.
  arma::mat actionDelta;

  //! Locally-stored recurrent delta.
  arma::mat rnnDelta;

  //! Locally-stored initial action input.
  arma::mat initialInput;

  //! Locally-stored reset visitor.
  ResetVisitor resetVisitor;

  //! Locally-stored attention gradient.
  arma::mat attentionGradient;

  //! Locally-stored intermediate gradient for the attention module.
  arma::mat intermediateGradient;
}; // class RecurrentAttention

} // namespace ann
} // namespace mlpack

// Include implementation.
#include "recurrent_attention_impl.hpp"

#endif


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008-2012 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#include "sparse_solver.h"
#include <Eigen/PaStiXSupport>
#include <unsupported/Eigen/SparseExtra>

template<typename T> void test_pastix_T()
{
  PastixLLT< SparseMatrix<T, ColMajor>, Eigen::Lower > pastix_llt_lower;
  PastixLDLT< SparseMatrix<T, ColMajor>, Eigen::Lower > pastix_ldlt_lower;
  PastixLLT< SparseMatrix<T, ColMajor>, Eigen::Upper > pastix_llt_upper;
  PastixLDLT< SparseMatrix<T, ColMajor>, Eigen::Upper > pastix_ldlt_upper;
  PastixLU< SparseMatrix<T, ColMajor> > pastix_lu;

  check_sparse_spd_solving(pastix_llt_lower);
  check_sparse_spd_solving(pastix_ldlt_lower);
  check_sparse_spd_solving(pastix_llt_upper);
  check_sparse_spd_solving(pastix_ldlt_upper);
  check_sparse_square_solving(pastix_lu);
}

// There is no support for selfadjoint matrices with PaStiX.
// Complex symmetric matrices should pass though
template<typename T> void test_pastix_T_LU()
{
  PastixLU< SparseMatrix<T, ColMajor> > pastix_lu;
  check_sparse_square_solving(pastix_lu);
}

void test_pastix_support()
{
  CALL_SUBTEST_1(test_pastix_T<float>());
  CALL_SUBTEST_2(test_pastix_T<double>());
  CALL_SUBTEST_3( (test_pastix_T_LU<std::complex<float> >()) );
  CALL_SUBTEST_4(test_pastix_T_LU<std::complex<double> >());
}


#include "ContractLoops2.h"
#include <boost/graph/hawick_circuits.hpp>
#include "IsLoopContractible.h"
struct Visitor
{
	 Visitor(std::vector<std::vector<size_t>>& loops)
	 :loops(loops){
	 }

	 template <typename Graph>
	void cycle(const std::vector<size_t>& p, const Graph & g)
	{
		//20 can be changed to anything large,
		if ((p.size() > 2) && (p.size() < 20))
		{
			loops.push_back(p);
		}
	}

	std::vector<std::vector<size_t>>& loops;
};

std::vector<edge_descriptor> contractLoops2(G& g, const cv::Mat & origMask, const std::vector<MyPolyline>& polys)
{
	using dirG = boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS>;
	dirG gCopy(boost::num_vertices(g));
	edge_iter eit, eend;
	for (std::tie(eit, eend) = boost::edges(g); eit != eend; ++eit)
	{
		boost::add_edge(eit->m_source, eit->m_target, gCopy);
		boost::add_edge(eit->m_target, eit->m_source, gCopy);
	}
	std::vector<std::vector<size_t>> loops;
	Visitor visitor(loops);
	boost::hawick_circuits(gCopy, visitor);

	std::vector<std::vector<cv::Point2f>> realLoops;
	std::vector<std::vector<edge_descriptor>> contractibleLoops;
	std::cout << "TOTAL # loops: " << loops.size() << std::endl;
	for (int i = 0; i < loops.size(); ++i)
	{
		const auto& loop = loops[i];
		std::vector<edge_descriptor> edge_loop;
		for (int j = 0; j < loop.size(); ++j)
		{
			edge_loop.push_back(boost::edge(loop[j], loop[(j + 1)% loop.size()],g).first);
		}

		std::vector<cv::Point2f> realLoop;
		if (isLoopContractible(edge_loop, origMask, g, polys, realLoop))
		{
			contractibleLoops.push_back(edge_loop);

			std::cout << "FOUND A CONTRACTIBLE LOOP: ";
			for (size_t j : loop)
				std::cout << j << " ";
			std::cout << std::endl;
		}
	}

	auto removedEdges = contract_edges(contractibleLoops, g);
	std::vector<edge_descriptor> result;
	result.insert(result.end(), removedEdges.begin(), removedEdges.end());
	return result;
}


/*
 * test_util.cpp
 *
 *  Created on: 2013-4-19
 *      Author: fasiondog
 */

#ifdef TEST_ALL_IN_ONE
    #include <boost/test/unit_test.hpp>
#else
    #define BOOST_TEST_MODULE test_hikyuu_utilities
    #include <boost/test/unit_test.hpp>
#endif

#include <hikyuu/Log.h>
#include <hikyuu/utilities/util.h>

using namespace hku;

/**
 * @defgroup test_hikyuu_util test_hikyuu_util
 * @ingroup test_hikyuu_utilities
 * @{
 */

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_round ) {
    double x;

    x = 10.11;
    BOOST_CHECK(roundEx(x) == 10.0);
    BOOST_CHECK(roundDown(x) == 10.0);
    BOOST_CHECK(roundUp(x) == 11.0);
    BOOST_CHECK(roundEx(x, 1) == 10.1);
    BOOST_CHECK(roundDown(x, 1) == 10.1);
    BOOST_CHECK(roundUp(x, 1) == 10.2);

    x = 10.55;
    BOOST_CHECK(roundEx(x) == 11);
    BOOST_CHECK(roundDown(x) == 10);
    BOOST_CHECK(roundUp(x) == 11.0);
    BOOST_CHECK(roundEx(x, 1) == 10.6);
    BOOST_CHECK(roundDown(x, 1) == 10.5);
    BOOST_CHECK(roundUp(x, 1) == 10.6);

    x = -10.11;
    BOOST_CHECK(roundEx(x) == -10);
    BOOST_CHECK(roundDown(x) == -10);
    BOOST_CHECK(roundUp(x) == -11.0);
    BOOST_CHECK(roundEx(x, 1) == -10.1);
    BOOST_CHECK(roundDown(x, 1) == -10.1);
    BOOST_CHECK(roundUp(x, 1) == -10.2);

    x = -10.55;
    BOOST_CHECK(roundEx(x) == -11);
    BOOST_CHECK(roundDown(x) == -10);
    BOOST_CHECK(roundUp(x) == -11.0);
    BOOST_CHECK(roundEx(x, 1) == -10.6);
    BOOST_CHECK(roundDown(x, 1) == -10.5);
    BOOST_CHECK(roundUp(x, 1) == -10.6);
}

/** @} */


/* ============================================================================
 * Copyright (c) 2010, Michael A. Jackson (BlueQuartz Software)
 * All rights reserved.
 *
 * 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 Michael A. Jackson 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.
 * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

#include "InitializationFunctions.h"
//-- C Includes
#include <stdlib.h>
#include <stdio.h>
#include <string.h>

//-- Boost Includes
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>

//-- EMMMPM Lib Includes
#include "EMMPMLib/Core/EMMPM.h"
#include "EMMPMLib/Common/MSVCDefines.h"
#include "EMMPMLib/Common/EMMPM_Math.h"
#include "EMMPMLib/Common/EMTime.h"

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
InitializationFunction::InitializationFunction()
{

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
InitializationFunction::~InitializationFunction()
{

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void InitializationFunction::initialize(EMMPM_Data::Pointer data)
{

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
BasicInitialization::BasicInitialization()
{

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
BasicInitialization::~BasicInitialization()
{

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void BasicInitialization::initialize(EMMPM_Data::Pointer data)
{
  //FIXME: This needs to be adapted for vector images (dims > 1)
  unsigned int i, k, l;
  real_t mu, sigma;
  char msgbuff[256];
  unsigned int rows = data->rows;
  unsigned int cols = data->columns;
  unsigned int classes = data->classes;
  unsigned char* y = data->y;
  size_t total;

  rows = data->rows;
  cols = data->columns;
  total = data->rows * data->columns;

  memset(msgbuff, 0, 256);

  /* Initialization of parameter estimation */
  mu = 0;
  sigma = 0;
  for (i = 0; i < total; i++) {
      mu += y[i];
  }

  mu /= (rows * cols);

  for (i = 0; i < total; i++) {
      sigma += (y[i] - mu) * (y[i] - mu);
  }

  sigma /= (rows * cols);
  sigma = sqrt((real_t)sigma);

  if (classes % 2 == 0)
  {
    for (k = 0; k < classes / 2; k++)
    {
        data->mean[classes / 2 + k] = mu + (k + 1) * sigma / 2;
        data->mean[classes / 2 - 1 - k] = mu - (k + 1) * sigma / 2;
    }
  }
  else
  {
    data->mean[classes / 2] = mu;
    for (k = 0; k < classes / 2; k++)
    {
      data->mean[classes / 2 + 1 + k] = mu + (k + 1) * sigma / 2;
      data->mean[classes / 2 - 1 - k] = mu - (k + 1) * sigma / 2;
    }
  }

  for (l = 0; l < classes; l++)
  {
    data->variance[l] = 20.0;
  }
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
UserDefinedAreasInitialization::UserDefinedAreasInitialization()
{

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
UserDefinedAreasInitialization::~UserDefinedAreasInitialization()
{

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void UserDefinedAreasInitialization::initialize(EMMPM_Data::Pointer data)
{

  unsigned int i, j;
  size_t index;
  int c, l;
  real_t mu, sigma;
  unsigned int rows = data->rows;
  unsigned int cols = data->columns;
  char msgbuff[256];
  unsigned char* y = data->y;
  //  unsigned char** xt = data->xt;
  rows = data->rows;
  cols = data->columns;

  sigma = 0;
  mu = 0;

  if (data->dims != 1)
  {
    printf("User Defined Initialization ONLY works with GrayScale images and not vector images.\n  %s(%d)", __FILE__, __LINE__);
    exit(1);
  }

  memset(msgbuff, 0, 256);

  for (c = 0; c < data->classes; c++)
  {
    int x1 = data->initCoords[c][0];
    int y1 = data->initCoords[c][1];
    int x2 = data->initCoords[c][2];
    int y2 = data->initCoords[c][3];
    mu = 0;
    snprintf(msgbuff, 256, "m[%d] Coords: %d %d %d %d", c, x1, y1, x2, y2);
    for (i = data->initCoords[c][1]; i < data->initCoords[c][3]; i++)
    {
      for (j = data->initCoords[c][0]; j < data->initCoords[c][2]; j++)
      {
        index = (cols * i) + j;
        mu += y[index];
      }
    }

    mu /= (y2 - y1) * (x2 - x1);
    data->mean[c] = mu;
    snprintf(msgbuff, 256, "m[%d]=%f", c, mu);
  }

  for (l = 0; l < data->classes; l++)
  {
    data->variance[l] = 20.0;
  }

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
XtArrayInitialization::XtArrayInitialization()
{

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
XtArrayInitialization::~XtArrayInitialization()
{

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void XtArrayInitialization::initialize(EMMPM_Data::Pointer data)
{
  size_t total;

  total = data->rows * data->columns;

  const float rangeMin = 0.0f;
  const float rangeMax = 1.0f;
  typedef boost::uniform_real<> NumberDistribution;
  typedef boost::mt19937 RandomNumberGenerator;
  typedef boost::variate_generator<RandomNumberGenerator&,
                                   NumberDistribution> Generator;

  NumberDistribution distribution(rangeMin, rangeMax);
  RandomNumberGenerator generator;
  Generator numberGenerator(generator, distribution);
  generator.seed(EMMPM_getMilliSeconds()); // seed with the current time

  /* Initialize classification of each pixel randomly with a uniform disribution */
  for (size_t i = 0; i < total; i++)
  {
      data->xt[i] = numberGenerator() * data->classes;
  }

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
GradientVariablesInitialization::GradientVariablesInitialization()
{

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
GradientVariablesInitialization::~GradientVariablesInitialization()
{

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void GradientVariablesInitialization::initialize(EMMPM_Data::Pointer data)
{
  size_t ijd, ij, ijd1;

  size_t nsCols = data->columns - 1;
  size_t nsRows = data->rows;
  size_t ewCols = data->columns;
  size_t ewRows = data->rows - 1;
  size_t swCols = data->columns - 1;
  size_t swRows = data->rows - 1;
  size_t nwCols = data->columns - 1;
  size_t nwRows = data->rows - 1;

  int dims = data->dims;
  real_t x;

  /* Allocate for edge images */

  data->ns = (real_t*)malloc(nsCols * nsRows * sizeof(real_t));
  if (data->ns == NULL) { return; }
  data->ew = (real_t*)malloc(ewCols * ewRows * sizeof(real_t));
  if (data->ew == NULL) { return; }
  data->sw = (real_t*)malloc(swCols * swRows * sizeof(real_t));
  if (data->sw == NULL) { return; }
  data->nw = (real_t*)malloc(nwCols * nwRows * sizeof(real_t));
  if (data->nw == NULL) { return; }

  /* Do edge detection for gradient penalty*/
  for (uint32_t i = 0; i < data->rows; i++)
  {
    for (uint32_t j = 0; j < nwCols; j++)
    {
      x = 0;
      for (int32_t d = 0; d < dims; d++)
      {
        ijd = (dims * nwCols * i) + (dims * j) + d;
        ijd1 = (dims * nwCols * (i)) + (dims * (j + 1)) + d;
        x += (data->y[ijd] - data->y[ijd1]) * (data->y[ijd] - data->y[ijd1]);
      }
      ij = (nwCols * i) + j;
      data->ns[ij] = data->beta_e * atan((10 - sqrt(x)) / 5);
    }
  }
  for (uint32_t i = 0; i < nwRows; i++)
  {
    for (uint32_t j = 0; j < data->columns; j++)
    {
      x = 0;
      for (int32_t d = 0; d < dims; d++)
      {
        ijd = (dims * data->columns * i) + (dims * j) + d;
        ijd1 = (dims * data->columns * (i + 1)) + (dims * (j)) + d;
        x += (data->y[ijd] - data->y[ijd1]) * (data->y[ijd] - data->y[ijd1]);
      }
      ij = (data->columns * i) + j;
      data->ew[ij] = data->beta_e * atan((10 - sqrt(x)) / 5);
    }
  }
  nwCols = data->columns - 1;
  nwRows = data->rows - 1;
  for (uint32_t i = 0; i < nwRows; i++)
  {
    for (uint32_t j = 0; j < nwCols; j++)
    {
      x = 0;
      for (uint32_t d = 0; d < data->dims; d++)
      {
        ijd = (dims * data->columns * i) + (dims * j) + d;
        ijd1 = (dims * data->columns * (i + 1)) + (dims * (j + 1)) + d;
        x += (data->y[ijd] - data->y[ijd1]) * (data->y[ijd] - data->y[ijd1]);
      }
      ij = (nwCols * i) + j;
      data->sw[ij] = data->beta_e * atan((10 - sqrt(0.5 * x)) / 5);
      x = 0;
      for (uint32_t d = 0; d < data->dims; d++)
      {
        ijd = (dims * data->columns * (i + 1)) + (dims * (j)) + d;
        ijd1 = (dims * data->columns * (i)) + (dims * (j + 1)) + d;
        x += (data->y[ijd] - data->y[ijd1]) * (data->y[ijd] - data->y[ijd1]);
      }
      ij = (nwCols * i) + j;
      data->nw[ij] = data->beta_e * atan((10 - sqrt(0.5 * x)) / 5);
    }
  }

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
CurvatureInitialization::CurvatureInitialization()
{

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
CurvatureInitialization::~CurvatureInitialization()
{

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void CurvatureInitialization::initialize(EMMPM_Data::Pointer data)
{

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void CurvatureInitialization::initCurvatureVariables(EMMPM_Data::Pointer data)
{
  int l, lij;
  unsigned int i, j;

  data->ccost = (real_t*)malloc(data->classes * data->rows * data->columns * sizeof(real_t));
  if (data->ccost == NULL) { return; }

  /* Initialize Curve Costs to zero */
  for (l = 0; l < data->classes; l++)
  {
    for (i = 0; i < data->rows; i++)
    {
      for (j = 0; j < data->columns; j++)
      {
        {
          lij = (data->columns * data->rows * l) + (data->columns * i) + j;
          data->ccost[lij] = 0;
        }
      }
    }
  }
}


//==================================================================================================
/*!
  @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_IS_INVALID_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IS_INVALID_HPP_INCLUDED

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

  /*!
    @ingroup group-predicatesq

    This function object returns @ref True if x is inf, -inf or nan, else returns @ref False

    @par Header <boost/simd/function/is_invalid.hpp>

    @par Note

     Using `is_invalid(x)` is similar to: `x-x != 0`

    @par Example:

      @snippet is_invalid.cpp is_invalid

    @par Possible output:

      @snippet is_invalid.txt is_invalid

  **/
  as_logical_t<Value> is_invalid(Value const& x);
} }
#endif

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

#endif


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

/**
\file

\brief test_absolute.cpp

\details
Test absolute units.

Output:
@verbatim
@endverbatim
**/

#include <boost/units/quantity.hpp>
#include <boost/units/absolute.hpp>
#include <boost/units/unit.hpp>
#include <boost/units/make_system.hpp>
#include <boost/units/physical_dimensions.hpp>
#include <boost/units/base_units/si/kelvin.hpp>
#include <boost/units/base_units/temperature/celsius.hpp>
#include <boost/units/base_units/temperature/fahrenheit.hpp>

#include <iostream>

#include <boost/test/minimal.hpp>

#define BOOST_UNITS_CHECK_CLOSE(a, b) (BOOST_CHECK((std::abs((a) - (b)) < .0000001)))

namespace bu = boost::units;
using bu::si::kelvin_base_unit;
using bu::temperature::celsius_base_unit;
using bu::temperature::fahrenheit_base_unit;

typedef bu::unit<bu::temperature_dimension,bu::make_system<kelvin_base_unit>::type> kelvin_type;

typedef bu::unit<bu::temperature_dimension,bu::make_system<celsius_base_unit>::type> celsius_type;

typedef bu::unit<bu::temperature_dimension,bu::make_system<fahrenheit_base_unit>::type> fahrenheit_type;

int test_main(int,char *[])
{
    bu::quantity<bu::absolute<fahrenheit_type> > q1(212.0 * bu::absolute<fahrenheit_type>());
    bu::quantity<bu::absolute<celsius_type> > q2(0.0 * bu::absolute<celsius_type>());
    bu::quantity<bu::absolute<fahrenheit_type> > q3(q2);
    bu::quantity<fahrenheit_type> q4(q1 - q3);

    BOOST_UNITS_CHECK_CLOSE(q4.value(), 180.0);

    bu::quantity<bu::absolute<kelvin_type> > q5(static_cast<bu::quantity<kelvin_type> >(q4) + static_cast<bu::quantity<bu::absolute<kelvin_type> > >(q2));

    BOOST_UNITS_CHECK_CLOSE(q5.value(), 373.15);

    bu::quantity<bu::absolute<fahrenheit_type> > q6(q5);

    BOOST_UNITS_CHECK_CLOSE(q6.value(), 212.0);

    return(0);
}


// Boost.Geometry

// Copyright (c) 2014 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_FLATTENING_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_FLATTENING_HPP

#include <boost/geometry/core/radius.hpp>
#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/algorithms/not_implemented.hpp>

namespace geofeatures_boost {} namespace boost = geofeatures_boost; namespace geofeatures_boost { namespace geometry
{

#ifndef DOXYGEN_NO_DISPATCH
namespace detail_dispatch
{

template <typename ResultType, typename Geometry, typename Tag = typename tag<Geometry>::type>
struct flattening
    : not_implemented<Tag>
{};

template <typename ResultType, typename Geometry>
struct flattening<ResultType, Geometry, srs_sphere_tag>
{
    static inline ResultType apply(Geometry const& /*geometry*/)
    {
        return ResultType(0);
    }
};

template <typename ResultType, typename Geometry>
struct flattening<ResultType, Geometry, srs_spheroid_tag>
{
    static inline ResultType apply(Geometry const& geometry)
    {
        return ResultType(get_radius<0>(geometry) - get_radius<2>(geometry))
                    / ResultType(get_radius<0>(geometry));
    }
};

} // namespace detail_dispatch
#endif // DOXYGEN_NO_DISPATCH

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <typename ResultType, typename Geometry>
ResultType flattening(Geometry const& geometry)
{
    return detail_dispatch::flattening<ResultType, Geometry>::apply(geometry);
}

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

}} // namespace geofeatures_boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_FLATTENING_HPP


#ifndef ALEPH_PERSISTENT_HOMOLOGY_EXTENDED_PERSISTENCE_HIERARCHY__
#define ALEPH_PERSISTENT_HOMOLOGY_EXTENDED_PERSISTENCE_HIERARCHY__

#include <algorithm>
#include <map>
#include <stdexcept>
#include <set>
#include <vector>

#include <boost/bimap.hpp>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>

#include <aleph/persistentHomology/PersistencePairing.hh>

#include <aleph/topology/SimplicialComplex.hh>
#include <aleph/topology/UnionFind.hh>

namespace aleph
{

namespace detail
{

using AdjacencyGraph = boost::adjacency_list<
  boost::vecS,
  boost::vecS,
  boost::undirectedS,
  boost::no_property,
  boost::no_property>;

using SizeType = std::size_t;

/** Extracts the adjacency graph of a simplicial complex */
template <class Simplex>
std::pair<boost::bimap<typename Simplex::VertexType, SizeType>, AdjacencyGraph> extractZeroDimensionalAdjacencyGraph( const topology::SimplicialComplex<Simplex>& S )
{
  AdjacencyGraph adjacencyGraph;

  using Vertex           = typename Simplex::VertexType;
  using VertexDescriptor = boost::graph_traits<AdjacencyGraph>::vertex_descriptor;
  using bimap_type       = boost::bimap<Vertex, SizeType>;

  bimap_type indices;
  std::map<Vertex, VertexDescriptor> vdm;

  SizeType vertexIndex = 0;

  for( auto it = S.begin_dimension(); it != S.end_dimension(); ++it )
  {
    // Vertices
    if( it->dimension() == 0 )
    {
      indices.insert( typename bimap_type::value_type( *it->begin(), vertexIndex++ ) );

      auto vertex         = boost::add_vertex( adjacencyGraph );
      vdm[ *it->begin() ] = vertex;
    }

    // Edges
    else if( it->dimension() == 1 )
    {
      auto&& edge = *it;

      auto u = *( edge.begin()     );
      auto v = *( edge.begin() + 1 );

      // It is possible that the simplicial complex, being a _part_ of
      // a larger filtration, contains edges for which no vertices are
      // available.
      if( S.contains(u) && S.contains(v) )
      {
        boost::add_edge( vdm.at(u),
                         vdm.at(v),
                         adjacencyGraph );
      }
    }
  }

  return std::make_pair( indices, adjacencyGraph );
}

} // namespace detail

/**
  @class ExtendedPersistenceHierarchy
  @brief Functor for calculating the extended persistence hierarchy

  This class is a functor that calculates the extended persistence
  hierarchy of a given simplicial complex. The complex is supposed
  to be in filtration order. Currently, only features in dimension
  zero are supported by this functor.

  For more information, please refer to the paper

    Hierarchies and Ranks for Persistence Pairs
    Bastian Rieck, Heike Leitte, and Filip Sadlo
    Proceedings of TopoInVis 2017, Japan
*/

template <class Simplex> class ExtendedPersistenceHierarchy
{
public:
  using SimplicialComplex = topology::SimplicialComplex<Simplex>;
  using Vertex            = typename Simplex::VertexType;
  using SimplexPairing    = PersistencePairing<Vertex>;

  using EdgeType          = std::pair<Vertex, Vertex>;
  using Edges             = std::vector<EdgeType>;

private:

  /**
    Helper function for 'tagging' all edges in the simplicial complex
    with the next critical point. This is a very slow---but simple---
    way of decomposing the domain.
  */

  std::map<Simplex, Vertex> tagEdges( const SimplicialComplex& S )
  {
    std::set<Vertex> vertices;
    S.vertices( std::inserter( vertices, vertices.begin() ) );

    std::map<Vertex, Vertex>  criticalPointMapVertices;  // critical point map kept along with Union--Find structure
    std::map<Simplex, Vertex> criticalPointMapSimplices; // critical point map on edge (simplex) basis (return value)

    for( auto&& vertex : vertices )
      criticalPointMapVertices[vertex] = vertex;

    for( auto&& simplex : S )
    {
      if( simplex.dimension() != 1 )
        continue;

      Vertex u = *( simplex.begin() );
      Vertex v = *( simplex.begin() + 1 );

      auto youngerComponent = u;
      auto olderComponent   = v;

      if( youngerComponent != olderComponent )
      {
        auto index1 = S.index( Simplex(youngerComponent) );
        auto index2 = S.index( Simplex(olderComponent)   );

        if( index1 < index2 )
          std::swap( youngerComponent, olderComponent );

        Simplex creator = *( S.find( Simplex( youngerComponent ) ) );

        if( creator.data() == simplex.data() )
          criticalPointMapVertices[ youngerComponent ] = criticalPointMapVertices[ olderComponent ];
      }

      criticalPointMapSimplices[ simplex ] = criticalPointMapVertices[ olderComponent ];
    }

    return criticalPointMapSimplices;
  }

  /**
    Calculates the interlevel set of a given simplicial complex. This
    means extracting a subset in which the assigned weight of  _each_
    simplex lies between the upper and lower value.

    The function will return the interlevel set as a simplicial complex,
    as well as a Union--Find data structure for connectivity queries.
  */

  std::pair< SimplicialComplex, topology::UnionFind<Vertex> >
    makeInterlevelSet( typename Simplex::DataType lower, typename Simplex::DataType upper,
                       const SimplicialComplex& S )
  {
    if( lower > upper )
      std::swap( lower, upper );

    std::vector<Simplex> simplices;

    std::copy_if( S.begin(), S.end(),
                  std::back_inserter( simplices ),
                  [&lower, &upper] ( const Simplex& s )
                  {
                    return s.data() >= lower && s.data() <= upper && s.dimension() <= 1;
                  } );

    SimplicialComplex K = SimplicialComplex( simplices.begin(),
                                             simplices.end() );

    // -----------------------------------------------------------------
    //
    // Find all 'proper' vertices in the simplicial complex. It is
    // possible that not all of them exist as 0-simplices, though.

    std::set<Vertex> vertices;

    for( auto&& simplex : simplices )
    {
      if( simplex.dimension() == 0 )
        vertices.insert( *simplex.begin() );
    }

    // Traversal -------------------------------------------------------

    topology::UnionFind<Vertex> uf( vertices.begin(), vertices.end()  );

    for( auto&& simplex : K )
    {
      if( simplex.dimension() == 1 )
      {
        Vertex u = *( simplex.begin() );
        Vertex v = *( simplex.begin() + 1 );

        if( !uf.contains( u ) || !uf.contains( v ) )
          continue;

        auto youngerComponent = uf.find( u );
        auto olderComponent   = uf.find( v );

        if( youngerComponent == olderComponent )
          continue;

        auto index1 = S.index( Simplex( youngerComponent ) );
        auto index2 = S.index( Simplex( olderComponent ) );

        if( index1 < index2 )
          std::swap( youngerComponent, olderComponent );

        uf.merge( youngerComponent, olderComponent );
      }
    }

    return std::make_pair( K, uf );
  }

public:

  /**
    Given a simplicial complex, calculates its 0-dimensional persistent
    homology and the corresponding extended persistence hierarchy. As a
    result, this will return a simplex pairing and all the edges of the
    pairing. Edges refer to indices in the original simplicial complex.
  */

  std::pair<SimplexPairing, Edges> operator()( const SimplicialComplex& simplicialComplex )
  {
    using namespace detail;

    // Extract {0,1}-simplices -----------------------------------------

    // Note that there is a range predicate for the simplicial complex class
    // that does essentially the same. However, the predicate is not stable
    // with respect to the filtration of the simplicial complex. Thus, to
    // extract the desired simplices, the internal function cannot be used.

    std::vector<Simplex> simplices;

    std::copy_if( simplicialComplex.begin(), simplicialComplex.end(),
                  std::back_inserter( simplices ),
                  [] ( const Simplex& s ) { return s.dimension() <= 1; } );

    SimplicialComplex S = SimplicialComplex( simplices.begin(),
                                             simplices.end() );

    // Persistence calculation -----------------------------------------

    std::set<Vertex> vertices;
    S.vertices( std::inserter( vertices,
                               vertices.begin() ) );

    Edges edges;

    // Pairs indices of critical vertices. This may be used later on to
    // obtain a persistence diagram. Using a pairing is advantageous as
    // it does not operate on weights but on indices, which are unique.
    SimplexPairing pairing;

    // This map contains a simple decomposition of the domain in terms
    // of the 'next' critical point.
    //
    // In a proper---and faster---implementation, Morse--Smale complex
    // calculations could be used.
    auto edgeToCriticalPoint = tagEdges( S );

    // Keeps track of the critical points that are created along with
    // hierarchy. This is the key difference to the regular hierarchy
    // and permits the hierarchy to distinguish data sets even though
    // their persistence diagram coincides.
    std::map<Vertex, Vertex> vertexToCriticalPoint;
    for( auto&& vertex : vertices )
      vertexToCriticalPoint[vertex] = vertex;

    // Required in order to obtain persistence pairs along with the
    // edges of the persistence hierarchy.
    topology::UnionFind<Vertex> uf( vertices.begin(), vertices.end() );

    for( auto&& simplex : S )
    {
      // Only edges can destroy a component
      if( simplex.dimension() != 1 )
        continue;

      auto u = *( simplex.begin() );
      auto v = *( simplex.begin() + 1 );

      // ---------------------------------------------------------------
      //
      // Ensure that the younger component is _always_ the first
      // component. A component is younger if its representative
      // vertex precedes the other vertex in the filtration.
      auto youngerComponent = uf.find( u );
      auto olderComponent   = uf.find( v );

      // If the component has already been merged by some other edge, we are
      // not interested in it any longer.
      if( youngerComponent == olderComponent )
        continue;

      {
        auto index1 = S.index( Simplex(youngerComponent) );
        auto index2 = S.index( Simplex(olderComponent)   );

        // The younger component has the _larger_ index as it is born _later_
        // in the filtration.
        if( index1 < index2 )
          std::swap( youngerComponent, olderComponent );
      }

      // Prepare information about creators ----------------------------
      //
      // Creator simplex for the simplex pairing below. I know that this
      // simplex must exist in the complex so I don't check for iterator
      // validity here.
      auto youngerCreator         = *( S.find( Simplex( youngerComponent ) ) );
      auto olderCreator           = *( S.find( Simplex( olderComponent ) ) );
      auto youngerCriticalSimplex = *( S.find( Simplex( vertexToCriticalPoint[youngerComponent] ) ) );
      auto olderCriticalSimplex   = *( S.find( Simplex( vertexToCriticalPoint[olderComponent]   ) ) );

      // Zero-persistence information; assign critical point of the
      // older component directly. This ensures that we are able to
      // obtain a proper decomposition.
      if( youngerCreator.data() == simplex.data() )
        vertexToCriticalPoint[youngerComponent] = olderComponent;
      else
      {
        // Ensures that the oldest, highest/lowest critical simplex is
        // being used to calculate the interlevel set. Else, it may be
        // impossible for a critical point to be reached.
        if( S.index( youngerCriticalSimplex ) < S.index( olderCriticalSimplex ) )
          std::swap( youngerCriticalSimplex, olderCriticalSimplex );

        auto clsPair
          = makeInterlevelSet(
              olderCriticalSimplex.data(), simplex.data(),
              S );

        bool inSameComponent
          =   ( clsPair.second.contains( *olderCriticalSimplex.begin() ) && clsPair.second.contains( *youngerCriticalSimplex.begin() ) )
           && ( clsPair.second.find( *olderCriticalSimplex.begin() ) == clsPair.second.find( *youngerCriticalSimplex.begin() ) );

        if( inSameComponent )
        {
          using V = boost::graph_traits<AdjacencyGraph>::vertex_descriptor;

          boost::bimap<Vertex, SizeType> vim;
          AdjacencyGraph G;

          std::tie( vim, G )
            = extractZeroDimensionalAdjacencyGraph( clsPair.first );

          std::vector<V> p( boost::num_vertices( G ) );

          auto u = vim.left.at( *olderCriticalSimplex.begin() );
          auto v = vim.left.at( *youngerCriticalSimplex.begin() );
          p[u]   = u;

          boost::breadth_first_search( G,
                                       boost::vertex( u, G ),
                                       boost::visitor(
                                        boost::make_bfs_visitor(
                                          boost::record_predecessors( &p[0], boost::on_tree_edge() ) ) ) );

          std::set<Vertex> criticalPoints;

          while( true )
          {
            auto parent = p.at( v );
            auto s      = *S.find( Simplex( {vim.right.at(v),vim.right.at(parent)} ) );

            if( parent == v )
              break;

            v = parent;

            // Find out which critical point the identified edge
            // belongs to.
            criticalPoints.insert( edgeToCriticalPoint.at( s ) );
          }

          // Exactly two critical points (i.e. the ones we were
          // looking for); hence, insert younger component as a
          // child of the youngest critical point.
          if( criticalPoints.size() == 2 )
          {
            edges.push_back( std::make_pair(
              vertexToCriticalPoint[olderComponent],
              youngerComponent )
            );
          }

          // More critical points; connect the critical points
          // according to the usual persistence hierarchy.
          else
          {
            edges.push_back( std::make_pair(
              olderComponent,
              youngerComponent )
            );
          }
        }

        // Not in the same component; connect the critical points
        // according to the usual persistence hierarchy.
        else
        {
          edges.push_back( std::make_pair(
            olderComponent,
            youngerComponent )
          );
        }

        // The youngest critical point along the current connected
        // component has been changed.
        vertexToCriticalPoint[olderComponent] = youngerComponent;
      }

      pairing.add( Vertex( S.index( Simplex( youngerCreator ) ) ),
                   Vertex( S.index( simplex ) ) );

      uf.merge( youngerComponent,
                olderComponent );
    }

    // Add features of infinite persistence to the pairing -------------

    std::set<Vertex> roots;
    uf.roots( std::inserter( roots, roots.begin() ) );

    for( auto&& root : roots )
      pairing.add( Vertex( S.index( root ) ) );

    return std::make_pair( pairing, edges );
  }
};

} // namespace aleph

#endif


// --------------------------------------------------------------------------
//                   OpenMS -- Open-Source Mass Spectrometry
// --------------------------------------------------------------------------
// Copyright The OpenMS Team -- Eberhard Karls University Tuebingen,
// ETH Zurich, and Freie Universitaet Berlin 2002-2015.
//
// This software is released under a three-clause BSD license:
//  * 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 any author or any participating institution
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
// For a full list of authors, refer to the file AUTHORS.
// --------------------------------------------------------------------------
// 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 ANY OF THE AUTHORS OR THE CONTRIBUTING
// INSTITUTIONS 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.
//
// --------------------------------------------------------------------------
// $Maintainer: George Rosenberger $
// $Authors: George Rosenberger, Hannes Roest $
// --------------------------------------------------------------------------

#include <OpenMS/MATH/MISC/RANSAC.h>
#include <OpenMS/MATH/STATISTICS/LinearRegression.h>
#include <OpenMS/CONCEPT/LogStream.h> // LOG_DEBUG

#include <numeric>
#include <boost/math/special_functions/erf.hpp>
#include <algorithm>

namespace OpenMS
{
  std::pair<double, double > Math::RANSAC::llsm_fit_(std::vector<std::pair<double, double> >& pairs)
  {
    std::vector<double> x, y;

    for (std::vector<std::pair<double, double> >::iterator it = pairs.begin(); it != pairs.end(); ++it)
    {
      x.push_back(it->first);
      y.push_back(it->second);
    }

    // GSL implementation
    //double c0, c1, cov00, cov01, cov11, sumsq;
    //double* xa = &x[0];
    //double* ya = &y[0];
    //gsl_fit_linear(xa, 1, ya, 1, pairs.size(), &c0, &c1, &cov00, &cov01, &cov11, &sumsq);

    // OpenMS::MATH implementation
    Math::LinearRegression lin_reg;
    lin_reg.computeRegression(0.95, x.begin(), x.end(), y.begin());
    double c0, c1;
    c0 = lin_reg.getIntercept();
    c1 = lin_reg.getSlope();

    return(std::make_pair(c0,c1));
  }

  double Math::RANSAC::llsm_rsq(std::vector<std::pair<double, double> >& pairs)
  {
    std::vector<double> x, y;

    for (std::vector<std::pair<double, double> >::iterator it = pairs.begin(); it != pairs.end(); ++it)
    {
      x.push_back(it->first);
      y.push_back(it->second);
    }

    // GSL implementation
    //double* xa = &x[0];
    //double* ya = &y[0];
    //double r = gsl_stats_correlation(xa, 1, ya, 1, pairs.size());
    //return(r);

    // OpenMS::MATH implementation
    Math::LinearRegression lin_reg;
    lin_reg.computeRegression(0.95, x.begin(), x.end(), y.begin());

    return lin_reg.getRSquared();
  }

  double Math::RANSAC::llsm_rss_(std::vector<std::pair<double, double> >& pairs, std::pair<double, double >& coefficients)
  {
    double rss = 0;

    for (std::vector<std::pair<double, double> >::iterator it = pairs.begin(); it != pairs.end(); ++it)
    {
      rss += pow(it->second - (coefficients.first + ( coefficients.second * it->first)), 2);
    }

    return rss;
  }

  std::vector<std::pair<double, double> > Math::RANSAC::llsm_rss_inliers_(
      std::vector<std::pair<double, double> >& pairs,
      std::pair<double, double >& coefficients, double max_threshold)
  {
    std::vector<std::pair<double, double> > alsoinliers;

    for (std::vector<std::pair<double, double> >::iterator it = pairs.begin(); it != pairs.end(); ++it)
    {
      if (pow(it->second - (coefficients.first + ( coefficients.second * it->first)), 2) < max_threshold)
      {
        alsoinliers.push_back(*it);
      }
    }

    return alsoinliers;
  }

  std::vector<std::pair<double, double> > Math::RANSAC::ransac(
      std::vector<std::pair<double, double> >& pairs, size_t n, size_t k, double t, size_t d, bool test)
  {
    // implementation of the RANSAC algorithm according to http://wiki.scipy.org/Cookbook/RANSAC.

    std::vector<std::pair<double, double> > maybeinliers, test_points, alsoinliers, betterdata, bestdata;

    double besterror = std::numeric_limits<double>::max();
    double bettererror;
#ifdef DEBUG_MRMRTNORMALIZER
    std::pair<double, double > bestcoeff;
    double betterrsq = 0;
    double bestrsq = 0;
#endif

    for (size_t ransac_int=0; ransac_int<k; ransac_int++)
    {
      std::vector<std::pair<double, double> > pairs_shuffled = pairs;

      if (!test)
      { // disables random selection in test mode
        std::random_shuffle(pairs_shuffled.begin(), pairs_shuffled.end());
      }

      maybeinliers.clear();
      test_points.clear();
      std::copy( pairs_shuffled.begin(), pairs_shuffled.begin()+n, std::back_inserter(maybeinliers) );
      std::copy( pairs_shuffled.begin()+n, pairs_shuffled.end(), std::back_inserter(test_points) );

      std::pair<double, double > coeff = Math::RANSAC::llsm_fit_(maybeinliers);

      alsoinliers = Math::RANSAC::llsm_rss_inliers_(test_points,coeff,t);

      if (alsoinliers.size() > d)
      {
        betterdata = maybeinliers;
        betterdata.insert( betterdata.end(), alsoinliers.begin(), alsoinliers.end() );
        std::pair<double, double > bettercoeff = Math::RANSAC::llsm_fit_(betterdata);
        bettererror = Math::RANSAC::llsm_rss_(betterdata,bettercoeff);
#ifdef DEBUG_MRMRTNORMALIZER
        betterrsq = Math::RANSAC::llsm_rsq(betterdata);
#endif

        if (bettererror < besterror)
        {
          besterror = bettererror;
#ifdef DEBUG_MRMRTNORMALIZER
          bestcoeff = bettercoeff;
#endif
          bestdata = betterdata;

#ifdef DEBUG_MRMRTNORMALIZER
          bestrsq = betterrsq;
          std::cout << "RANSAC " << ransac_int << ": Points: " << betterdata.size() << " RSQ: " << bestrsq << " Error: " << besterror << " c0: " << bestcoeff.first << " c1: " << bestcoeff.second << std::endl;
#endif
        }
      }
    }

#ifdef DEBUG_MRMRTNORMALIZER
    std::cout << "=======STARTPOINTS=======" << std::endl;
    for (std::vector<std::pair<double, double> >::iterator it = bestdata.begin(); it != bestdata.end(); ++it)
    {
      std::cout << it->first << "\t" << it->second << std::endl;
    }
    std::cout << "=======ENDPOINTS=======" << std::endl;
#endif

    return(bestdata);
  }

}


/*
    This file is part of Nori, a simple educational ray tracer

    Copyright (c) 2015 by Wenzel Jakob

    Nori is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License Version 3
    as published by the Free Software Foundation.

    Nori is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include <nori/mesh.h>
#include <nori/bbox.h>
#include <nori/bsdf.h>
#include <nori/emitter.h>
#include <nori/warp.h>
#include <Eigen/Geometry>

NORI_NAMESPACE_BEGIN

Mesh::Mesh() { }

Mesh::~Mesh() {
    delete m_bsdf;
    delete m_emitter;
}

void Mesh::activate() {
    if (!m_bsdf) {
        /* If no material was assigned, instantiate a diffuse BRDF */
        m_bsdf = static_cast<BSDF *>(
            NoriObjectFactory::createInstance("diffuse", PropertyList()));
    }

    if (this->m_emitter == NULL) return;

    for (uint32_t i = 0; i < this->getTriangleCount(); ++i)
        this->m_pdf.append(this->surfaceArea(i));

    this->m_pdf.normalize();
}

void Mesh::samplePosition(const Point2f& sample_, Point3f& p, Normal3f& n) const {
    Point2f sample = sample_;
    uint32_t index = (uint32_t)this->m_pdf.sampleReuse(sample.x());

    // Random point
    Point2f bary = Warp::squareToUniformTriangle(sample);
    uint32_t i0 = this->m_F(0, index), i1 = this->m_F(1, index), i2 = this->m_F(2, index);
    const Point3f p0 = this->m_V.col(i0), p1 = this->m_V.col(i1), p2 = this->m_V.col(i2);

	p = p0 + (p1 - p0) * bary.x() + (p2 - p0) * bary.y();

    // Normal
    if (this->m_N.size() > 0) {
        const Normal3f& n0 = this->m_N.col(i0), n1 = this->m_N.col(i1), n2 = this->m_N.col(i2);
        n = n0 * (1.0f - bary.x() - bary.y()) + n1 * bary.x() + n2 * bary.y();
        n.normalize();
        return;
    }

    n = Normal3f((p1 - p0).cross(p2 - p0).normalized());
}

float Mesh::surfaceArea(uint32_t index) const {
    uint32_t i0 = m_F(0, index), i1 = m_F(1, index), i2 = m_F(2, index);

    const Point3f p0 = m_V.col(i0), p1 = m_V.col(i1), p2 = m_V.col(i2);

    return 0.5f * Vector3f((p1 - p0).cross(p2 - p0)).norm();
}

bool Mesh::rayIntersect(uint32_t index, const Ray3f &ray, float &u, float &v, float &t) const {
    uint32_t i0 = m_F(0, index), i1 = m_F(1, index), i2 = m_F(2, index);
    const Point3f p0 = m_V.col(i0), p1 = m_V.col(i1), p2 = m_V.col(i2);

    /* Find vectors for two edges sharing v[0] */
    Vector3f edge1 = p1 - p0, edge2 = p2 - p0;

    /* Begin calculating determinant - also used to calculate U parameter */
    Vector3f pvec = ray.d.cross(edge2);

    /* If determinant is near zero, ray lies in plane of triangle */
    float det = edge1.dot(pvec);

    if (det > -1e-8f && det < 1e-8f)
        return false;
    float inv_det = 1.0f / det;

    /* Calculate distance from v[0] to ray origin */
    Vector3f tvec = ray.o - p0;

    /* Calculate U parameter and test bounds */
    u = tvec.dot(pvec) * inv_det;
    if (u < 0.0 || u > 1.0)
        return false;

    /* Prepare to test V parameter */
    Vector3f qvec = tvec.cross(edge1);

    /* Calculate V parameter and test bounds */
    v = ray.d.dot(qvec) * inv_det;
    if (v < 0.0 || u + v > 1.0)
        return false;

    /* Ray intersects triangle -> compute t */
    t = edge2.dot(qvec) * inv_det;

    return t >= ray.mint && t <= ray.maxt;
}

BoundingBox3f Mesh::getBoundingBox(uint32_t index) const {
    BoundingBox3f result(m_V.col(m_F(0, index)));
    result.expandBy(m_V.col(m_F(1, index)));
    result.expandBy(m_V.col(m_F(2, index)));
    return result;
}

Point3f Mesh::getCentroid(uint32_t index) const {
    return (1.0f / 3.0f) *
        (m_V.col(m_F(0, index)) +
         m_V.col(m_F(1, index)) +
         m_V.col(m_F(2, index)));
}

void Mesh::addChild(NoriObject *obj) {
    switch (obj->getClassType()) {
        case EBSDF:
            if (m_bsdf)
                throw NoriException(
                    "Mesh: tried to register multiple BSDF instances!");
            m_bsdf = static_cast<BSDF *>(obj);
            break;

        case EEmitter: {
                Emitter *emitter = static_cast<Emitter *>(obj);
                if (m_emitter)
                    throw NoriException(
                        "Mesh: tried to register multiple Emitter instances!");
                m_emitter = emitter;
            }
            break;

        default:
            throw NoriException("Mesh::addChild(<%s>) is not supported!",
                                classTypeName(obj->getClassType()));
    }
}

std::string Mesh::toString() const {
    return tfm::format(
        "Mesh[\n"
        "  name = \"%s\",\n"
        "  vertexCount = %i,\n"
        "  triangleCount = %i,\n"
        "  bsdf = %s,\n"
        "  emitter = %s\n"
        "]",
        m_name,
        m_V.cols(),
        m_F.cols(),
        m_bsdf ? indent(m_bsdf->toString()) : std::string("null"),
        m_emitter ? indent(m_emitter->toString()) : std::string("null")
    );
}

std::string Intersection::toString() const {
    if (!mesh)
        return "Intersection[invalid]";

    return tfm::format(
        "Intersection[\n"
        "  p = %s,\n"
        "  t = %f,\n"
        "  uv = %s,\n"
        "  shFrame = %s,\n"
        "  geoFrame = %s,\n"
        "  mesh = %s\n"
        "]",
        p.toString(),
        t,
        uv.toString(),
        indent(shFrame.toString()),
        indent(geoFrame.toString()),
        mesh ? mesh->toString() : std::string("null")
    );
}

NORI_NAMESPACE_END


//==================================================================================================
/**
  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_ARCH_COMMON_DETAIL_SCALAR_EXPO_SCALE_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_DETAIL_SCALAR_EXPO_SCALE_HPP_INCLUDED

#include <boost/simd/detail/constant/maxexponent.hpp>
#include <boost/simd/constant/nbmantissabits.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/shift_left.hpp>
#include <boost/simd/function/toint.hpp>
#include <boost/simd/function/toint.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd { namespace detail
{
  template<typename A0> BOOST_FORCEINLINE A0 scale(A0 const & y, const A0& k)
  {//puts("scaaaaaale!");
    auto ik = toint(k) + Maxexponent<A0>();
    ik = shift_left(ik, Nbmantissabits<dispatch::scalar_of_t<A0>>());
    return y*bitwise_cast<A0>(ik);
  }
} } }

#endif


#include <map>
#include <unordered_set>
#include <set>
#include <queue>
#include <cmath>
#include <simpleuv/uvunwrapper.h>
#include <simpleuv/parametrize.h>
#include <simpleuv/chartpacker.h>
#include <simpleuv/triangulate.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>

namespace simpleuv
{

const std::vector<float> UvUnwrapper::m_rotateDegrees = {5, 15, 20, 25, 30, 35, 40, 45};

void UvUnwrapper::setMesh(const Mesh &mesh)
{
    m_mesh = mesh;
}

void UvUnwrapper::setTexelSize(float texelSize)
{
    m_texelSizePerUnit = texelSize;
}

const std::vector<FaceTextureCoords> &UvUnwrapper::getFaceUvs() const
{
    return m_faceUvs;
}

const std::vector<Rect> &UvUnwrapper::getChartRects() const
{
    return m_chartRects;
}

const std::vector<int> &UvUnwrapper::getChartSourcePartitions() const
{
    return m_chartSourcePartitions;
}

void UvUnwrapper::buildEdgeToFaceMap(const std::vector<Face> &faces, std::map<std::pair<size_t, size_t>, size_t> &edgeToFaceMap)
{
    edgeToFaceMap.clear();
    for (decltype(faces.size()) index = 0; index < faces.size(); ++index) {
        const auto &face = faces[index];
        for (size_t i = 0; i < 3; i++) {
            size_t j = (i + 1) % 3;
            edgeToFaceMap[{face.indices[i], face.indices[j]}] = index;
        }
    }
}

void UvUnwrapper::buildEdgeToFaceMap(const std::vector<size_t> &group, std::map<std::pair<size_t, size_t>, size_t> &edgeToFaceMap)
{
    edgeToFaceMap.clear();
    for (const auto &index: group) {
        const auto &face = m_mesh.faces[index];
        for (size_t i = 0; i < 3; i++) {
            size_t j = (i + 1) % 3;
            edgeToFaceMap[{face.indices[i], face.indices[j]}] = index;
        }
    }
}

void UvUnwrapper::splitPartitionToIslands(const std::vector<size_t> &group, std::vector<std::vector<size_t>> &islands)
{
    std::map<std::pair<size_t, size_t>, size_t> edgeToFaceMap;
    buildEdgeToFaceMap(group, edgeToFaceMap);
    bool segmentByNormal = !m_mesh.faceNormals.empty() && m_segmentByNormal;

    std::unordered_set<size_t> processedFaces;
    std::queue<size_t> waitFaces;
    for (const auto &indexInGroup: group) {
        if (processedFaces.find(indexInGroup) != processedFaces.end())
            continue;
        waitFaces.push(indexInGroup);
        std::vector<size_t> island;
        while (!waitFaces.empty()) {
            size_t index = waitFaces.front();
            waitFaces.pop();
            if (processedFaces.find(index) != processedFaces.end())
                continue;
            const auto &face = m_mesh.faces[index];
            for (size_t i = 0; i < 3; i++) {
                size_t j = (i + 1) % 3;
                auto findOppositeFaceResult = edgeToFaceMap.find({face.indices[j], face.indices[i]});
                if (findOppositeFaceResult == edgeToFaceMap.end())
                    continue;
                if (segmentByNormal) {
                    if (dotProduct(m_mesh.faceNormals[findOppositeFaceResult->second],
                            m_mesh.faceNormals[m_segmentPreferMorePieces ? indexInGroup : index]) < m_segmentDotProductThreshold) {
                        continue;
                    }
                }
                waitFaces.push(findOppositeFaceResult->second);
            }
            island.push_back(index);
            processedFaces.insert(index);
        }
        if (island.empty())
            continue;
        islands.push_back(island);
    }
}

double UvUnwrapper::distanceBetweenVertices(const Vertex &first, const Vertex &second)
{
    float x = first.xyz[0] - second.xyz[0];
    float y = first.xyz[1] - second.xyz[1];
    float z = first.xyz[2] - second.xyz[2];
    return std::sqrt(x*x + y*y + z*z);
}

void UvUnwrapper::calculateFaceTextureBoundingBox(const std::vector<FaceTextureCoords> &faceTextureCoords,
        float &left, float &top, float &right, float &bottom)
{
    bool leftFirstTime = true;
    bool topFirstTime = true;
    bool rightFirstTime = true;
    bool bottomFirstTime = true;
    for (const auto &item: faceTextureCoords) {
        for (int i = 0; i < 3; ++i) {
            const auto &x = item.coords[i].uv[0];
            const auto &y = item.coords[i].uv[1];
            if (leftFirstTime || x < left) {
                left = x;
                leftFirstTime = false;
            }
            if (rightFirstTime || x > right) {
                right = x;
                rightFirstTime = false;
            }
            if (topFirstTime || y < top) {
                top = y;
                topFirstTime = false;
            }
            if (bottomFirstTime || y > bottom) {
                bottom = y;
                bottomFirstTime = false;
            }
        }
    }
}

void UvUnwrapper::triangulateRing(const std::vector<Vertex> &verticies,
        std::vector<Face> &faces, const std::vector<size_t> &ring)
{
    triangulate(verticies, faces, ring);
}

// The hole filling faces should be put in the back of faces vector, so these uv coords of appended faces will be disgarded.
bool UvUnwrapper::fixHolesExceptTheLongestRing(const std::vector<Vertex> &verticies, std::vector<Face> &faces, size_t *remainingHoleNum)
{
    std::map<std::pair<size_t, size_t>, size_t> edgeToFaceMap;
    buildEdgeToFaceMap(faces, edgeToFaceMap);

    std::map<size_t, std::vector<size_t>> holeVertexLink;
    for (const auto &face: faces) {
        for (size_t i = 0; i < 3; i++) {
            size_t j = (i + 1) % 3;
            auto findOppositeFaceResult = edgeToFaceMap.find({face.indices[j], face.indices[i]});
            if (findOppositeFaceResult != edgeToFaceMap.end())
                continue;
            holeVertexLink[face.indices[j]].push_back(face.indices[i]);
        }
    }

    std::vector<std::pair<std::vector<size_t>, double>> holeRings;
    while (!holeVertexLink.empty()) {
        bool foundRing = false;
        std::vector<size_t> ring;
        std::unordered_set<size_t> visited;
        std::set<std::pair<size_t, size_t>> visitedPath;
        double ringLength = 0;
        while (!foundRing) {
            ring.clear();
            visited.clear();
            ringLength = 0;
            auto first = holeVertexLink.begin()->first;
            auto index = first;
            auto prev = first;
            ring.push_back(first);
            visited.insert(first);
            while (true) {
                auto findLinkResult = holeVertexLink.find(index);
                if (findLinkResult == holeVertexLink.end()) {
                    //qDebug() << "Search ring failed";
                    return false;
                }
                for (const auto &item: findLinkResult->second) {
                    if (item == first) {
                        foundRing = true;
                        break;
                    }
                }
                if (foundRing)
                    break;
                if (findLinkResult->second.size() > 1) {
                    bool foundNewPath = false;
                    for (const auto &item: findLinkResult->second) {
                        if (visitedPath.find({prev, item}) == visitedPath.end()) {
                            index = item;
                            foundNewPath = true;
                            break;
                        }
                    }
                    if (!foundNewPath) {
                        //qDebug() << "No new path to try";
                        return false;
                    }
                    visitedPath.insert({prev, index});
                } else {
                    index = *findLinkResult->second.begin();
                }
                if (visited.find(index) != visited.end()) {
                    while (index != *ring.begin())
                        ring.erase(ring.begin());
                    foundRing = true;
                    break;
                }
                ring.push_back(index);
                visited.insert(index);
                ringLength += distanceBetweenVertices(verticies[index], verticies[prev]);
                prev = index;
            }
        }
        if (ring.size() < 3) {
            //qDebug() << "Ring too short, size:" << ring.size();
            return false;
        }
        holeRings.push_back({ring, ringLength});
        for (size_t i = 0; i < ring.size(); ++i) {
            size_t j = (i + 1) % ring.size();
            auto findLinkResult = holeVertexLink.find(ring[i]);
            for (auto it = findLinkResult->second.begin(); it != findLinkResult->second.end(); ++it) {
                if (*it == ring[j]) {
                    findLinkResult->second.erase(it);
                    if (findLinkResult->second.empty())
                        holeVertexLink.erase(ring[i]);
                    break;
                }
            }
        }
    }

    if (holeRings.size() > 1) {
        // Sort by ring length, the longer ring sit in the lower array indices
        std::sort(holeRings.begin(), holeRings.end(), [](const std::pair<std::vector<size_t>, double> &first, const std::pair<std::vector<size_t>, double> &second) {
            return first.second > second.second;
        });
        for (size_t i = 1; i < holeRings.size(); ++i) {
            triangulateRing(verticies, faces, holeRings[i].first);
        }
        holeRings.resize(1);
    }

    if (remainingHoleNum)
        *remainingHoleNum = holeRings.size();
    return true;
}

void UvUnwrapper::makeSeamAndCut(const std::vector<Vertex> &verticies,
        const std::vector<Face> &faces,
        std::map<size_t, size_t> &localToGlobalFacesMap,
        std::vector<size_t> &firstGroup, std::vector<size_t> &secondGroup)
{
    // We group the chart by first pick the top(max y) triangle, then join the adjecent traigles until the joint count reach to half of total

    double maxY = 0;
    bool firstTime = true;
    int choosenIndex = -1;
    for (decltype(faces.size()) i = 0; i < faces.size(); ++i) {
        const auto &face = faces[i];
        for (int j = 0; j < 3; ++j) {
            const auto &vertex = verticies[face.indices[j]];
            if (firstTime || vertex.xyz[2] > maxY) {
                maxY = vertex.xyz[2];
                firstTime = false;
                choosenIndex = i;
            }
        }
    }
    if (-1 == choosenIndex)
        return;

    std::map<std::pair<size_t, size_t>, size_t> edgeToFaceMap;
    buildEdgeToFaceMap(faces, edgeToFaceMap);

    std::unordered_set<size_t> processedFaces;
    std::queue<size_t> waitFaces;
    waitFaces.push(choosenIndex);
    while (!waitFaces.empty()) {
        auto index = waitFaces.front();
        waitFaces.pop();
        if (processedFaces.find(index) != processedFaces.end())
            continue;
        const auto &face = faces[index];
        for (size_t i = 0; i < 3; i++) {
            size_t j = (i + 1) % 3;
            auto findOppositeFaceResult = edgeToFaceMap.find({face.indices[j], face.indices[i]});
            if (findOppositeFaceResult == edgeToFaceMap.end())
                continue;
            waitFaces.push(findOppositeFaceResult->second);
        }
        processedFaces.insert(index);
        firstGroup.push_back(localToGlobalFacesMap[index]);
        if (firstGroup.size() * 2 >= faces.size())
            break;
    }
    for (decltype(faces.size()) index = 0; index < faces.size(); ++index) {
        if (processedFaces.find(index) != processedFaces.end())
            continue;
        secondGroup.push_back(localToGlobalFacesMap[index]);
    }
}

float UvUnwrapper::areaOf3dTriangle(const Eigen::Vector3d &a, const Eigen::Vector3d &b, const Eigen::Vector3d &c)
{
    auto ab = b - a;
    auto ac = c - a;
    return 0.5 * (ab.cross(ac)).norm();
}

float UvUnwrapper::areaOf2dTriangle(const Eigen::Vector2d &a, const Eigen::Vector2d &b, const Eigen::Vector2d &c)
{
    return areaOf3dTriangle(Eigen::Vector3d(a.x(), a.y(), 0),
        Eigen::Vector3d(b.x(), b.y(), 0),
        Eigen::Vector3d(c.x(), c.y(), 0));
}

void UvUnwrapper::calculateSizeAndRemoveInvalidCharts()
{
    auto charts = m_charts;
    auto chartSourcePartitions = m_chartSourcePartitions;
    m_charts.clear();
    chartSourcePartitions.clear();
    for (size_t chartIndex = 0; chartIndex < charts.size(); ++chartIndex) {
        auto &chart = charts[chartIndex];
        float left, top, right, bottom;
        left = top = right = bottom = 0;
        calculateFaceTextureBoundingBox(chart.second, left, top, right, bottom);
        std::pair<float, float> size = {right - left, bottom - top};
        if (size.first <= 0 || std::isnan(size.first) || std::isinf(size.first) ||
                size.second <= 0 || std::isnan(size.second) || std::isinf(size.second)) {
            //qDebug() << "Found invalid chart size:" << size.first << "x" << size.second;
            continue;
        }
        float surfaceArea = 0;
        for (const auto &item: chart.first) {
            const auto &face = m_mesh.faces[item];
            surfaceArea += areaOf3dTriangle(Eigen::Vector3d(m_mesh.vertices[face.indices[0]].xyz[0],
                    m_mesh.vertices[face.indices[0]].xyz[1],
                    m_mesh.vertices[face.indices[0]].xyz[2]),
                Eigen::Vector3d(m_mesh.vertices[face.indices[1]].xyz[0],
                    m_mesh.vertices[face.indices[1]].xyz[1],
                    m_mesh.vertices[face.indices[1]].xyz[2]),
                Eigen::Vector3d(m_mesh.vertices[face.indices[2]].xyz[0],
                    m_mesh.vertices[face.indices[2]].xyz[1],
                    m_mesh.vertices[face.indices[2]].xyz[2]));
        }
        float uvArea = 0;
        for (auto &item: chart.second) {
            for (int i = 0; i < 3; ++i) {
                item.coords[i].uv[0] -= left;
                item.coords[i].uv[1] -= top;
            }
            uvArea += areaOf2dTriangle(Eigen::Vector2d(item.coords[0].uv[0], item.coords[0].uv[1]),
                Eigen::Vector2d(item.coords[1].uv[0], item.coords[1].uv[1]),
                Eigen::Vector2d(item.coords[2].uv[0], item.coords[2].uv[1]));
        }
        if (m_enableRotation) {
            Eigen::Vector3d center(size.first * 0.5, size.second * 0.5, 0);
            float minRectArea = size.first * size.second;
            float minRectLeft = 0;
            float minRectTop = 0;
            bool rotated = false;
            for (const auto &degree: m_rotateDegrees) {
                Eigen::Matrix3d matrix;
                matrix = Eigen::AngleAxisd(degree * 180.0 / 3.1415926, Eigen::Vector3d::UnitZ());
                std::vector<FaceTextureCoords> rotatedUvs;
                for (auto &item: chart.second) {
                    FaceTextureCoords rotatedCoords;
                    for (int i = 0; i < 3; ++i) {
                        Eigen::Vector3d point(item.coords[i].uv[0], item.coords[i].uv[1], 0);
                        point -= center;
                        point = matrix * point;
                        rotatedCoords.coords[i].uv[0] = point.x();
                        rotatedCoords.coords[i].uv[1] = point.y();
                    }
                    rotatedUvs.push_back(rotatedCoords);
                }
                left = top = right = bottom = 0;
                calculateFaceTextureBoundingBox(rotatedUvs, left, top, right, bottom);
                std::pair<float, float> newSize = {right - left, bottom - top};
                float newRectArea = newSize.first * newSize.second;
                if (newRectArea < minRectArea) {
                    minRectArea = newRectArea;
                    size = newSize;
                    minRectLeft = left;
                    minRectTop = top;
                    rotated = true;
                    chart.second = rotatedUvs;
                }
            }
            if (rotated) {
                for (auto &item: chart.second) {
                    for (int i = 0; i < 3; ++i) {
                        item.coords[i].uv[0] -= minRectLeft;
                        item.coords[i].uv[1] -= minRectTop;
                    }
                }
            }
        }
        //qDebug() << "left:" << left << "top:" << top << "right:" << right << "bottom:" << bottom;
        //qDebug() << "width:" << size.first << "height:" << size.second;
        float ratioOfSurfaceAreaAndUvArea = uvArea > 0 ? surfaceArea / uvArea : 1.0;
        float scale = ratioOfSurfaceAreaAndUvArea * m_texelSizePerUnit;
        m_chartSizes.push_back(size);
        m_scaledChartSizes.push_back(std::make_pair(size.first * scale, size.second * scale));
        m_charts.push_back(chart);
        m_chartSourcePartitions.push_back(chartSourcePartitions[chartIndex]);
    }
}

void UvUnwrapper::packCharts()
{
    ChartPacker chartPacker;
    chartPacker.setCharts(m_scaledChartSizes);
    m_resultTextureSize = chartPacker.pack();
    m_chartRects.resize(m_chartSizes.size());
    const std::vector<std::tuple<float, float, float, float, bool>> &packedResult = chartPacker.getResult();
    for (decltype(m_charts.size()) i = 0; i < m_charts.size(); ++i) {
        const auto &chartSize = m_chartSizes[i];
        auto &chart = m_charts[i];
        if (i >= packedResult.size()) {
            for (auto &item: chart.second) {
                for (int i = 0; i < 3; ++i) {
                    item.coords[i].uv[0] = 0;
                    item.coords[i].uv[1] = 0;
                }
            }
            continue;
        }
        const auto &result = packedResult[i];
        auto &left = std::get<0>(result);
        auto &top = std::get<1>(result);
        auto &width = std::get<2>(result);
        auto &height = std::get<3>(result);
        auto &flipped = std::get<4>(result);
        if (flipped)
            m_chartRects[i] = {left, top, height, width};
        else
            m_chartRects[i] = {left, top, width, height};
        if (flipped) {
            for (auto &item: chart.second) {
                for (int i = 0; i < 3; ++i) {
                    std::swap(item.coords[i].uv[0], item.coords[i].uv[1]);
                }
            }
        }
        for (auto &item: chart.second) {
            for (int i = 0; i < 3; ++i) {
                item.coords[i].uv[0] /= chartSize.first;
                item.coords[i].uv[1] /= chartSize.second;
                item.coords[i].uv[0] *= width;
                item.coords[i].uv[1] *= height;
                item.coords[i].uv[0] += left;
                item.coords[i].uv[1] += top;
            }
        }
    }
}

void UvUnwrapper::finalizeUv()
{
    m_faceUvs.resize(m_mesh.faces.size());
    for (const auto &chart: m_charts) {
        for (decltype(chart.second.size()) i = 0; i < chart.second.size(); ++i) {
            auto &faceUv = m_faceUvs[chart.first[i]];
            faceUv = chart.second[i];
        }
    }
}

void UvUnwrapper::partition()
{
    m_partitions.clear();
    if (m_mesh.facePartitions.empty()) {
        for (decltype(m_mesh.faces.size()) i = 0; i < m_mesh.faces.size(); i++) {
            m_partitions[0].push_back(i);
        }
    } else {
        for (decltype(m_mesh.faces.size()) i = 0; i < m_mesh.faces.size(); i++) {
            int partition = m_mesh.facePartitions[i];
            m_partitions[partition].push_back(i);
        }
    }
}

void UvUnwrapper::unwrapSingleIsland(const std::vector<size_t> &group, int sourcePartition, bool skipCheckHoles)
{
    if (group.empty())
        return;

    std::vector<Vertex> localVertices;
    std::vector<Face> localFaces;
    std::map<size_t, size_t> globalToLocalVerticesMap;
    std::map<size_t, size_t> localToGlobalFacesMap;
    for (decltype(group.size()) i = 0; i < group.size(); ++i) {
        const auto &globalFace = m_mesh.faces[group[i]];
        Face localFace;
        for (size_t j = 0; j < 3; j++) {
            int globalVertexIndex = globalFace.indices[j];
            if (globalToLocalVerticesMap.find(globalVertexIndex) == globalToLocalVerticesMap.end()) {
                localVertices.push_back(m_mesh.vertices[globalVertexIndex]);
                globalToLocalVerticesMap[globalVertexIndex] = (int)localVertices.size() - 1;
            }
            localFace.indices[j] = globalToLocalVerticesMap[globalVertexIndex];
        }
        localFaces.push_back(localFace);
        localToGlobalFacesMap[localFaces.size() - 1] = group[i];
    }

    //if (skipCheckHoles) {
    //    parametrizeSingleGroup(localVertices, localFaces, localToGlobalFacesMap, localFaces.size());
    //    return;
    //}

    decltype(localFaces.size()) faceNumBeforeFix = localFaces.size();
    size_t remainingHoleNumAfterFix = 0;
    if (!fixHolesExceptTheLongestRing(localVertices, localFaces, &remainingHoleNumAfterFix)) {
        //qDebug() << "fixHolesExceptTheLongestRing failed";
        return;
    }
    if (1 == remainingHoleNumAfterFix) {
        parametrizeSingleGroup(localVertices, localFaces, localToGlobalFacesMap, faceNumBeforeFix, sourcePartition);
        return;
    }

    if (0 == remainingHoleNumAfterFix) {
        std::vector<size_t> firstGroup;
        std::vector<size_t> secondGroup;
        makeSeamAndCut(localVertices, localFaces, localToGlobalFacesMap, firstGroup, secondGroup);
        if (firstGroup.empty() || secondGroup.empty()) {
            //qDebug() << "Cut mesh failed";
            return;
        }
        unwrapSingleIsland(firstGroup, sourcePartition, true);
        unwrapSingleIsland(secondGroup, sourcePartition, true);
        return;
    }
}

void UvUnwrapper::parametrizeSingleGroup(const std::vector<Vertex> &verticies,
        const std::vector<Face> &faces,
        std::map<size_t, size_t> &localToGlobalFacesMap,
        size_t faceNumToChart,
        int sourcePartition)
{
    std::vector<TextureCoord> localVertexUvs;
    if (!parametrize(verticies, faces, localVertexUvs))
        return;
    std::pair<std::vector<size_t>, std::vector<FaceTextureCoords>> chart;
    for (size_t i = 0; i < faceNumToChart; ++i) {
        const auto &localFace = faces[i];
        auto globalFaceIndex = localToGlobalFacesMap[i];
        FaceTextureCoords faceUv;
        for (size_t j = 0; j < 3; j++) {
            const auto &localVertexIndex = localFace.indices[j];
            const auto &vertexUv = localVertexUvs[localVertexIndex];
            faceUv.coords[j].uv[0] = vertexUv.uv[0];
            faceUv.coords[j].uv[1] = vertexUv.uv[1];
        }
        chart.first.push_back(globalFaceIndex);
        chart.second.push_back(faceUv);
    }
    if (chart.first.empty())
        return;
    m_charts.push_back(chart);
    m_chartSourcePartitions.push_back(sourcePartition);
}

float UvUnwrapper::getTextureSize() const
{
    return m_resultTextureSize;
}

void UvUnwrapper::unwrap()
{
    partition();

    m_faceUvs.resize(m_mesh.faces.size());
    for (const auto &group: m_partitions) {
        std::vector<std::vector<size_t>> islands;
        splitPartitionToIslands(group.second, islands);
        for (const auto &island: islands)
            unwrapSingleIsland(island, group.first);
    }

    calculateSizeAndRemoveInvalidCharts();
    packCharts();
    finalizeUv();
}

}


#include <boost/test/unit_test.hpp>
#include "shared_operations.h"
#include "libs/cleanup_operatiosn/base_normalized_task.h"
#include "libs/sources_handling/source_data.h"
#include "libs/sources_handling/input_data.h"
#include "libs/cleanup_operatiosn/operetional_pipline.h"
#include "inputs/normalized_max.h"
#include "inputs/normalized_minmax.h"
#include "inputs/datainput.h"
#include "inputs/normalized_znorm.h"
#include <cmath>

namespace tt = boost::test_tools;

BOOST_AUTO_TEST_CASE( test_no_normalization_needed )
{
    // test that you don't do anything when you don't need to do it
    auto input_data = make_test_data(24u, "test input");
    auto ret = preform_action(normalized_nothing, input_data);
    BOOST_TEST(input_data == data(ret),
            "we do expect that we would not change anything in this case");
}

BOOST_AUTO_TEST_CASE( test_normalized_empty )
{
    normalized_data::arg_type input;
    z_normaliztion_conf znorm;
    auto ret = preform_action(znorm, input);
    const auto& data_after_norm = data(ret);
    BOOST_TEST(data_after_norm == input, "empty data set, no action");
    max_normalization mnorm;
    ret = preform_action(mnorm, input);
    const auto& data_after_norm2 = data(ret);
    BOOST_TEST(data_after_norm2 == input, "we expecting that we do nothing on empty set");
    max_normalization mxnorm;
    ret = preform_action(mxnorm, input);
    const auto& data_after_norm3 = data(ret);
    BOOST_TEST(data_after_norm3 == input, "we expecting that we do nothing on empty set");
    minmax_normalization mmnorm;
    ret = preform_action(mmnorm, input);
    const auto& data_after_norm4 = data(ret);
    BOOST_TEST(data_after_norm4 == input, "we expecting that we do nothing on empty set");
}

BOOST_AUTO_TEST_CASE( test_normalized_max )
{
    auto input_data = from_double_array(std::begin(inputdata), std::end(inputdata), "max_normalization");
    auto fixed_data = from_double_array(std::begin(max_normalizated), std::end(max_normalizated), "max normalization");
    max_normalization normalizer{1};
    auto after_fix = preform_action(normalizer, input_data);
    const auto& data_after_norm = data(after_fix);

    BOOST_TEST_REQUIRE(size(input_data) == size(fixed_data), "we expecting that the input data and the data that we fix are the same");
    BOOST_TEST_REQUIRE(size(input_data) == size(data_after_norm), "we are expecting that the data after fix did not changed");
    auto in_be = begin(input_data);
    auto in_end = end(input_data);
    auto exp_be = begin(fixed_data);
    auto exp_end = end(fixed_data);
    for (auto i = begin(data_after_norm); i != end(data_after_norm); ++i) {
        BOOST_TEST_REQUIRE(*i != *in_be, "the fixed data "<<*i<<" do equals "<<*in_be);
        BOOST_TEST_REQUIRE(in_be != in_end, "we should not finish yet!!");
        auto delta_exp = std::fabs(i->value - exp_be->value);
        BOOST_TEST_REQUIRE((delta_exp < 0.001), "the fixed data "<<*i<<" do not equals "<<*exp_be<<" ("<<delta_exp<<")");
        BOOST_TEST_REQUIRE(exp_be != exp_end, "we should not finish yet!!");
        ++in_be;
        ++exp_be;
    }
}

BOOST_AUTO_TEST_CASE( test_normalized_minmax )
{
    auto input_data = from_double_array(std::begin(inputdata), std::end(inputdata), "minmax_normalization");
    auto fixed_data = from_double_array(std::begin(minmax_normalized_data), std::end(minmax_normalized_data), "minmax normalization");
    minmax_normalization normalizer{1, -10.0, 10.0};
    auto after_fix = preform_action(normalizer, input_data);
    const auto& data_after_norm = data(after_fix);

    BOOST_TEST_REQUIRE(size(input_data) == size(fixed_data), "we expecting that the input data and the data that we fix are the same");
    BOOST_TEST_REQUIRE(size(input_data) == size(data_after_norm), "we are expecting that the data after fix did not changed");
    auto in_be = begin(input_data);
    auto in_end = end(input_data);
    auto exp_be = begin(fixed_data);
    auto exp_end = end(fixed_data);
    for (auto i = begin(data_after_norm); i != end(data_after_norm); ++i) {
        BOOST_TEST_REQUIRE(*i != *in_be, "the fixed data "<<*i<<" do equals "<<*in_be);
        BOOST_TEST_REQUIRE(in_be != in_end, "we should not finish yet!!");
        auto delta_exp = std::fabs(i->value - exp_be->value);
        BOOST_TEST_REQUIRE((delta_exp < 0.001), "the fixed data "<<*i<<" do not equals "<<*exp_be<<" ("<<delta_exp<<")");
        BOOST_TEST_REQUIRE(exp_be != exp_end, "we should not finish yet!!");
        ++in_be;
        ++exp_be;
    }
}

BOOST_AUTO_TEST_CASE( test_normalized_znorm )
{
    auto input_data = from_double_array(std::begin(inputdata), std::end(inputdata), "z_normalization");
    auto fixed_data = from_double_array(std::begin(znorm_values), std::end(znorm_values), "z normalization");
    z_normaliztion_conf normalizer{1};
    auto after_fix = preform_action(normalizer, input_data);
    const auto& data_after_norm = data(after_fix);

    BOOST_TEST_REQUIRE(size(input_data) == size(fixed_data), "we expecting that the input data and the data that we fix are the same");
    BOOST_TEST_REQUIRE(size(input_data) == size(data_after_norm), "we are expecting that the data after fix did not changed");
    auto in_be = begin(input_data);
    auto in_end = end(input_data);
    auto exp_be = begin(fixed_data);
    auto exp_end = end(fixed_data);
    for (auto i = begin(data_after_norm); i != end(data_after_norm); ++i) {
        BOOST_TEST_REQUIRE(*i != *in_be, "the fixed data "<<*i<<" do equals "<<*in_be);
        BOOST_TEST_REQUIRE(in_be != in_end, "we should not finish yet!!");
        auto delta_exp = std::fabs(i->value - exp_be->value);
        BOOST_TEST_REQUIRE((delta_exp < 0.001), "the fixed data "<<*i<<" do not equals "<<*exp_be<<" ("<<delta_exp<<")");
        BOOST_TEST_REQUIRE(exp_be != exp_end, "we should not finish yet!!");
        ++in_be;
        ++exp_be;
    }
}