#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/iterator.h>
#include <CGAL/point_generators_2.h>

#include <boost/bind.hpp>

typedef CGAL::Exact_predicates_exact_constructions_kernel K;
typedef K::Point_2                     Point;
typedef K::Segment_2                   Segment;

typedef CGAL::Creator_uniform_2<double,Point>              Pt_creator;
typedef CGAL::Random_points_on_segment_2<Point,Pt_creator> P1;
typedef CGAL::Random_points_on_circle_2<Point,Pt_creator>  P2;
typedef CGAL::Creator_uniform_2< Point, Segment>           Seg_creator;
typedef CGAL::Join_input_iterator_2< P1, P2, Seg_creator>  Seg_iterator;

struct Intersector{
  typedef CGAL::cpp11::result_of<K::Intersect_2(Segment,Segment)>::type result_type;
  const Segment& s;
  K::Intersect_2 intersect;

  Intersector(const Segment& seg): s(seg) {}

  result_type
  operator() ( const Segment& other) const
  {
    return intersect(s, other);
  }
};

int main()
{
  std::vector<Segment> input;

  // Prepare point generator for the horizontal segment, length 200.
  P1 p1( Point(-100,0), Point(100,0));

  // Prepare point generator for random points on circle, radius 250.
  P2 p2( 250);

  // Create segments.
  Seg_iterator g( p1, p2);
  std::copy_n( g, 200, std::back_inserter(input));

  // splitting results with Dispatch_output_iterator
  std::vector<Point> points;
  std::vector<Segment> segments;

  typedef CGAL::Dispatch_output_iterator<
    std::tuple<Point,Segment>, std::tuple< std::back_insert_iterator<std::vector<Point> >,
                                               std::back_insert_iterator<std::vector<Segment> > > >
    Dispatcher;

  Dispatcher disp = CGAL::dispatch_output<Point,Segment>( std::back_inserter(points),
                                                          std::back_inserter(segments) );

  // intersects the first segment of input with all other segments
  // The resulting points or segments are written in the vectors with the same names
  std::transform( input.begin(), input.end(), disp,
                  Intersector(input.front()) );

  std::cout << "Point intersections: " << points.size() << std::endl;
  std::cout << "Segment intersections: " << segments.size() << std::endl;

  return 0;
}


//    Copyright 2017 Rainer Gemulla
//
//    Licensed under the Apache License, Version 2.0 (the "License");
//    you may not use this file except in compliance with the License.
//    You may obtain a copy of the License at
//
//        http://www.apache.org/licenses/LICENSE-2.0
//
//    Unless required by applicable law or agreed to in writing, software
//    distributed under the License is distributed on an "AS IS" BASIS,
//    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//    See the License for the specific language governing permissions and
//    limitations under the License.
/** \file
 *
 * Illustrates matrix factorization with SGD. We first creates factors and then a data matrix
 * from these factors. THis process ensures that we know the best factorization of the input.
 * We then try to reconstruct the factors using SGD.
 */
#include <iostream>

#include <boost/math/distributions/normal.hpp>
#include <boost/random/uniform_real.hpp>

#include <util/evaluation.h>
#include <util/io.h>

#include <mf/mf.h>

using namespace std;
using namespace mf;
using namespace rg;
using namespace boost::numeric::ublas;

log4cxx::LoggerPtr logger(log4cxx::Logger::getLogger("main"));

int main(int argc, char* argv[]) {
#ifndef NDEBUG
	LOG4CXX_WARN(logger, "Warning: Debug mode activated (runtimes may be slow).");
#endif

	// parameters for the factorization
	mf_size_type size1 = 10000;
	mf_size_type size2 = 10000;
	mf_size_type nnz = 1000000;
	double sigma = 1; // standard deviation
	double lambda = 1/sigma/sigma;
	mf_size_type r = 10;

	// parameters for SGD
	double eps0 = 0.1;
	unsigned epochs = 20;
	SgdOrder order = SGD_ORDER_WOR;
	typedef UpdateTruncate<UpdateNzsl> Update;
	typedef RegularizeL2 Regularize;
	typedef SumLoss<NzslLoss, L2Loss> Loss;
	typedef NzslLoss TestLoss;
	Update update = Update(UpdateNzsl(), -10*sigma, 10*sigma); // truncate for numerical stability
	Regularize regularize = Regularize(lambda);
	Loss loss((NzslLoss()), L2Loss(lambda));
	TestLoss testLoss;
	mf_size_type testNnz = nnz/100;
	BalanceType balanceType = BALANCE_NONE;
	BalanceMethod balanceMethod = BALANCE_SIMPLE;

	// generate original factors by sampling from a normal(0,sigma) distribution
	Random32 random; // note: this takes a default seed (not randomized!)
	DenseMatrix wIn(size1, r);
	DenseMatrixCM hIn(r, size2);
	generateRandom(wIn, random, boost::normal_distribution<>(0, sigma));
	generateRandom(hIn, random, boost::normal_distribution<>(0, sigma));

	// generate a sparse matrix by selecting random entries from the generated factors
	// and add small Gaussian noise
	SparseMatrix v;
	generateRandom(v, nnz, wIn, hIn, random);
	addRandom(v, random, boost::normal_distribution<>(0, 0.1));
	LOG4CXX_INFO(logger, "Data matrix: "
		<< v.size1() << " x " << v.size2() << ", " << v.nnz() << " nonzeros");
	LOG4CXX_INFO(logger, "Loss with original factors: " << loss((FactorizationData<>(v, wIn, hIn))));

	// create a test matrix (without noise)
	SparseMatrix vTest;
	generateRandom(vTest, testNnz, wIn, hIn, random);
	LOG4CXX_INFO(logger, "Test matrix: "
		<< v.size1() << " x " << v.size2() << ", " << vTest.nnz() << " nonzeros");

	// take a small sample and remove empty rows/columns
	ProjectedSparseMatrix Vsample;
	projectRandomSubmatrix(random, v, Vsample, v.size1()/5, v.size2()/5);
	projectFrequent(Vsample, 0);
	LOG4CXX_INFO(logger, "Sample matrix: "
		<< Vsample.data.size1() << " x " << Vsample.data.size2()
		<< ", " << Vsample.data.nnz() << " nonzeros");

	// generate initial factors by sampling from a uniform[-0.5,0.5] distribution
	DenseMatrix w(size1, r);
	DenseMatrixCM h(r, size2);
	generateRandom(w, random, boost::uniform_real<>(-0.5, 0.5));
	generateRandom(h, random, boost::uniform_real<>(-0.5, 0.5));

	// initialize the SGD
	Timer t;
	SgdRunner sgdRunner(random);
	SgdJob<Update,Regularize> job(v, w, h, update, regularize, order);
	FactorizationData<> testJob(vTest, w, h);
	DecayAuto<Update,Regularize,Loss> decay(job, loss, Vsample, eps0, 4);
	Trace trace;

	// print the test loss
	LOG4CXX_INFO(logger, "Initial test loss: "
			<< testLoss(FactorizationData<>(vTest,w,h)));

	// run SGD to try to reconstruct the original factors
	t.start();
	sgdRunner.run(job, loss, epochs, decay, trace, balanceType, balanceMethod, &testJob, &testLoss);
	t.stop();
	LOG4CXX_INFO(logger, "Total time: " << t);

	// print the test loss
	LOG4CXX_INFO(logger, "Final test loss: "
			<< testLoss(FactorizationData<>(vTest,w,h)));

	// write trace to an R file
	LOG4CXX_INFO(logger, "Writing trace to " << "/tmp/sgd-trace.R");
	trace.toRfile("/tmp/sgd-trace.R", "sgd");

	return 0;
}


/*******************************************************************************
 * Copyright (c) 2018-, UT-Battelle, LLC.
 * All rights reserved. This program and the accompanying materials
 * are made available under the terms of the MIT License
 * which accompanies this distribution.
 *
 * Contributors:
 *   Alexander J. McCaskey - initial API and implementation
 *   Thien Nguyen - implementation
 *******************************************************************************/
#include "qsim_utils.hpp"
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <Eigen/QR>
#include <cassert>
namespace qcor {
namespace QuaSiMo {
std::shared_ptr<CostFunctionEvaluator>
getEvaluator(Operator *observable, const HeterogeneousMap &params) {
  // If an evaluator was provided explicitly:
  if (params.pointerLikeExists<CostFunctionEvaluator>("evaluator")) {
    return xacc::as_shared_ptr(
        params.getPointerLike<CostFunctionEvaluator>("evaluator"));
  }

  // Cost Evaluator was provided by name:
  if (params.stringExists("evaluator")) {
    return getObjEvaluator(observable, params.getString("evaluator"));
  }

  // No specific evaluator/evaluation method was requested,
  // use the default one (partial tomography based).
  return getObjEvaluator(observable);
}

PronyResult pronyFit(const std::vector<std::complex<double>> &in_signal) {
  assert(!in_signal.empty());

  // Returns the Hankel matrix constructed from the first column c, and
  // (optionally) the last row r.
  auto hankelMat =
      [](const std::vector<std::complex<double>> &c,
         const std::vector<std::complex<double>> &r) -> Eigen::MatrixXcd {
    Eigen::MatrixXcd result = Eigen::MatrixXcd::Zero(c.size(), r.size());
    const auto m = c.size();
    for (int i = 0; i < result.rows(); ++i) {
      for (int j = 0; j < result.cols(); ++j) {
        // H(i,j) = c(i+j),  i+j < m;
        if (i + j < m) {
          result(i, j) = c[i + j];
        } else {
          // H(i,j) = r(i+j+1-m),  otherwise
          result(i, j) = r[i + j + 1 - m];
        }
      }
    }

    return result;
  };

  // Pythonic vector slice:
  auto vectorSlice = [](const std::vector<std::complex<double>> &in_vec,
                        int in_start, int in_end) {
    std::vector<std::complex<double>> result;
    auto startIter = in_vec.begin() + in_start;
    auto endIter =
        in_end > 0 ? in_vec.begin() + in_end : (in_vec.end() + in_end);
    result.assign(startIter, endIter);
    return result;
  };

  const size_t num_freqs = in_signal.size() / 2;
  auto hankel0 = hankelMat(vectorSlice(in_signal, 0, num_freqs),
                           vectorSlice(in_signal, num_freqs - 1, -1));
  auto hankel1 = hankelMat(vectorSlice(in_signal, 1, num_freqs + 1),
                           vectorSlice(in_signal, num_freqs, 0));
  // std::cout << "Hankel Matrix 0: \n" << hankel0 << "\n";
  // std::cout << "Hankel Matrix 1: \n" << hankel1 << "\n";

  hankel0.transposeInPlace();
  hankel1.transposeInPlace();
  Eigen::MatrixXcd shift_matrix =
      Eigen::MatrixXcd::Zero(hankel0.cols(), hankel1.cols());

  for (size_t i = 0; i < hankel1.cols(); ++i) {
    Eigen::VectorXcd shift_matrix_col =
        hankel0.fullPivHouseholderQr().solve(hankel1.col(i));
    shift_matrix.col(i) = shift_matrix_col;
  }
  // std::cout << "Shift matrix: \n" << shift_matrix << "\n";

  shift_matrix.transposeInPlace();
  Eigen::ComplexEigenSolver<Eigen::MatrixXcd> s(shift_matrix);
  auto phases = s.eigenvalues();
  Eigen::MatrixXcd generation_matrix =
      Eigen::MatrixXcd::Zero(in_signal.size(), phases.size());
  for (int i = 0; i < generation_matrix.rows(); ++i) {
    for (int j = 0; j < generation_matrix.cols(); ++j) {
      generation_matrix(i, j) = std::pow(phases[j], i);
    }
  }

  auto signalData = in_signal;
  Eigen::VectorXcd signal =
      Eigen::Map<Eigen::VectorXcd>(signalData.data(), signalData.size());
  Eigen::VectorXcd amplitudes =
      generation_matrix.fullPivHouseholderQr().solve(signal);
  assert(phases.size() == amplitudes.size());
  // std::cout << "Amplitude:\n" << amplitudes << "\n";
  // std::cout << "Phases:\n" << phases << "\n";
  PronyResult finalResult;
  for (size_t i = 0; i < phases.size(); ++i) {
    finalResult.emplace_back(std::make_pair(amplitudes(i), phases(i)));
  }

  // Sort by amplitude:
  std::sort(finalResult.begin(), finalResult.end(),
            [](const auto &a, const auto &b) {
              return std::abs(a.first) < std::abs(b.first);
            });

  return finalResult;
}
} // namespace QuaSiMo
} // namespace qcor


#include "TriangleMuscleConstraint.h"
#include <Eigen/SVD>
#include <Eigen/Sparse>
#include <Eigen/Geometry>
#include <iostream>

using namespace FEM;
TriangleMuscleConstraint::
TriangleMuscleConstraint(double stiffness,const Eigen::Vector2d& fiber_direction,int i0,int i1,int i2,double area,const Eigen::Matrix2d& invDm)
	:Constraint(stiffness),mFiberDirection(fiber_direction),
	mi0(i0),mi1(i1),mi2(i2),mArea(area),mInvDm(invDm),mActivationLevel(0.0)
{

}
int
TriangleMuscleConstraint::
GetDof()
{
	return 1;
}

