/**
 * @file
 * This file is part of SeisSol.
 *
 * @author Sebastian Wolf (wolf.sebastian AT tum.de, https://www5.in.tum.de/wiki/index.php/Sebastian_Wolf,_M.Sc.)
 * @section LICENSE
 * Copyright (c) 2019 - 2020, SeisSol Group
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder 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.
 *
 * @section DESCRIPTION
 **/

#ifndef MODEL_ANISOTROPIC_DATASTRUCTURES_H_
#define MODEL_ANISOTROPIC_DATASTRUCTURES_H_

#include <Model/common_datastructures.hpp>
#include <Equations/elastic/Model/datastructures.hpp>
#include <Eigen/Eigen>
#include <Eigen/Eigenvalues>
#include <generated_code/init.h>
#include <generated_code/tensor.h>
#include <generated_code/kernel.h>

namespace seissol {
  namespace model {
    struct AnisotropicMaterial : Material {
      double c11;
      double c12;
      double c13;
      double c14;
      double c15;
      double c16;
      double c22;
      double c23;
      double c24;
      double c25;
      double c26;
      double c33;
      double c34;
      double c35;
      double c36;
      double c44;
      double c45;
      double c46;
      double c55;
      double c56;
      double c66;

      AnisotropicMaterial() {}

      explicit AnisotropicMaterial(ElasticMaterial m) {
        rho = m.rho;
        c11 = m.lambda + 2*m.mu;
        c12 = m.lambda;
        c13 = m.lambda;
        c14 = 0;
        c15 = 0;
        c16 = 0;
        c22 = m.lambda + 2*m.mu;
        c23 = m.lambda;
        c24 = 0;
        c25 = 0;
        c26 = 0;
        c33 = m.lambda + 2*m.mu;
        c34 = 0;
        c35 = 0;
        c36 = 0;
        c44 = m.mu;
        c45 = 0;
        c46 = 0;
        c55 = m.mu;
        c56 = 0;
        c66 = m.mu;
      }

      AnisotropicMaterial( double* materialValues, int numMaterialValues)
      {
        assert(numMaterialValues == 22);

        this->rho = materialValues[0];
        this->c11 = materialValues[1];
        this->c12 = materialValues[2];
        this->c13 = materialValues[3];
        this->c14 = materialValues[4];
        this->c15 = materialValues[5];
        this->c16 = materialValues[6];
        this->c22 = materialValues[7];
        this->c23 = materialValues[8];
        this->c24 = materialValues[9];
        this->c25 = materialValues[10];
        this->c26 = materialValues[11];
        this->c33 = materialValues[12];
        this->c34 = materialValues[13];
        this->c35 = materialValues[14];
        this->c36 = materialValues[15];
        this->c44 = materialValues[16];
        this->c45 = materialValues[17];
        this->c46 = materialValues[18];
        this->c55 = materialValues[19];
        this->c56 = materialValues[20];
        this->c66 = materialValues[21];
      }

      virtual ~AnisotropicMaterial() {};

      void getFullStiffnessTensor(std::array<real, 81>& fullTensor) const final {
        auto stiffnessTensorView = init::stiffnessTensor::view::create(fullTensor.data());
        stiffnessTensorView.setZero();
        stiffnessTensorView(0,0,0,0) = c11;
        stiffnessTensorView(0,0,0,1) = c16;
        stiffnessTensorView(0,0,0,2) = c15;
        stiffnessTensorView(0,0,1,0) = c16;
        stiffnessTensorView(0,0,1,1) = c12;
        stiffnessTensorView(0,0,1,2) = c14;
        stiffnessTensorView(0,0,2,0) = c15;
        stiffnessTensorView(0,0,2,1) = c14;
        stiffnessTensorView(0,0,2,2) = c13;
        stiffnessTensorView(0,1,0,0) = c16;
        stiffnessTensorView(0,1,0,1) = c66;
        stiffnessTensorView(0,1,0,2) = c56;
        stiffnessTensorView(0,1,1,0) = c66;
        stiffnessTensorView(0,1,1,1) = c26;
        stiffnessTensorView(0,1,1,2) = c46;
        stiffnessTensorView(0,1,2,0) = c56;
        stiffnessTensorView(0,1,2,1) = c46;
        stiffnessTensorView(0,1,2,2) = c36;
        stiffnessTensorView(0,2,0,0) = c15;
        stiffnessTensorView(0,2,0,1) = c56;
        stiffnessTensorView(0,2,0,2) = c55;
        stiffnessTensorView(0,2,1,0) = c56;
        stiffnessTensorView(0,2,1,1) = c25;
        stiffnessTensorView(0,2,1,2) = c45;
        stiffnessTensorView(0,2,2,0) = c55;
        stiffnessTensorView(0,2,2,1) = c45;
        stiffnessTensorView(0,2,2,2) = c35;
        stiffnessTensorView(1,0,0,0) = c16;
        stiffnessTensorView(1,0,0,1) = c66;
        stiffnessTensorView(1,0,0,2) = c56;
        stiffnessTensorView(1,0,1,0) = c66;
        stiffnessTensorView(1,0,1,1) = c26;
        stiffnessTensorView(1,0,1,2) = c46;
        stiffnessTensorView(1,0,2,0) = c56;
        stiffnessTensorView(1,0,2,1) = c46;
        stiffnessTensorView(1,0,2,2) = c36;
        stiffnessTensorView(1,1,0,0) = c12;
        stiffnessTensorView(1,1,0,1) = c26;
        stiffnessTensorView(1,1,0,2) = c25;
        stiffnessTensorView(1,1,1,0) = c26;
        stiffnessTensorView(1,1,1,1) = c22;
        stiffnessTensorView(1,1,1,2) = c24;
        stiffnessTensorView(1,1,2,0) = c25;
        stiffnessTensorView(1,1,2,1) = c24;
        stiffnessTensorView(1,1,2,2) = c23;
        stiffnessTensorView(1,2,0,0) = c14;
        stiffnessTensorView(1,2,0,1) = c46;
        stiffnessTensorView(1,2,0,2) = c45;
        stiffnessTensorView(1,2,1,0) = c46;
        stiffnessTensorView(1,2,1,1) = c24;
        stiffnessTensorView(1,2,1,2) = c44;
        stiffnessTensorView(1,2,2,0) = c45;
        stiffnessTensorView(1,2,2,1) = c44;
        stiffnessTensorView(1,2,2,2) = c34;
        stiffnessTensorView(2,0,0,0) = c15;
        stiffnessTensorView(2,0,0,1) = c56;
        stiffnessTensorView(2,0,0,2) = c55;
        stiffnessTensorView(2,0,1,0) = c56;
        stiffnessTensorView(2,0,1,1) = c25;
        stiffnessTensorView(2,0,1,2) = c45;
        stiffnessTensorView(2,0,2,0) = c55;
        stiffnessTensorView(2,0,2,1) = c45;
        stiffnessTensorView(2,0,2,2) = c35;
        stiffnessTensorView(2,1,0,0) = c14;
        stiffnessTensorView(2,1,0,1) = c46;
        stiffnessTensorView(2,1,0,2) = c45;
        stiffnessTensorView(2,1,1,0) = c46;
        stiffnessTensorView(2,1,1,1) = c24;
        stiffnessTensorView(2,1,1,2) = c44;
        stiffnessTensorView(2,1,2,0) = c45;
        stiffnessTensorView(2,1,2,1) = c44;
        stiffnessTensorView(2,1,2,2) = c34;
        stiffnessTensorView(2,2,0,0) = c13;
        stiffnessTensorView(2,2,0,1) = c36;
        stiffnessTensorView(2,2,0,2) = c35;
        stiffnessTensorView(2,2,1,0) = c36;
        stiffnessTensorView(2,2,1,1) = c23;
        stiffnessTensorView(2,2,1,2) = c34;
        stiffnessTensorView(2,2,2,0) = c35;
        stiffnessTensorView(2,2,2,1) = c34;
        stiffnessTensorView(2,2,2,2) = c33;
      }

      //calculate maximal wave speed
      //Wavespeeds for anisotropic materials depend on the direction of propagation.
      //An analytic solution for the maximal wave speed is hard to obtain.
      //Instead of solving an optimization problem we sample the velocitiy for
      //different directions and take the maximum.
      double getMaxWaveSpeed() const final{
        auto samplingDirections = init::samplingDirections::view::create(const_cast<real*>(init::samplingDirections::Values));

        Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 3, 3>> saes;

        double maxEv = 0;

        std::array<real, 81> fullTensor;
        getFullStiffnessTensor(fullTensor);
        kernel::computeChristoffel computeChristoffel;
        computeChristoffel.stiffnessTensor = fullTensor.data();

        for(unsigned j = 0; j < 200; ++j)
        {
          real n[3] = { samplingDirections(j, 0),
                        samplingDirections(j, 1),
                        samplingDirections(j, 2)
          };
          real M[9];
          computeChristoffel.direction = n;
          computeChristoffel.christoffel = M;
          computeChristoffel.execute();

          saes.compute(Eigen::Matrix<real, 3, 3>(M).cast<double>());
          auto eigenvalues = saes.eigenvalues();
          for(unsigned i = 0; i < 3; ++i) {
            maxEv = std::max(eigenvalues(i), maxEv);
          }
        }
        return sqrt(maxEv / rho);
      }

      //calculate P-wave speed based on averaged material parameters
      double getPWaveSpeed() const final {
        double muBar = (c44 + c55 + c66) / 3.0;
        double lambdaBar = (c11 + c22 + c33) / 3.0 - 2.0*muBar;
        return std::sqrt((lambdaBar + 2*muBar) / rho);
      }

      //calculate S-wave speed based on averaged material parameters
      double getSWaveSpeed() const final {
        double muBar = (c44 + c55 + c66) / 3.0;
        return std::sqrt(muBar / rho);
      }

      MaterialType getMaterialType() const {
        return MaterialType::anisotropic;
      }
    };

  }
}

#endif


/* Software License Agreement (BSD License)
 *
 * Copyright (c) 2014, Ross Linscott (rossklin@gmail.com)
 *
 * 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.
 *
 *     The names of its contributors may not 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.
 */

#if defined(NDEBUG)
#undef NDEBUG
#endif

#include <cstdlib>
#include <cmath>
#include <cstring>

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

#ifdef STANDALONE
#include <RInside.h>
#endif

#include "../inst/include/SimpleSDESampler.h"

NumericMatrix lpoly_sde(nlopt_stepper &stepper
			, NumericVector start
			, double from, double to, int steps
			, double x_tol
			, const char* algorithm) {

  const double dt = (to - from)/steps;
  vector<double> state = as<vector<double> >(start);
  NumericMatrix result(steps+1, start.size());

  for(int j = 0; j < start.size(); ++j){
    result(0, j) = state[j];
  }

  for(int i = 1; i <= steps; ++i) {
    stepper.do_step(state, (i-1)*dt);
    for(int j = 0; j < start.size(); ++j){
      result(i, j) = state[j];
    }
  }

  return result;
}


/*
 * Copyright (c) 2011-2019, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/master/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   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.
 *   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.
 */

#ifndef DART_MATH_RANDOM_HPP_
#define DART_MATH_RANDOM_HPP_

#include <random>

#include <Eigen/Core>

#ifdef DART_USE_ARBITRARY_PRECISION
#include "mpreal.h"
#endif

namespace dart {
namespace math {

class Random final
{
public:
  using GeneratorType = std::mt19937;

  template <typename FloatType>
  using UniformRealDist = std::uniform_real_distribution<FloatType>;

  template <typename IntType>
  using UniformIntDist = std::uniform_int_distribution<IntType>;

  template <typename FloatType>
  using NormalRealDist = std::normal_distribution<FloatType>;

  /// Returns a mutable reference to the random generator
  static GeneratorType& getGenerator();

  /// Sets the seed value.
  ///
  /// The same seed gives the same sequence of random values so that you can
  /// regenerate the same sequencial random values as long as you knot the seed
  /// value.
  static void setSeed(unsigned int seed);

  /// Generates a seed value using the default random device.
  ///
  /// \param[in] applyGeneratedSeed Whether to apply the generated seed.
  /// \return The new seed value.
  static unsigned int generateSeed(bool applyGeneratedSeed = false);

  /// \return The current seed value.
  static unsigned int getSeed();