void
TriangleMuscleConstraint::
EvaluateJMatrix(int index, std::vector<Eigen::Triplet<double>>& J_triplets)
{
	Eigen::MatrixXd Ai(3,9);

	Eigen::Vector2d v = mInvDm*mFiberDirection;

	Ai<<
		-v[0]-v[1],0,0,v[0],0,0,v[1],0,0,
		0,-v[0]-v[1],0,0,v[0],0,0,v[1],0,
		0,0,-v[0]-v[1],0,0,v[0],0,0,v[1];

	Eigen::MatrixXd MuAiT = mStiffness*mArea*Ai.transpose();

	int idx[3] = {mi0,mi1,mi2};

	for(int i=0;i<3;i++)
	{
		J_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+0, 3*(index)+0, MuAiT(3*i+0,0)));
		J_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+0, 3*(index)+1, MuAiT(3*i+0,1)));
		J_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+0, 3*(index)+2, MuAiT(3*i+0,2)));
		J_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+1, 3*(index)+0, MuAiT(3*i+1,0)));
		J_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+1, 3*(index)+1, MuAiT(3*i+1,1)));
		J_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+1, 3*(index)+2, MuAiT(3*i+1,2)));
		J_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+2, 3*(index)+0, MuAiT(3*i+2,0)));
		J_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+2, 3*(index)+1, MuAiT(3*i+2,1)));
		J_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+2, 3*(index)+2, MuAiT(3*i+2,2)));
	}
}
void
TriangleMuscleConstraint::
EvaluateLMatrix(std::vector<Eigen::Triplet<double>>& L_triplets)
{
	Eigen::MatrixXd Ai(3,9);

	Eigen::Vector2d v = mInvDm*mFiberDirection;
	Ai<<
		-v[0]-v[1],0,0,v[0],0,0,v[1],0,0,
		0,-v[0]-v[1],0,0,v[0],0,0,v[1],0,
		0,0,-v[0]-v[1],0,0,v[0],0,0,v[1];

	Eigen::MatrixXd MuAiTAi = mStiffness*mArea*((Ai.transpose())*Ai);

	int idx[3] = {mi0,mi1,mi2};
	for(int i =0;i<3;i++)
	{
		for(int j=0;j<3;j++)
		{
			L_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+0, 3*idx[j]+0, MuAiTAi(3*i+0, 3*j+0)));
			L_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+0, 3*idx[j]+1, MuAiTAi(3*i+0, 3*j+1)));
			L_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+0, 3*idx[j]+2, MuAiTAi(3*i+0, 3*j+2)));
			L_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+1, 3*idx[j]+0, MuAiTAi(3*i+1, 3*j+0)));
			L_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+1, 3*idx[j]+1, MuAiTAi(3*i+1, 3*j+1)));
			L_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+1, 3*idx[j]+2, MuAiTAi(3*i+1, 3*j+2)));
			L_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+2, 3*idx[j]+0, MuAiTAi(3*i+2, 3*j+0)));
			L_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+2, 3*idx[j]+1, MuAiTAi(3*i+2, 3*j+1)));
			L_triplets.push_back(Eigen::Triplet<double>(3*idx[i]+2, 3*idx[j]+2, MuAiTAi(3*i+2, 3*j+2)));
		}
	}
}
void
TriangleMuscleConstraint::
EvaluateDVector(const Eigen::VectorXd& x)
{
	Eigen::Vector3d x0(x.segment<3>(mi0*3));

	Eigen::Matrix32d Ds, P;
	Ds.col(0) = x.segment<3>(mi1*3) - x0;
	Ds.col(1) = x.segment<3>(mi2*3) - x0;

	P.col(0) = Ds.col(0).normalized();
	P.col(1) = (Ds.col(1)-Ds.col(1).dot(P.col(0))*P.col(0)).normalized();

	Eigen::Matrix2d F = P.transpose()*Ds*mInvDm;

	md = (1.0-mActivationLevel)*P*F*mFiberDirection;
}
void
TriangleMuscleConstraint::
GetDVector(int& index,Eigen::VectorXd& d)
{
	d.segment<3>(3*(index)) = md;
	index++;
}


#include <Kokkos_Core.hpp>
#include <iostream>

#include <boost/program_options.hpp>

#define PI 3.1415926535897932384

typedef int int_t;

//#include "Tile3D.h"
KOKKOS_FUNCTION double left_flux(double left, double self, double cfl)
{
    return -cfl * left + cfl * self;
}

KOKKOS_FUNCTION double right_flux(double self, double right, double cfl)
{
    return -cfl * right + cfl * self;
}

KOKKOS_FUNCTION double stencil6p(double self, double xm, double xp, double ym,
    double yp, double zm, double zp, double cfl)
{
    return (1.0 - 6.0 * cfl) * self + cfl * (xm + xp + ym + yp + zm + zp);
}

int main(int argc, char* argv[])
{
    double cfl = 0.1;    // must be less than 1/6

    namespace bpo = boost::program_options;
    bpo::options_description desc("ABFT3D");

    desc.add_options()(
        "xsize", bpo::value<int>()->default_value(98), "X Dimension");
    desc.add_options()(
        "ysize", bpo::value<int>()->default_value(98), "Y Dimension");
    desc.add_options()(
        "zsize", bpo::value<int>()->default_value(98), "Z Dimension");

    bpo::variables_map vm;

    // Setup commandline arguments
    bpo::store(bpo::parse_command_line(argc, argv, desc), vm);
    bpo::notify(vm);

    int xsize = vm["xsize"].as<int>();
    int ysize = vm["ysize"].as<int>();
    int zsize = vm["zsize"].as<int>();

    bool do_check_sum = true;

    Kokkos::initialize(argc, argv);
    {
        using range_policy = Kokkos::RangePolicy<>;

        Kokkos::View<double***> stencil_0(
            "data1", xsize + 2, ysize + 2, zsize + 2);
        Kokkos::View<double***> stencil_1(
            "data2", xsize + 2, ysize + 2, zsize + 2);

        // Soft copy
        auto stencil_old = stencil_0;
        auto stencil_new = stencil_1;
        auto stencil_tmp = stencil_new;
        Kokkos::View<double*, Kokkos::MemoryTraits<Kokkos::Atomic>> checksum(
            "checksum", 1);

        Kokkos::View<double*, Kokkos::DefaultHostExecutionSpace> hchecksum(
            "h_checksum", 1);
        hchecksum[0] = 0.;

        Kokkos::deep_copy(checksum, hchecksum);

        // Initialize stencil
        Kokkos::parallel_for(
            "init", range_policy(0, ysize + 2), KOKKOS_LAMBDA(int j) {
                for (int k = 0; k < zsize + 2; ++k)
                {
                    for (int i = 0; i < xsize + 2; ++i)
                    {
                        stencil_old(i, j, k) = std::sin(1.0 * PI * (double) i /
                            (double) (xsize + 1));    //*
                        stencil_new(i, j, k) = stencil_old(i, j, k);
                    }
                }
            });
        Kokkos::fence();

        Kokkos::Timer timer;

        //
        // How to catch the error ... order matters.  where does the error come from?
        // Physical location could matter (plus interaction with OS)
        //

        // Original checksum
        if (do_check_sum)
        {
            Kokkos::parallel_for(
                "init_checksum", range_policy(1, xsize + 1),
                KOKKOS_LAMBDA(int i) {
                    for (int j = 1; j <= ysize; ++j)
                    {
                        for (int k = 1; k <= zsize; ++k)
                        {
                            checksum[0] += stencil_old(i, j, k);
                        }
                    }
                });
            Kokkos::fence();
        }

        Kokkos::deep_copy(hchecksum, checksum);

        std::cout << "Initial Checksum " << std::scientific << hchecksum[0]
                  << std::endl;

        double chk_ = 0;
        for (int its = 0; its < 10; ++its)
        {
            if (do_check_sum)
            {
                // Deduct the checksum from the boundary
                Kokkos::parallel_for(
                    "first_loop", range_policy(1, zsize + 1),
                    KOKKOS_LAMBDA(int k) {
                        for (int j = 1; j <= ysize; ++j)
                        {
                            checksum[0] -= left_flux(stencil_old(0, j, k),
                                               stencil_old(1, j, k), cfl) +
                                right_flux(stencil_old(xsize, j, k),
                                    stencil_old(xsize + 1, j, k), cfl);
                        }
                    });
                Kokkos::fence();

                Kokkos::parallel_for(
                    "second_loop", range_policy(1, zsize + 1),
                    KOKKOS_LAMBDA(int k) {
                        for (int i = 1; i <= xsize; ++i)
                        {
                            checksum[0] -= left_flux(stencil_old(i, 0, k),
                                               stencil_old(i, 1, k), cfl) +
                                right_flux(stencil_old(i, ysize, k),
                                    stencil_old(i, ysize + 1, k), cfl);
                        }
                    });
                Kokkos::fence();

                Kokkos::parallel_for(
                    "third_loop", range_policy(1, ysize + 1),
                    KOKKOS_LAMBDA(int j) {
                        for (int i = 1; i <= xsize; ++i)
                        {
                            checksum[0] -= left_flux(stencil_old(i, j, 0),
                                               stencil_old(i, j, 1), cfl) +
                                right_flux(stencil_old(i, j, zsize),
                                    stencil_old(i, j, zsize + 1), cfl);
                        }
                    });
                Kokkos::fence();
            }

            // Apply stencil
            // Replace the loops by parallel for
            //
            Kokkos::parallel_reduce(
                "stencil_op", range_policy(1, xsize + 1),
                KOKKOS_LAMBDA(int i, double& chk_) {
                    for (int j = 1; j <= ysize; ++j)
                    {
                        for (int k = 1; k <= zsize; ++k)
                        {
                            //std::cout << "Old " << i << " " << j << " " << k << ": " <<  stencil_old(i,j,k) << std::endl;
                            stencil_new(i, j, k) = stencil6p(
                                stencil_old(i, j, k), stencil_old(i - 1, j, k),
                                stencil_old(i + 1, j, k),
                                stencil_old(i, j - 1, k),
                                stencil_old(i, j + 1, k),
                                stencil_old(i, j, k - 1),
                                stencil_old(i, j, k + 1), cfl);

                            chk_ += stencil_new(i, j, k);
                        }
                    }
                },
                chk_);
            Kokkos::fence();

            // chk_ = 0;

            if (do_check_sum)
            {
                Kokkos::deep_copy(hchecksum, checksum);

                // Testing the checksum
                auto error = std::abs((chk_ - hchecksum[0]));

                std::cout << "New Checksum " << std::scientific << chk_
                          << " and analytical checksum " << hchecksum[0]
                          << " with Error: " << error << std::endl;

                if (error > 1e-2)
                {
                    std::cout << "Failed " << error << std::endl;
                }

                // Alternate stencil assignment
                if (its % 2 == 0)
                {
                    stencil_old = stencil_1;
                    stencil_new = stencil_0;
                }
                else
                {
                    stencil_old = stencil_0;
                    stencil_new = stencil_1;
                }

                chk_ = 0.;
            }
        }

        double time_taken = timer.seconds();

        std::cout << "Executed in: " << time_taken << std::endl;
    }

    Kokkos::finalize();
    return 0;
}


/**
 * \file libsanm/tensor_polymat.cpp
 * This file is part of SANM, a symbolic asymptotic numerical solver.
 */

#include "libsanm/stl.h"
#include "libsanm/tensor_impl_helper.h"

#include <Eigen/Dense>
#include <complex>
#include <functional>
#include <mutex>
#include <span>

using namespace sanm;