  /// Returns a random number from an uniform distribution.
  ///
  ///
  /// This template function can generate different scalar types of random
  /// numbers as:
  /// - Floating-point number: \c float, \c s_t, \c long s_t
  /// - Integer number: [\c unsigned] \c short, [\c unsigned] \c int,
  ///   [\c unsigned] \c long, [\c unsigned] \c long \c long
  ///
  /// and vectors and matrices as:
  /// - Fixed-size: Eigen::Vector3i, Eigen::Vector3s, Eigen::Matrix4d, and so
  ///   on.
  /// - Dynamic-size: Eigen::VectorXi, Eigen::VectorXs, Eigen::MatrixXs, and so
  ///   on.
  ///
  /// Example:
  /// \code
  /// // Generate a random int in [0, 10]
  /// int intVal1 = Random::uniform(0, 10);
  /// int intVal2 = Random::uniform<int>(0, 10);
  ///
  /// // Generate a random s_t in [0.0, 10.0)
  /// s_t dblVal1 = Random::uniform(0.0, 10.0);
  /// s_t dblVal2 = Random::uniform<s_t>(0, 10);
  ///
  /// // Generate a random vector in [lb, ub)
  /// Eigen::Vector3s lb = Eigen::Vector3s::Constant(1);
  /// Eigen::Vector3s ub = Eigen::Vector3s::Constant(4);
  /// Eigen::Vector3s vecVal1 = Random::uniform(lb, ub);
  /// Eigen::Vector3s vecVal2 = Random::uniform<Eigen::Vector3s>(lb, ub);
  ///
  /// // Generate a random matrix in [lb, ub)
  /// Eigen::Matrix4f lb = Eigen::Matrix4f::Constant(1);
  /// Eigen::Matrix4f ub = Eigen::Matrix4f::Constant(4);
  /// Eigen::Matrix4f vecVal1 = Random::uniform(lb, ub);
  /// Eigen::Matrix4f vecVal2 = Random::uniform<Eigen::Matrix4f>(lb, ub);
  /// \endcode
  ///
  /// Note that the end of the range is closed for integer types (i.e.,
  /// [int_min, int_max]), but open for floating-point types (i.e., [float_min,
  /// float_max)).
  ///
  /// \tparam S The type of random value.
  /// \param[in] min Lower bound of the distribution.
  /// \param[in] max Upper bound of the distribution.
  ///
  /// \sa normal()
  template <typename S>
  static S uniform(S min, S max);

  /// Returns a random vector or matrix from an uniform distribution.
  ///
  /// This is a helper function for the case that the each of lower and upper
  /// bound has an uniform element value in it. For example, the lower bound is
  /// [1, 1, 1] or [-2, -2].
  ///
  /// This variant is meant to be used for fixed-size vector or matrix types.
  /// For dynamic-size types, please use other variants that takes the size of
  /// vector or matrix.
  ///
  /// Example:
  /// \code
  /// // Generate random vectors
  /// Eigen::VectorXi vecXi = Random::uniform<Eigen::VectorXi>(0, 10);
  /// Eigen::VectorXs vecXd = Random::uniform<Eigen::VectorXs>(0.0, 10.0);
  /// \endcode
  ///
  /// \tparam FixedSizeT The type of fixed-size vector or fixed-size matrix.
  /// \param[in] min The constant value of the lower bound.
  /// \param[in] max The constant value of the upper bound.
  ///
  /// \sa uniform()
  template <typename FixedSizeT>
  static FixedSizeT uniform(
      typename FixedSizeT::Scalar min, typename FixedSizeT::Scalar max);

  /// Returns a random vector from an uniform distribution.
  ///
  /// This variant is meant to be used for dynamic-size vector.
  ///
  /// Example:
  /// \code
  /// // Generate random matrices
  /// Eigen::MatrixXi matXi = Random::uniform<Eigen::MatrixXi>(0, 10);
  /// Eigen::MatrixXs matXd = Random::uniform<Eigen::MatrixXs>(0.0, 10.0);
  /// \endcode
  ///
  /// \tparam DynamicSizeVectorT The type of dynamic-size vector.
  /// \param[in] size The size of the vectors.
  /// \param[in] min The constant value of the lower bound vector.
  /// \param[in] max The constant value of the upper bound vector.
  template <typename DynamicSizeVectorT>
  static DynamicSizeVectorT uniform(
      int size,
      typename DynamicSizeVectorT::Scalar min,
      typename DynamicSizeVectorT::Scalar max);

  /// Returns a random matrix from an uniform distribution.
  ///
  /// This variant is meant to be used for dynamic-size matrix.
  ///
  /// \tparam DynamicSizeMatrixT The type of dynamic-size matrix.
  /// \param[in] rows The row size of the matrices.
  /// \param[in] cols The col size of the matrices.
  /// \param[in] min The constant value of the lower bound matrix.
  /// \param[in] max The constant value of the upper bound matrix.
  ///
  /// \sa uniform()
  template <typename DynamicSizeMatrixT>
  static DynamicSizeMatrixT uniform(
      int rows,
      int cols,
      typename DynamicSizeMatrixT::Scalar min,
      typename DynamicSizeMatrixT::Scalar max);

  /// Returns a random number from a normal distribution.
  ///
  /// This template function can generate different scalar types of random
  /// numbers as:
  /// - Floating-point number: \c float, \c s_t, \c long s_t
  /// - Integer number: [\c unsigned] \c short, [\c unsigned] \c int,
  ///   [\c unsigned] \c long, [\c unsigned] \c long \c long
  ///
  /// Example:
  /// \code
  /// // Generate a random int
  /// int intVal = Random::normal(0, 10);
  ///
  /// // Generate a random s_t
  /// s_t dblVal = Random::normal(0.0, 10.0);
  /// \endcode
  ///
  /// \param[in] mean Mean of the normal distribution.
  /// \param[in] sigma Standard deviation of the distribution.
  ///
  /// \sa uniform()
  template <typename S>
  static S normal(S mean, S sigma);

private:
  /// \return A mutable reference to the seed.
  static unsigned int& getSeedMutable();
};

} // namespace math
} // namespace dart

#include "dart/math/detail/Random-impl.hpp"

#endif // DART_MATH_RANDOM_HPP_


//==============================================================================
//
//   (c) Copyright, 2010 University Corporation for Atmospheric Research (UCAR).
//       All rights reserved.
//       Do not copy or distribute without authorization.
//
//       File: $RCSfile: lambertGridLatLong.cc,v $
//       Version: $Revision: 1.1 $  Dated: $Date: 2012-01-04 00:33:41 $
//
//==============================================================================

/**
 * @file lambertGridLatLong.cc
 *
 * Convert lat-longs cloud grid coordinates
 *
 * @date 3/25/10
 */

// Include files
#include <stdio.h>
#include <string>
#include <vector>
#include <projects.h>
#include <boost/format.hpp>
#include "LambertConfigReader.hh"
#include "Proj4Wrap.hh"
#include "writeBinary.hh"

using boost::format;
using std::string;
using std::vector;

// Functions

void usage(char *programName)
{
  fprintf(stderr, "Usage: %s configFile latFile lonFile\n", programName);
}