namespace {

using cfp_t = std::complex<fp_t>;

/*!
 * \brief compute the DFT of given coeffs using the FFT algorithm
 *
 * Evaluate the polynomial defined by the \p coeffs at the points \f$\omega_n^0,
 * \ldots, \omega_n^{n-1}\f$ where \f$n\f$ is \p nr_term and must be a power of
 * two.
 *
 * \return the real part and the imaginary part
 */
std::pair<TensorArray, TensorArray> fft(const TensorArray& coeffs,
                                        size_t nr_term) {
    sanm_assert(!(nr_term & (nr_term - 1)));
    if (coeffs.size() == 1) {
        return {TensorArray(nr_term, coeffs[0]),
                TensorArray(nr_term, coeffs[0].fill_with(0))};
    }
    if (nr_term == 1) {
        TensorND sum = coeffs[0];
        for (size_t i = 1; i < coeffs.size(); ++i) {
            sum += coeffs[i];
        }
        return {{sum}, {sum.fill_with(0)}};
    }

    TensorArray coeffs_even, coeffs_odd;
    coeffs_even.reserve((coeffs.size() + 1) / 2);
    coeffs_odd.reserve(coeffs.size() / 2);
    for (size_t i = 0; i < coeffs.size(); i += 2) {
        coeffs_even.emplace_back(coeffs[i]);
        if (i + 1 < coeffs.size()) {
            coeffs_odd.emplace_back(coeffs[i + 1]);
        }
    }

    auto result_even = fft(coeffs_even, nr_term / 2),
         result_odd = fft(coeffs_odd, nr_term / 2);

    TensorArray ret_real(nr_term), ret_imag(nr_term);
    for (size_t si = 0; si < nr_term / 2; ++si) {
        const TensorND& a_real = result_even.first[si];
        const TensorND& a_imag = result_even.second[si];
        const TensorND& b_real = result_odd.first[si];
        const TensorND& b_imag = result_odd.second[si];

        for (size_t i : {si, si + nr_term / 2}) {
            TensorND& y_real = ret_real[i];
            TensorND& y_imag = ret_imag[i];

            fp_t angle = fp_t(i) * (M_PI * 2) / fp_t(nr_term),
                 x_real = std::cos(angle), x_imag = std::sin(angle);

            // y = a + x * b
            y_real.set_shape(coeffs[0].shape());
            y_imag.set_shape(coeffs[0].shape());
            as_vector_w(y_real) = as_vector_r(a_real) +
                                  as_vector_r(b_real) * x_real -
                                  as_vector_r(b_imag) * x_imag;
            as_vector_w(y_imag) = as_vector_r(a_imag) +
                                  as_vector_r(b_imag) * x_real +
                                  as_vector_r(b_real) * x_imag;
        }
    }
    return {std::move(ret_real), std::move(ret_imag)};
}

size_t next_pow2(size_t x) {
    size_t y = 1;
    while (y < x) {
        y <<= 1;
    }
    return y;
}

TensorND compute_polymat_det_coeff_with_fft(const TensorArray& coeffs,
                                            size_t nr_term_pow2,
                                            size_t target_order) {
    std::pair<TensorArray, TensorArray> polymat_dft;
    {
        SANM_SCOPED_PROFILER("polymat_det_fft");
        polymat_dft = fft(coeffs, nr_term_pow2);
    }

    const size_t batch = coeffs[0].shape(0);
    const Eigen::Index mdim = coeffs[0].shape(1);
    ScopedAllowMalloc allow_malloc;
    Eigen::Matrix<cfp_t, Eigen::Dynamic, Eigen::Dynamic> eigmat(mdim, mdim);

    TensorND ret{TensorShape{batch, 1}};
    auto ret_ptr = ret.woptr();
    for (size_t ib = 0; ib < batch; ++ib) {
        // do the inverse dft to solve the target coefficient
        cfp_t accum = 0;
        for (size_t i = 0; i < nr_term_pow2; ++i) {
            EigenMatDyn mreal{const_cast<fp_t*>(polymat_dft.first[i].ptr()) +
                                      ib * mdim * mdim,
                              mdim, mdim},
                    mimag{const_cast<fp_t*>(polymat_dft.second[i].ptr()) +
                                  ib * mdim * mdim,
                          mdim, mdim};
            eigmat.real() = mreal;
            eigmat.imag() = mimag;
            cfp_t dfti = eigmat.determinant();
            fp_t angle =
                    -(M_PI * 2) * fp_t(i * target_order) / fp_t(nr_term_pow2);
            accum += dfti * cfp_t{std::cos(angle), std::sin(angle)};
        }
        accum /= fp_t(nr_term_pow2);
        sanm_assert(std::fabs(accum.imag()) <
                            1e-4 * std::max<fp_t>(1, std::fabs(accum.real())),
                    "IDFT not real: real=%g imag=%g", accum.real(),
                    accum.imag());
        ret_ptr[ib] = accum.real();
    }

    return ret;
}

TensorArray transpose_coeffs(const TensorArray& coeffs) {
    TensorArray ret;
    ret.resize(coeffs.size());
    for (size_t i = 0; i < coeffs.size(); ++i) {
        TensorND& dst = ret[i];
        const TensorND& src = coeffs[i];
        sanm_assert(src.rank() == 3);
        size_t n = src.shape(0), m0 = src.shape(1), m1 = src.shape(2);
        dst.set_shape({m0, m1, n});
        EigenMatDyn mdst{dst.woptr(), static_cast<Eigen::Index>(n),
                         static_cast<Eigen::Index>(m0 * m1)},
                msrc{const_cast<fp_t*>(src.ptr()),
                     static_cast<Eigen::Index>(m0 * m1),
                     static_cast<Eigen::Index>(n)};
        mdst = msrc.transpose();
    }
    return ret;
}

using EigenVecArr = std::span<EigenVec>;

void conv(EigenVecArr dst, EigenVecArr x, EigenVecArr y) {
    for (EigenVec& i : dst) {
        i.setZero();
    }
    for (size_t i = 0; i < x.size(); ++i) {
        for (size_t j = 0; j < y.size() && i + j < dst.size(); ++j) {
            dst[i + j].array() += x[i].array() * y[j].array();
        }
    }
}

void conv_k(size_t k, EigenVec& dst, EigenVecArr x, EigenVecArr y) {
    bool first = true;
    for (size_t i = std::max(0, int(k) + 1 - int(y.size()));
         i < x.size() && i <= k; ++i) {
        if (first) {
            dst = x[i].array() * y[k - i].array();
            first = false;
        } else {
            dst.array() += x[i].array() * y[k - i].array();
        }
    }
    if (first) {
        dst.setZero();
    }
}

class EigenVecArrStorage : public ObjArray<EigenVec> {
public:
    explicit EigenVecArrStorage(size_t size)
            : ObjArray<EigenVec>(size, nullptr, 0, 1) {}
};

/*!
 * \brief Compute a single term in the expansion of determinant
 *
 * The result is the product of mat[i][row_indices[i]]
 * Result is negated if row_indices[0]<0 or row_indices[1]<0
 *
 * \param coeffs_trans transposed coefficients in (m, m, batch) shape
 * \param k target term order
 */
class DetSingleTermCompute {
    TensorArray m_coeffs_trans;
    const size_t m_batch;
    const size_t m_k;
    std::unique_ptr<fp_t[]> m_buf_storage;
    TensorND m_ret;
    EigenVecArrStorage m_buf0, m_buf1, m_opr0, m_opr1;

public:
    DetSingleTermCompute(const TensorArray& coeffs, size_t k)
            : m_coeffs_trans{transpose_coeffs(coeffs)},
              m_batch{m_coeffs_trans[0].shape(2)},
              m_k{k},
              m_buf_storage{new fp_t[2 * (k + 1) * m_batch]},
              m_ret{TensorShape{m_batch, 1}},
              m_buf0{k + 1},
              m_buf1{k + 1},
              m_opr0{coeffs.size()},
              m_opr1{coeffs.size()} {
        auto ptr = m_buf_storage.get();
        for (size_t i = 0; i <= k; ++i) {
            reset(m_buf0[i], ptr + (i * 2) * m_batch, m_batch);
            reset(m_buf1[i], ptr + (i * 2 + 1) * m_batch, m_batch);
        }
    }

    TensorND operator()(std::span<const int> row_indices) {
        auto extract = [&row_indices, this, msize = m_coeffs_trans[0].shape(1)](
                               EigenVecArr dst, int r) {
            for (size_t i = 0; i < m_coeffs_trans.size(); ++i) {
                size_t c = std::abs(row_indices[r]);
                reset(dst[i],
                      m_coeffs_trans[i].ptr() + (r * msize + c) * m_batch,
                      m_batch);
            }
        };

        auto conv_ret = [this, &row_indices](EigenVecArr x, EigenVecArr y) {
            auto ret_vec = as_vector_w(m_ret);
            conv_k(m_k, ret_vec, x, y);
            if (row_indices[0] < 0 || row_indices[1] < 0) {
                m_ret.inplace_neg();
            }
            return m_ret;
        };

        extract(m_opr0, 0);
        extract(m_opr1, 1);
        if (row_indices.size() == 2) {
            return conv_ret(m_opr0, m_opr1);
        }

        EigenVecArrStorage *prod = &m_buf0, *prod_next = &m_buf1;
        conv(*prod, m_opr0, m_opr1);

        for (size_t i = 2; i + 1 < row_indices.size(); ++i) {
            extract(m_opr0, i);
            conv(*prod_next, *prod, m_opr0);
            std::swap(prod, prod_next);
        }
        extract(m_opr0, row_indices.size() - 1);
        return conv_ret(*prod, m_opr0);
    }
};

/*!
 * \brief get the terms in the expansion of determinant
 * \return vector of size m! * m; the sign of items i*m are the sign of the
 *      terms
 */
const std::vector<int>& get_det_terms(size_t m) {
    static std::mutex mutex;
    static std::vector<std::vector<int>> results{std::vector<int>{0}};
    static auto compute = [&](int size) {
        sanm_assert(results.size() == static_cast<size_t>(size - 1));
        std::vector<int>& cur = results.emplace_back();
        const auto& prev = results[size - 2];
        for (int i = 0; i < size; ++i) {
            for (size_t j = 0; j < prev.size();) {
                size_t r0 = cur.size();
                bool neg = i % 2;
                cur.push_back(i);
                for (int jdt = 0; jdt < size - 1; ++jdt) {
                    int p = prev[j + jdt];
                    if (p < 0) {
                        neg = !neg;
                        p = -p;
                    }
                    cur.push_back(p + (p >= i));
                }
                if (neg) {
                    if (cur[r0]) {
                        cur[r0] = -cur[r0];
                    } else {
                        cur[r0 + 1] = -cur[r0 + 1];
                    }
                }
                j += size - 1;
            }
        }
#if 0
        printf("det(%d):\n", size);
        for (size_t i = 0; i < cur.size(); ++i) {
            printf("%d", cur[i]);
            if ((i + 1) % size == 0) {
                printf("\n");
            } else {
                printf(" ");
            }
        }
#endif
    };

    sanm_assert(m >= 1);
    std::lock_guard<std::mutex> mutex_lg{mutex};
    if (m - 1 < results.size()) {
        return results[m - 1];
    }
    for (size_t i = results.size() + 1; i <= m; ++i) {
        compute(i);
    }
    return results[m - 1];
}

TensorND compute_polymat_det_coeff_by_expanding(const TensorArray& coeffs,
                                                size_t target_order) {
    DetSingleTermCompute tc{coeffs, target_order};
    size_t m = coeffs[0].shape(1);
    const auto& terms = get_det_terms(m);

    TensorND ret;
    for (size_t i = 0; i < terms.size(); i += m) {
        TensorND cur = tc({terms.data() + i, m});
        if (!i) {
            ret = cur;
        } else {
            ret += cur;
        }
    }
    return ret;
}
}  // anonymous namespace

TensorND sanm::compute_polymat_det_coeff(const TensorArray& coeffs,
                                         size_t order) {
    SANM_SCOPED_PROFILER("polymat_det");
    sanm_assert(!coeffs.empty() && coeffs[0].rank() == 3 &&
                coeffs[0].shape(1) == coeffs[0].shape(2));
    for (size_t i = 1; i < coeffs.size(); ++i) {
        sanm_assert(coeffs[i].shape() == coeffs[0].shape());
    }

    const size_t batch = coeffs[0].shape(0), mdim = coeffs[0].shape(1),
                 nr_term = (coeffs.size() - 1) * mdim + 1;
    sanm_assert(mdim >= 2);

    if (order >= nr_term) {
        return TensorND{TensorShape{batch, 1}}.fill_with_inplace(0);
    }
    if (order == 0) {
        return coeffs[0].batched_determinant();
    }
    if (order == 1) {
        TensorND ret{TensorShape{batch, 1}},
                src = coeffs[0].batched_cofactor() * coeffs[1];
        EigenMatDyn smat{const_cast<fp_t*>(src.ptr()),
                         static_cast<Eigen::Index>(mdim * mdim),
                         static_cast<Eigen::Index>(batch)};
        as_vector_w(ret) = smat.colwise().sum().transpose();
        return ret;
    }

    if (mdim <= 4) {
        return compute_polymat_det_coeff_by_expanding(coeffs, order);
    }

    return compute_polymat_det_coeff_with_fft(coeffs, next_pow2(nr_term),
                                              order);
}


/*=========================================================================

  Program:   Small Body Geophysical Analysis
  Module:    SBGATSphericalHarmo.hpp

  Class derived from VTK's vtkPolyDataAlgorithm by Benjamin Bercovici

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
/**
 @class  SBGATSphericalHarmo
 @author Benjamin Bercovici
 @author Jay McMahon
@date October 2018
 @brief  Computes/evaluates the outer spherical harmonics expansion of the exterior gravity
 field around a constant density polyhedron

 @details  Computes/evaluates the outer spherical harmonics expansion of the exterior gravity
field around a constant density polyhedron. Normalized or non-normalized coefficients can be computed.
The computed coefficients are completely independent of the mass and density of the considered object
as they are only a geometric construct, thanks to the constant-density assumption. This class will always use results expressed in `meters` as their distance unit (e.g accelerations in m/s^2, potentials in m^2/s^2,...) . Unit consistency is enforced through the use of the SetScaleMeters()
and SetScaleKiloMeters() method.

Adapted from the works of Yu Takahashi and Siamak Hesar by Benjamin Bercovici, University of Colorado Boulder
for more details, see
Werner, R. a. (1997).
Spherical harmonic coefficients for the potential of a constant-density polyhedron.
Computers & Geosciences,
23(10),
1071–1077.
https://doi.org/10.1016/S0098-3004(97)00110-6
@copyright MIT License, Benjamin Bercovici and Jay McMahon
*/

#ifndef SBGATSphericalHarmo_h
#define SBGATSphericalHarmo_h

#include <vtkFiltersCoreModule.h> // For export macro
#include <vtkPolyDataAlgorithm.h>
#include <armadillo>

class VTKFILTERSCORE_EXPORT SBGATSphericalHarmo : public vtkPolyDataAlgorithm{
public:
  /**
   * Constructs with initial values of zero.
   */
  static SBGATSphericalHarmo *New();

  vtkTypeMacro(SBGATSphericalHarmo,vtkPolyDataAlgorithm);
  void PrintSelf(std::ostream& os, vtkIndent indent) override;
  void PrintHeader(std::ostream& os, vtkIndent indent) override;
  void PrintTrailer(std::ostream& os, vtkIndent indent) override;

  /**
  Sets degree of spherical harmonics expansion
  @param deg degree of spherical harmonics expansion
  */
  void SetDegree(const unsigned int deg){
    this -> degree = deg;
    this -> degreeSet = true;
  }

  /**
  Sets reference radius in spherical harmonics expansion. Must be consistent
  with the units in which the shape coordinates are expressed
  @param ref_radius reference radius in spherical harmonics expansion
  */
  void SetReferenceRadius(const double ref_radius){
    this -> referenceRadius = ref_radius;
    this -> referenceRadiusSet = true;
  }

  /**
  Sets polyhedron density
  @param density bulk density of polyhedron (kg/m^3)
  */
  void SetDensity(const double density){
    this -> density = density;
    this -> densitySet = true;
  }

  /*
  Will return normalized coefficients next (default is normalized)
  */
  void IsNormalized(){
    this -> normalized = true;
  }

  /*
  Will return non-normalized coefficients next (default is normalized)
  */
  void IsNonNormalized(){
    this -> normalized = false;
  }

  /*
  Return Cnm array of coefficients ordered like in the exemple below where degree = 5

    1     0     0     0    0      0
  C_10  C_11  C_12  C_13  C_14  C_15
  C_20  C_21  C_22  C_23  C_24  C_25
  C_30  C_31  C_32  C_33  C_34  C_35
  C_40  C_41  C_42  C_43  C_44  C_45
  C_50  C_51  C_52  C_53  C_54  C_55

  @return Cnm array of coefficients
  */
  arma::mat GetCnm() {this -> Update(); return this -> Cnm;}

  /*
  Return Cnm array of coefficients ordered like in the exemple below where degree = 5

    1     0     0     0    0      0
  C_10  C_11  C_12  C_13  C_14  C_15
  C_20  C_21  C_22  C_23  C_24  C_25
  C_30  C_31  C_32  C_33  C_34  C_35
  C_40  C_41  C_42  C_43  C_44  C_45
  C_50  C_51  C_52  C_53  C_54  C_55

  @param[out] Cnm array of coefficients
  */
  void GetCnm(arma::mat & C_nm) {this -> Update(); C_nm = this -> Cnm;}

  /*

  Return Snm array of coefficients ordered like in the exemple below where degree = 5

    0     0     0     0    0      0
    0  S_11  S_12  S_13  S_14  S_15
    0  S_21  S_22  S_23  S_24  S_25
    0  S_31  S_32  S_33  S_34  S_35
    0  S_41  S_42  S_43  S_44  S_45
    0  S_51  S_52  S_53  S_54  S_55

  @return Snm array of coefficients
  */
  arma::mat GetSnm() {this -> Update(); return this -> Snm;}

  /*
  Return Snm array of coefficients ordered like in the exemple below where degree = 5

    0     0     0     0    0      0
    0  S_11  S_12  S_13  S_14  S_15
    0  S_21  S_22  S_23  S_24  S_25
    0  S_31  S_32  S_33  S_34  S_35
    0  S_41  S_42  S_43  S_44  S_45
    0  S_51  S_52  S_53  S_54  S_55

  @param[out] Snm array of coefficients
  */
  void GetSnm(arma::mat & S_nm) {this -> Update(); S_nm = this -> Snm;}

  /**
  Return the acceleration due to gravity at the specified point
  @param pos position at which the acceleration must be evaluated (meters)
  @return acceleration (m / s ^ 2)
  */
  arma::vec::fixed<3> GetAcceleration(const arma::vec::fixed<3> & pos);

  /**
  Evaluates the gravity gradient matrix (the partial derivative of the spherical
  harmonics acceleration with respect to the position vector) at the prescribed
  location. Note that this gravity gradient matrix is expressed in the body-fixed frame of the considered object.
  @param[in] pos position at which the gravity gradient matrix must be evaluated, expressed in the same frame/same unit L as
  the shape used to build the spherical harmonics expansion.
  @param[out] dAccdPos container holding the gravity gradient matrix (1 / s ^ 2)
  */
  void GetGravityGradientMatrix(const arma::vec::fixed<3> & pos,
    arma::mat::fixed<3,3> & dAccdPos);

  /**
  Evaluates the partial derivative of the spherical harmonics acceleration
  with respect to the spherical harmonics coefficients. The coefficients and the partials of the acceleration w/r to the coefficients
  are ordered like so:

  Cnm :
(C_00 == 1)  0     0    0     0     0
  C_10     C_11    0    0     0     0
  C_20     C_21  C_22   0     0     0
  C_30     C_31  C_32  C_33   0     0
  C_40     C_41  C_42  C_43  C_44   0
  C_50     C_51  C_52  C_53  C_54  C_55

  Snm :
  0    0     0     0    0      0
  0  S_11    0     0    0      0
  0  S_21  S_22    0    0      0
  0  S_31  S_32  S_33   0      0
  0  S_41  S_42  S_43  S_44    0
  0  S_51  S_52  S_53  S_54  S_55

  so

  partial_C = [ dA/C_00,dA/C_10,dA/C_11,dA/C_20,dA/C_21,dA/C_22,...]
  partial_S = [ dA/S_11,dA/C_21,dA/S_22,dA/S_31,dA/S_32,dA/S_33,...]

  @param[in] pos position at which the partial derivatives must be evaluated, expressed in the same frame/same unit as
  the shape used to build the spherical harmonics expansion.
  @param[out] partial_C container holding the partial derivative of the acceleration
  with respect to the Cnm spherical harmonic coefficients. If the degree/order is n, then there are (n+1) * (n + 2)/2 non-zero Cnm coefficients

  @param[out] partial_S container holding the partial derivative of the acceleration
  with respect to the Snm spherical harmonic coefficients. If the degree/order is n, then there are (n+1) * (n + 2)/2 - n non-zero Snm coefficients

  */
  void GetPartialHarmonics(const arma::vec::fixed<3> & pos,
    arma::mat & partial_C,
    arma::mat & partial_S);
  /**
  Sets the scale factor to 1, indicative that the polydata has its coordinates expressed in meters
  */
  void SetScaleMeters() { this -> scaleFactor = 1; this -> scaleFactorSet = true;}

  /**
  Sets the scale factor to 1000, indicative that the polydata has its coordinates expressed in kilometers
  */
  void SetScaleKiloMeters() { this -> scaleFactor = 1000; this -> scaleFactorSet = true;}

  /**
  Exports the computed spherical harmonics expansion to
  a JSON file. The saved fields are:
  - facets == number of facets
  - vertices == number of vertices
  - totalMass : {value, unit}
  - density : {value, unit}
  - reference_radius : {value, unit}
  - normalized == true if the coefficients are normalized
  - degree == degree of the spherical expansion
  - Cnm_coefs - vector of coefficients triplets {n,m,Cnm}
  - Snm_coefs - vector of coefficients triplets {n,m,Snm}
  @param path JSON file where the spherical harmonics model will be saved

  */
  void SaveToJson(std::string path) const;

  /**
  Loads a previously computed spherical harmonics expansion
  from a JSON file. Will set the appropriate fields in the SBGATSphericalHarmo object to
  allow calls to other methods.
  The loadable fields are:
  - facets == number of facets (not needed for evaluation)
  - vertices == number of vertices (not needed for evaluation)
  - totalMass : {value, unit}
  - density : {value, unit}
  - reference_radius : {value, unit}
  - normalized == true if the coefficients are normalized
  - degree == degree of the spherical expansion
  - Cnm_coefs - vector of coefficients triplets {n,m,Cnm}
  - Snm_coefs - vector of coefficients triplets {n,m,Snm}
  @param path JSON file storing the spherical harmonics model
  */
  void LoadFromJson(std::string path);

  /**
  Sets the Cnm coefficients. There is normally no need to use this method outside of Sbgat's tests
  @param[in] Cnm coefficients
  */
  void SetCnm(arma::mat Cnm){this ->Cnm =Cnm;}

  /**
  Sets the Snm coefficients. There is normally no need to use this method outside of Sbgat's tests
  @param[in] Snm coefficients
  */
  void SetSnm(arma::mat Snm){this ->Snm =Snm;}

protected:
  SBGATSphericalHarmo();
  ~SBGATSphericalHarmo() override;

  int RequestData(vtkInformation* request,
    vtkInformationVector** inputVector,
    vtkInformationVector* outputVector) override;

  arma::mat Cnm;
  arma::mat Snm;

  double referenceRadius;
  double density;
  double totalMass;
  double scaleFactor = 1;

  bool normalized;
  unsigned int degree;

  int n_facets;
  int n_vertices;

  bool degreeSet;
  bool densitySet;
  bool referenceRadiusSet;
  bool scaleFactorSet;
  bool setFromJSON;

private:
  SBGATSphericalHarmo(const SBGATSphericalHarmo&) = delete;
  void operator=(const SBGATSphericalHarmo&) = delete;
};

#endif


#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <complex>
#include "larsson_iccv19.h"
#include "kukelova_iccv13.h"
#include "../misc/univariate.h"
#include "../misc/distortion.h"

using namespace Eigen;
using namespace radialpose;
using std::complex;

static const double SMALL_NUMBER = 1e-8;
static const double DAMP_FACTOR = 1e-8;

static void linsolve_known_pose_dist(const Points2D &x, const Points3D &X, double t3, int Np, int Nd, double damp, Camera* camera)
{
	int n_pts = x.cols();
	int n_param = 1 + Np + Nd;

	int n_rows = n_pts;
	if (damp > 0)
		n_rows += n_param - 1;

	// System matrix for normal equations
	Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> A(n_rows, n_param);
	Eigen::Matrix<double, Eigen::Dynamic, 1> b(n_rows, 1);
	A.setZero();  b.setZero();

	double r_pow[4];
	for (int i = 0; i < n_pts; i++) {

		double uu, uv;
		if (std::abs(x(0, i)) < SMALL_NUMBER) {
			uv = x(1, i);
			uu = X(1, i) / (t3 + X(2, i));
		}
		else {
			uv = x(0, i);
			uu = X(0, i) / (t3 + X(2, i));
		}

		//double rd2 = x(0, i) * x(0, i) + x(1, i) * x(1, i);
		double ru2 = (X(0, i) * X(0, i) + X(1, i) * X(1, i)) / ((X(2, i) + t3) * (X(2, i) + t3));

		// compute powers
		r_pow[0] = ru2;            // r^2
		r_pow[1] = ru2 * ru2;      // r^4
		r_pow[2] = ru2 * r_pow[1]; // r^6
		r_pow[3] = ru2 * r_pow[2]; // r^8

		A(i, 0) = uu;
		for (int k = 0; k < Np; k++)
			A(i, 1 + k) = r_pow[k] * uu;
		for (int k = 0; k < Nd; k++)
			A(i, 1 + Np + k) = -uv * r_pow[k];
		b(i) = uv;
	}
	if (damp > 0) {
		for (int i = 1; i < n_param; i++)
			A(n_pts - 1 + i, i) += damp;
	}

	//Eigen::JacobiSVD<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>> svd(A);
	//std::cout << "A: " << A << "\n";
	//std::cout << "svd(A): " << svd.singularValues() << "\n";

	Eigen::Matrix<double, Eigen::Dynamic, 1> sol = A.fullPivHouseholderQr().solve(b);
	camera->focal = sol(0);
	for (int i = 1; i < n_param; i++) {
		if (i <= Np)
			camera->dist_params.push_back(sol(i) / camera->focal);
		else
			camera->dist_params.push_back(sol(i));
	}
}

static void linsolve_known_pose_undist(const Points2D &x, const Points3D &X, double t3, int Np, int Nd, double damp, Camera* camera)
{
	int n_pts = x.cols();
	int n_param = 1 + Np + Nd;

	int n_rows = n_pts;
	if (damp > 0)
		n_rows += n_param - 1;

	// System matrix for normal equations
	Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> A(n_rows, n_param);
	Eigen::Matrix<double, Eigen::Dynamic, 1> b(n_rows, 1);
	A.setZero();  b.setZero();

	double r_pow[4];
	for (int i = 0; i < n_pts; i++) {

		double uu, uv;
		if (std::abs(x(0, i)) < SMALL_NUMBER) {
			uv = x(1, i);
			uu = X(1, i) / (t3 + X(2, i));
		} else {
			uv = x(0, i);
			uu = X(0, i) / (t3 + X(2, i));
		}

		double rd2 = x(0, i) * x(0, i) + x(1, i) * x(1, i);

		// compute powers
		r_pow[0] = rd2;            // r^2
		r_pow[1] = rd2 * rd2;      // r^4
		r_pow[2] = rd2 * r_pow[1]; // r^6
		r_pow[3] = rd2 * r_pow[2]; // r^8

		A(i, 0) = uu;
		for (int k = 0; k < Np; k++)
			A(i, 1 + k) = -r_pow[k] * uv;
		for (int k = 0; k < Nd; k++)
			A(i, 1 + Np + k) = uu * r_pow[k];
		b(i) = uv;
	}
	if (damp > 0) {
		for (int i = 1; i < n_param; i++)
			A(n_pts - 1 + i, i) += damp;
	}

	Eigen::Matrix<double, Eigen::Dynamic, 1> sol = A.fullPivHouseholderQr().solve(b);
	camera->focal = sol(0);
	double f2 = camera->focal * camera->focal;
	double f2k = f2;
	// We have the real mu_k = sol * f^2k and lambda_k = sol * f^2k-1
	for (int i = 1; i < n_param; i++) {
		if (i == Np+1)
			f2k = camera->focal;
		camera->dist_params.push_back(sol(i) * f2k);
		f2k = f2k * f2;
	}
}