int main(int argc, char **argv)
{
  if (argc != 4)
    {
      usage(argv[0]);
      exit(2);
    }

  LambertConfigReader cfg(argv[1]);
  if (cfg.error != string(""))
    {
      printf("Error: configuration file error %s\n", cfg.error.c_str());
      return 1;
    }

  // Uses latitude where lambert conformal projection is true and
  // median aligned with cartesian y-axis
  string paramString = str(format("+proj=lcc +R=6371200 +lon_0=%1% +lat_0=%2% +lat_1=%3% +lat_2=%4%") % cfg.lov % cfg.latin1 % cfg.latin1 % cfg.latin2);

  p4w::Proj4Wrap lambertProj(paramString, p4w::Proj4Wrap::LON_LAT_TYPE, cfg.lo1, cfg.la1, cfg.dx, cfg.dy);
  double xc;
  double yc;
  double lon;
  double lat;

  vector<float> xvec;
  vector<float> yvec;

  for (int i=0; i<cfg.ny; i++)
    {
      for (int j=0; j<cfg.nx; j++)
	{
	  xc = j;
	  yc = i;
	  lambertProj.xy2ll(xc, yc, &lon, &lat);
	  xvec.push_back(lon);
	  yvec.push_back(lat);
	}
    }

  string error;
  writeBinary(yvec, argv[2], error);
  writeBinary(xvec, argv[3], error);
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2014 Daniele Panozzo <daniele.panozzo@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.

#include "nrosy.h"

#include <igl/copyleft/comiso/nrosy.h>
#include <igl/triangle_triangle_adjacency.h>
#include <igl/edge_topology.h>
#include <igl/per_face_normals.h>

#include <stdexcept>
#include "../../PI.h"

#include <Eigen/Geometry>
#include <Eigen/Sparse>
#include <queue>
#include <vector>

#include <gmm/gmm.h>
#include <CoMISo/Solver/ConstrainedSolver.hh>
#include <CoMISo/Solver/MISolver.hh>
#include <CoMISo/Solver/GMM_Tools.hh>

namespace igl
{
namespace copyleft
{

namespace comiso
{
class NRosyField
{
public:
  // Init
  IGL_INLINE NRosyField(const Eigen::MatrixXd& _V, const Eigen::MatrixXi& _F);

  // Generate the N-rosy field
  // N degree of the rosy field
  // round separately: round the integer variables one at a time, slower but higher quality
  IGL_INLINE void solve(int N = 4);

  // Set a hard constraint on fid
  // fid: face id
  // v: direction to fix (in 3d)
  IGL_INLINE void setConstraintHard(int fid, const Eigen::Vector3d& v);

  // Set a soft constraint on fid
  // fid: face id
  // w: weight of the soft constraint, clipped between 0 and 1
  // v: direction to fix (in 3d)
  IGL_INLINE void setConstraintSoft(int fid, double w, const Eigen::Vector3d& v);

  // Set the ratio between smoothness and soft constraints (0 -> smoothness only, 1 -> soft constr only)
  IGL_INLINE void setSoftAlpha(double alpha);

  // Reset constraints (at least one constraint must be present or solve will fail)
  IGL_INLINE void resetConstraints();

  // Return the current field
  IGL_INLINE Eigen::MatrixXd getFieldPerFace();

  // Compute singularity indexes
  IGL_INLINE void findCones(int N);

  // Return the singularities
  IGL_INLINE Eigen::VectorXd getSingularityIndexPerVertex();

private:
  // Compute angle differences between reference frames
  IGL_INLINE void computek();

  // Remove useless matchings
  IGL_INLINE void reduceSpace();

  // Prepare the system matrix
  IGL_INLINE void prepareSystemMatrix(int N);

  // Solve with roundings using CoMIso
  IGL_INLINE void solveRoundings();

  // Convert a vector in 3d to an angle wrt the local reference system
  IGL_INLINE double convert3DtoLocal(unsigned fid, const Eigen::Vector3d& v);

  // Convert an angle wrt the local reference system to a 3d vector
  IGL_INLINE Eigen::Vector3d convertLocalto3D(unsigned fid, double a);

  // Compute the per vertex angle defect
  IGL_INLINE Eigen::VectorXd angleDefect();

  // Temporary variable for the field
  Eigen::VectorXd angles;

  // Hard constraints
  Eigen::VectorXd hard;
  std::vector<bool> isHard;

  // Soft constraints
  Eigen::VectorXd soft;
  Eigen::VectorXd wSoft;
  double softAlpha;

  // Face Topology
  Eigen::MatrixXi TT, TTi;

  // Edge Topology
  Eigen::MatrixXi EV, FE, EF;
  std::vector<bool> isBorderEdge;

  // Per Edge information
  // Angle between two reference frames
  Eigen::VectorXd k;

  // Jumps
  Eigen::VectorXi p;
  std::vector<bool> pFixed;

  // Mesh
  Eigen::MatrixXd V;
  Eigen::MatrixXi F;

  // Normals per face
  Eigen::MatrixXd N;

  // Singularity index
  Eigen::VectorXd singularityIndex;

  // Reference frame per triangle
  std::vector<Eigen::MatrixXd> TPs;

  // System stuff
  Eigen::SparseMatrix<double> A;
  Eigen::VectorXd b;
  Eigen::VectorXi tag_t;
  Eigen::VectorXi tag_p;

};

} // NAMESPACE COMISO
} // NAMESPACE COPYLEFT
} // NAMESPACE IGL

igl::copyleft::comiso::NRosyField::NRosyField(const Eigen::MatrixXd& _V, const Eigen::MatrixXi& _F)
{
  V = _V;
  F = _F;

  assert(V.rows() > 0);
  assert(F.rows() > 0);

  // Generate topological relations
  igl::triangle_triangle_adjacency(F,TT,TTi);
  igl::edge_topology(V,F, EV, FE, EF);

  // Flag border edges
  isBorderEdge.resize(EV.rows());
  for(unsigned i=0; i<EV.rows(); ++i)
    isBorderEdge[i] = (EF(i,0) == -1) || ((EF(i,1) == -1));

  // Generate normals per face
  igl::per_face_normals(V, F, N);

  // Generate reference frames
  for(unsigned fid=0; fid<F.rows(); ++fid)
  {
    // First edge
    Eigen::Vector3d e1 = V.row(F(fid,1)) - V.row(F(fid,0));
    e1.normalize();
    Eigen::Vector3d e2 = N.row(fid);
    e2 = e2.cross(e1);
    e2.normalize();

    Eigen::MatrixXd TP(2,3);
    TP << e1.transpose(), e2.transpose();
    TPs.push_back(TP);
  }

  // Alloc internal variables
  angles = Eigen::VectorXd::Zero(F.rows());
  p = Eigen::VectorXi::Zero(EV.rows());
  pFixed.resize(EV.rows());
  k = Eigen::VectorXd::Zero(EV.rows());
  singularityIndex = Eigen::VectorXd::Zero(V.rows());

  // Reset the constraints
  resetConstraints();

  // Compute k, differences between reference frames
  computek();
  softAlpha = 0.5;
}

void igl::copyleft::comiso::NRosyField::setSoftAlpha(double alpha)
{
  assert(alpha >= 0 && alpha < 1);
  softAlpha = alpha;
}

void igl::copyleft::comiso::NRosyField::prepareSystemMatrix(const int N)
{
  double Nd = N;

  // Minimize the MIQ energy
  // Energy on edge ij is
  //     (t_i - t_j + kij + pij*(2*pi/N))^2
  // Partial derivatives:
  //   t_i: 2     ( t_i - t_j + kij + pij*(2*pi/N)) = 0
  //   t_j: 2     (-t_i + t_j - kij - pij*(2*pi/N)) = 0
  //   pij: 4pi/N ( t_i - t_j + kij + pij*(2*pi/N)) = 0
  //
  //          t_i      t_j         pij       kij
  // t_i [     2       -2           4pi/N      2    ]
  // t_j [    -2        2          -4pi/N     -2    ]
  // pij [   4pi/N   -4pi/N    2*(2pi/N)^2   4pi/N  ]

  // Count and tag the variables
  tag_t = Eigen::VectorXi::Constant(F.rows(),-1);
  std::vector<int> id_t;
  size_t count = 0;
  for(unsigned i=0; i<F.rows(); ++i)
    if (!isHard[i])
    {
      tag_t(i) = count++;
      id_t.push_back(i);
    }

  size_t count_t = id_t.size();

  tag_p = Eigen::VectorXi::Constant(EF.rows(),-1);
  std::vector<int> id_p;
  for(unsigned i=0; i<EF.rows(); ++i)
  {
    if (!pFixed[i])
    {
      // if it is not fixed then it is a variable
      tag_p(i) = count++;
    }

    // if it is not a border edge,
    if (!isBorderEdge[i])
    {
      // and it is not between two fixed faces
      if (!(isHard[EF(i,0)] && isHard[EF(i,1)]))
      {
          // then it participates in the energy!
          id_p.push_back(i);
      }
    }
  }

  size_t count_p = count - count_t;
  // System sizes: A (count_t + count_p) x (count_t + count_p)
  //               b (count_t + count_p)

  b.resize(count_t + count_p);
  b.setZero();

  std::vector<Eigen::Triplet<double> > T;
  T.reserve(3 * 4 * count_p);

  for(auto eid : id_p)
  {
    int i = EF(eid, 0);
    int j = EF(eid, 1);
    bool isFixed_i = isHard[i];
    bool isFixed_j = isHard[j];
    bool isFixed_p = pFixed[eid];
    int row;
    // (i)-th row: t_i [     2       -2           4pi/N      2    ]
    if (!isFixed_i)
    {
      row = tag_t[i];
      T.emplace_back(row, tag_t[i], 2);
      if (isFixed_j)
        b(row) +=  2 * hard[j];
      else
        T.emplace_back(row, tag_t[j], -2);
      if (isFixed_p)
        b(row) += -((4. * igl::PI) / Nd) * p[eid];
      else
        T.emplace_back(row, tag_p[eid], ((4. * igl::PI) / Nd));
      b(row) += -2 * k[eid];
      assert(hard[i] == hard[i]);
      assert(hard[j] == hard[j]);
      assert(p[eid] == p[eid]);
      assert(k[eid] == k[eid]);
      assert(b(row) == b(row));
    }
    // (j)+1 -th row: t_j [    -2        2          -4pi/N     -2    ]
    if (!isFixed_j)
    {
      row = tag_t[j];
      T.emplace_back(row, tag_t[j], 2);
      if (isFixed_i)
        b(row) += 2 * hard[i];
      else
        T.emplace_back(row, tag_t[i], -2);
      if (isFixed_p)
        b(row) += ((4. * igl::PI) / Nd) * p[eid];
      else
        T.emplace_back(row, tag_p[eid], -((4. * igl::PI) / Nd));
      b(row) += 2 * k[eid];
      assert(k[eid] == k[eid]);
      assert(b(row) == b(row));
    }
    // (r*3)+2 -th row: pij [   4pi/N   -4pi/N    2*(2pi/N)^2   4pi/N  ]
    if (!isFixed_p)
    {
      row = tag_p[eid];
      T.emplace_back(row, tag_p[eid], (2. * pow(((2. * igl::PI) / Nd), 2)));
      if (isFixed_i)
        b(row) += -(4. * igl::PI) / Nd * hard[i];
      else
        T.emplace_back(row, tag_t[i], (4. * igl::PI) / Nd);
      if (isFixed_j)
        b(row) += (4. * igl::PI) / Nd * hard[j];
      else
        T.emplace_back(row,tag_t[j], -(4. * igl::PI) / Nd);
      b(row) += - (4 * igl::PI)/Nd * k[eid];
      assert(k[eid] == k[eid]);
      assert(b(row) == b(row));
    }
  }

  A.resize(count_t + count_p, count_t + count_p);
  A.setFromTriplets(T.begin(), T.end());

  // Soft constraints
  bool addSoft = false;

  for(unsigned i=0; i<wSoft.size();++i)
    if (wSoft[i] != 0)
      addSoft = true;

  if (addSoft)
  {
    Eigen::VectorXd bSoft = Eigen::VectorXd::Zero(count_t + count_p);
    std::vector<Eigen::Triplet<double> > TSoft;
    TSoft.reserve(2 * count_p);

    for(unsigned i=0; i<F.rows(); ++i)
    {
      int varid = tag_t[i];
      if (varid != -1) // if it is a variable in the system
      {
        TSoft.emplace_back(varid, varid, wSoft[i]);
        bSoft[varid] += wSoft[i] * soft[i];
      }
    }
    Eigen::SparseMatrix<double> ASoft(count_t + count_p, count_t + count_p);
    ASoft.setFromTriplets(TSoft.begin(), TSoft.end());

    A = (1.0 - softAlpha) * A + softAlpha * ASoft;
    b = b * (1.0 - softAlpha) + bSoft * softAlpha;
  }
}

void igl::copyleft::comiso::NRosyField::solveRoundings()
{
  unsigned n = A.rows();

  gmm::col_matrix< gmm::wsvector< double > > gmm_A(n, n);
  std::vector<double> gmm_b(n);
  std::vector<int> ids_to_round;
  std::vector<double> x(n);

  // Copy A
  for (int k=0; k<A.outerSize(); ++k)
    for (Eigen::SparseMatrix<double>::InnerIterator it(A, k); it; ++it)
    {
      gmm_A(it.row(),it.col()) += it.value();
    }

  // Copy b
  for(unsigned int i = 0; i < n;++i)
    gmm_b[i] = b[i];

  // Set variables to round
  ids_to_round.clear();
  for(unsigned i=0; i<tag_p.size();++i)
    if(tag_p[i] != -1)
      ids_to_round.push_back(tag_p[i]);

  // Empty constraints
  gmm::row_matrix< gmm::wsvector< double > > gmm_C(0, n);

  COMISO::ConstrainedSolver cs;
  cs.solve(gmm_C, gmm_A, x, gmm_b, ids_to_round, 0.0, false, true);

  // Copy the result back
  for(unsigned i=0; i<F.rows(); ++i)
    if (tag_t[i] != -1)
      angles[i] = x[tag_t[i]];
    else
      angles[i] = hard[i];

  for(unsigned i=0; i<EF.rows(); ++i)
    if(tag_p[i]  != -1)
      p[i] = (int)std::round(x[tag_p[i]]);
}

void igl::copyleft::comiso::NRosyField::solve(const int N)
{
  // Reduce the search space by fixing matchings
  reduceSpace();

  // Build the system
  prepareSystemMatrix(N);

  // Solve with integer roundings
  solveRoundings();

  // Find the cones
  findCones(N);
}

void igl::copyleft::comiso::NRosyField::setConstraintHard(const int fid, const Eigen::Vector3d& v)
{
  isHard[fid] = true;
  hard(fid) = convert3DtoLocal(fid, v);
}

void igl::copyleft::comiso::NRosyField::setConstraintSoft(const int fid, const double w, const Eigen::Vector3d& v)
{
  wSoft(fid) = w;
  soft(fid) = convert3DtoLocal(fid, v);
}

void igl::copyleft::comiso::NRosyField::resetConstraints()
{
  isHard.resize(F.rows());
  for(unsigned i = 0; i < F.rows(); ++i)
    isHard[i] = false;
  hard   = Eigen::VectorXd::Zero(F.rows());
  wSoft = Eigen::VectorXd::Zero(F.rows());
  soft = Eigen::VectorXd::Zero(F.rows());
}

Eigen::MatrixXd igl::copyleft::comiso::NRosyField::getFieldPerFace()
{
  Eigen::MatrixXd result(F.rows(),3);
  for(unsigned int i = 0; i < F.rows(); ++i)
    result.row(i) = convertLocalto3D(i, angles(i));
  return result;
}

void igl::copyleft::comiso::NRosyField::computek()
{
  // For every non-border edge
  for (unsigned eid = 0; eid < EF.rows(); ++eid)
  {
    if (!isBorderEdge[eid])
    {
      int fid0 = EF(eid,0);
      int fid1 = EF(eid,1);

      Eigen::Vector3d N0 = N.row(fid0);
      Eigen::Vector3d N1 = N.row(fid1);

      // find common edge on triangle 0 and 1
      int fid0_vc = -1;
      int fid1_vc = -1;
      for (unsigned i=0;i<3;++i)
      {
        if (EV(eid,0) == F(fid0,i))
          fid0_vc = i;
        if (EV(eid,1) == F(fid1,i))
          fid1_vc = i;
      }
      assert(fid0_vc != -1);
      assert(fid1_vc != -1);

      Eigen::Vector3d common_edge = V.row(F(fid0,(fid0_vc+1)%3)) - V.row(F(fid0,fid0_vc));
      common_edge.normalize();

      // Map the two triangles in a new space where the common edge is the x axis and the N0 the z axis
      Eigen::MatrixXd P(3,3);
      Eigen::VectorXd o = V.row(F(fid0,fid0_vc));
      Eigen::VectorXd tmp = -N0.cross(common_edge);
      P << common_edge, tmp, N0;
      P.transposeInPlace();

      Eigen::MatrixXd V0(3,3);
      V0.row(0) = V.row(F(fid0,0)).transpose() -o;
      V0.row(1) = V.row(F(fid0,1)).transpose() -o;
      V0.row(2) = V.row(F(fid0,2)).transpose() -o;

      V0 = (P*V0.transpose()).transpose();

      assert(V0(0,2) < 10e-10);
      assert(V0(1,2) < 10e-10);
      assert(V0(2,2) < 10e-10);

      Eigen::MatrixXd V1(3,3);
      V1.row(0) = V.row(F(fid1,0)).transpose() -o;
      V1.row(1) = V.row(F(fid1,1)).transpose() -o;
      V1.row(2) = V.row(F(fid1,2)).transpose() -o;
      V1 = (P*V1.transpose()).transpose();

      assert(V1(fid1_vc,2) < 10e-10);
      assert(V1((fid1_vc+1)%3,2) < 10e-10);

      // compute rotation R such that R * N1 = N0
      // i.e. map both triangles to the same plane
      double alpha = -std::atan2(V1((fid1_vc + 2) % 3, 2), V1((fid1_vc + 2) % 3, 1));

      Eigen::MatrixXd R(3,3);
      R << 1,          0,            0,
           0, std::cos(alpha), -std::sin(alpha) ,
           0, std::sin(alpha),  std::cos(alpha);
      V1 = (R*V1.transpose()).transpose();

      assert(V1(0,2) < 10e-10);
      assert(V1(1,2) < 10e-10);
      assert(V1(2,2) < 10e-10);

      // measure the angle between the reference frames
      // k_ij is the angle between the triangle on the left and the one on the right
      Eigen::VectorXd ref0 = V0.row(1) - V0.row(0);
      Eigen::VectorXd ref1 = V1.row(1) - V1.row(0);

      ref0.normalize();
      ref1.normalize();

      double ktemp = std::atan2(ref1(1), ref1(0)) - std::atan2(ref0(1), ref0(0));

      // just to be sure, rotate ref0 using angle ktemp...
      Eigen::MatrixXd R2(2,2);
      R2 << std::cos(ktemp), -std::sin(ktemp), std::sin(ktemp), std::cos(ktemp);

      tmp = R2*ref0.head<2>();

      assert(tmp(0) - ref1(0) < 10^10);
      assert(tmp(1) - ref1(1) < 10^10);

      k[eid] = ktemp;
    }
  }

}

void igl::copyleft::comiso::NRosyField::reduceSpace()
{
  // All variables are free in the beginning
  for(unsigned int i = 0; i < EV.rows(); ++i)
    pFixed[i] = false;

  std::vector<bool> visited(EV.rows(), false);
  std::vector<bool> starting(EV.rows(), false);

  std::queue<int> q;
  for(unsigned int i = 0; i < F.rows(); ++i)
    if (isHard[i] || wSoft[i] != 0)
    {
      q.push(i);
      starting[i] = true;
    }

  // Reduce the search space (see MI paper)
  while (!q.empty())
  {
    int c = q.front();
    q.pop();

    visited[c] = true;
    for(int i=0; i<3; ++i)
    {
      int eid = FE(c,i);
      int fid = TT(c,i);

      // skip borders
      if (fid != -1)
      {
        assert((EF(eid,0) == c && EF(eid,1) == fid) || (EF(eid,1) == c && EF(eid,0) == fid));
        // for every neighbouring face
        if (!visited[fid] && !starting[fid])
        {
          pFixed[eid] = true;
          p[eid] = 0;
          visited[fid] = true;
          q.push(fid);
        }
      }
      else
      {
        // fix borders
        pFixed[eid] = true;
        p[eid] = 0;
      }
    }
  }

  // Force matchings between fixed faces
  for(unsigned int i = 0; i < F.rows();++i)
  {
    if (isHard[i])
    {
      for(unsigned int j = 0; j < 3; ++j)
      {
        int fid = TT(i,j);
        if ((fid!=-1) && (isHard[fid]))
        {
          // i and fid are adjacent and fixed
          int eid = FE(i,j);
          int fid0 = EF(eid,0);
          int fid1 = EF(eid,1);

          pFixed[eid] = true;
          p[eid] = (int)std::round(2.0 / igl::PI * (hard(fid1) - hard(fid0) - k(eid)));
        }
      }
    }
  }
}

double igl::copyleft::comiso::NRosyField::convert3DtoLocal(unsigned fid, const Eigen::Vector3d& v)
{
  // Project onto the tangent plane
  Eigen::Vector2d vp = TPs[fid] * v;

  // Convert to angle
  return std::atan2(vp(1), vp(0));
}

Eigen::Vector3d igl::copyleft::comiso::NRosyField::convertLocalto3D(unsigned fid, double a)
{
  Eigen::Vector2d vp(std::cos(a), std::sin(a));
  return vp.transpose() * TPs[fid];
}

Eigen::VectorXd igl::copyleft::comiso::NRosyField::angleDefect()
{
  Eigen::VectorXd A = Eigen::VectorXd::Constant(V.rows(),-2*igl::PI);

  for (unsigned int i = 0; i < F.rows(); ++i)
  {
    for (int j = 0; j < 3; ++j)
    {
      Eigen::VectorXd a = V.row(F(i,(j+1)%3)) - V.row(F(i,j));
      Eigen::VectorXd b = V.row(F(i,(j+2)%3)) - V.row(F(i,j));
      double t = a.transpose() * b;
      if(a.norm() > 0. && b.norm() > 0.)
        t /= (a.norm() * b.norm());
      else
        throw std::runtime_error("igl::copyleft::comiso::NRosyField::angleDefect: Division by zero!");
      A(F(i, j)) += std::acos(std::max(std::min(t, 1.), -1.));
    }
  }

  return A;
}

void igl::copyleft::comiso::NRosyField::findCones(int N)
{
  // Compute I0, see http://www.graphics.rwth-aachen.de/media/papers/bommes_zimmer_2009_siggraph_011.pdf for details

  singularityIndex = Eigen::VectorXd::Zero(V.rows());

  // first the k
  for (unsigned i = 0; i < EV.rows(); ++i)
  {
    if (!isBorderEdge[i])
    {
      singularityIndex(EV(i, 0)) -= k(i);
      singularityIndex(EV(i, 1)) += k(i);
    }
  }

  // then the A
  Eigen::VectorXd A = angleDefect();
  singularityIndex += A;
  // normalize
  singularityIndex /= (2 * igl::PI);

  // round to integer (remove numerical noise)
  for (unsigned i = 0; i < singularityIndex.size(); ++i)
    singularityIndex(i) = round(singularityIndex(i));

  for (unsigned i = 0; i < EV.rows(); ++i)
  {
    if (!isBorderEdge[i])
    {
      singularityIndex(EV(i, 0)) -= double(p(i)) / double(N);
      singularityIndex(EV(i, 1)) += double(p(i)) / double(N);
    }
  }

  // Clear the vertices on the edges
  for (unsigned i = 0; i < EV.rows(); ++i)
  {
    if (isBorderEdge[i])
    {
      singularityIndex(EV(i,0)) = 0;
      singularityIndex(EV(i,1)) = 0;
    }
  }
}

Eigen::VectorXd igl::copyleft::comiso::NRosyField::getSingularityIndexPerVertex()
{
  return singularityIndex;
}

IGL_INLINE void igl::copyleft::comiso::nrosy(
  const Eigen::MatrixXd& V,
  const Eigen::MatrixXi& F,
  const Eigen::VectorXi& b,
  const Eigen::MatrixXd& bc,
  const Eigen::VectorXi& b_soft,
  const Eigen::VectorXd& w_soft,
  const Eigen::MatrixXd& bc_soft,
  const int N,
  const double soft,
  Eigen::MatrixXd& R,
  Eigen::VectorXd& S
  )
{
  // Init solver
  igl::copyleft::comiso::NRosyField solver(V, F);

  // Add hard constraints
  for (unsigned i = 0; i < b.size(); ++i)
    solver.setConstraintHard(b(i), bc.row(i));

  // Add soft constraints
  for (unsigned i = 0; i < b_soft.size(); ++i)
    solver.setConstraintSoft(b_soft(i), w_soft(i), bc_soft.row(i));

  // Set the soft constraints global weight
  solver.setSoftAlpha(soft);

  // Interpolate
  solver.solve(N);

  // Copy the result back
  R = solver.getFieldPerFace();

  // Extract singularity indices
  S = solver.getSingularityIndexPerVertex();
}

IGL_INLINE void igl::copyleft::comiso::nrosy(
                           const Eigen::MatrixXd& V,
                           const Eigen::MatrixXi& F,
                           const Eigen::VectorXi& b,
                           const Eigen::MatrixXd& bc,
                           const int N,
                           Eigen::MatrixXd& R,
                           Eigen::VectorXd& S
                           )
{
  // Init solver
  igl::copyleft::comiso::NRosyField solver(V, F);

  // Add hard constraints
  for (unsigned i= 0; i < b.size(); ++i)
    solver.setConstraintHard(b(i), bc.row(i));

  // Interpolate
  solver.solve(N);

  // Copy the result back
  R = solver.getFieldPerFace();

  // Extract singularity indices
  S = solver.getSingularityIndexPerVertex();
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TRIGONOMETRIC_CONSTANTS_PIO2_2T_HPP_INCLUDED
#define NT2_TRIGONOMETRIC_CONSTANTS_PIO2_2T_HPP_INCLUDED

#include <nt2/include/functor.hpp>
#include <boost/simd/constant/hierarchy.hpp>
#include <boost/simd/constant/register.hpp>
namespace nt2
{
  namespace tag
  {
   /*!
     @brief Pio2_2t generic tag

     Represents the Pio2_2t constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    // 2.02226624879595063154e-21
    BOOST_SIMD_CONSTANT_REGISTER( Pio2_2t, double
                                , 0, 0x2e85a308
                                , 0x3BA3198A2E037073ll
                                )
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::Pio2_2t, Site> dispatching_Pio2_2t(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::Pio2_2t, Site>();
   }
   template<class... Args>
   struct impl_Pio2_2t;
  }
  /*!
    Constant used in modular computation involving \f$\pi\f$

    @par Semantic:

    For type T0:

    @code
    T0 r = Pio2_2t<T0>();
    @endcode

    @return a value of type T0
  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Pio2_2t, Pio2_2t);
}

#endif


#include "Astronomy.h"
#include "AstronomyHelperFunctions.h"
#include "Exception.h"
#include <boost/date_time/c_local_time_adjustor.hpp>
#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/local_time_adjustor.hpp>
#include <boost/date_time/posix_time/posix_time_types.hpp>
#include <cmath>
#include <limits>
#include <vector>

/*=== Public interface =====================*/

namespace Fmi
{
namespace Astronomy
{
namespace
{
std::vector<double> quad(double ym, double yz, double yp)
{
  try
  {
    double nz(0.0);
    double z1(0.0);
    double z2(0.0);
    double a = 0.5 * (ym + yp) - yz;
    double b = 0.5 * (yp - ym);
    double c = yz;
    double xe = -b / (2 * a);
    double ye = (a * xe + b) * xe + c;
    double dis = b * b - 4 * a * c;
    if (dis > 0)
    {
      double dx = 0.5 * sqrt(dis) / fabs(a);
      z1 = xe - dx;
      z2 = xe + dx;
      nz = fabs(z1) < 1 ? nz + 1 : nz;
      nz = fabs(z2) < 1 ? nz + 1 : nz;
      z1 = z1 < -1 ? z2 : z1;
    }

    std::vector<double> ret;
    ret.push_back(nz);
    ret.push_back(z1);
    ret.push_back(z2);
    ret.push_back(xe);
    ret.push_back(ye);

    return ret;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

/**
 *      returns an angle in degrees in the range 0 to 360
 */
double degRange(double x)
{
  try
  {
    double b = x / 360;
    double a = 360 * (b - static_cast<int>(b));
    double retVal = (a < 0 ? a + 360 : a);
    return retVal;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

double lmst(double mjd, double glon)
{
  try
  {
    double d = mjd - 51544.5;
    double t = d / 36525;
    double lst =
        degRange(280.46061839 + 360.98564736629 * d + 0.000387933 * t * t - t * t * t / 38710000);
    return lst / 15 + glon / 15;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

/**
 *      returns the self::fractional part of x
 */
double frac(double x)
{
  try
  {
    x -= static_cast<int>(x);
    return x < 0 ? x + 1 : x;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

/**
 * takes t and returns the geocentric ra and dec in an array mooneq
 * claimed good to 5' (angle) in ra and 1' in dec
 * tallies with another approximate method and with ICE for a couple of dates
 */
std::vector<double> minimoon(double t)
{
  try
  {
    double p2 = 6.283185307;
    double arc = 206264.8062;
    double coseps = 0.91748;
    double sineps = 0.39778;

    double lo = frac(0.606433 + 1336.855225 * t);
    double l = p2 * frac(0.374897 + 1325.552410 * t);
    double l2 = l * 2;
    double ls = p2 * frac(0.993133 + 99.997361 * t);
    double d = p2 * frac(0.827361 + 1236.853086 * t);
    double d2 = d * 2;
    double f = p2 * frac(0.259086 + 1342.227825 * t);
    double f2 = f * 2;

    double sinls = sin(ls);
    double sinf2 = sin(f2);

    double dl = 22640 * sin(l);
    dl += -4586 * sin(l - d2);
    dl += 2370 * sin(d2);
    dl += 769 * sin(l2);
    dl += -668 * sinls;
    dl += -412 * sinf2;
    dl += -212 * sin(l2 - d2);
    dl += -206 * sin(l + ls - d2);
    dl += 192 * sin(l + d2);
    dl += -165 * sin(ls - d2);
    dl += -125 * sin(d);
    dl += -110 * sin(l + ls);
    dl += 148 * sin(l - ls);
    dl += -55 * sin(f2 - d2);

    double s = f + (dl + 412 * sinf2 + 541 * sinls) / arc;
    double h = f - d2;
    double n = -526 * sin(h);
    n += 44 * sin(l + h);
    n += -31 * sin(-l + h);
    n += -23 * sin(ls + h);
    n += 11 * sin(-ls + h);
    n += -25 * sin(-l2 + f);
    n += 21 * sin(-l + f);

    double L_moon = p2 * frac(lo + dl / 1296000);
    double B_moon = (18520.0 * sin(s) + n) / arc;

    double cb = cos(B_moon);
    double x = cb * cos(L_moon);
    double v = cb * sin(L_moon);
    double w = sin(B_moon);
    double y = coseps * v - sineps * w;
    double z = sineps * v + coseps * w;
    double rho = sqrt(1 - z * z);
    double dec = (360 / p2) * atan(z / rho);
    double ra = (48 / p2) * atan(y / (x + rho));
    ra = ra < 0 ? ra + 24 : ra;

    std::vector<double> retval;
    retval.push_back(dec);
    retval.push_back(ra);

    return retval;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

double sinAlt(double mjd, double hour, double glon, double cglat, double sglat)
{
  try
  {
    mjd += (hour / 24.0);
    double t = (mjd - 51544.5) / 36525;
    std::vector<double> objpos = minimoon(t);

    double ra = objpos[1];
    double dec = objpos[0];
    double decRad = deg2rad(dec);
    double tau = 15 * (lmst(mjd, glon) - ra);

    return sglat * sin(decRad) + cglat * cos(decRad) * cos(deg2rad(tau));
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

}  // namespace

const boost::local_time::local_date_time& lunar_time_t::risesettime(SetAndRiseOccurence occ) const
{
  try
  {
    if (occ == FIRST_RISE)
      return moonrise;
    if (occ == SECOND_RISE)
      return moonrise2;
    if (occ == FIRST_SET)
      return moonset;

    return moonset2;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

std::string lunar_time_t::as_string(SetAndRiseOccurence occ) const
{
  try
  {
    std::stringstream ss;

    bool rise_occurence(occ == FIRST_RISE || occ == SECOND_RISE);

    const boost::local_time::local_date_time& occ_ldt =
        (rise_occurence ? (occ == FIRST_RISE ? moonrise : moonrise2)
                        : (occ == FIRST_SET ? moonset : moonset2));

    if (!occ_ldt.is_not_a_date_time())
    {
      ss << std::setfill('0') << std::setw(2) << occ_ldt.local_time().time_of_day().hours()
         << std::setfill('0') << std::setw(2) << occ_ldt.local_time().time_of_day().minutes();
    }

    return ss.str();
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

std::string lunar_time_t::as_string_long(SetAndRiseOccurence occ) const
{
  try
  {
    std::stringstream ss;

    bool rise_occurence(occ == FIRST_RISE || occ == SECOND_RISE);

    const boost::local_time::local_date_time& occ_ldt =
        (rise_occurence ? (occ == FIRST_RISE ? moonrise : moonrise2)
                        : (occ == FIRST_SET ? moonset : moonset2));

    std::cout << occ_ldt;

    if (occ_ldt.is_not_a_date_time())
      ss << occ_ldt;
    else
      ss << occ_ldt.date() << " " << as_string(occ);

    return ss.str();
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

std::ostream& operator<<(std::ostream& ostream, const lunar_time_t& lt)
{
  try
  {
    std::string risestr(lt.as_string(FIRST_RISE));
    std::string setstr(lt.as_string(FIRST_SET));

    if (risestr.empty())
    {
      if (!lt.moonset_today())
      {
        if (lt.above_hz_24h)
          risestr = "****";
        else
          risestr = "----";
      }
      else
      {
        risestr = "    ";
      }
    }
    if (setstr.empty())
    {
      if (!lt.moonrise_today())
      {
        if (lt.above_hz_24h)
          setstr = "****";
        else
          setstr = "----";
      }
      else
      {
        setstr = "    ";
      }
    }

    ostream << risestr << " " << setstr;

    return ostream;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

bool dst_on(const boost::posix_time::ptime& theTime, const boost::local_time::local_date_time& ldt)
{
  try
  {
    auto zone = ldt.zone();

    bool dst_on(false);
    if (zone->has_dst())
    {
      auto dst_starttime = zone->dst_local_start_time(ldt.local_time().date().year());
      auto dst_endtime = zone->dst_local_end_time(ldt.local_time().date().year());

      if (dst_starttime < dst_endtime)
      {
        if (theTime >= dst_starttime && theTime <= dst_endtime)
          dst_on = true;
      }
      else
      {
        if (theTime >= dst_starttime || theTime <= dst_endtime)
          dst_on = true;
      }
    }
    return dst_on;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

double timezone_offset(const boost::local_time::local_date_time& ldt)
{
  try
  {
    auto zone = ldt.zone();

    double base_offset(zone->base_utc_offset().hours() +
                       (static_cast<float>(zone->base_utc_offset().minutes()) / 60.0));
    double dst_offset(dst_on(ldt.local_time(), ldt)
                          ? (zone->dst_offset().hours() + zone->dst_offset().minutes())
                          : 0.0);

    return (base_offset + dst_offset);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

void get_hours_and_minutes(double hours, int& hr, int& min)
{
  try
  {
    double hrs(hours);

    // if time is greater than 23:59:29 use value 23:59, so that we do not move to 00:00 (backwards)
    if (hrs > 23.991 && hrs < 24.0)
      hrs = 23.991;

    hr = static_cast<int>(floor(hrs));
    min = static_cast<int>(round(60.0 * (hrs - hr)));
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

boost::local_time::local_date_time parse_local_date_time(
    const boost::local_time::local_date_time& ldt, double hours)
{
  try
  {
    double hrs(hours);
    int hour(0);
    int min(0);

    get_hours_and_minutes(hrs, hour, min);
    // utc time of the beginning of the day
    auto utc_ptime = ldt.utc_time();
    // add hour offset
    utc_ptime += boost::posix_time::time_duration(hour, min, 0, 0);

    // return local time
    boost::local_time::local_date_time ldt_riseset(utc_ptime, ldt.zone());

    return ldt_riseset;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

/*
 * Calculate moonrise, moonset for given location
 */
lunar_time_t lunar_time_calculation(const boost::local_time::local_date_time& ldt,
                                    double offset,
                                    double lon,
                                    double lat)
{
  try
  {
    double utrise(0.0);
    double utset(0.0);
    double utrise2(0.0);
    double utset2(0.0);

    // beginning of the day
    boost::local_time::local_date_time ldt_beg(
        ldt.local_time().date(),
        boost::posix_time::time_duration(0, 0, 0, 0),
        ldt.zone(),
        boost::local_time::local_date_time::NOT_DATE_TIME_ON_ERROR);

    double date = ldt_beg.local_time().date().modjulian_day();

    date -= (static_cast<float>(offset) / 24.0);

    double latRad = deg2rad(lat);
    double sinho = 0.0023271056;
    double sglat = sin(latRad);
    double cglat = cos(latRad);

    bool rise = false;
    bool set = false;
    bool rise2 = false;
    bool set2 = false;
    bool above = false;
    double hour = 1.0;

    double ym = sinAlt(date, hour - 1.0, lon, cglat, sglat) - sinho;

    above = (ym > 0);
    while (hour < 25)
    {
      double yz = sinAlt(date, hour, lon, cglat, sglat) - sinho;
      double yp = sinAlt(date, hour + 1.0, lon, cglat, sglat) - sinho;

      std::vector<double> quadout = quad(ym, yz, yp);
      double nz = quadout[0];
      double z1 = quadout[1];
      double z2 = quadout[2];
      // double xe = quadout[3];
      double ye = quadout[4];

      if (nz == 1)
      {
        if (ym < 0)
        {
          if (!set || !rise)
          {
            utrise = hour + z1;
            rise = true;
          }
          else
          {
            utrise2 = hour + z1;
            rise2 = true;
          }
        }
        else
        {
          if (!set || !rise)
          {
            utset = hour + z1;
            set = true;
          }
          else
          {
            utset2 = hour + z1;
            set2 = true;
          }
        }
      }

      if (nz == 2)
      {
        if (ye < 0)
        {
          if (!set || !rise)
          {
            utrise = hour + z2;
            utset = hour + z1;
          }
          else
          {
            utrise2 = hour + z2;
            utset2 = hour + z1;
          }
        }
        else
        {
          if (!set || !rise)
          {
            utrise = hour + z1;
            utset = hour + z2;
          }
          else
          {
            utrise2 = hour + z1;
            utset2 = hour + z2;
          }
        }
      }
#ifdef MYDEBUG
      std::cout << "nz: " << nz << endl;
      std::cout << "z1: " << z1 << endl;
      std::cout << "z2: " << z2 << endl;
      // std::cout << "xe: " << xe << endl;
      std::cout << "ye: " << ye << endl;
      std::cout << "utrise: " << utrise << endl;
      std::cout << "utset: " << utset << endl;
      std::cout << "utrise2: " << utrise2 << endl;
      std::cout << "utset2: " << utset2 << endl;
      std::cout << "rise: " << rise << endl;
      std::cout << "rise2: " << rise2 << endl;
      std::cout << "set: " << set << endl;
      std::cout << "set2: " << set2 << endl << endl;
#endif

      ym = yp;
      hour += 2.0;
    }

#ifdef MYDEBUG
    cout << "\noffset: " << offset << endl;
    cout << "rise: " << rise << endl;
    cout << "rise2: " << rise2 << endl;
    cout << "utrise: " << utrise << endl;
    cout << "utrise2: " << utrise2 << endl;
    cout << "set: " << set << endl;
    cout << "set2: " << set2 << endl;
    cout << "utset: " << utset << endl;
    cout << "utset2: " << utset2 << endl;
    cout << "julian: " << date << endl;
    cout << "ldt: " << ldt << endl;
    cout << "ldt.local_time(): " << ldt.local_time() << endl;
    cout << "ldt.utc_time(): " << ldt.utc_time() << endl;
    cout << "ldt_beg: " << ldt_beg << endl;
#endif

    lunar_time_t retval(
        (rise ? parse_local_date_time(ldt_beg, utrise)
              : boost::local_time::local_date_time(boost::posix_time::not_a_date_time)),
        (set ? parse_local_date_time(ldt_beg, utset)
             : boost::local_time::local_date_time(boost::posix_time::not_a_date_time)),
        (rise2 ? parse_local_date_time(ldt_beg, utrise2)
               : boost::local_time::local_date_time(boost::posix_time::not_a_date_time)),
        (set2 ? parse_local_date_time(ldt_beg, utset2)
              : boost::local_time::local_date_time(boost::posix_time::not_a_date_time)),
        rise,
        set,
        rise2,
        set2,
        (!rise && !set && above));

    return retval;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

lunar_time_t lunar_time_i(const boost::local_time::local_date_time& ldt, double lon, double lat)
{
  try
  {
    // beginning of the day
    boost::local_time::local_date_time ldt_beg(
        ldt.local_time().date(),
        boost::posix_time::time_duration(0, 0, 0, 0),
        ldt.zone(),
        boost::local_time::local_date_time::NOT_DATE_TIME_ON_ERROR);
    boost::local_time::local_date_time ldt_end(
        ldt.local_time().date(),
        boost::posix_time::time_duration(23, 59, 59, 0),
        ldt.zone(),
        boost::local_time::local_date_time::NOT_DATE_TIME_ON_ERROR);

    boost::posix_time::ptime dst_endtime(
        ldt.zone()->dst_local_end_time(ldt.local_time().date().year()));
    bool dst_ends_today(ldt_beg.local_time().date() == dst_endtime.date());

    double offset_before_dst_ends = timezone_offset(ldt_beg);

    lunar_time_t retval;

    // if dst ends today we have to handle the extended time
    if (dst_ends_today)
    {
      double offset_after_dst_ends = timezone_offset(ldt_end);

      lunar_time_t lt_before = lunar_time_calculation(ldt_beg, offset_before_dst_ends, lon, lat);

      // if rise and set is found return
      if (lt_before.moonrise_today() && lt_before.moonset_today())
        return lt_before;

      // do calculation using wintertime offset
      lunar_time_t lt_after = lunar_time_calculation(ldt_beg, offset_after_dst_ends, lon, lat);
      if (lt_after.moonrise_today())
        lt_after.moonrise += boost::posix_time::hours(1);
      if (lt_after.moonset_today())
        lt_after.moonset += boost::posix_time::hours(1);
      if (lt_after.moonrise2_today())
        lt_after.moonrise2 += boost::posix_time::hours(1);
      if (lt_after.moonset2_today())
        lt_after.moonset2 += boost::posix_time::hours(1);

      lunar_time_t lt_combined(
          lt_before.moonrise_today() ? lt_before.moonrise : lt_after.moonrise,
          lt_before.moonset_today() ? lt_before.moonset : lt_after.moonset,
          lt_before.moonrise2_today() ? lt_before.moonrise2 : lt_after.moonrise2,
          lt_before.moonset2_today() ? lt_before.moonset2 : lt_after.moonset2,
          lt_before.rise_today,
          lt_before.set_today,
          lt_before.rise2_today,
          lt_before.set2_today,
          lt_before.above_hz_24h && lt_after.above_hz_24h);

      retval = lt_combined;
    }
    else
    {
      retval = lunar_time_calculation(ldt_beg, offset_before_dst_ends, lon, lat);
    }
    return retval;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

lunar_time_t lunar_time(const boost::local_time::local_date_time& ldt,
                        double lon,
                        double lat,
                        bool allow_missing_dates /*= false*/)
{
  try
  {
    lunar_time_t lt_ret = lunar_time_i(ldt, lon, lat);

    if (allow_missing_dates)
      return lt_ret;

    unsigned int iteration_limit(366);
    if (!lt_ret.moonrise_today())
    {
      unsigned int counter(0);  // just in case something goes wrong
      boost::local_time::local_date_time ldt_iter(ldt);
      if (lt_ret.above_hz_24h || lt_ret.moonset_today())
      {
        // find the previous moonrise
        lunar_time_t lt_prev;
        while (!lt_prev.moonrise_today() && counter < iteration_limit)
        {
          ldt_iter -= boost::posix_time::hours(24);
          lt_prev = lunar_time_i(ldt_iter, lon, lat);
          counter++;
        }
        if (counter < iteration_limit)
          lt_ret.moonrise = lt_prev.moonrise;
      }
      else
      {
        // find the next moonrise
        lunar_time_t lt_next;
        while (!lt_next.moonrise_today() && counter < iteration_limit)
        {
          ldt_iter += boost::posix_time::hours(24);
          lt_next = lunar_time_i(ldt_iter, lon, lat);
          counter++;
        }
        if (counter < iteration_limit)
          lt_ret.moonrise = lt_next.moonrise;
      }
    }
    if (!lt_ret.moonset_today())
    {
      unsigned int counter(0);  // just in case something goes wrong
      boost::local_time::local_date_time ldt_iter(ldt);
      if (lt_ret.above_hz_24h || lt_ret.moonrise_today())
      {
        // find the next moonset
        lunar_time_t lt_next;
        while (!lt_next.moonset_today() && counter < iteration_limit)
        {
          ldt_iter += boost::posix_time::hours(24);
          lt_next = lunar_time_i(ldt_iter, lon, lat);
          counter++;
        }
        if (counter < iteration_limit)
          lt_ret.moonset = lt_next.moonset;
      }
      else
      {
        // find the previous moonset
        lunar_time_t lt_prev;
        while (!lt_prev.moonset_today() && counter < iteration_limit)
        {
          ldt_iter -= boost::posix_time::hours(24);
          lt_prev = lunar_time_i(ldt_iter, lon, lat);
          counter++;
        }
        if (counter < iteration_limit)
          lt_ret.moonset = lt_prev.moonset;
      }
    }
    return lt_ret;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

}  // namespace Astronomy
}  // namespace Fmi

// ======================================================================


// original source:https://github.com/AgileDrones/OptiTrack-Motive-2-Client
// by Winter Guerra
// winterg@mit.edu
//https://winter.industries/

// Include motion capture framework
#include "optitrack_motive_2_client/motionCaptureClientFramework.h"
// Include ACL message types (https://bitbucket.org/brettlopez/acl_msgs.git)
#include "acl_msgs/ViconState.h"

// Includes for ROS
#include "ros/ros.h"

// Includes for node
#include <boost/program_options.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <iostream>
#include <fstream>

namespace po = boost::program_options;
using namespace Eigen;
const double pi  = 3.1415926535897932384626433832795028841971;

// Used to convert mocap frame (NUE) to LCM NED.
// static Eigen::Matrix3d R_NUE2NED = [] {
//     Eigen::Matrix3d tmp;
//     tmp <<  1, 0, 0,
//             0, 0, 1,
//             0, -1, 0;
//     return tmp;
// }();

// Used to convert mocap frame (NUE) to ROS ENU.
static Eigen::Matrix3d R_NUE2ENU = [] {
    Eigen::Matrix3d tmp;
    tmp <<  0, 0, 1,
            1, 0, 0,
            0, 1, 0;
    return tmp;
}();

Vector3d positionConvertNUE2ENU(double* positionNUE){
  Vector3d positionNUEVector, positionENUVector;
  positionNUEVector << positionNUE[0], positionNUE[1], positionNUE[2];

  positionENUVector = R_NUE2ENU * positionNUEVector;
  return positionENUVector;
}

Quaterniond quaternionConvertNUE2ENU(double* quaternionNUE){
    Quaterniond quaternionInNUE;
    quaternionInNUE.x() = quaternionNUE[0];
    quaternionInNUE.y() = quaternionNUE[1];
    quaternionInNUE.z() = quaternionNUE[2];
    quaternionInNUE.w() = quaternionNUE[3];

    Quaterniond quaternionInENU = Quaterniond(R_NUE2ENU * quaternionInNUE.normalized().toRotationMatrix()
                              * R_NUE2ENU.transpose());
    return quaternionInENU;
}

int main(int argc, char *argv[])
{
  // Keep track of ntime offset.
  int64_t offset_between_windows_and_linux = std::numeric_limits<int64_t>::max();
  ROS_INFO("1");
  // Init ROS
  ros::init(argc, argv, "optitrack_motive_2_client_node");
  ros::NodeHandle n;

  std::string szMyIPAddress;
  std::string szServerIPAddress;

  // Get CMDline arguments for server and local IP addresses.

 //自己加的，方便调整IP read ip from launch, added by Peter Li
  n.getParam(
      "/optitrack_motive_2_client_node/MyIPAddress",
      szMyIPAddress); //因为这个参数不是server那边发送的，是这个节点私有的，因此要加范围
  ROS_INFO("MyIPAddress is %s", szMyIPAddress.c_str());

  n.getParam(
      "/optitrack_motive_2_client_node/ServerIPAddress",
      szServerIPAddress); //因为这个参数不是server那边发送的，是这个节点私有的，因此要加范围
  ROS_INFO("ServerIPAddress is %s", szServerIPAddress.c_str());

/*
// test for direct input here，added by Peter Li
szMyIPAddress = "192.168.50.170";
szServerIPAddress = "192.168.50.171";
*/

  try {
    po::options_description desc ("Options");
    desc.add_options()
        ("help,h", "print usage message")
        ("local",po::value<std::string>(&szMyIPAddress),"local IP Address")
        ("server",po::value<std::string>(&szServerIPAddress), "server address");

    po::variables_map vm;
    try {
      po::store(po::parse_command_line(argc, argv, desc), vm);
      po::notify (vm);
    }
    catch (po::error& e) {
      std::cerr << e.what() << std::endl;
      return 0;
    }
    if (vm.count("help")) {
      std::cout << desc << "\n";
      return 0;
    }
  }
  catch (...) {}

  // Init mocap framework
  agile::motionCaptureClientFramework mocap_ = agile::motionCaptureClientFramework(szMyIPAddress, szServerIPAddress);

  // Some vars to calculate twist/acceleration and dts
  // Also keeps track of the various publishers
  std::map<int, ros::Publisher> rosPublishers;
  std::map<int, acl_msgs::ViconState> pastStateMessages;

  while (true){
    // Wait for mocap packet
    mocap_.spin();

    std::vector<agile::Packet> mocap_packets = mocap_.getPackets();

    for (agile::Packet mocap_packet : mocap_packets){

      // @TODO: Make getPackets return a list.

      // Skip this rigid body if tracking is invalid
      if (!mocap_packet.tracking_valid)
        continue;

      // estimate the windows to linux constant offset by taking the minimum seen offset.
      // @TODO: Make offset a rolling average instead of a latching offset.
      int64_t offset = mocap_packet.transmit_timestamp - mocap_packet.receive_timestamp;
      if (offset < offset_between_windows_and_linux ){
        offset_between_windows_and_linux = offset;
      }
      uint64_t packet_ntime = mocap_packet.mid_exposure_timestamp - offset_between_windows_and_linux;

      // Get past state and publisher (if they exist)
      bool hasPreviousMessage = (rosPublishers.find(mocap_packet.rigid_body_id) != rosPublishers.end());
      ros::Publisher publisher;
      acl_msgs::ViconState lastState;
      acl_msgs::ViconState currentState;

      // Initialize publisher for rigid body if not exist.
      if (!hasPreviousMessage){
        std::string topic = "/" + mocap_packet.model_name + "/vicon";

        publisher = n.advertise<acl_msgs::ViconState>(topic, 1);
        rosPublishers[mocap_packet.rigid_body_id] = publisher;
      } else {
        // Get saved publisher and last state
        publisher = rosPublishers[mocap_packet.rigid_body_id];
        lastState = pastStateMessages[mocap_packet.rigid_body_id];
      }

      // Add timestamp
      currentState.header.stamp = ros::Time(packet_ntime/1e9, packet_ntime%(int64_t)1e9);

      // Convert rigid body position from NUE to ROS ENU
      Vector3d positionENUVector = positionConvertNUE2ENU(mocap_packet.pos);
      currentState.pose.position.x = positionENUVector(0);
      currentState.pose.position.y = positionENUVector(1);
      currentState.pose.position.z = positionENUVector(2);
      // Convert rigid body rotation from NUE to ROS ENU
      Quaterniond quaternionENUVector = quaternionConvertNUE2ENU(mocap_packet.orientation);
      currentState.pose.orientation.x = quaternionENUVector.x();
      currentState.pose.orientation.y = quaternionENUVector.y();
      currentState.pose.orientation.z = quaternionENUVector.z();
      currentState.pose.orientation.w = quaternionENUVector.w();
     /***************************************************/
      //加上欧拉角 convert Quaterniond to Eulers, added by Peter Li
      //四元数->旋转矩阵->欧拉角 Quaterniond->RotationMatrix->Eulers
      ////!!!!!Be aware you can convert rad to degree here， which is convenient for comparing with Mtoive panels.
      ///Custom for your need.注意此处是 rad。
      Matrix3d RotationMatrix_cur = quaternionENUVector.toRotationMatrix();
      Vector3d Eulers_cur = RotationMatrix_cur.eulerAngles(2,1,0);
      currentState.Eulers.z = Eulers_cur(0);//yaw(why only within [0,pi] why?)
      currentState.Eulers.y = Eulers_cur(1);//pitch (-pi,pi]
      currentState.Eulers.x = Eulers_cur(2);//roll (-pi,pi]
      /***************************************************/
      currentState.has_pose = true;

      // Loop through markers and convert positions from NUE to ENU
      // @TODO since the state message does not understand marker locations.

      if (hasPreviousMessage){
        int64_t dt_nsec = packet_ntime - (lastState.header.stamp.sec*1e9 + lastState.header.stamp.nsec);

       // printf("time interval = %ld",dt_nsec);

        // Calculate twist. Requires last state message.
        currentState.twist.linear.x = (currentState.pose.position.x - lastState.pose.position.x)/(dt_nsec / 1e9);
        currentState.twist.linear.y = (currentState.pose.position.y - lastState.pose.position.y)/(dt_nsec / 1e9);
        currentState.twist.linear.z = (currentState.pose.position.z - lastState.pose.position.z)/(dt_nsec / 1e9);

        // @TODO: not sure how to calculate the angular twist. Is it in local frame or global frame?
        //计算角速度 calculate the derivation of Eulers in global frame, i.e. angular velocity, added by Peter Li

        currentState.twist.angular.x = (currentState.Eulers.x - lastState.Eulers.x)/(dt_nsec / 1e9);//roll
        currentState.twist.angular.y = (currentState.Eulers.y - lastState.Eulers.y)/(dt_nsec / 1e9);//pitch
        currentState.twist.angular.z = (currentState.Eulers.z - lastState.Eulers.z)/(dt_nsec / 1e9);//yaw
        currentState.has_twist = true;

        // Calculate accelerations of both positon and Eulers. Requires last state message.
        currentState.pos_accel.x = (currentState.twist.linear.x - lastState.twist.linear.x)/(dt_nsec / 1e9);
        currentState.pos_accel.y = (currentState.twist.linear.y - lastState.twist.linear.y)/(dt_nsec / 1e9);
        currentState.pos_accel.z = (currentState.twist.linear.z - lastState.twist.linear.z)/(dt_nsec / 1e9);
        currentState.Eulers_accel.x = (currentState.twist.angular.x - lastState.twist.angular.x)/(dt_nsec / 1e9);
        currentState.Eulers_accel.y = (currentState.twist.angular.y - lastState.twist.angular.y)/(dt_nsec / 1e9);
        currentState.Eulers_accel.z = (currentState.twist.angular.z - lastState.twist.angular.z)/(dt_nsec / 1e9);

        currentState.has_accel = true;
      }

      // Save state for future acceleration and twist computations
      pastStateMessages[mocap_packet.rigid_body_id] = currentState;

      // Publish ROS state.
      publisher.publish(currentState);

    }
  }
}


#pragma once

#include "splines.h"
#include <algorithm>
#include <iterator>
#include <functional>
#include <cmath>			// for fmin/fmax
#include <cassert>
#ifdef MSVC_LIMITATIONS
#include <boost/math/special_functions/binomial.hpp>
#endif
#include "general math.h"	// for lerp
#include "misc.h"			// for Reserve()

#pragma region CompositePoint
namespace Math::Splines
{
	template<class Pos, class ...Attribs>
	struct CompositePoint<Pos, Attribs...>::AddAssign
	{
		template<typename Dst, typename Src>
		constexpr void operator ()(Dst &dst, const Src &src) const { dst += src; }
	};

	template<class Pos, class ...Attribs>
	struct CompositePoint<Pos, Attribs...>::SubAssign
	{
		template<typename Dst, typename Src>
		constexpr void operator ()(Dst &dst, const Src &src) const { dst -= src; }
	};

	template<class Pos, class ...Attribs>
	struct CompositePoint<Pos, Attribs...>::MulAssign
	{
		template<typename Dst, typename Src>
		constexpr void operator ()(Dst &dst, const Src &src) const { dst *= src; }
	};

	template<class Pos, class ...Attribs>
	struct CompositePoint<Pos, Attribs...>::DivAssign
	{
		template<typename Dst, typename Src>
		constexpr void operator ()(Dst &dst, const Src &src) const { dst /= src; }
	};

	// op point
	template<class Pos, class ...Attribs>
	template<class Functor, size_t ...idx>
	inline auto CompositePoint<Pos, Attribs...>::OpPoint(std::index_sequence<idx...>) const
#ifdef MSVC_LIMITATIONS
		-> CompositePoint<Pos, Attribs...>
#else
		-> CompositePoint<std::decay_t<decltype(std::declval<Functor>()(pos))>, std::decay_t<decltype(std::declval<Functor>()(std::get<idx>(attribs)))>...>
#endif
	{
		constexpr Functor op;
		return{ op(pos), op(std::get<idx>(attribs))... };
	}

	// point op point
	template<class Pos, class ...Attribs>
	template<class Functor, size_t ...idx, class RightPos, class ...RightAttribs>
	inline auto CompositePoint<Pos, Attribs...>::PointOpPoint(std::index_sequence<idx...>, const CompositePoint<Pos, Attribs...> &left, const CompositePoint<RightPos, RightAttribs...> &right)
#ifdef MSVC_LIMITATIONS
		-> CompositePoint<Pos, Attribs...>
#else
		-> CompositePoint<std::decay_t<decltype(std::declval<Functor>()(left.pos, right.pos))>, std::decay_t<decltype(std::declval<Functor>()(std::get<idx>(left.attribs), std::get<idx>(right.attribs)))>...>
#endif
	{
		constexpr Functor op;
		return{ op(left.pos, right.pos), op(std::get<idx>(left.attribs), std::get<idx>(right.attribs))... };
	}

	// point op scalar
	template<class Pos, class ...Attribs>
	template<class Functor, size_t ...idx, typename Scalar>
	inline auto CompositePoint<Pos, Attribs...>::PointOpScalar(std::index_sequence<idx...>, const CompositePoint<Pos, Attribs...> &left, const Scalar &right)
#ifdef MSVC_LIMITATIONS
		-> CompositePoint<Pos, Attribs...>
#else
		-> CompositePoint<std::decay_t<decltype(std::declval<Functor>()(left.pos, right))>, std::decay_t<decltype(std::declval<Functor>()(std::get<idx>(left.attribs), right))>...>
#endif
	{
		constexpr Functor op;
		return{ op(left.pos, right), op(std::get<idx>(left.attribs), right)... };
	}

	// scalar op point
	template<class Pos, class ...Attribs>
	template<class Functor, size_t ...idx, typename Scalar>
	inline auto CompositePoint<Pos, Attribs...>::ScalarOpPoint(std::index_sequence<idx...>, const Scalar &left, const CompositePoint<Pos, Attribs...> &right)
#ifdef MSVC_LIMITATIONS
		-> CompositePoint<Pos, Attribs...>
#else
		-> CompositePoint<std::decay_t<decltype(std::declval<Functor>()(left, right.pos))>, std::decay_t<decltype(std::declval<Functor>()(left, std::get<idx>(right.attribs)))>...>
#endif
	{
		constexpr Functor op;
		return { op(left, right.pos), op(left, std::get<idx>(right.attribs))... };
	}

	// point op= point
	template<class Pos, class ...Attribs>
	template<class Functor, size_t idx, class SrcPos, class ...SrcAttribs>
	inline auto CompositePoint<Pos, Attribs...>::PointOpPoint(const CompositePoint<SrcPos, SrcAttribs...> &src) -> std::enable_if_t<idx < sizeof...(Attribs), CompositePoint &>
	{
		Functor()(std::get<idx>(attribs), std::get<idx>(src.attribs));
		return PointOpPoint<Functor, idx + 1>(src);
	}

	template<class Pos, class ...Attribs>
	template<class Functor, size_t idx, class SrcPos, class ...SrcAttribs>
	inline auto CompositePoint<Pos, Attribs...>::PointOpPoint(const CompositePoint<SrcPos, SrcAttribs...> &src) -> std::enable_if_t<idx == sizeof...(Attribs), CompositePoint &>
	{
		Functor()(pos, src.pos);
		return *this;
	}

	// point op= scalar
	template<class Pos, class ...Attribs>
	template<class Functor, size_t idx, typename Scalar>
	inline auto CompositePoint<Pos, Attribs...>::PointOpScalar(const Scalar &src) -> std::enable_if_t<idx < sizeof...(Attribs), CompositePoint &>
	{
		Functor()(std::get<idx>(attribs), src);
		return PointOpScalar<Functor, idx + 1>(src);
	}

	template<class Pos, class ...Attribs>
	template<class Functor, size_t idx, typename Scalar>
	inline auto CompositePoint<Pos, Attribs...>::PointOpScalar(const Scalar &src) -> std::enable_if_t<idx == sizeof...(Attribs), CompositePoint &>
	{
		Functor()(pos, src);
		return *this;
	}

	template<class Pos, class ...Attribs>
	template<class SrcPos, class ...SrcAttribs>
	inline CompositePoint<Pos, Attribs...>::CompositePoint(SrcPos &&pos, SrcAttribs &&...attribs) :
	pos(std::forward<SrcPos>(pos)), attribs(std::forward<SrcAttribs>(attribs)...) {}

	template<class Pos, class ...Attribs>
	template<class SrcPos, class ...SrcAttribs>
	inline CompositePoint<Pos, Attribs...>::CompositePoint(const CompositePoint<SrcPos, SrcAttribs...> &src) :
	pos(src.pos), attribs(src.attribs) {}

	template<class Pos, class ...Attribs>
	template<class SrcPos, class ...SrcAttribs>
	inline CompositePoint<Pos, Attribs...>::CompositePoint(CompositePoint<SrcPos, SrcAttribs...> &&src) :
	pos(std::move(src.pos)), attribs(std::move(src.attribs)) {}

	template<class Pos, class ...Attribs>
	template<class SrcPos, class ...SrcAttribs>
	inline auto CompositePoint<Pos, Attribs...>::operator =(const CompositePoint<SrcPos, SrcAttribs...> &src) -> CompositePoint &
	{
		pos = src.pos;
		attribs = src.attribs;
		return *this;
	}

	template<class Pos, class ...Attribs>
	template<class SrcPos, class ...SrcAttribs>
	inline auto CompositePoint<Pos, Attribs...>::operator =(CompositePoint<SrcPos, SrcAttribs...> &&src) -> CompositePoint &
	{
		pos = std::move(src.pos);
		attribs = std::move(src.attribs);
		return *this;
	}

	template<class Pos, class ...Attribs>
	inline auto CompositePoint<Pos, Attribs...>::operator -() const
	{
		return OpPoint<std::negate<>>(std::index_sequence_for<Attribs...>());
	}

	// point += point
	template<class Pos, class ...Attribs>
	template<class SrcPos, class ...SrcAttribs>
	inline auto CompositePoint<Pos, Attribs...>::operator +=(const CompositePoint<SrcPos, SrcAttribs...> &src) -> CompositePoint &
	{
		return PointOpPoint<AddAssign>(src);
	}

	// point -= point
	template<class Pos, class ...Attribs>
	template<class SrcPos, class ...SrcAttribs>
	inline auto CompositePoint<Pos, Attribs...>::operator -=(const CompositePoint<SrcPos, SrcAttribs...> &src) -> CompositePoint &
	{
		return PointOpPoint<SubAssign>(src);
	}

	// point *= scalar
	template<class Pos, class ...Attribs>
	template<typename Scalar>
	inline auto CompositePoint<Pos, Attribs...>::operator *=(const Scalar &src) -> CompositePoint &
	{
		return PointOpScalar<MulAssign>(src);
	}

	// point /= scalar
	template<class Pos, class ...Attribs>
	template<typename Scalar>
	inline auto CompositePoint<Pos, Attribs...>::operator /=(const Scalar &src) -> CompositePoint &
	{
		return PointOpScalar<DivAssign>(src);
	}

	// point + point
	template<class LeftPos, class ...LeftAttribs, class RightPos, class ...RightAttribs>
	inline auto operator +(const CompositePoint<LeftPos, LeftAttribs...> &left, const CompositePoint<RightPos, RightAttribs...> &right)
	{
		return CompositePoint<LeftPos, LeftAttribs...>::PointOpPoint<std::plus<>>(std::index_sequence_for<LeftAttribs...>(), left, right);
	}

	// point - point
	template<class LeftPos, class ...LeftAttribs, class RightPos, class ...RightAttribs>
	inline auto operator -(const CompositePoint<LeftPos, LeftAttribs...> &left, const CompositePoint<RightPos, RightAttribs...> &right)
	{
		return CompositePoint<LeftPos, LeftAttribs...>::PointOpPoint<std::minus<>>(std::index_sequence_for<LeftAttribs...>(), left, right);
	}

	// point * scalar
	template<class Pos, class ...Attribs, typename Scalar>
	inline auto operator *(const CompositePoint<Pos, Attribs...> &left, const Scalar &right)
	{
		return CompositePoint<Pos, Attribs...>::PointOpScalar<std::multiplies<>>(std::index_sequence_for<Attribs...>(), left, right);
	}

	// scalar * point
	template<class Pos, class ...Attribs, typename Scalar>
	inline auto operator *(const Scalar &left, const CompositePoint<Pos, Attribs...> &right)
	{
		return CompositePoint<Pos, Attribs...>::ScalarOpPoint<std::multiplies<>>(std::index_sequence_for<Attribs...>(), left, right);
	}

	// point / scalar
	template<class Pos, class ...Attribs, typename Scalar>
	inline auto operator /(const CompositePoint<Pos, Attribs...> &left, const Scalar &right)
	{
		return CompositePoint<Pos, Attribs...>::PointOpScalar<std::divides<>>(std::index_sequence_for<Attribs...>(), left, right);
	}
}
#pragma endregion

template<typename ScalarType, unsigned int dimension, unsigned int degree, class ...Attribs>
template<size_t ...idx>
inline Math::Splines::CBezier<ScalarType, dimension, degree, Attribs...>::CBezier(const typename ControlPoints::value_type (&src)[degree + 1], std::index_sequence<idx...>) :
	controlPoints{ typename ControlPoints::value_type(src[idx])... } {}

template<typename ScalarType, unsigned int dimension, unsigned int degree, class ...Attribs>
Math::Splines::CBezier<ScalarType, dimension, degree, Attribs...>::CBezier(const typename ControlPoints::value_type (&src)[degree + 1]) :
	CBezier(src, std::make_index_sequence<degree + 1>()) {}

template<typename ScalarType, unsigned int dimension, unsigned int degree, class ...Attribs>
Math::Splines::CBezier<ScalarType, dimension, degree, Attribs...>::CBezier(const ControlPoints &controlPoints) :
	controlPoints(controlPoints) {}

template<typename ScalarType, unsigned int dimension, unsigned int degree, class ...Attribs>
Math::Splines::CBezier<ScalarType, dimension, degree, Attribs...>::CBezier(ControlPoints &&controlPoints) :
	controlPoints(std::move(controlPoints)) {}

template<typename ScalarType, unsigned int dimension, unsigned int degree, class ...Attribs>
template<class ...Points>
Math::Splines::CBezier<ScalarType, dimension, degree, Attribs...>::CBezier(Points &&...controlPoints) :
controlPoints{ std::forward<Points>(controlPoints)... }
{
	constexpr auto count = degree + 1;
	static_assert(sizeof...(Points) >= count, "too few control points");
	static_assert(sizeof...(Points) <= count, "too many control points");
}

#ifndef MSVC_LIMITATIONS
template<typename ScalarType, unsigned int dimension, unsigned int degree, class ...Attribs>
template<size_t ...idx>
inline auto Math::Splines::CBezier<ScalarType, dimension, degree, Attribs...>::operator ()(ScalarType u, std::index_sequence<idx...>) const -> typename ControlPoints::value_type
{
	static_assert(degree >= 2);
	ScalarType factor1 = u;
	const ScalarType factors2[degree] = { 1 - u, factors2[idx] * factors2[0] ... };
	using Combinatorics::C;
	return C<degree, 0> * factors2[degree - 1] * controlPoints[0] +
		C<degree, 1> * factor1 * factors2[degree - 2] * controlPoints[1] +
		(C<degree, idx + 2> * (factor1 *= u) * factors2[degree - 3 - idx] * controlPoints[2 + idx] + ... + C<degree, degree> * factor1 * u * controlPoints[degree]);
}
#endif

template<typename ScalarType, unsigned int dimension, unsigned int degree, class ...Attribs>
auto Math::Splines::CBezier<ScalarType, dimension, degree, Attribs...>::operator ()(ScalarType u) const -> typename ControlPoints::value_type
{
#ifdef MSVC_LIMITATIONS
	ScalarType factor1 = 1, factors2[degree + 1];
	factors2[degree] = 1;
	for (signed i = degree - 1; i >= 0; i--)
		factors2[i] = factors2[i + 1] * (1 - u);
	auto result = typename ControlPoints::value_type();	// value init
	for (unsigned i = 0; i <= degree; i++, factor1 *= u)
		result += boost::math::binomial_coefficient<ScalarType>(degree, i) * factor1 * factors2[i] * controlPoints[i];
	return result;
#else
	if constexpr (degree == 0)
		return controlPoints[0];
	else if constexpr (degree == 1)
		return lerp(controlPoints[0], controlPoints[1], u);
	else
		return operator ()(u, std::make_index_sequence<degree - 2>());
#endif
}

template<typename ScalarType, unsigned int dimension, unsigned int degree, class ...Attribs>
template<typename Iterator>
void Math::Splines::CBezier<ScalarType, dimension, degree, Attribs...>::Tessellate(Iterator output, ScalarType delta, bool emitFirstPoint) const
{
	if (emitFirstPoint) *output++ = controlPoints[0];
	Subdiv(output, delta, controlPoints);
}

template<typename ScalarType, unsigned int dimension, unsigned int degree, class ...Attribs>
template<typename Iterator, size_t ...idx>
void Math::Splines::CBezier<ScalarType, dimension, degree, Attribs...>::Subdiv(Iterator output, ScalarType delta, const ControlPoints &controlPoints, std::index_sequence<idx...>)
{
	// TODO: move to Math
	const auto point_line_dist = [](typename ControlPoints::const_reference point, typename ControlPoints::const_reference lineBegin, typename ControlPoints::const_reference lineEnd) -> ScalarType
	{
		const auto point_dir = GetPos(point) - GetPos(lineBegin), line_dir = GetPos(lineEnd) - GetPos(lineBegin);
		// project point_dir on line_dir
		const auto proj = dot(point_dir, line_dir) / dot(line_dir, line_dir) * line_dir;
		return length(point_dir - proj);
	};

	const auto stop_test = [&point_line_dist, &controlPoints, delta]
	{
#ifdef MSVC_LIMITATIONS
		for (unsigned i = 1; i < degree; i++)
		{
			if (point_line_dist(controlPoints[i], controlPoints[0], controlPoints[degree]) > delta)
				return false;
		}
		return true;
#else
		return (point_line_dist(controlPoints[idx + 1], controlPoints[0], controlPoints[degree]) > delta || ...);
#endif
	};

	if (stop_test())
		*output++ = (controlPoints[degree]);
	else
	{
		constexpr auto intermediate_points_count = (degree + 1) * 2 - 1;
		typename ControlPoints::value_type intermediate_points[intermediate_points_count];	// will hold control points for 2 subdivided curves (with 1 common point)
		for (unsigned i = 0; i <= degree; i++)
			intermediate_points[i * 2] = controlPoints[i];
		for (unsigned insert_idx_begin = 1, insert_idx_end = intermediate_points_count; insert_idx_begin <= degree; insert_idx_begin++, insert_idx_end--)
		{
			for (unsigned insert_idx = insert_idx_begin; insert_idx < insert_idx_end; insert_idx += 2)
				intermediate_points[insert_idx] = lerp(intermediate_points[insert_idx - 1], intermediate_points[insert_idx + 1], ScalarType(.5));
		}
		Subdiv(output, delta, intermediate_points + 0);
		Subdiv(output, delta, intermediate_points + degree);
	}
}

template<template<typename ScalarType, unsigned int dimension, class ...Attribs> class CBezierInterpolationImpl, typename ScalarType, unsigned int dimension, class ...Attribs>
template<typename Iterator>
void Math::Splines::Impl::CBezierInterpolationCommon<CBezierInterpolationImpl, ScalarType, dimension, Attribs...>::Tessellate(Iterator output, ScalarType delta) const
{
	for (Points::size_type i = 1; i < points.size() - 2; i++)
		Segment(i).Tessellate(output, delta, i == 1);
}

template<typename ScalarType, unsigned int dimension, class ...Attribs>
template<typename Iterator>
Math::Splines::Impl::CCatmullRom<ScalarType, dimension, Attribs...>::CCatmullRom(Iterator begin, Iterator end) :
points(begin, end)
{
	assert(points.size() >= 4);
}

template<typename ScalarType, unsigned int dimension, class ...Attribs>
Math::Splines::Impl::CCatmullRom<ScalarType, dimension, Attribs...>::CCatmullRom(std::initializer_list<Point> points) :
points(points)
{
	assert(points.size() >= 4);
}

template<typename ScalarType, unsigned int dimension, class ...Attribs>
auto Math::Splines::Impl::CCatmullRom<ScalarType, dimension, Attribs...>::operator ()(ScalarType u) const -> Point
{
	//assert(u >= 0 && u <= 1);
	// [0..1]->[0..m-2]->[1..m-1]
	u *= points.size() - 3, u++;
	// ensure 1 <= i < m
	//const Points::size_type i = std::min<Points::size_type>(floor(u), points.size() - 3);
	const ScalarType i = fmin(fmax(floor(u), 1), points.size() - 3);
	return Segment(i)(u - i);
}

template<typename ScalarType, unsigned int dimension, class ...Attribs>
auto Math::Splines::Impl::CCatmullRom<ScalarType, dimension, Attribs...>::Segment(typename Points::size_type i) const -> Bezier
{
	assert(i >= 1 && i < points.size() - 2);
	return { points[i], points[i] + (points[i + 1] - points[i - 1]) / 6, points[i + 1] - (points[i + 2] - points[i]) / 6, points[i + 1] };
}

template<typename ScalarType, unsigned int dimension, class ...Attribs>
template<typename Iterator>
Math::Splines::Impl::CBesselOverhauser<ScalarType, dimension, Attribs...>::CBesselOverhauser(Iterator begin, Iterator end)
{
	Reserve(points, begin, end);
	Init(begin, end);
}

template<typename ScalarType, unsigned int dimension, class ...Attribs>
Math::Splines::Impl::CBesselOverhauser<ScalarType, dimension, Attribs...>::CBesselOverhauser(std::initializer_list<Point> points)
{
	this->points.reserve(points.size());
	Init(points.begin(), points.end());
}

template<typename ScalarType, unsigned int dimension, class ...Attribs>
template<typename Iterator>
void Math::Splines::Impl::CBesselOverhauser<ScalarType, dimension, Attribs...>::Init(Iterator begin, Iterator end)
{
	assert(begin != end);
	points.emplace_back(typename Points::value_type(0, *begin));
	transform(std::next(begin), end, std::back_inserter(points), [this](const Point &curPoint)
	{
		/*
			it seems that 'std' namespace used here ('VectorMath::' before 'distance' required)
			TODO: try with other compilers
		*/
		return typename Points::value_type(points.back().first + VectorMath::distance(GetPos(points.back().second), GetPos(curPoint)), curPoint);
	});
	assert(points.size() >= 4);
}

template<typename ScalarType, unsigned int dimension, class ...Attribs>
auto Math::Splines::Impl::CBesselOverhauser<ScalarType, dimension, Attribs...>::operator ()(ScalarType u) const -> Point
{
	//assert(u >= 0 && u <= 1);
	// [0..1]->[u_begin..u_end]
	const ScalarType u_begin = std::next(points.begin())->first, u_end = std::prev(points.end(), 2)->first;
	u *= u_end - u_begin, u += u_begin;
	// ensure u in [u_begin..u_end]
	//u = fmin(fmax(u, u_begin), u_end);
	struct
	{
		bool operator ()(ScalarType left, typename Points::const_reference right) const
		{
			return left < right.first;
		}
		bool operator ()(typename Points::const_reference left, ScalarType right) const
		{
			return left.first < right;
		}
	} point_order;
	auto p1 = std::lower_bound(points.begin(), points.end(), u, point_order);
	if (std::distance(points.begin(), p1) < 2)
		p1 = std::next(points.begin(), 2);
	else if (std::distance(p1, points.end()) < 2)
		p1 = std::prev(points.end(), 2);
	const auto p0 = std::prev(p1);
	return Segment(std::distance(points.begin(), p0))((u - p0->first) / (p1->first - p0->first));
}

template<typename ScalarType, unsigned int dimension, class ...Attribs>
auto Math::Splines::Impl::CBesselOverhauser<ScalarType, dimension, Attribs...>::Segment(typename Points::size_type i) const -> Bezier
{
	assert(i >= 1 && i < points.size() - 2);
	const auto v_segment = [this](typename Points::size_type j)
	{
		return (points[j + 1].second - points[j].second) / (points[j + 1].first - points[j].first);
	};
	const Point segment_vels[3] = {v_segment(i - 1), v_segment(i), v_segment(i + 1)};
	const auto offset = [this, &segment_vels, i](typename Points::size_type shift)
	{
		return
			(
				(points[i + 1 + shift].first - points[i + shift].first) * segment_vels[0 + shift] +
				(points[i + shift].first - points[i - 1 + shift].first) * segment_vels[1 + shift]
			) /
			((points[i + 1 + shift].first - points[i - 1 + shift].first) * 3) *
			(points[i + 1].first - points[i].first);
	};
	return { points[i].second, points[i].second + offset(0), points[i + 1].second - offset(1), points[i + 1].second };
}