// Small refinement on the minimal sample. TODO: Move this to refinement.cc
template<int Np, int Nd>
void radial_refinement_dist(Camera &p, const Points2D &x, const Points3D &X, double damp_factor) {
	// It is assumed that X is already rotated by p
	constexpr int n_pts = std::max(2 + Np + Nd, 5);

	Matrix<double, 2 * n_pts, 2 + Np + Nd> J;
	Matrix<double, 2 * n_pts, 1> res;
	Matrix<double, 2 + Np + Nd, 1> dx;

	for (int i = 0; i < Np; ++i)
		p.dist_params[i] *= p.focal;

	for (int iter = 0; iter < 5; ++iter) {
		for (int i = 0; i < n_pts; ++i) {
			double d = X(2, i) + p.t(2);
			double d2 = d * d;
			double r2 = X.block<2, 1>(0, i).squaredNorm();

			double num = p.focal;
			double denom = d;

			double dnum_dt = 0.0;
			double ddenom_dt = 1.0;

			for (int k = 0; k < Np; k++) {
				double r2d2 = std::pow(r2 / d2, k + 1);

				num += p.dist_params[k] * r2d2;
				dnum_dt += - p.dist_params[k] * 2 * (k + 1) * r2d2 / d;

			}
			for (int k = 0; k < Nd; k++) {
				double r2d2 = std::pow(r2 / d2, k + 1);

				denom += d * p.dist_params[Np + k] * r2d2;
				ddenom_dt += -p.dist_params[Np + k] * (2 * k + 1)*r2d2;
			}

			double factor = num / denom;
			double dfactor_dt = (dnum_dt*denom - num * ddenom_dt) / (denom*denom);
			double dfactor_df = 1 / denom;

			res(2 * i + 0) = factor * X(0, i) - x(0, i);
			res(2 * i + 1) = factor * X(1, i) - x(1, i);

			J(2 * i + 0, 0) = dfactor_dt * X(0, i);
			J(2 * i + 1, 0) = dfactor_dt * X(1, i);
			J(2 * i + 0, 1) = dfactor_df * X(0, i);
			J(2 * i + 1, 1) = dfactor_df * X(1, i);

			for (int k = 0; k < Np; ++k) {
				double r2d2 = std::pow(r2 / d2, k + 1);
				double dfactor_dmu = r2d2 / denom;

				J(2 * i + 0, 2 + k) = dfactor_dmu * X(0, i);
				J(2 * i + 1, 2 + k) = dfactor_dmu * X(1, i);
			}

			for (int k = 0; k < Nd; ++k) {
				double r2d2 = std::pow(r2 / d2, k + 1);
				double dfactor_dlambda = -d * r2d2 * num / (denom*denom);
				J(2 * i + 0, 2 + Np + k) = dfactor_dlambda * X(0, i);
				J(2 * i + 1, 2 + Np + k) = dfactor_dlambda * X(1, i);
			}

		}

		if (res.norm() < SMALL_NUMBER)
			break;

		//std::cout << "res = " << res << "\n";
		//std::cout << "jac = " << J << "\n";

		Matrix<double, 2 + Np + Nd, 2 + Np + Nd> H = J.transpose()*J;
		H.diagonal().array() += 1e-6; // LM dampening
		Matrix<double, 2 + Np + Nd, 1> g = -J.transpose()*res;

		if (Nd > 0 && Np > 0 && damp_factor > 0) {
			// For rational models we add a small dampening factor
			H.template block<Np + Nd,Np + Nd>(2, 2).diagonal().array() += damp_factor;
			for(int i = 0; i < Np+Nd; i++)
				g(2+i) -= damp_factor * p.dist_params[i];
		}

		dx = H.ldlt().solve(g);

		p.t(2) += dx(0);
		p.focal += dx(1);
		for (int i = 0; i < Np; ++i)
			p.dist_params[i] += dx(2 + i);
		for (int i = 0; i < Nd; ++i)
			p.dist_params[i + Np] += dx(Np + 2 + i);

		if (dx.array().abs().maxCoeff() < SMALL_NUMBER)
			break;
	}

	for (int i = 0; i < Np; ++i)
		p.dist_params[i] /= p.focal;
}

template<int Np, int Nd>
void radial_refinement_undist(Camera &p, const Points2D &x, const Points3D &X, double damp_factor) {
	// It is assumed that X is already rotated by p
	constexpr int n_pts = std::max(2 + Np + Nd, 5);

	Matrix<double, 2 * n_pts, 2 + Np + Nd> J;
	Matrix<double, 2 * n_pts, 1> res;
	Matrix<double, 2 + Np + Nd, 1> dx;

	// Change of variables
	double f2 = p.focal * p.focal;
	double f2k = f2;
	for (int i = 0; i < Np; ++i) {
		p.dist_params[i] /= f2k;
		f2k *= f2;
	}
	f2k = p.focal;
	for (int i = 0; i < Nd; ++i) {
		p.dist_params[Np + i] /= f2k;
		f2k *= f2;
	}

	double r_pow[3];

	for (int iter = 0; iter < 5; ++iter) {
		for (int i = 0; i < n_pts; ++i) {
			double d = X(2, i) + p.t(2);
			double r2 = x.col(i).squaredNorm();

			double num = 1.0;
			double denom = p.focal;

			r_pow[0] = r2; // 2
			r_pow[1] = r2 * r2; // 4
			r_pow[2] = r2 * r_pow[1]; // 6

			for (int k = 0; k < Np; k++) {
				num += p.dist_params[k] * r_pow[k];
			}
			for (int k = 0; k < Nd; k++) {
				denom += p.dist_params[Np + k] * r_pow[k];
			}

			double factor = num / denom;

			double dfactor_df = -factor / denom;

			res(2 * i + 0) = factor * x(0, i) - X(0, i) / d;
			res(2 * i + 1) = factor * x(1, i) - X(1, i) / d;

			J(2 * i + 0, 0) = X(0, i) / (d*d); // t
			J(2 * i + 1, 0) = X(1, i) / (d*d);
			J(2 * i + 0, 1) = dfactor_df * x(0, i);
			J(2 * i + 1, 1) = dfactor_df * x(1, i);

			for (int k = 0; k < Np; ++k) {
				double dfactor_dmu = r_pow[k] / denom;
				J(2 * i + 0, 2 + k) = dfactor_dmu * x(0, i);
				J(2 * i + 1, 2 + k) = dfactor_dmu * x(1, i);
			}
			for (int k = 0; k < Nd; ++k) {
				double dfactor_dlambda = -r_pow[k] * num / (denom*denom);
				J(2 * i + 0, 2 + Np + k) = dfactor_dlambda * x(0, i);
				J(2 * i + 1, 2 + Np + k) = dfactor_dlambda * x(1, i);
			}
		}

		//std::cout << "res = " << res << "\n";
		//std::cout << "jac = " << J << "\n";
		if (res.norm() < SMALL_NUMBER)
			break;

		Matrix<double, 2 + Np + Nd, 2 + Np + Nd> H = J.transpose()*J;
		H.diagonal().array() += 1e-6; // LM dampening
		Matrix<double, 2 + Np + Nd, 1> g = -J.transpose()*res;

		if (Nd > 0 && Np > 0 && damp_factor > 0) {
			// For rational models we add a small dampening factor
			H.template block<Np + Nd,Np + Nd>(2, 2).diagonal().array() += damp_factor;
			for (int i = 0; i < Np + Nd; i++)
				g(2 + i) -= damp_factor * p.dist_params[i];
		}

		dx = H.ldlt().solve(g);

		p.t(2) += dx(0);
		p.focal += dx(1);
		for (int i = 0; i < Np; ++i)
			p.dist_params[i] += dx(2 + i);
		for (int i = 0; i < Nd; ++i)
			p.dist_params[i + Np] += dx(Np + 2 + i);

		if (dx.array().abs().maxCoeff() < SMALL_NUMBER)
			break;
	}

	// Revert change of variables
	f2 = p.focal * p.focal;
	f2k = f2;
	for (int i = 0; i < Np; ++i) {
		p.dist_params[i] *= f2k;
		f2k *= f2;
	}
	f2k = p.focal;
	for (int i = 0; i < Nd; ++i) {
		p.dist_params[Np + i] *= f2k;
		f2k *= f2;
	}
}

inline double simple_preconditioner(const Points2D &x, const Points3D &X) {
	// Simple preconditioner using the first two points
	double nx1 = x.col(0).squaredNorm();
	double nx2 = x.col(1).squaredNorm();
	double sx1 = X.block<2, 1>(0, 0).dot(x.col(0));
	double sx2 = X.block<2, 1>(0, 1).dot(x.col(1));
	return (X(2, 1)*nx2*sx1 - X(2, 0)*nx1*sx2) / (nx1 * sx2 - nx2 * sx1);
}

template<int Np, int Nd, bool DistortionModel>
int radialpose::larsson_iccv19::Solver<Np,Nd,DistortionModel>::solve(const Points2D& image_points, const Points3D& world_points, std::vector<Camera>* poses) const
{

	std::vector<Camera> initial_poses;
	std::vector<double> t3;

	if (use_radial_solver) {
		kukelova_iccv13::Radial1DSolver::p5p_radial_impl(image_points, world_points, &initial_poses);
	} else {
		initial_poses.push_back(Camera(Matrix3d::Identity(), Vector3d::Zero()));
	}
	Matrix<double, 3, Dynamic> X;

	for (int k = 0; k < initial_poses.size(); k++) {
		t3.clear();

		X = initial_poses[k].R * world_points;
		X.colwise() += initial_poses[k].t;

		//std::cout << "Initial pose " << k + 1 << "/" << initial_poses.size() << "\n";
		//std::cout << "R=" << initial_poses[k].R << "\nt=" << initial_poses[k].t << "\n";
		//std::cout << "X=" << X << "\n";

		double t0 = 0;
		if (use_precond) {
			t0 = simple_preconditioner(image_points, X);
			X.row(2).array() += t0;
		}

		solver_impl(image_points, X, &t3);

		for (int i = 0; i < t3.size(); ++i) {
			Camera pose;
			pose.R = initial_poses[k].R;
			pose.t = initial_poses[k].t;
			pose.t(2) = t3[i];

			if (DistortionModel) {
				linsolve_known_pose_dist(image_points, X, t3[i], Np, Nd, damp_factor, &pose);
				if(root_refinement)
					radial_refinement_dist<Np, Nd>(pose, image_points, X, damp_factor);
			} else {
				linsolve_known_pose_undist(image_points, X, t3[i], Np, Nd, damp_factor, &pose);
				if(root_refinement)
					radial_refinement_undist<Np, Nd>(pose, image_points, X, damp_factor);
			}

			if (pose.focal < 0) {
				// flipped solution
				pose.focal = -pose.focal;
				pose.R.row(0) = -pose.R.row(0);
				pose.R.row(1) = -pose.R.row(1);
				pose.t(0) = -pose.t(0);
				pose.t(1) = -pose.t(1);
			}

			// Revert precond
			pose.t(2) += t0;

			//std::cout << "solution[" << i << "], t3=" << pose.t(2) << "\n";
			poses->push_back(pose);
		}
	}
	return poses->size();
}

// Template instantiations
template class radialpose::larsson_iccv19::Solver<1, 0, true>;
template class radialpose::larsson_iccv19::Solver<2, 0, true>;
template class radialpose::larsson_iccv19::Solver<3, 0, true>;
template class radialpose::larsson_iccv19::Solver<3, 3, true>;
template class radialpose::larsson_iccv19::Solver<1, 0, false>;
//template class radialpose::larsson_iccv19::Solver<2, 0, false>;
//template class radialpose::larsson_iccv19::Solver<3, 0, false>;
//template class radialpose::larsson_iccv19::Solver<3, 3, false>;

template class radialpose::PoseEstimator<radialpose::larsson_iccv19::Solver<1, 0, true>>;
template class radialpose::PoseEstimator<radialpose::larsson_iccv19::Solver<2, 0, true>>;
template class radialpose::PoseEstimator<radialpose::larsson_iccv19::Solver<3, 0, true>>;
template class radialpose::PoseEstimator<radialpose::larsson_iccv19::Solver<3, 3, true>>;
template class radialpose::PoseEstimator<radialpose::larsson_iccv19::Solver<1, 0, false>>;

/*
 This is broken?
// These are implemented in larsson_iccv19_impl.cc
extern template int radialpose::larsson_iccv19::Solver<1, 0, true>::solver_impl(Eigen::Matrix<double, 2, Eigen::Dynamic>, Eigen::Matrix<double, 3, Eigen::Dynamic>, std::vector<double>*);
extern template int radialpose::larsson_iccv19::Solver<2, 0, true>::solver_impl(Eigen::Matrix<double, 2, Eigen::Dynamic>, Eigen::Matrix<double, 3, Eigen::Dynamic>, std::vector<double>*);
extern template int radialpose::larsson_iccv19::Solver<3, 0, true>::solver_impl(Eigen::Matrix<double, 2, Eigen::Dynamic>, Eigen::Matrix<double, 3, Eigen::Dynamic>, std::vector<double>*);
extern template int radialpose::larsson_iccv19::Solver<3, 3, true>::solver_impl(Eigen::Matrix<double, 2, Eigen::Dynamic>, Eigen::Matrix<double, 3, Eigen::Dynamic>, std::vector<double>*);
extern template int radialpose::larsson_iccv19::Solver<1, 0, false>::solver_impl(Eigen::Matrix<double, 2, Eigen::Dynamic>, Eigen::Matrix<double, 3, Eigen::Dynamic>, std::vector<double>*);
*/


#include "mex.h"
#include <Eigen/Dense>
#include "re3q3/re3q3.h"

void printUsage() {
	mexPrintf("[x] = re3q3(C);\n");
	mexPrintf("    coefficient order is [ x^2, xy, xz, y^2, yz, z^2, x, y, z, 1.0 ]\n");
}

void mexFunction(int nlhs,mxArray *plhs[], int nrhs, const mxArray *prhs[]) {

	if (nrhs != 1) {
		printUsage();
		mexErrMsgTxt("Please, specify 3 x 10N coefficient matrix");
	}
	if (nlhs != 1) {
		printUsage();
		mexErrMsgTxt("Require one output argument.");
	}

	if ((mxGetM(prhs[0]) != 3) || (mxGetN(prhs[0]) % 10 != 0)) {
		printUsage();
		mexErrMsgTxt("One input 3 x 10N matrix is required.");
	}

	int n_instances = mxGetN(prhs[0]) / 10;

	double *data = mxGetPr(prhs[0]);

	Eigen::Matrix<double, 3, 10> coeffs;
	Eigen::Matrix<double, 3, 8> solutions;

	if (n_instances == 1) {
		// we are solving a single instance, output is 3xNsols
		coeffs = Eigen::Map<Eigen::Matrix<double, 3, 10>>(data);

		int n_sols = re3q3::re3q3(coeffs, &solutions);
		plhs[0] = mxCreateDoubleMatrix(3, n_sols, mxREAL);

		Eigen::Map<Eigen::MatrixXd> output_matrix = Eigen::Map<Eigen::MatrixXd>(mxGetPr(plhs[0]), 3, n_sols);
		output_matrix = solutions.block(0, 0, 3, n_sols);
	} else {
		// we are solving multiple instances, output is 3x8N
		plhs[0] = mxCreateDoubleMatrix(3, 8 * n_instances, mxREAL);
		double *output = mxGetPr(plhs[0]);

		Eigen::Map<Eigen::MatrixXd> output_matrix = Eigen::Map<Eigen::MatrixXd>(mxGetPr(plhs[0]), 3, 8*n_instances);
		output_matrix.setZero();

		for (int i = 0; i < n_instances; i++) {
			coeffs = Eigen::Map<Eigen::Matrix<double, 3, 10>>(data + 30 * i);
			solutions.setZero();
			int n_sols = re3q3::re3q3(coeffs, &solutions);
			output_matrix.block(0, i * 8, 3, n_sols) = solutions.block(0,0,3,n_sols);
		}
	}
}


#include <CGAL/Polyhedron_3.h>
#include <CGAL/Polyhedron_items_with_id_3.h>
#include <CGAL/boost/graph/graph_traits_Polyhedron_3.h>
#include <CGAL/Simple_cartesian.h>
#include <CGAL/extract_mean_curvature_flow_skeleton.h>
#include <CGAL/Polygon_mesh_processing/connected_components.h>
#include <CGAL/IO/Polyhedron_iostream.h>

#include <boost/foreach.hpp>

#include <fstream>
#include <set>
#include <queue>

typedef CGAL::Simple_cartesian<double>                               Kernel;
typedef Kernel::Point_3                                              Point;
typedef Kernel::Vector_3                                             Vector;
typedef CGAL::Polyhedron_3<Kernel, CGAL::Polyhedron_items_with_id_3> Polyhedron;
typedef boost::graph_traits<Polyhedron>::vertex_descriptor           vertex_descriptor;

typedef CGAL::Mean_curvature_flow_skeletonization<Polyhedron>        Mean_curvature_skeleton;
typedef Mean_curvature_skeleton::Skeleton                            Skeleton;
typedef boost::graph_traits<Skeleton>::vertex_descriptor             vertex_desc;
typedef boost::graph_traits<Skeleton>::edge_descriptor             edge_desc;

// The input of the skeletonization algorithm must be a pure triangular closed
// mesh and has only one component.
bool is_mesh_valid(Polyhedron& pMesh)
{
  if (!pMesh.is_closed())
  {
    std::cerr << "The mesh is not closed.";
    return false;
  }
  if (!pMesh.is_pure_triangle())
  {
    std::cerr << "The mesh is not a pure triangle mesh.";
    return false;
  }

  // the algorithm is only applicable on a mesh
  // that has only one connected component
  std::size_t num_component;
  CGAL::Counting_output_iterator output_it(&num_component);
  CGAL::internal::corefinement::extract_connected_components(pMesh, output_it);
  ++output_it;
  if (num_component != 1)
  {
    std::cerr << "The mesh is not a single closed mesh. It has "
              << num_component << " components.";
    return false;
  }
  return true;
}

int main()
{
  Polyhedron mesh;
  std::ifstream input("data/elephant.off");

  if ( !input || !(input >> mesh) || mesh.empty() ) {
    std::cerr << "Cannot open data/elephant.off" << std::endl;
    return EXIT_FAILURE;
  }
  if (!is_mesh_valid(mesh)) {
    return EXIT_FAILURE;
  }

  Skeleton skeleton;

  CGAL::extract_mean_curvature_flow_skeleton(mesh, skeleton);

  if (num_vertices(skeleton) == 0)
  {
    std::cerr << "The number of skeletal points is zero!\n";
    return EXIT_FAILURE;
  }

// check all vertices are seen exactly once
{
  std::set<vertex_descriptor> visited;
  BOOST_FOREACH(vertex_desc v, vertices(skeleton))
  {
    BOOST_FOREACH(vertex_descriptor vd, skeleton[v].vertices)
      if (!visited.insert(vd).second)
      {
        std::cerr << "A vertex was seen twice!\n";
        return EXIT_FAILURE;
      }
  }

  BOOST_FOREACH(vertex_descriptor vd, vertices(mesh))
  {
    if (!visited.count(vd))
    {
      std::cerr << "A vertex was not seen!\n";
      return EXIT_FAILURE;
    }
  }
}

// check the skeleton is connected
{
  std::queue<vertex_desc> qu;
  std::set<vertex_desc> visited;

  qu.push(*vertices(skeleton).first);
  visited.insert(qu.back());

  while (!qu.empty())
  {
    vertex_desc cur = qu.front();
    qu.pop();

    BOOST_FOREACH(edge_desc ed, in_edges(cur, skeleton))
    {
      vertex_desc next = source(ed, skeleton);
      if (visited.insert(next).second)
        qu.push(next);
    }
  }

  BOOST_FOREACH(vertex_desc vd, vertices(skeleton))
  {
    if (!visited.count(vd))
    {
      std::cerr << "Skeleton curve is not fully connected!\n";
      return EXIT_FAILURE;
    }
  }
}
  std::cout << "Pass connectivity test.\n";
  return EXIT_SUCCESS;
}


/******************************************************
    Author : shipeng_liu
    Email : 1196075299@qq.com
    Description: Adaptive dynamic programming
******************************************************/
#include <cmath>
#include<iostream>
#include <fstream>
#include <iomanip>
#include <Eigen/Dense>
#include "rambot_controller/src/controller/adp_learning/critic_network.cpp"
using namespace std;

/******************************************************************
    Action Network
    Input: S, diff_s
    number of hidden_layer: 1
    number of active_function: hidden_number
    output: steer_cmd
    parameter_layer1: parameters from input to hidden_layer
    parameter_layer2: parameters from hidden_layer to output
******************************************************************/

class action_network {

  private:

    double learning_rate = 0.00001 ; //learning rate
    double reward_count = 1;
    Eigen::MatrixXd parameter_layer1;
    Eigen::MatrixXd parameter_layer2;
    Eigen::MatrixXd input_variable;
    Eigen::MatrixXd hidden_layer;
    Eigen::MatrixXd p_hidden_layer;
    double output;
    int hidden_number;
    double deriva_J_u;
    static double active_function(double input) {
        // 1 - exp( - qi(t) ) / 1 + exp( - qi(t) ) )
        double ret = std::exp(- input);
        double tmp = (2)/(1 + ret) - 1;
        return tmp;
    }

  public:

    action_network()
    {
        hidden_number = 4;
        init();
    }

    void init()
    {
        parameter_layer1 = Eigen::MatrixXd::Random(2, hidden_number);
        parameter_layer2 = Eigen::MatrixXd::Random(hidden_number, 1);

        hidden_layer = Eigen::MatrixXd::Random(1, hidden_number);
        p_hidden_layer = Eigen::MatrixXd::Random(1, hidden_number);
        input_variable = Eigen::MatrixXd::Random(1,2);
        output = 0;

    }

    double output_cmd(double s, double diff_s)
    {
        input_variable(0,0) = s;
        input_variable(0,1) = diff_s;
        //cout << parameter_layer1 << endl;
        //cout << parameter_layer2 << endl;
        hidden_layer = input_variable * parameter_layer1;

        for (int i = 0; i < hidden_number; i++)
        {
            p_hidden_layer(0,i) = active_function(hidden_layer(0,i));
            // limit the p_Hidden_layer
            if (p_hidden_layer(0,i) < 0.000001 && p_hidden_layer(0,i) > 0 )
                p_hidden_layer(0,i) = 0.000001;
            else if (p_hidden_layer(0,i) > -0.000001 && p_hidden_layer(0,i) < 0)
                p_hidden_layer(0,i) = - 0.000001;
        }
        Eigen::MatrixXd test = p_hidden_layer * parameter_layer2;
        output = test(0,0);
        output = active_function(output);
        return output;
    }

    void update_weight(double J_cost, critic_network *critic_network)
    {

        double reference_signal = 0;
        double eat = J_cost - reference_signal;
        cout << "\naction_network_information:\n";
        cout << left << setw(20) << "[ refer: " << right
             << setw(20) << reference_signal << "]" << endl;
        cout << left << setw(20) << "[ J_cost: " << right
             << setw(20) << J_cost << "]" << endl;
        cout << left << setw(20) << "[ eat: " << right
             << setw(20) << eat << "]" << endl;
        cout << left << setw(20) << "[ p_hidden_layer: " << right
             << setw(20) << p_hidden_layer << "]" << endl;
        cout << "[ p_hidden_layer: " << endl;
        cout << p_hidden_layer << endl;

        // cout <<  "[ parameter_layer1: " << endl;
        // cout << parameter_layer1 << endl;

        // cout << "[ parameter_layer2: " << endl;
        // cout << parameter_layer2 << endl;

        // compute the derivative of J to U
        for (int l = 0; l < 4; l++)
        {
            deriva_J_u += ( critic_network->return_parameter_layer2()(l,0) ) * 0.5 * (1 - pow(critic_network->return_p_hidden_layer()(0,l),2)) * critic_network->return_parameter_layer1()(2,l);
        }
        for (int j = 0; j < hidden_number; j++)
        {
            //compute the parameter_layer1
            for (int k = 0; k < 2; k++)
            {

                double derivative_1 = reward_count * eat *  0.5 * (1-pow(output,2)) * parameter_layer2(j,0) * 0.5 * (1 - pow(p_hidden_layer(0,j),2)) * input_variable(0,k) * deriva_J_u;
                double delta_weight1 = learning_rate * (- derivative_1);

                parameter_layer1(k,j) += delta_weight1;
            }

            // compute the parameter_layer2

            double derivative =  reward_count * eat * 0.5 * (1-pow(output,2)) *(p_hidden_layer(0,j)) * deriva_J_u;

            double delta_weight = learning_rate * (- derivative);
            parameter_layer2(j,0) += delta_weight;

        }

    }
};


#pragma once

#include <Eigen/Core>
#include <Eigen/Dense>
#include <vector>
#include <random>
#include <numeric>
#include <iostream>

using namespace std;
using namespace Eigen;

template <typename state>
struct ParticleFilter{
    ParticleFilter(int number_of_particles, int states, function<void (int id)> process, function<double (int id)> cost,
                   VectorXd &start, double std):
            number_of_particles(number_of_particles), process(process), cost(cost), start(start){
        particles.resize(number_of_particles, VectorXd(states));
        weights.resize(number_of_particles, 1.0/number_of_particles);
        costs.resize(number_of_particles,RAND_MAX);
        cumsum.resize(number_of_particles);
        indx.resize(number_of_particles);

        distribution = normal_distribution<double>(0,std);
        state_length = particles.begin()->size();

        resample(true);
    }

    void step(){
        double sum_of_elems = 0.0;
        for(uint i=0;i<number_of_particles;i++){
            process(i);
            costs[i] = cost(i);
            sum_of_elems += costs[i];
        }

        double Neff = 0.0, weight_sum = 0.0;
        for(uint i=0;i<number_of_particles;i++){
            weights[i] = costs[i]/sum_of_elems;
            weight_sum += weights[i];
            Neff += pow(weights[i],2.0);
        }

        Neff = 1.0/Neff;

        if(Neff < number_of_particles/2.0) {
            cout <<"resample, Neff: " << Neff << endl;
            resample(false, Neff);
        }
    }

    double result(state &winner, int *index){
        double val;
        minimum<double>(costs, val, index);
        winner = particles[*index];
        return val;
    }

    function<double (int id)> cost;
    function<void (int id)> process;

    void resample(bool all, int N = 0){
        if(all){ //resample all
            for(state &particle:particles){
                particle = start;
                for(int i=0;i<state_length;i++){
                    particle(i) += distribution(generator);
                }
            }
        }else{ // Sampling Importance Resampling
            vector<size_t> indexes = sort_indexes<double>(weights);
            for(uint i=0;i<N;i++){
                particles[indexes[i]] = particles[indexes[number_of_particles-1-i]];
                double w0 = weights[indexes[i]], w1 = indexes[number_of_particles-1-i];

            }

        }
    }

    template<typename T>
    void minimum(vector<T> &v, T &val, int *index ){
        *index = 0;
        val = v[0];
        for(uint i=1; i<v.size(); i++){
            if(v[i]<val){
                val = v[i];
                *index = i;
            }
        }
    }

    template <typename T>
    vector<size_t> sort_indexes(const vector<T> &v) {

        // initialize original index locations
        vector<size_t> idx(v.size());
        iota(idx.begin(), idx.end(), 0);

        // sort indexes based on comparing values in v
        sort(idx.begin(), idx.end(),
             [&v](size_t i1, size_t i2) {return v[i1] > v[i2];});

        return idx;
    }

    vector<state> particles;
    vector<double> weights, costs;
    vector<double> cumsum;
    vector<int> indx;
    int number_of_particles;
    default_random_engine generator;
    normal_distribution<double> distribution;
    int state_length;
    VectorXd start;
};


/*
 * Path-related utility functions.
 *
 * Copyright (C) 2014-2015 DubinsPathPlanner.
 * Created by David Goodman <dagoodma@gmail.com>
 * Redistribution and use of this file is allowed according to the terms of the MIT license.
 * For details see the LICENSE file distributed with DubinsPathPlanner.
 */
#include <stdio.h>
#include <math.h>
#include <algorithm>

#include <Eigen/Dense>

#include <DubinsCurves.h>

#include <dpp/basic/basic.h>
#include <dpp/basic/Logger.h>
#include <dpp/basic/Path.h>
#include <dpp/basic/Util.h>

using Eigen::Vector3d;
using Eigen::Vector2d;

#define HEADING_TOLERANCE          1E-10 // upperbound for Euclidean metric, radius safe?

namespace dpp {

/**
 * Calculate the shortest Dubins' path distance to the node. Note that all angles
 * used in this function are heading angles from 0 at the y-axis, and
 * counter-clockwise is positive.
 * @FIXME Add RLR and LRL curves. Does this fix dist > 3*r?
 */
double dubinsPathLength(VehicleConfiguration &Cs, VehicleConfiguration &Ce,
    double turnRadius) {
    double r = turnRadius; // shorter name
    Vector2d Ps = Vector2d(Cs.x(), Cs.y()),
        Pe = Vector2d(Ce.x(), Ce.y());
    double Xs = Cs.m_heading,
        Xe = Ce.m_heading;
    double dist = (Ps - Pe).norm();

    Logger::logDebug(DPP_LOGGER_VERBOSE_3) << "Given Cs=" << Cs << ", Ce=" << Ce << ", r=" << r << std::endl;
    Logger::logDebug(DPP_LOGGER_VERBOSE_3) << "Got dist=" << dist << " compared to r=" << r << "." << std::endl;

    // Added tolerance to avoid numerical instability for paths with no curviture
    // TODO compute curviture as ratio w.r.t. turn radius?
    // FIXME this does not take into account the feasability of the path!
    //        Should add an isReachable( func)
    if (fabs(Xs - Xe) <= HEADING_TOLERANCE) {
        Logger::logDebug(DPP_LOGGER_VERBOSE_3) << "Using straight line L=" << (Pe - Ps).norm() << ". |Xs - Xe| = "
            << fabs(Xs - Xe) << " <= " << HEADING_TOLERANCE << std::endl;
        return (Pe - Ps).norm();
    }

    //DPP_ASSERT(dist >= 3.0 * r);
    // FIXME: return an unfeasible path macro, eg -1
    // FIXME: improve this function so that we don't have to use the dubins-curves library
    //         this is needed for boustrophedon algorithm which does not care if the
    //         point dist is >= 3.0 * r
    // note: Detect if the final heading is close to the opposite of the initial,
    //       to allow for implementing CCC-type paths (ie. U-turns) via Dubins
    //       Corollarry to theorem 1. Though this may not satisfy all cases where
    //       no feasible paths exist. "U-turn" is either: {RLR or LRL}?
    if (dist < 3.0 * r) {
        //return DPP_MAX_EDGE_COST;
        double *q0, *q1;
        Cs.asArray(&q0);
        Ce.asArray(&q1);
        q0[2] = dpp::headingToAngle(q0[2]);
        q1[2] = dpp::headingToAngle(q1[2]);
        DubinsCurves::DubinsPath path;
        DubinsCurves::dubins_init( q0, q1, turnRadius, &path);
        double expectedLength = DubinsCurves::dubins_path_length(&path);
        return expectedLength;
    }
    //if (dist < 3.0 * r) {
    //  std::domain_error("distance must be larger than 3*r");
    //}

    // Convert headings to circular angles
    double alpha = headingToAngle(Xs),
           beta = headingToAngle(Xe);

    // Find circle center points for each case
    Vector3d R_rs(cos(alpha - M_PI/2.0), sin(alpha - M_PI/2.0), 0.0),
        R_ls(cos(alpha + M_PI/2.0), sin(alpha + M_PI/2.0), 0.0),
        R_re(cos(beta - M_PI/2.0), sin(beta - M_PI/2.0), 0.0),
        R_le(cos(beta + M_PI/2.0), sin(beta + M_PI/2.0), 0.0);

    Vector3d PC_rs(Cs.x(), Cs.y(), 0.0),
        PC_ls(Cs.x(), Cs.y(), 0.0),
        PC_re(Ce.x(), Ce.y(), 0.0),
        PC_le(Ce.x(), Ce.y(), 0.0);

    PC_rs = PC_rs.transpose() + r*R_rs.transpose();
    PC_ls = PC_ls.transpose() + r*R_ls.transpose();
    PC_re = PC_re.transpose() + r*R_re.transpose();
    PC_le = PC_le.transpose() + r*R_le.transpose();

    Logger::logDebug(DPP_LOGGER_VERBOSE_3) << "PC_rs: (" << PC_rs.x() << "," << PC_rs.y()
        << "," << PC_rs.z() << "," <<")." << std::endl;
    Logger::logDebug(DPP_LOGGER_VERBOSE_3) << "PC_ls: (" << PC_ls.x() << "," << PC_ls.y()
        << "," << PC_ls.z() << "," <<")." << std::endl;
    Logger::logDebug(DPP_LOGGER_VERBOSE_3) << "PC_re: (" << PC_re.x() << "," << PC_re.y()
        << "," << PC_re.z() << "," <<")." << std::endl;
    Logger::logDebug(DPP_LOGGER_VERBOSE_3) << "PC_le: (" << PC_le.x() << "," << PC_le.y()
        << "," << PC_le.z() << "," <<")." << std::endl;

    // Case I, R-S-R
    double x = headingBetween(PC_rs, PC_re);
    double L1 = (PC_rs - PC_re).norm()
        + r*wrapAngle(2.0 * M_PI + wrapAngle(x - M_PI/2.0) - wrapAngle(Xs - M_PI/2.0))
        + r*wrapAngle(2.0 * M_PI + wrapAngle(Xe - M_PI/2.0) - wrapAngle(x - M_PI/2.0));

    Logger::logDebug(DPP_LOGGER_VERBOSE_3) << "L1: " << L1 << ", with x=" << x << std::endl;

    // Case II, R-S-L
    double ls = (PC_le - PC_rs).norm();
    x = headingBetween(PC_rs, PC_le);
    double x2 = x - M_PI/2.0 + asin(2.0*r/ls);
    double L2 = sqrt(ls*ls - 4*r*r) + r*wrapAngle(2.0*M_PI + wrapAngle(x2)
        - wrapAngle(Xs - M_PI/2.0)) + r*wrapAngle(2.0*M_PI + wrapAngle(x2 + M_PI)
        - wrapAngle(Xe + M_PI/2.0));
    Logger::logDebug(DPP_LOGGER_VERBOSE_3) << "L2: " << L2 << " with ls=" << ls << ", x=" << x << ", x2=" << x2 << std::endl;

    // Case III, L-S-R
    ls = (PC_re - PC_ls).norm();
    x = headingBetween(PC_ls, PC_re);
    double ratioOA = 2.0*r/ls;
    // Bound the ratio from -1 to 1
    ratioOA = std::max<double> (-1.0, ratioOA);
    ratioOA = std::min<double> (1.0, ratioOA);
    DPP_ASSERT(ratioOA <= 1.0 && ratioOA >= -1.0);
    x2 = acos(ratioOA);
    double L3 = sqrt(ls*ls - 4*r*r) + r*wrapAngle(2.0*M_PI + wrapAngle(Xs + M_PI/2.0)
        - wrapAngle(x + x2)) + r*wrapAngle(2.0*M_PI + wrapAngle(Xe - M_PI/2.0)
        - wrapAngle(x + x2 - M_PI));

    Logger::logDebug(DPP_LOGGER_VERBOSE_3) << "L3: " << L3 << " with ls=" << ls << ", x=" << x << ", x2="
        << x2 << std::endl;

    // Case IV, L-S-L
    x = headingBetween(PC_ls, PC_le);
    double L4 = (PC_ls - PC_le).norm() + r*wrapAngle(2.0*M_PI + wrapAngle(Xs + M_PI/2.0)
        - wrapAngle(x + M_PI/2.0)) + r*wrapAngle(2.0*M_PI + wrapAngle(x + M_PI/2.0)
        - wrapAngle(Xe + M_PI/2.0));

    Logger::logDebug(DPP_LOGGER_VERBOSE_3) << "L4: " << L4 << ", with x=" << x << std::endl;
    Logger::logDebug(DPP_LOGGER_VERBOSE_3) << "Comparing L1=" << L1 << " L2=" << L2 << " L3=" << L3 << " L4="
        << L4 << std::endl;

    return std::min({L1, L2, L3, L4});
}

/**
 * Finds the cost of the shortest dubins path over the given tour with headings.
 * If returnCost is true, the cost of returning back to the first node in the
 * tour will be included.
 * @param[in] G graph with nodes to tour
 * @param[in] GA attributes of the graph
 * @param[in] Tour ordered list of nodes to visit
 * @param[in] Headings for vehicle at each node
 * @param[in] turnRadius of the vehicle
 * @param[in] returnEdge whether to add a return edge back to the first node
 */
double dubinsTourCost(ogdf::Graph &G, ogdf::GraphAttributes &GA,
    ogdf::List<ogdf::node> &Tour, ogdf::NodeArray<double> &Headings,
    double turnRadius, bool returnCost) {
    ogdf::ListIterator<ogdf::node> iter;
    double cost = 0.0;

    if (Tour.size() < 2) {
        Logger::logWarn(DPP_LOGGER_VERBOSE_1) << "Zero cost for an empty tour." << std::endl;
        return 0.0;
    }

    // Add the return edge if necessary
    ogdf::List<ogdf::node> modTour(Tour);
    ogdf::node lastNode = modTour.back();
    if (returnCost && lastNode != *(modTour.begin())) {
        modTour.pushBack(*(Tour.begin()));
    }
    else if (!returnCost && lastNode == *(modTour.begin())) {
        modTour.popBack();
    }

    int m = modTour.size() - 1;
    int i = 0; // edge index
    for ( iter = modTour.begin(); (i < m && iter != modTour.end()); iter++ ) {
        ogdf::node u = *iter, v = *(iter.succ());

        VehicleConfiguration Cu(GA.x(u), GA.y(u), Headings(u)),
                      Cv(GA.x(v), GA.y(v), Headings(v));
        cost += dubinsPathLength(Cu, Cv, turnRadius);
        i++;

        Logger::logDebug(DPP_LOGGER_VERBOSE_3) << "Found cost " << cost << " from node "
            << GA.idNode(u) << "->" << GA.idNode(v) << ", where headings "
            << GA.idNode(u) << ": " << Headings(u) << ", " << GA.idNode(v) << ": "
            << Headings(v) << std::endl;
    }
    Logger::logDebug(DPP_LOGGER_VERBOSE_2) << "Found total cost "
    << cost << " for tour." << std::endl;

    return cost;
}

/**
 * Adds weighted edges to the graph with the cost of the shortest dubins path
 * between each node in the tour. If returnEdge is true, an edge returning to the
 * origin is added. Added edges are saved into the list of edges.
 * @param[in] G graph with nodes to tour
 * @param[in] GA attributes of the graph
 * @param[in] Tour ordered list of nodes to visit
 * @param[in] Headings for vehicle at each node
 * @param[in] turnRadius of the vehicle
 * @param[out] Edges ordered list of edges to build
 * @param[in] returnEdge whether to add a return edge back to the first node
 */
double createDubinsTourEdges(ogdf::Graph &G, ogdf::GraphAttributes &GA,
    ogdf::List<ogdf::node> &Tour, ogdf::NodeArray<double> &Headings,
    double turnRadius, ogdf::List<ogdf::edge> &Edges, bool returnEdge) {
    ogdf::ListIterator<ogdf::node> iter;
    double total_cost = 0.0;

    if (Tour.size() < 2) return 0.0;
    DPP_ASSERT(G.numberOfEdges() < 1);
    //    std::range_error("Cannot have existing edges in graph");
    //}

    // Add the return edge if necessary
    ogdf::List<ogdf::node> modTour(Tour);
    ogdf::node lastNode = modTour.back();
    if (returnEdge && lastNode != *(modTour.begin())) {
        modTour.pushBack(*(Tour.begin()));
    }
    else if (!returnEdge && lastNode == *(modTour.begin())) {
        modTour.popBack();
    }

    int m = modTour.size() - 1;
    int i = 0; // edge index
    for ( iter = modTour.begin(); (i < m && iter != modTour.end()); iter++ ) {
        ogdf::node u = *iter, v = *(iter.succ());

        VehicleConfiguration Cu(GA.x(u), GA.y(u), Headings(u)),
                      Cv(GA.x(v), GA.y(v), Headings(v));
        double cost = dubinsPathLength(Cu, Cv, turnRadius);

        Logger::logDebug(DPP_LOGGER_VERBOSE_3) << "Found cost " << cost << " from node "
            << GA.idNode(u) << "->" << GA.idNode(v) << ", where headings "
            << GA.idNode(u) << ": " << Headings(u) << ", " << GA.idNode(v) << ": "
            << Headings(v) << std::endl;
        //printf("Found cost %0.1f from node %d->%d, where headings %d: %0.1f, %d: %0.1f\n",
        //    cost, GA.idNode(u), GA.idNode(v), GA.idNode(u), Headings(u), GA.idNode(v), Headings(v));

        // Add the edge
        ogdf::edge e = G.newEdge(u,v);
        GA.doubleWeight(e) = cost;
        Edges.pushBack(e);
        total_cost += cost;
        i++;
    }
    Logger::logDebug(DPP_LOGGER_VERBOSE_2) << "Created tour edges with total cost "
    << total_cost << ": " << std::endl;
    Logger::logDebug(DPP_LOGGER_VERBOSE_2) << printEdges(G, GA, Edges);

    return total_cost;
}

/**
 * Computes an adjacency matrix of Dubins path lengths between nodes with the
 * given headings (for ATSP).
 */
void buildDubinsAdjacencyMatrix(ogdf::Graph &G, ogdf::GraphAttributes &GA,
    NodeMatrix<double> &A, ogdf::NodeArray<double> &Headings, double turnRadius) {

    ogdf::node i, j;
    forall_nodes(i, G) {
        VehicleConfiguration Ci(GA.x(i), GA.y(i), Headings(i));

        forall_nodes(j, G) {
            if (i == j) {
                A[i][i] = DPP_MAX_EDGE_COST;
                continue;
            }
            VehicleConfiguration Cj(GA.x(j), GA.y(j), Headings(j));

            double w = dubinsPathLength(Ci, Cj, turnRadius);
            A[i][j] = w;
        }
    }
}

} // namespace DPP


///////////////////////////////////////////////////////////////////////////////
// weighted_variance.hpp
//
//  Copyright 2005 Daniel Egloff, Eric Niebler. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_VARIANCE_HPP_EAN_28_10_2005
#define BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_VARIANCE_HPP_EAN_28_10_2005

#include <boost/mpl/placeholders.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/accumulators/statistics/weighted_sum.hpp>
#include <boost/accumulators/statistics/weighted_mean.hpp>
#include <boost/accumulators/statistics/weighted_moment.hpp>

namespace boost { namespace accumulators
{

namespace impl
{
    //! Lazy calculation of variance of weighted samples.
    /*!
        The default implementation of the variance of weighted samples is based on the second moment
        \f$\widehat{m}_n^{(2)}\f$ (weighted_moment<2>) and the mean\f$ \hat{\mu}_n\f$ (weighted_mean):
        \f[
            \hat{\sigma}_n^2 = \widehat{m}_n^{(2)}-\hat{\mu}_n^2,
        \f]
        where \f$n\f$ is the number of samples.
    */
    template<typename Sample, typename Weight, typename MeanFeature>
    struct lazy_weighted_variance_impl
      : accumulator_base
    {
        typedef typename numeric::functional::multiplies<Sample, Weight>::result_type weighted_sample;
        // for boost::result_of
        typedef typename numeric::functional::average<weighted_sample, Weight>::result_type result_type;

        lazy_weighted_variance_impl(dont_care) {}

        template<typename Args>
        result_type result(Args const &args) const
        {
            extractor<MeanFeature> const some_mean = {};
            result_type tmp = some_mean(args);
            return weighted_moment<2>(args) - tmp * tmp;
        }
    };

    //! Iterative calculation of variance of weighted samples.
    /*!
        Iterative calculation of variance of weighted samples:
        \f[
            \hat{\sigma}_n^2 =
                \frac{\bar{w}_n - w_n}{\bar{w}_n}\hat{\sigma}_{n - 1}^2
              + \frac{w_n}{\bar{w}_n - w_n}\left(X_n - \hat{\mu}_n\right)^2
            ,\quad n\ge2,\quad\hat{\sigma}_0^2 = 0.
        \f]
        where \f$\bar{w}_n\f$ is the sum of the \f$n\f$ weights \f$w_i\f$ and \f$\hat{\mu}_n\f$
        the estimate of the mean of the weighted smaples. Note that the sample variance is not defined for
        \f$n <= 1\f$.
    */
    template<typename Sample, typename Weight, typename MeanFeature, typename Tag>
    struct weighted_variance_impl
      : accumulator_base
    {
        typedef typename numeric::functional::multiplies<Sample, Weight>::result_type weighted_sample;
        // for boost::result_of
        typedef typename numeric::functional::average<weighted_sample, Weight>::result_type result_type;

        template<typename Args>
        weighted_variance_impl(Args const &args)
          : weighted_variance(numeric::average(args[sample | Sample()], numeric::one<Weight>::value))
        {
        }

        template<typename Args>
        void operator ()(Args const &args)
        {
            std::size_t cnt = count(args);

            if(cnt > 1)
            {
                extractor<MeanFeature> const some_mean = {};

                result_type tmp = args[parameter::keyword<Tag>::get()] - some_mean(args);

                this->weighted_variance =
                    numeric::average(this->weighted_variance * (sum_of_weights(args) - args[weight]), sum_of_weights(args))
                  + numeric::average(tmp * tmp * args[weight], sum_of_weights(args) - args[weight] );
            }
        }

        result_type result(dont_care) const
        {
            return this->weighted_variance;
        }

    private:
        result_type weighted_variance;
    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::weighted_variance
// tag::immediate_weighted_variance
//
namespace tag
{
    struct lazy_weighted_variance
      : depends_on<weighted_moment<2>, weighted_mean>
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::lazy_weighted_variance_impl<mpl::_1, mpl::_2, weighted_mean> impl;
    };

    struct weighted_variance
      : depends_on<count, immediate_weighted_mean>
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::weighted_variance_impl<mpl::_1, mpl::_2, immediate_weighted_mean, sample> impl;
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::weighted_variance
// extract::immediate_weighted_variance
//
namespace extract
{
    extractor<tag::lazy_weighted_variance> const lazy_weighted_variance = {};
    extractor<tag::weighted_variance> const weighted_variance = {};
}

using extract::lazy_weighted_variance;
using extract::weighted_variance;

// weighted_variance(lazy) -> lazy_weighted_variance
template<>
struct as_feature<tag::weighted_variance(lazy)>
{
    typedef tag::lazy_weighted_variance type;
};

// weighted_variance(immediate) -> weighted_variance
template<>
struct as_feature<tag::weighted_variance(immediate)>
{
    typedef tag::weighted_variance type;
};

////////////////////////////////////////////////////////////////////////////
//// droppable_accumulator<weighted_variance_impl>
////  need to specialize droppable lazy weighted_variance to cache the result at the
////  point the accumulator is dropped.
///// INTERNAL ONLY
/////
//template<typename Sample, typename Weight, typename MeanFeature>
//struct droppable_accumulator<impl::weighted_variance_impl<Sample, Weight, MeanFeature> >
//  : droppable_accumulator_base<
//        with_cached_result<impl::weighted_variance_impl<Sample, Weight, MeanFeature> >
//    >
//{
//    template<typename Args>
//    droppable_accumulator(Args const &args)
//      : droppable_accumulator::base(args)
//    {
//    }
//};

}} // namespace boost::accumulators

#endif


#include <Rcpp.h>
#include <boost/multi_array.hpp>

using namespace Rcpp;

// [[Rcpp::plugins("cpp11")]]
// [[Rcpp::depends(BH)]]

//'Estep in girt.
//'
//'@param xall Item response matrix.
//'@param t0 item parameter vector
//'@param Xm node of theta dist.
//'@param Wm weight of theta dist.
//'@param group a vector.
//'@param ind a design matrix for group.
//'@param resp a design matrix for person.
//'@param D factor constant.
//'@param MLL a vector
//'@export
// [[Rcpp::export]]

List Estep_irt(
  IntegerMatrix xall,
  NumericMatrix t0, // a, b, c
  NumericVector Xm,
  NumericMatrix Wm,
  IntegerVector group,
  IntegerMatrix ind, // design matrix
  IntegerMatrix resp, // design matrix
  double D,
  NumericVector MLL
){

  int nj = xall.ncol();// item n
  int ni = xall.nrow(); // subject n
  int N  = Xm.length(); // node n
  int ng = max(group); // group n

  double a,b,c,t,tt,u,x;
  int g,i,m,j;

  boost::multi_array <double, 3> Lim (boost::extents[ng][ni][N]);
  boost::multi_array <double, 3> Gim (boost::extents[ng][ni][N]);
  boost::multi_array <double, 3> Njm (boost::extents[ng][nj][N]);
  boost::multi_array <double, 3> rjm (boost::extents[ng][nj][N]);

  for(g=0; g<ng; g++){
    for(i=0; i<ni; i++){
      if(group[i] != g+1) continue; // 集団に属さない受験者の部分はスキップ
      for(m=0; m<N; m++){
        t = 1;
        x = Xm[m];
        tt = 0;
        for(j=0; j<nj; j++){
          //if(resp(i,j)==0) continue; // NAの反応パタンのところは計算ループから外れる。
          a = t0(j,0);
          if(a == 0) continue;
          b = t0(j,1);
          c = t0(j,2);
          u = xall(i,j);
          if(u == 1){ // 正答した場合の尤度
            tt = c+(1.0-c)/(1.0+exp(-D*a*(x-b)));
          } else if (u == 0) { // 誤答した場合の尤度
            tt = 1.0-(c+(1.0-c)/(1.0+exp(-D*a*(x-b))));
          } else { // 欠測値の場合は尤度を計算しないので，1
            tt = 1;
          }
          t = t * tt; // sum
        }
        Lim[g][i][m] = t;
      }
    }
  }

  // 各受検者のthetaごとに事後分布の重みを計算する。
  double uu;
  double f = 0; // 周辺対数尤度代入用
  double l,w;
  for(int g=0; g<ng; g++){
    for(int i=0; i<ni; i++){
      if(group[i] != g+1) continue; // 集団に属さない受験者の部分はスキップ
      u = 0;
      for(int m=0; m<N; m++){ // 総和を1にするための分母の計算 // sum
        l = Lim[g][i][m];
        w = Wm(m,g);
        uu = l*w;
        u += uu;
      }
      f += log(u);
      for(int m=0; m<N; m++){
        l = Lim[g][i][m];
        w = Wm(m,g);
        Gim[g][i][m] =  l * w / u;
      }

    }
  }
  MLL.push_back(f);
  if(traits::is_nan<REALSXP>(f)){
    // 対数尤度の計算に失敗したら，計算を中止する。
    stop("Can't calculate marginal log likelihood.");
  }

  //Rcout<<"expected frequency of subjects in each nodes calculation.\n";
  double k,kk;
  for(int g=0; g<ng; g++){
    for(int j=0; j<nj; j++){ // 各分点の期待度数
      if(ind(g,j) == 0) continue;
      for(int m=0; m<N; m++){
        k = 0;
        for(int i=0; i<ni; i++){ // 欠測値がある場合，項目ごとに受検者数が異なる。
          if(resp(i,j)==0) continue;
          //double d = resp(i,j);
          kk = Gim[g][i][m];
          k += kk; //* d;
        }
        Njm[g][j][m]= k;
      }
    }
  }

  double h,gg,hh;
  for(int g=0; g<ng; g++){
    for(int j=0; j<nj; j++){ // 各分点の正答受検者の期待度数
      if(ind(g,j) == 0) continue;
      for(int m=0; m<N; m++){
        h = 0;
        for(int i=0; i<ni; i++){ // sum
          //double d = resp(i,j);
          if(resp(i,j) == 0) continue;
          u = xall(i,j); // そもそもその項目に回答していない場合は，度数に数え上げない
          gg = Gim[g][i][m];
          hh = u*gg;
          h += hh;
        }
        rjm[g][j][m] = h;
      }
    }
  }

  return List::create(_["Njm"]=Njm, _["rjm"]=rjm, _["Lim"]=Lim, _["Gim"]=Gim, _["MLL"]=MLL);

}


// Boost.Geometry
// Unit Test

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

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

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

#include "test_formula.hpp"
#include "intersection_cases.hpp"

#include <boost/geometry/formulas/andoyer_inverse.hpp>
#include <boost/geometry/formulas/gnomonic_intersection.hpp>
#include <boost/geometry/formulas/sjoberg_intersection.hpp>
#include <boost/geometry/formulas/thomas_direct.hpp>
#include <boost/geometry/formulas/thomas_inverse.hpp>
#include <boost/geometry/formulas/vincenty_direct.hpp>
#include <boost/geometry/formulas/vincenty_inverse.hpp>

void check_inverse(expected_result const& result, expected_result const& expected, expected_result const& reference, double reference_error)
{
    check_one(result.lon, expected.lon, reference.lon, reference_error);
    check_one(result.lat, expected.lat, reference.lat, reference_error);
}

void test_all(expected_results const& results)
{
    double const d2r = bg::math::d2r<double>();
    double const r2d = bg::math::r2d<double>();

    double lona1r = results.p1.lon * d2r;
    double lata1r = results.p1.lat * d2r;
    double lona2r = results.p2.lon * d2r;
    double lata2r = results.p2.lat * d2r;
    double lonb1r = results.q1.lon * d2r;
    double latb1r = results.q1.lat * d2r;
    double lonb2r = results.q2.lon * d2r;
    double latb2r = results.q2.lat * d2r;

    expected_result result;

    // WGS84
    bg::srs::spheroid<double> spheroid(6378137.0, 6356752.3142451793);

    bg::formula::gnomonic_intersection<double, bg::formula::vincenty_inverse, bg::formula::vincenty_direct>
        ::apply(lona1r, lata1r, lona2r, lata2r, lonb1r, latb1r, lonb2r, latb2r, result.lon, result.lat, spheroid);
    result.lon *= r2d;
    result.lat *= r2d;
    check_inverse(result, results.gnomonic_vincenty, results.gnomonic_karney, 0.00000001);

    bg::formula::gnomonic_intersection<double, bg::formula::thomas_inverse, bg::formula::thomas_direct>
        ::apply(lona1r, lata1r, lona2r, lata2r, lonb1r, latb1r, lonb2r, latb2r, result.lon, result.lat, spheroid);
    result.lon *= r2d;
    result.lat *= r2d;
    check_inverse(result, results.gnomonic_thomas, results.gnomonic_karney, 0.0000001);

    bg::formula::sjoberg_intersection<double, bg::formula::vincenty_inverse, 4>
        ::apply(lona1r, lata1r, lona2r, lata2r, lonb1r, latb1r, lonb2r, latb2r, result.lon, result.lat, spheroid);
    result.lon *= r2d;
    result.lat *= r2d;
    check_inverse(result, results.sjoberg_vincenty, results.sjoberg_karney, 0.00000001);

    bg::formula::sjoberg_intersection<double, bg::formula::thomas_inverse, 2>
        ::apply(lona1r, lata1r, lona2r, lata2r, lonb1r, latb1r, lonb2r, latb2r, result.lon, result.lat, spheroid);
    result.lon *= r2d;
    result.lat *= r2d;
    check_inverse(result, results.sjoberg_thomas, results.sjoberg_karney, 0.0000001);

    bg::formula::sjoberg_intersection<double, bg::formula::andoyer_inverse, 1>
        ::apply(lona1r, lata1r, lona2r, lata2r, lonb1r, latb1r, lonb2r, latb2r, result.lon, result.lat, spheroid);
    result.lon *= r2d;
    result.lat *= r2d;
    check_inverse(result, results.sjoberg_andoyer, results.sjoberg_karney, 0.0001);
}

int test_main(int, char*[])
{
    for (size_t i = 0; i < expected_size; ++i)
    {
        test_all(expected[i]);
    }

    return 0;
}