#define _USE_MATH_DEFINES
#include <algorithm> // std::sort
#include <vector>

#include <Eigen/Dense>
#include <ccd/ccd.h>
#include <gtest/gtest.h>
#include <math.h>

#include "dart/collision/dart/DARTCollide.hpp"
#include "dart/dynamics/BallJoint.hpp"
#include "dart/dynamics/FreeJoint.hpp"
#include "dart/realtime/Ticker.hpp"
#include "dart/server/GUIWebsocketServer.hpp"
#include "dart/utils/DartResourceRetriever.hpp"
#include "dart/utils/sdf/sdf.hpp"
#include "dart/utils/urdf/urdf.hpp"

#include "TestHelpers.hpp"

using namespace dart;
using namespace realtime;

#define ALL_TESTS

Eigen::MatrixXs skelPosPosJacFD(
    std::shared_ptr<dynamics::Skeleton> skel,
    Eigen::VectorXs pos,
    Eigen::VectorXs vel,
    s_t dt)
{
  int dofs = skel->getNumDofs();
  Eigen::MatrixXs jac = Eigen::MatrixXs::Zero(dofs, dofs);
  s_t EPS = 1e-7;
  for (int i = 0; i < dofs; i++)
  {
    Eigen::VectorXs perturbed = pos;
    perturbed(i) += EPS;
    Eigen::VectorXs plus = skel->integratePositionsExplicit(perturbed, vel, dt);

    perturbed = pos;
    perturbed(i) -= EPS;
    Eigen::VectorXs minus
        = skel->integratePositionsExplicit(perturbed, vel, dt);

    jac.col(i) = (plus - minus) / (2 * EPS);
  }

  return jac;
}

Eigen::MatrixXs skelVelPosJacFD(
    std::shared_ptr<dynamics::Skeleton> skel,
    Eigen::VectorXs pos,
    Eigen::VectorXs vel,
    s_t dt)
{
  int dofs = skel->getNumDofs();
  Eigen::MatrixXs jac = Eigen::MatrixXs::Zero(dofs, dofs);
  s_t EPS = 1e-7;
  for (int i = 0; i < dofs; i++)
  {
    Eigen::VectorXs perturbed = vel;
    perturbed(i) += EPS;
    Eigen::VectorXs plus = skel->integratePositionsExplicit(pos, perturbed, dt);

    perturbed = vel;
    perturbed(i) -= EPS;
    Eigen::VectorXs minus
        = skel->integratePositionsExplicit(pos, perturbed, dt);

    jac.col(i) = (plus - minus) / (2 * EPS);
  }

  return jac;
}

//==============================================================================
#ifdef ALL_TESTS
TEST(FreeJointGradients, ATLAS_JACOBIANS)
{
  std::shared_ptr<simulation::World> world = simulation::World::create();
  std::shared_ptr<dynamics::Skeleton> atlas
      = dart::utils::SdfParser::readSkeleton(
          "dart://sample/sdf/atlas/atlas_v3_no_head.sdf");
  world->addSkeleton(atlas);

  int dofs = atlas->getNumDofs();
  s_t dt = 0.01;
  for (int i = 0; i < 10; i++)
  {
    Eigen::VectorXs pos = Eigen::VectorXs::Random(dofs);
    Eigen::VectorXs vel = Eigen::VectorXs::Random(dofs);

    Eigen::MatrixXs posPosAnalytical = atlas->getPosPosJac(pos, vel, dt);
    Eigen::MatrixXs velPosAnalytical = atlas->getVelPosJac(pos, vel, dt);
    Eigen::MatrixXs posPosFD = skelPosPosJacFD(atlas, pos, vel, dt);
    Eigen::MatrixXs velPosFD = skelVelPosJacFD(atlas, pos, vel, dt);

    const s_t tol = 3e-9;

    if (!equals(posPosAnalytical, posPosFD, tol))
    {
      std::cout << "Pos-Pos Analytical (top-left 6x6): " << std::endl
                << posPosAnalytical.block<6, 6>(0, 0) << std::endl;
      std::cout << "Pos-Pos FD (top-left 6x6): " << std::endl
                << posPosFD.block<6, 6>(0, 0) << std::endl;
      std::cout << "Pos-Pos Diff (" << (posPosAnalytical - posPosFD).minCoeff()
                << " - " << (posPosAnalytical - posPosFD).maxCoeff()
                << ") (top-left 6x6): " << std::endl
                << (posPosAnalytical - posPosFD).block<6, 6>(0, 0) << std::endl;
      break;
    }
    EXPECT_TRUE(equals(posPosAnalytical, posPosFD, tol));

    if (!equals(velPosAnalytical, velPosFD, tol))
    {
      std::cout << "Vel-Pos Analytical (top-left 6x6): " << std::endl
                << velPosAnalytical.block<6, 6>(0, 0) << std::endl;
      std::cout << "Vel-Pos FD (top-left 6x6): " << std::endl
                << velPosFD.block<6, 6>(0, 0) << std::endl;
      std::cout << "Vel-Pos Diff (" << (velPosAnalytical - velPosFD).minCoeff()
                << " - " << (velPosAnalytical - velPosFD).maxCoeff()
                << ") (top-left 6x6): " << std::endl
                << (velPosAnalytical - velPosFD).block<6, 6>(0, 0) << std::endl;
      break;
    }
    EXPECT_TRUE(equals(velPosAnalytical, velPosFD, tol));

    world->step();
  }
}
#endif

//==============================================================================
#ifdef ALL_TESTS
TEST(FreeJointGradients, INTEGRATE_POSITIONS_EXPLICIT)
{
  std::shared_ptr<simulation::World> world = simulation::World::create();
  std::shared_ptr<dynamics::Skeleton> atlas
      = dart::utils::SdfParser::readSkeleton(
          "dart://sample/sdf/atlas/atlas_v3_no_head.sdf");
  world->addSkeleton(atlas);

  int dofs = atlas->getNumDofs();
  s_t dt = 0.01;
  for (int i = 0; i < 1000; i++)
  {
    Eigen::VectorXs pos = Eigen::VectorXs::Random(dofs);
    Eigen::VectorXs vel = Eigen::VectorXs::Random(dofs);
    atlas->setPositions(pos);
    atlas->setVelocities(vel);
    atlas->integratePositions(dt);
    Eigen::VectorXs implicitNextPos = atlas->getPositions();

    // Scramble positions
    atlas->setPositions(Eigen::VectorXs::Random(dofs));
    atlas->setVelocities(Eigen::VectorXs::Random(dofs));
    Eigen::VectorXs explicitNextPos
        = atlas->integratePositionsExplicit(pos, vel, dt);

    EXPECT_TRUE(implicitNextPos.isApprox(explicitNextPos, 1e-10));
  }
}
#endif

//==============================================================================
Eigen::Vector6s integratePos(
    Eigen::Vector6s pos, Eigen::Vector6s vel, s_t dt)
{
  const Eigen::Isometry3s mQ = FreeJoint::convertToTransform(pos);
  const Eigen::Isometry3s Qnext = mQ * FreeJoint::convertToTransform(vel * dt);

  return FreeJoint::convertToPositions(Qnext);
}

Eigen::Vector6s integratePosByParts(
    Eigen::Vector6s pos, Eigen::Vector6s vel, s_t dt)
{
  const Eigen::Matrix3s mR = BallJoint::convertToRotation(pos.head<3>());
  const Eigen::Matrix3s Rnext
      = mR * BallJoint::convertToRotation(vel.head<3>() * dt);

  Eigen::Vector6s ret = Eigen::Vector6s::Zero();
  ret.head<3>() = BallJoint::convertToPositions(Rnext);
  ret.tail<3>() = pos.tail<3>() + (mR * vel.tail<3>() * dt);

  return ret;
}

//==============================================================================
#ifdef ALL_TESTS
TEST(FreeJointGradients, FREE_JOINT_BY_PARTS)
{
  s_t dt = 0.01;

  for (int i = 0; i < 1000; i++)
  {
    Eigen::Vector6s pos = Eigen::Vector6s::Random();
    Eigen::Vector6s vel = Eigen::Vector6s::Random();
    Eigen::Vector6s nextPos = integratePos(pos, vel, dt);
    Eigen::Vector6s nextPosByParts = integratePosByParts(pos, vel, dt);

    EXPECT_TRUE(nextPosByParts.isApprox(nextPos, 1e-9));
  }
}
#endif

Eigen::Vector3s rotateBall(Eigen::Vector3s pos, Eigen::Vector3s vel, s_t dt)
{
  const Eigen::Matrix3s mR = BallJoint::convertToRotation(pos.head<3>());
  const Eigen::Matrix3s Rnext
      = mR * BallJoint::convertToRotation(vel.head<3>() * dt);
  return BallJoint::convertToPositions(Rnext);
}

Eigen::Matrix3s rotatePosPosJacFD(
    Eigen::Vector3s pos, Eigen::Vector3s vel, s_t dt)
{
  Eigen::Matrix3s jac = Eigen::Matrix3s::Zero();

  const s_t EPS = 1e-7;
  for (int i = 0; i < 3; i++)
  {
    Eigen::Vector3s perturbed = pos;
    perturbed(i) += EPS;
    Eigen::Vector3s outPos = rotateBall(perturbed, vel, dt);

    perturbed = pos;
    perturbed(i) -= EPS;
    Eigen::Vector3s outNeg = rotateBall(perturbed, vel, dt);

    jac.col(i) = (outPos - outNeg) / (2 * EPS);
  }

  return jac;
}

Eigen::Matrix3s rotateVelPosJacFD(
    Eigen::Vector3s pos, Eigen::Vector3s vel, s_t dt)
{
  Eigen::Matrix3s jac = Eigen::Matrix3s::Zero();

  const s_t EPS = 1e-7;
  for (int i = 0; i < 3; i++)
  {
    Eigen::Vector3s perturbed = vel;
    perturbed(i) += EPS;
    Eigen::Vector3s outPos = rotateBall(pos, perturbed, dt);

    perturbed = vel;
    perturbed(i) -= EPS;
    Eigen::Vector3s outNeg = rotateBall(pos, perturbed, dt);

    jac.col(i) = (outPos - outNeg) / (2 * EPS);
  }

  return jac;
}

//==============================================================================
#ifdef ALL_TESTS
TEST(FreeJointGradients, ROTATION_JOINT_JAC)
{
  s_t dt = 0.01;
  Eigen::Vector3s pos = Eigen::Vector3s::UnitX();
  Eigen::Vector3s vel = Eigen::Vector3s::UnitY();

  // Just check these don't crash
  rotatePosPosJacFD(pos, vel, dt);
  rotateVelPosJacFD(pos, vel, dt);
}
#endif

/*
//==============================================================================
#ifdef ALL_TESTS
TEST(FreeJointGradients, GUI_EXPLORE)
{
  // Create a world
  std::shared_ptr<simulation::World> world = simulation::World::create();

  // Set gravity of the world
  // world->setPenetrationCorrectionEnabled(true);
  world->setGravity(Eigen::Vector3s(0.0, -9.81, 0));

  std::shared_ptr<BoxShape> boxShape(
      new BoxShape(Eigen::Vector3s(1.0, 1.0, 1.0)));

  std::shared_ptr<dynamics::Skeleton> box = dynamics::Skeleton::create("box");
  auto pair = box->createJointAndBodyNodePair<dynamics::FreeJoint>();
  pair.second->createShapeNodeWith<VisualAspect, CollisionAspect>(boxShape);
  pair.second->setFrictionCoeff(0.0);

  std::shared_ptr<dynamics::Skeleton> groundBox
      = dynamics::Skeleton::create("groundBox");
  auto groundPair
      = groundBox->createJointAndBodyNodePair<dynamics::WeldJoint>();
  std::shared_ptr<BoxShape> groundShape(
      new BoxShape(Eigen::Vector3s(10.0, 1.0, 10.0)));
  groundPair.second->createShapeNodeWith<VisualAspect, CollisionAspect>(
      groundShape);
  groundPair.second->setFrictionCoeff(1.0);
  Eigen::Isometry3s groundTransform = Eigen::Isometry3s::Identity();
  groundTransform.translation()(1) = -0.999;
  groundPair.first->setTransformFromParentBodyNode(groundTransform);

  // world->addSkeleton(atlas);
  world->addSkeleton(box);
  world->addSkeleton(groundBox);

  // Disable the ground from casting its own shadows
  groundBox->getBodyNode(0)->getShapeNode(0)->getVisualAspect()->setCastShadows(
      false);

  world->step();

  std::vector<Eigen::Vector3s> pointsX;
  pointsX.push_back(Eigen::Vector3s::Zero());
  pointsX.push_back(Eigen::Vector3s::UnitX() * 10);
  std::vector<Eigen::Vector3s> pointsY;
  pointsY.push_back(Eigen::Vector3s::Zero());
  pointsY.push_back(Eigen::Vector3s::UnitY() * 10);
  std::vector<Eigen::Vector3s> pointsZ;
  pointsZ.push_back(Eigen::Vector3s::Zero());
  pointsZ.push_back(Eigen::Vector3s::UnitZ() * 10);

  server::GUIWebsocketServer server;
  server.renderWorld(world);
  server.createLine("unitX", pointsX, Eigen::Vector3s::UnitX());
  server.createLine("unitY", pointsY, Eigen::Vector3s::UnitY());
  server.createLine("unitZ", pointsZ, Eigen::Vector3s::UnitZ());
  server.serve(8070);

  Ticker ticker(0.01);
  ticker.registerTickListener([&](long time) {
    s_t diff = sin(((s_t)time / 2000));
    diff = diff * diff;
    // atlas->setPosition(0, diff * dart::math::constantsd::pi());
    // s_t diff2 = sin(((s_t)time / 4000));
    // atlas->setPosition(4, diff2 * 1);

    Eigen::Isometry3s fromRoot = Eigen::Isometry3s::Identity();
    fromRoot.translation() = Eigen::Vector3s::UnitZ() * diff;
    pair.first->setTransformFromParentBodyNode(fromRoot);

    server.renderWorld(world);
  });
  server.registerConnectionListener([&]() { ticker.start(); });

  while (server.isServing())
  {
  }
}
#endif
*/


#include <boost/math/common_factor_ct.hpp>


#ifndef CX_DQMC_DENSITY_HPP
#define CX_DQMC_DENSITY_HPP

#include "la.hpp"
#include "parameters.hpp"
#include "cx_workspace.hpp"

#include <Eigen/Eigenvalues>

#include <boost/foreach.hpp>

#include <cmath>
#include <exception>
#include <iomanip>

namespace cx_dqmc {
    namespace density {
	inline void density_matrix(dqmc::parameters& p,
				   cx_dqmc::workspace& ws) {
	    using namespace std;
	    srand(23);
	    double rand_hop;
	    double rand_eps = 0;

	    pvec_t eigval(p.N);
	    pmat_t lowest_eigenvectors(p.N, p.particles);
	    pmat_t density(p.N, p.N), eigvec(p.N, p.N);

	    density.setZero();

	    if (ws.density.cols() != p.N) {
		throw std::runtime_error("Dimensions of density are incorrect");
	    }

	    for (int attempt = 0; attempt < 4; ++attempt) {
		alps::graph_helper<>::bond_iterator itr1, itr1_end;
		for (boost::tie(itr1, itr1_end) = p.graph.bonds(); itr1 != itr1_end; ++itr1) {
		    rand_hop = (1 + rand_eps * ( -0.5 + (std::rand() % RAND_MAX)/double(RAND_MAX)));

		    density(p.graph.source(*itr1), p.graph.target(*itr1)) =
			-p.ts[p.graph.bond_type(*itr1)] * rand_hop;
		    density(p.graph.target(*itr1), p.graph.source(*itr1)) =
			-p.ts[p.graph.bond_type(*itr1)] * rand_hop;
		}
		eigvec = density;

		Eigen::SelfAdjointEigenSolver<mat> eig(density);
		eig.compute(density);
		eigval = eig.eigenvalues();
		eigvec = eig.eigenvectors();

		bool error = false;
		for (int i = 0; i < eigval.size() - 1; ++i) {
		    double diff;
		    diff = fabs(eigval(i + 1) - eigval(i));
		    if (diff < 1e-4) {
			if (attempt == 0) {
			    rand_eps = 1e-3;
			}
			else if (attempt == 1) {
			    rand_eps = 1e-2;
			}
			else if (attempt == 2) {
			    rand_eps = 1e-1;
			}
			else if (attempt == 3) {
			    rand_eps = 5e-1;
			}
			else if (attempt == 4) {
			    if (p.rank < 1) {
				cout << attempt << " @ rand_eps = " << rand_eps << " - " << diff << endl;
			    }
			    throw std::runtime_error("Degeneracy not lifted");
			}
			error = true;
			break;
		    }
		}
		if (!error) {
		    break;
		}
	    }

	    for (int i = 0; i < p.N; i++) {
	    	for (int j = 0; j < p.real_particles; j++) {
		    lowest_eigenvectors(i, j) = eigvec(i, j);
	    	}
	    }

	    for (int row = 0; row < p.N; row++) {
		for (int col = 0; col < p.N; col++) {
		    ws.density(row, col) = cx_double(0, 0);

		    for (int i = 0; i < p.real_particles; i++) {
			ws.density(row, col) +=
			    cx_double(lowest_eigenvectors(row, i) * lowest_eigenvectors(col, i), 0);
			ws.den_U(row, i) = cx_double(lowest_eigenvectors(row, i), 0);
		    }
		}
	    }

	    // cout << p.outp << "And den_u" << endl << ws.den_U << endl << endl;
 	}

	inline void cx_density_matrix(dqmc::parameters& p, cx_dqmc::workspace& ws) {
	    using namespace std;

	    srand(23);
	    double rand_hop;
	    double rand_eps = 0;

	    cx_vec eigval(p.N);
	    cx_mat_t lowest_eigenvectors(p.N, p.real_particles);
	    cx_mat_t density(p.N, p.N), eigvec(p.N, p.N);

	    if (ws.density.cols() != p.N) {
		throw std::runtime_error("Dimensions of density are incorrect");
	    }

	    ws.density.setZero();
	    for (int attempt = 0; attempt < 4; ++attempt) {
		alps::graph_helper<>::bond_iterator itr1, itr1_end;
		int s, t, b;
		double pref;

		for (boost::tie(itr1, itr1_end) = p.graph.bonds(); itr1 != itr1_end; ++itr1) {
		    rand_hop = (1 + rand_eps * ( -0.5 + (std::rand() % RAND_MAX)/double(RAND_MAX)));
		    s = p.graph.source(*itr1);
		    t = p.graph.target(*itr1);
		    b = p.graph.bond_type(*itr1);

		    ws.density(s, t) = -1. * cx_double(p.ts[b] * rand_hop, -p.im_ts[b]* rand_hop);
		    ws.density(t, s) = -1. * cx_double(p.ts[b] * rand_hop, p.im_ts[b] * rand_hop);
		}
		ws.eigvec = ws.density;

		Eigen::SelfAdjointEigenSolver<cx_mat_t> eig(ws.density);
		eig.compute(ws.density);
		vec revals = eig.eigenvalues();
		ws.eigvec = eig.eigenvectors();

		if (attempt == 0) {
		    ofstream mat_file("eigenvectors.mat", ios::out | ios::trunc);
		    mat_file << ws.eigvec;
		    mat_file.close();

		    ofstream vec_file("eigenvalues.vec", ios::out | ios::trunc);
		    vec_file << revals;
		    vec_file.close();
		}

		std::vector<max_pair > sorting;
		sorting.reserve(revals.size());
		for (int i = 0; i < revals.size(); i++) {
		    double val = revals(i);
		    sorting.push_back(max_pair(val,i));
		}
		std::sort(sorting.begin(),sorting.end());
		cx_mat_t sorted_eigvec;
		sorted_eigvec.resizeLike(ws.eigvec);
		for (int i = 0; i < revals.size(); i++) {
		    revals.coeffRef(i,0) = sorting[i].first;
		    sorted_eigvec.col(i) = ws.eigvec.col(sorting[i].second);
		}
		if (attempt == 0) {
		    ofstream sorted_mat_file("sorted_eigenvectors.mat", ios::out | ios::trunc);
		    sorted_mat_file << sorted_eigvec;
		    sorted_mat_file.close();

		    ofstream sorted_file("sorted_eigenvalues.vec", ios::out | ios::trunc);
		    sorted_file << revals;
		    sorted_file.close();
		}

		// eigval = revals;
		ws.eigvec = sorted_eigvec;

		bool error = false;
		for (int i = 0; i < revals.size() - 1; ++i) {
		    // break;
		    double diff;
		    diff = abs(revals(i + 1) - revals(i));
		    if (diff < 1e-5) {

			if (attempt == 0) {
			    rand_eps = 1e-3;
			}
			else if (attempt == 1) {
			    rand_eps = 1e-2;
			}
			else if (attempt == 2) {
			    rand_eps = 1e-1;
			}
			else if (attempt == 3) {
			    throw std::runtime_error("Degeneracy not lifted");
			}
			error = true;
			break;
		    }
		}
		if (!error) {
		    break;
		}
	    }
	    ws.den_U = ws.eigvec.block(0, 0, p.N, p.real_particles);
	    ws.density = ws.den_U * ws.den_U.adjoint();
 	}
    }
}
#endif


#include <libv/lma/lma.hpp>
#include <libv/lma/numeric/divers.hpp>
#include <libv/core/miscmath.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_same.hpp>

using namespace lma;

template<class Derivator>
struct F : Derivator
{
  double obs;
  F(double obs_):obs(obs_){}

  template<class X, class Y, class Res> bool error(const X& x, const Y& y, Res& res) const
  {
    res = x * y - obs;
    return true;
  }

  bool operator()(double x, double y, double& res) const
  {
    return error(x,y,res);
  }

  template<class Mat>
  void analytical_derivative(double x, double y, Mat& mat1, Mat& mat2) const
  {
    static_assert(boost::is_same<Analytical,Derivator>::value,"Only for analytical mode");
    mat1(0,0) = y;
    mat2(0,0) = x;
  }

  template<class AD>
  bool automatic(double, AD x[1], double, AD y[1], AD res[1]) const
  {
    static_assert(boost::is_same<Automatic,Derivator>::value,"Only for automatic mode");
    return error(x[0],y[0],res[0]);
  }
};

template<class F> std::tuple<double,double> solve(double y, double x, std::vector<double> obs)
{
  Solver<F> solver;
  for(double o : obs)
    solver.add(F(o),&x,&y);
  solver.solve(DENSE,minimal_verbose());
  return std::make_tuple(x,y);
}

std::ostream& operator<<(std::ostream& o, std::tuple<double,double> tuple)
{
  return o << std::get<0>(tuple) << "," << std::get<1>(tuple) << std::endl;
}

struct None{};

int main()
{
  std::vector<double> obs = {0.1,0.2,-0.05,-0.15,0.0};
  std::cout << solve<F<None>>(1,1,obs) << std::endl;
  std::cout << solve<F<NumericCentral>>(1,1,obs) << std::endl;
  std::cout << solve<F<NumericForward>>(1,1,obs) << std::endl;
  std::cout << solve<F<Analytical>>(1,1,obs) << std::endl;

  #if __cplusplus > 201103L
  std::cout << solve<F<Automatic>>(1,1,obs) << std::endl;
  #endif
  return EXIT_SUCCESS;
}


/*
 *  (C) Copyright Nick Thompson 2018.
 *  Use, modification and distribution are 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 "multiprecision_config.hpp"

#ifndef DISABLE_MP_TESTS
#include <boost/integer/extended_euclidean.hpp>
#include <boost/cstdint.hpp>
#include <boost/core/lightweight_test.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/integer/common_factor.hpp>

using boost::multiprecision::int128_t;
using boost::multiprecision::int256_t;
using boost::integer::extended_euclidean;
using boost::integer::gcd;

template<class Z>
void test_extended_euclidean()
{
    // Stress test:
    //Z max_arg = std::numeric_limits<Z>::max();
    Z max_arg = 500;
    for (Z m = max_arg; m > 0; --m)
    {
        for (Z n = max_arg; n > 0; --n)
        {
            boost::integer::euclidean_result_t<Z> u = extended_euclidean(m, n);
            int256_t gcdmn = gcd(m, n);
            int256_t x = u.x;
            int256_t y = u.y;
            BOOST_TEST_EQ(u.gcd, gcdmn);
            BOOST_TEST_EQ(m*x + n*y, gcdmn);
        }
    }
}

int main()
{
    test_extended_euclidean<boost::int16_t>();
    test_extended_euclidean<boost::int32_t>();
    test_extended_euclidean<boost::int64_t>();
    test_extended_euclidean<int128_t>();

    return boost::report_errors();;
}
#else
int main()
{
    return 0;
}
#endif


#include <boost/algorithm/hex.hpp>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

#include <nil/crypto3/zk/components/blueprint.hpp>
#include <nil/crypto3/zk/components/blueprint_variable.hpp>

#include <nil/crypto3/zk/snark/schemes/ppzksnark/r1cs_gg_ppzksnark.hpp>
#include <nil/crypto3/zk/snark/schemes/ppzksnark/r1cs_gg_ppzksnark/marshalling.hpp>

#include <nil/crypto3/zk/snark/algorithms/generate.hpp>
#include <nil/crypto3/zk/snark/algorithms/verify.hpp>
#include <nil/crypto3/zk/snark/algorithms/prove.hpp>

#include "./types.h"
#include "./utils.cpp"
#include "./circuit.h"

using namespace std;

using namespace nil::crypto3::zk::components;
using namespace nil::crypto3::zk::snark;

ushort INVALID_PROOF_RETURN_CODE = 200;

int setup_keys(boost::filesystem::path pk_path, boost::filesystem::path vk_path) {
    blueprint<field_type> bp;
    LocationCircuit circuit(bp);
    circuit.generate_r1cs_constraints(bp);

    cout << "Blueprint size: " << bp.num_variables() << endl;
    cout << "Generating constraint system..." << endl;
    const r1cs_constraint_system<field_type> constraint_system = bp.get_constraint_system();
    cout << "Number of R1CS constraints: " << constraint_system.num_constraints() << endl;

    cout << "Generating keypair..." << endl;
    scheme_type::keypair_type keypair = generate<scheme_type>(constraint_system);

    cout << "Saving proving key to a file " << pk_path<< endl;
    save_proving_key(keypair.first, pk_path);

    cout << "Saving verification key to a file " << vk_path << endl;
    save_verification_key(keypair.second, vk_path);

    return 0;
}

int create_proof(boost::filesystem::path pk_path, boost::filesystem::path proof_path, boost::filesystem::path pi_path,float minLat, float maxLat, float minLng, float maxLng, float posLat, float posLng) {
    cout << "Loading proving key from a file " << pk_path << endl;
    typename scheme_type::proving_key_type pk = load_proving_key(pk_path);

    blueprint<field_type> bp;
    LocationCircuit circuit(bp);
    cout << "Generating constraint system..." << endl;
    circuit.generate_r1cs_constraints(bp);
    cout << "Generating witness..." << endl;
    circuit.generate_r1cs_witness(bp, minLat, maxLat, minLng, maxLng, posLat, posLng);

    cout << "Blueprint is satisfied: " << bp.is_satisfied() << endl;
    if (!bp.is_satisfied()) {
        return INVALID_PROOF_RETURN_CODE;
    }

    cout << "Generating proof..." << endl;
    const scheme_type::proof_type proof = prove<scheme_type>(pk, bp.primary_input(), bp.auxiliary_input());

    cout << "Saving proof to file " << proof_path << endl;
    save_proof(proof, proof_path);

    cout << "Saving primary input to file " << pi_path << endl;
    save_primary_input(bp.primary_input(), pi_path);
    return 0;
}

int verify_proof(boost::filesystem::path proof_path, boost::filesystem::path vk_path, boost::filesystem::path pi_path) {
    cout << "Loading proof from a file " << proof_path << endl;
    typename scheme_type::proof_type proof = load_proof(proof_path);

    cout << "Loading primary input from a file " << pi_path << endl;
    r1cs_primary_input<field_type> input = load_primary_input(pi_path);

    cout << "Loading verification key from a file " << vk_path << endl;
    typename scheme_type::verification_key_type vk = load_verification_key(vk_path);

    // verify
    using basic_proof_system = r1cs_gg_ppzksnark<curve_type>;
    const bool verified = verify<basic_proof_system>(vk, input, proof);
    cout << "Verification status: " << verified << endl;

    return verified ? 0 : INVALID_PROOF_RETURN_CODE;
}

int main(int argc, char *argv[]) {
    float minLat, maxLat, minLng, maxLng, posLat, posLng;
    boost::filesystem::path pk_path, vk_path, proof_path, pi_path;
    // bool hexFlag;

    boost::program_options::options_description options("CLI Proof Generator");
    options.add_options()
    // ("hex,h", boost::program_options::bool_switch(&hexFlag), "print only hex proof to output")
    ("minLat,minLat", boost::program_options::value<float>(&minLat)->default_value(0))
    ("maxLat,maxLat", boost::program_options::value<float>(&maxLat)->default_value(0))
    ("minLng,minLng", boost::program_options::value<float>(&minLng)->default_value(0))
    ("maxLng,maxLng", boost::program_options::value<float>(&maxLng)->default_value(0))
    ("posLat,posLat", boost::program_options::value<float>(&posLat)->default_value(0))
    ("posLng,posLng", boost::program_options::value<float>(&posLng)->default_value(0))
    ("proving-key-path,pk", boost::program_options::value<boost::filesystem::path>(&pk_path)->default_value("proving.key"))
    ("verification-key-path,vk", boost::program_options::value<boost::filesystem::path>(&vk_path)->default_value("verification.key"))
    ("proof-path,p", boost::program_options::value<boost::filesystem::path>(&proof_path)->default_value("proof"))
    ("primary-input-path,pi", boost::program_options::value<boost::filesystem::path>(&pi_path)->default_value("primary.input"));

    boost::program_options::variables_map vm;
    boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(options).run(), vm);
    boost::program_options::notify(vm);

    cout << setprecision(16);

    if (!argv[1]) {
        cout << "Please select a command: [setup/prove/verify]" << endl;
        return 0;
    }
    else if (string(argv[1]) == "setup") {
        // Generate proving.key & verification.key
        return setup_keys(pk_path, vk_path);
    } else if (string(argv[1]) == "prove") {
        // Generate and  save proof and primary_input to file
        return create_proof(pk_path, proof_path, pi_path, minLat, maxLat, minLng, maxLng, posLat, posLng);
    } else if (string(argv[1]) == "verify") {
        // Check the status of the proof
        return verify_proof(proof_path, pi_path, vk_path);
    }

    return 0;
}


#define EIGEN_RUNTIME_NO_MALLOC
#include <Eigen/Core>
#include <catch2/catch.hpp>
#include "ear/dsp/delay_buffer.hpp"
#include "ear/dsp/ptr_adapter.hpp"
#include "ear/helpers/assert.hpp"

using namespace ear;
using namespace ear::dsp;

TEST_CASE("delay_buffer") {
  int delay = 128;
  DelayBuffer db(5, delay);

  REQUIRE(db.get_delay() == delay);

  Eigen::VectorXi block_sizes(3);
  block_sizes << 64, 128, 256;

  int testlen = block_sizes.sum();
  Eigen::MatrixXf input = Eigen::MatrixXf::Random(testlen, 5);
  Eigen::MatrixXf output = Eigen::MatrixXf::Zero(testlen, 5);
  Eigen::MatrixXf expected_output = Eigen::MatrixXf::Zero(testlen, 5);

  expected_output(Eigen::seqN(delay, testlen - delay), Eigen::all) =
      input(Eigen::seqN(0, testlen - delay), Eigen::all);

  PtrAdapter in_ptrs(5), out_ptrs(5);

  Eigen::internal::set_is_malloc_allowed(false);
  int offset = 0;
  for (auto &block_size : block_sizes) {
    auto block = Eigen::seqN(offset, block_size);
    in_ptrs.set_eigen(input(block, Eigen::all));
    out_ptrs.set_eigen(output(block, Eigen::all));
    db.process(block_size, in_ptrs.ptrs(), out_ptrs.ptrs());
    offset += block_size;
  }
  Eigen::internal::set_is_malloc_allowed(true);

  REQUIRE(output == expected_output);
}

TEST_CASE("single_channel") {
  int delay = 128;
  DelayBuffer db(1, delay);

  int testlen = 512;
  Eigen::VectorXf input = Eigen::VectorXf::Random(testlen);
  Eigen::VectorXf output = Eigen::VectorXf::Zero(testlen);
  Eigen::VectorXf expected_output = Eigen::VectorXf::Zero(testlen);

  expected_output(Eigen::seqN(delay, testlen - delay)) =
      input(Eigen::seqN(0, testlen - delay));

  PtrAdapter in_ptrs(1), out_ptrs(1);

  Eigen::internal::set_is_malloc_allowed(false);
  in_ptrs.set_eigen(input);
  out_ptrs.set_eigen(output);
  db.process(testlen, in_ptrs.ptrs(), out_ptrs.ptrs());
  Eigen::internal::set_is_malloc_allowed(true);

  REQUIRE(output == expected_output);
}


/**
 * \ file AttackReleaseHysteresisFilter.cpp
 */

#include <ATK/Dynamic/AttackReleaseHysteresisFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#include <boost/math/constants/constants.hpp>
#include <boost/scoped_array.hpp>

#define PROCESSSIZE (1024*64)

BOOST_AUTO_TEST_CASE( AttackReleaseHysteresisFilter_triangle_test )
{
  boost::scoped_array<float> data(new float[PROCESSSIZE]);
  for(int64_t i = 0; i < PROCESSSIZE/2; ++i)
  {
    data[i] = i / 48000;
  }
  for(int64_t i = 0; i < PROCESSSIZE/2; ++i)
  {
    data[PROCESSSIZE/2 + i] = (PROCESSSIZE/2 - i) / 48000;
  }

  ATK::InPointerFilter<float> generator(data.get(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  boost::scoped_array<float> outdata(new float[PROCESSSIZE]);

  ATK::AttackReleaseHysteresisFilter<float> filter(1);
  filter.set_attack(std::exp(-1./(48000 * 1e-3)));
  filter.set_release(std::exp(-1./(48000 * 100e-3)));
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);

  ATK::OutPointerFilter<float> output(outdata.get(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(int64_t i = 0; i < PROCESSSIZE/2; ++i)
  {
    BOOST_REQUIRE_GE(data[i], outdata[i]);
  }
  for(int64_t i = 0; i < PROCESSSIZE/2; ++i)
  {
    BOOST_REQUIRE_GE(outdata[PROCESSSIZE/2+i], outdata[PROCESSSIZE/2+i-1]);
  }
}

#define CUSTOMPROCESSSIZE 7

BOOST_AUTO_TEST_CASE( AttackReleaseHysteresisFilter_release_custom_test )
{
  float data[] = {0., 1., .5, .4, .3, .2, .1};
  float target[] = {0., 1., 1., .46, .46, .226, .1126};

  ATK::InPointerFilter<float> generator(data, 1, CUSTOMPROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  boost::scoped_array<float> outdata(new float[CUSTOMPROCESSSIZE]);

  ATK::AttackReleaseHysteresisFilter<float> filter(1);
  filter.set_attack(0);
  filter.set_release(.1);
  filter.set_release_hysteresis(.5);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);

  ATK::OutPointerFilter<float> output(outdata.get(), 1, CUSTOMPROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(CUSTOMPROCESSSIZE);

  for(int64_t i = 0; i < CUSTOMPROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(target[i], outdata[i], .001);
  }
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"
#include <Eigen/SVD>

template<typename MatrixType, typename JacobiScalar>
void jacobi(const MatrixType& m = MatrixType())
{
  Index rows = m.rows();
  Index cols = m.cols();

  enum {
    RowsAtCompileTime = MatrixType::RowsAtCompileTime,
    ColsAtCompileTime = MatrixType::ColsAtCompileTime
  };

  typedef Matrix<JacobiScalar, 2, 1> JacobiVector;

  const MatrixType a(MatrixType::Random(rows, cols));

  JacobiVector v = JacobiVector::Random().normalized();
  JacobiScalar c = v.x(), s = v.y();
  JacobiRotation<JacobiScalar> rot(c, s);

  {
    Index p = internal::random<Index>(0, rows-1);
    Index q;
    do {
      q = internal::random<Index>(0, rows-1);
    } while (q == p);

    MatrixType b = a;
    b.applyOnTheLeft(p, q, rot);
    VERIFY_IS_APPROX(b.row(p), c * a.row(p) + numext::conj(s) * a.row(q));
    VERIFY_IS_APPROX(b.row(q), -s * a.row(p) + numext::conj(c) * a.row(q));
  }

  {
    Index p = internal::random<Index>(0, cols-1);
    Index q;
    do {
      q = internal::random<Index>(0, cols-1);
    } while (q == p);

    MatrixType b = a;
    b.applyOnTheRight(p, q, rot);
    VERIFY_IS_APPROX(b.col(p), c * a.col(p) - s * a.col(q));
    VERIFY_IS_APPROX(b.col(q), numext::conj(s) * a.col(p) + numext::conj(c) * a.col(q));
  }
}

void test_jacobi()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1(( jacobi<Matrix3f, float>() ));
    CALL_SUBTEST_2(( jacobi<Matrix4d, double>() ));
    CALL_SUBTEST_3(( jacobi<Matrix4cf, float>() ));
    CALL_SUBTEST_3(( jacobi<Matrix4cf, std::complex<float> >() ));

    int r = internal::random<int>(2, internal::random<int>(1,EIGEN_TEST_MAX_SIZE)/2),
        c = internal::random<int>(2, internal::random<int>(1,EIGEN_TEST_MAX_SIZE)/2);
    CALL_SUBTEST_4(( jacobi<MatrixXf, float>(MatrixXf(r,c)) ));
    CALL_SUBTEST_5(( jacobi<MatrixXcd, double>(MatrixXcd(r,c)) ));
    CALL_SUBTEST_5(( jacobi<MatrixXcd, std::complex<double> >(MatrixXcd(r,c)) ));
    // complex<float> is really important to test as it is the only way to cover conjugation issues in certain unaligned paths
    CALL_SUBTEST_6(( jacobi<MatrixXcf, float>(MatrixXcf(r,c)) ));
    CALL_SUBTEST_6(( jacobi<MatrixXcf, std::complex<float> >(MatrixXcf(r,c)) ));

    TEST_SET_BUT_UNUSED_VARIABLE(r);
    TEST_SET_BUT_UNUSED_VARIABLE(c);
  }
}


#ifndef STAN_MATH_PRIM_SCAL_FUN_INC_BETA_HPP
#define STAN_MATH_PRIM_SCAL_FUN_INC_BETA_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/err.hpp>
#include <stan/math/prim/scal/fun/boost_policy.hpp>
#include <boost/math/special_functions/beta.hpp>

namespace stan {
namespace math {

/**
 * The normalized incomplete beta function of a, b, with outcome x.
 *
 * Used to compute the cumulative density function for the beta
 * distribution.
 *
 * @param a Shape parameter a >= 0; a and b can't both be 0
 * @param b Shape parameter b >= 0
 * @param x Random variate. 0 <= x <= 1
 * @throws if constraints are violated or if any argument is NaN
 * @return The normalized incomplete beta function.
 */
inline double inc_beta(double a, double b, double x) {
  check_not_nan("inc_beta", "a", a);
  check_not_nan("inc_beta", "b", b);
  check_not_nan("inc_beta", "x", x);
  return boost::math::ibeta(a, b, x, boost_policy_t());
}

}  // namespace math
}  // namespace stan
#endif


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "massmatrix.h"
#include "normalize_row_sums.h"
#include "sparse.h"
#include "doublearea.h"
#include "repmat.h"
#include <Eigen/Geometry>
#include <iostream>

template <typename DerivedV, typename DerivedF, typename Scalar>
IGL_INLINE void igl::massmatrix(
  const Eigen::MatrixBase<DerivedV> & V,
  const Eigen::MatrixBase<DerivedF> & F,
  const MassMatrixType type,
  Eigen::SparseMatrix<Scalar>& M)
{
  using namespace Eigen;
  using namespace std;

  const int n = V.rows();
  const int m = F.rows();
  const int simplex_size = F.cols();

  MassMatrixType eff_type = type;
  // Use voronoi of for triangles by default, otherwise barycentric
  if(type == MASSMATRIX_TYPE_DEFAULT)
  {
    eff_type = (simplex_size == 3?MASSMATRIX_TYPE_VORONOI:MASSMATRIX_TYPE_BARYCENTRIC);
  }

  // Not yet supported
  assert(type!=MASSMATRIX_TYPE_FULL);

  Matrix<int,Dynamic,1> MI;
  Matrix<int,Dynamic,1> MJ;
  Matrix<Scalar,Dynamic,1> MV;
  if(simplex_size == 3)
  {
    // Triangles
    // edge lengths numbered same as opposite vertices
    Matrix<Scalar,Dynamic,3> l(m,3);
    // loop over faces
    for(int i = 0;i<m;i++)
    {
      l(i,0) = (V.row(F(i,1))-V.row(F(i,2))).norm();
      l(i,1) = (V.row(F(i,2))-V.row(F(i,0))).norm();
      l(i,2) = (V.row(F(i,0))-V.row(F(i,1))).norm();
    }
    Matrix<Scalar,Dynamic,1> dblA;
    doublearea(l,dblA);

    switch(eff_type)
    {
      case MASSMATRIX_TYPE_BARYCENTRIC:
        // diagonal entries for each face corner
        MI.resize(m*3,1); MJ.resize(m*3,1); MV.resize(m*3,1);
        MI.block(0*m,0,m,1) = F.col(0);
        MI.block(1*m,0,m,1) = F.col(1);
        MI.block(2*m,0,m,1) = F.col(2);
        MJ = MI;
        repmat(dblA,3,1,MV);
        MV.array() /= 6.0;
        break;
      case MASSMATRIX_TYPE_VORONOI:
        {
          // diagonal entries for each face corner
          // http://www.alecjacobson.com/weblog/?p=874
          MI.resize(m*3,1); MJ.resize(m*3,1); MV.resize(m*3,1);
          MI.block(0*m,0,m,1) = F.col(0);
          MI.block(1*m,0,m,1) = F.col(1);
          MI.block(2*m,0,m,1) = F.col(2);
          MJ = MI;

          // Holy shit this needs to be cleaned up and optimized
          Matrix<Scalar,Dynamic,3> cosines(m,3);
          cosines.col(0) =
            (l.col(2).array().pow(2)+l.col(1).array().pow(2)-l.col(0).array().pow(2))/(l.col(1).array()*l.col(2).array()*2.0);
          cosines.col(1) =
            (l.col(0).array().pow(2)+l.col(2).array().pow(2)-l.col(1).array().pow(2))/(l.col(2).array()*l.col(0).array()*2.0);
          cosines.col(2) =
            (l.col(1).array().pow(2)+l.col(0).array().pow(2)-l.col(2).array().pow(2))/(l.col(0).array()*l.col(1).array()*2.0);
          Matrix<Scalar,Dynamic,3> barycentric = cosines.array() * l.array();
          normalize_row_sums(barycentric,barycentric);
          Matrix<Scalar,Dynamic,3> partial = barycentric;
          partial.col(0).array() *= dblA.array() * 0.5;
          partial.col(1).array() *= dblA.array() * 0.5;
          partial.col(2).array() *= dblA.array() * 0.5;
          Matrix<Scalar,Dynamic,3> quads(partial.rows(),partial.cols());
          quads.col(0) = (partial.col(1)+partial.col(2))*0.5;
          quads.col(1) = (partial.col(2)+partial.col(0))*0.5;
          quads.col(2) = (partial.col(0)+partial.col(1))*0.5;

          quads.col(0) = (cosines.col(0).array()<0).select( 0.25*dblA,quads.col(0));
          quads.col(1) = (cosines.col(0).array()<0).select(0.125*dblA,quads.col(1));
          quads.col(2) = (cosines.col(0).array()<0).select(0.125*dblA,quads.col(2));

          quads.col(0) = (cosines.col(1).array()<0).select(0.125*dblA,quads.col(0));
          quads.col(1) = (cosines.col(1).array()<0).select(0.25*dblA,quads.col(1));
          quads.col(2) = (cosines.col(1).array()<0).select(0.125*dblA,quads.col(2));

          quads.col(0) = (cosines.col(2).array()<0).select(0.125*dblA,quads.col(0));
          quads.col(1) = (cosines.col(2).array()<0).select(0.125*dblA,quads.col(1));
          quads.col(2) = (cosines.col(2).array()<0).select( 0.25*dblA,quads.col(2));

          MV.block(0*m,0,m,1) = quads.col(0);
          MV.block(1*m,0,m,1) = quads.col(1);
          MV.block(2*m,0,m,1) = quads.col(2);

          break;
        }
      case MASSMATRIX_TYPE_FULL:
        assert(false && "Implementation incomplete");
        break;
      default:
        assert(false && "Unknown Mass matrix eff_type");
    }

  }else if(simplex_size == 4)
  {
    assert(V.cols() == 3);
    assert(eff_type == MASSMATRIX_TYPE_BARYCENTRIC);
    MI.resize(m*4,1); MJ.resize(m*4,1); MV.resize(m*4,1);
    MI.block(0*m,0,m,1) = F.col(0);
    MI.block(1*m,0,m,1) = F.col(1);
    MI.block(2*m,0,m,1) = F.col(2);
    MI.block(3*m,0,m,1) = F.col(3);
    MJ = MI;
    // loop over tets
    for(int i = 0;i<m;i++)
    {
      // http://en.wikipedia.org/wiki/Tetrahedron#Volume
      Matrix<Scalar,3,1> v0m3 = V.row(F(i,0)) - V.row(F(i,3));
      Matrix<Scalar,3,1> v1m3 = V.row(F(i,1)) - V.row(F(i,3));
      Matrix<Scalar,3,1> v2m3 = V.row(F(i,2)) - V.row(F(i,3));
      Scalar v = fabs(v0m3.dot(v1m3.cross(v2m3)))/6.0;
      MV(i+0*m) = v/4.0;
      MV(i+1*m) = v/4.0;
      MV(i+2*m) = v/4.0;
      MV(i+3*m) = v/4.0;
    }
  }else
  {
    // Unsupported simplex size
    assert(false && "Unsupported simplex size");
  }
  sparse(MI,MJ,MV,n,n,M);
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template specialization
// generated by autoexplicit.sh
template void igl::massmatrix<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 4, 0, -1, 4>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 4, 0, -1, 4> > const&, igl::MassMatrixType, Eigen::SparseMatrix<double, 0, int>&);
// generated by autoexplicit.sh
template void igl::massmatrix<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, igl::MassMatrixType, Eigen::SparseMatrix<double, 0, int>&);
template void igl::massmatrix<Eigen::Matrix<double, -1, 3, 1, -1, 3>, Eigen::Matrix<int, -1, 3, 1, -1, 3>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 1, -1, 3> > const&, igl::MassMatrixType, Eigen::SparseMatrix<double, 0, int>&);
template void igl::massmatrix<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, igl::MassMatrixType, Eigen::SparseMatrix<double, 0, int>&);
#endif


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Vincent Rouvreau
 *
 *    Copyright (C) 2014 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#include <boost/program_options.hpp>
#include <boost/variant.hpp>

#include <gudhi/Alpha_complex_3d.h>
#include <gudhi/Simplex_tree.h>
#include <gudhi/Persistent_cohomology.h>
#include <gudhi/Points_3D_off_io.h>

#include <fstream>
#include <string>
#include <vector>
#include <limits>  // for numeric_limits<>

// gudhi type definition
using Simplex_tree = Gudhi::Simplex_tree<Gudhi::Simplex_tree_options_fast_persistence>;
using Filtration_value = Simplex_tree::Filtration_value;
using Persistent_cohomology =
    Gudhi::persistent_cohomology::Persistent_cohomology<Simplex_tree, Gudhi::persistent_cohomology::Field_Zp>;

void program_options(int argc, char *argv[], std::string &off_file_points, bool &exact, bool &safe,
                     std::string &weight_file, std::string &cuboid_file, std::string &output_file_diag,
                     Filtration_value &alpha_square_max_value, int &coeff_field_characteristic,
                     Filtration_value &min_persistence);

bool read_weight_file(const std::string &weight_file, std::vector<double> &weights) {
  // Read weights information from file
  std::ifstream weights_ifstr(weight_file);
  if (weights_ifstr.good()) {
    double weight = 0.0;
    // Attempt read the weight in a double format, return false if it fails
    while (weights_ifstr >> weight) {
      weights.push_back(weight);
    }
  } else {
    return false;
  }
  return true;
}

bool read_cuboid_file(const std::string &cuboid_file, double &x_min, double &y_min, double &z_min, double &x_max,
                      double &y_max, double &z_max) {
  // Read weights information from file
  std::ifstream iso_cuboid_str(cuboid_file);
  if (iso_cuboid_str.is_open()) {
    if (!(iso_cuboid_str >> x_min >> y_min >> z_min >> x_max >> y_max >> z_max)) {
      return false;
    }
  } else {
    return false;
  }
  return true;
}

template <typename AlphaComplex3d>
std::vector<typename AlphaComplex3d::Bare_point_3> read_off(const std::string &off_file_points) {
  // Read the OFF file (input file name given as parameter) and triangulate points
  Gudhi::Points_3D_off_reader<typename AlphaComplex3d::Bare_point_3> off_reader(off_file_points);
  // Check the read operation was correct
  if (!off_reader.is_valid()) {
    std::cerr << "Unable to read OFF file " << off_file_points << std::endl;
    exit(-1);
  }
  return off_reader.get_point_cloud();
}

int main(int argc, char **argv) {
  std::string off_file_points;
  std::string weight_file;
  std::string cuboid_file;
  std::string output_file_diag;
  Filtration_value alpha_square_max_value = 0.;
  int coeff_field_characteristic = 0;
  Filtration_value min_persistence = 0.;
  bool exact_version = false;
  bool fast_version = false;
  bool weighted_version = false;
  bool periodic_version = false;

  program_options(argc, argv, off_file_points, exact_version, fast_version, weight_file, cuboid_file, output_file_diag,
                  alpha_square_max_value, coeff_field_characteristic, min_persistence);

  std::vector<double> weights;
  if (weight_file != std::string()) {
    if (!read_weight_file(weight_file, weights)) {
      std::cerr << "Unable to read weights file " << weight_file << std::endl;
      exit(-1);
    }
    weighted_version = true;
  }

  double x_min = 0., y_min = 0., z_min = 0., x_max = 0., y_max = 0., z_max = 0.;
  std::ifstream iso_cuboid_str(argv[3]);
  if (cuboid_file != std::string()) {
    if (!read_cuboid_file(cuboid_file, x_min, y_min, z_min, x_max, y_max, z_max)) {
      std::cerr << "Unable to read cuboid file " << cuboid_file << std::endl;
      exit(-1);
    }
    periodic_version = true;
  }

  Gudhi::alpha_complex::complexity complexity = Gudhi::alpha_complex::complexity::SAFE;
  if (exact_version) {
    if (fast_version) {
      std::cerr << "You cannot set the exact and the fast version." << std::endl;
      exit(-1);
    }
    complexity = Gudhi::alpha_complex::complexity::EXACT;
  }
  if (fast_version) {
    complexity = Gudhi::alpha_complex::complexity::FAST;
  }

  Simplex_tree simplex_tree;

  switch (complexity) {
    case Gudhi::alpha_complex::complexity::FAST:
      if (weighted_version) {
        if (periodic_version) {
          using Alpha_complex_3d =
              Gudhi::alpha_complex::Alpha_complex_3d<Gudhi::alpha_complex::complexity::FAST, true, true>;
          auto points = read_off<Alpha_complex_3d>(off_file_points);
          Alpha_complex_3d alpha_complex(points, weights, x_min, y_min, z_min, x_max, y_max, z_max);
          alpha_complex.create_complex(simplex_tree, alpha_square_max_value);
        } else {
          using Alpha_complex_3d =
              Gudhi::alpha_complex::Alpha_complex_3d<Gudhi::alpha_complex::complexity::FAST, true, false>;
          auto points = read_off<Alpha_complex_3d>(off_file_points);
          Alpha_complex_3d alpha_complex(points, weights);
          alpha_complex.create_complex(simplex_tree, alpha_square_max_value);
        }
      } else {
        if (periodic_version) {
          using Alpha_complex_3d =
              Gudhi::alpha_complex::Alpha_complex_3d<Gudhi::alpha_complex::complexity::FAST, false, true>;
          auto points = read_off<Alpha_complex_3d>(off_file_points);
          Alpha_complex_3d alpha_complex(points, x_min, y_min, z_min, x_max, y_max, z_max);
          alpha_complex.create_complex(simplex_tree, alpha_square_max_value);
        } else {
          using Alpha_complex_3d =
              Gudhi::alpha_complex::Alpha_complex_3d<Gudhi::alpha_complex::complexity::FAST, false, false>;
          auto points = read_off<Alpha_complex_3d>(off_file_points);
          Alpha_complex_3d alpha_complex(points);
          alpha_complex.create_complex(simplex_tree, alpha_square_max_value);
        }
      }
      break;
    case Gudhi::alpha_complex::complexity::EXACT:
      if (weighted_version) {
        if (periodic_version) {
          using Alpha_complex_3d =
              Gudhi::alpha_complex::Alpha_complex_3d<Gudhi::alpha_complex::complexity::EXACT, true, true>;
          auto points = read_off<Alpha_complex_3d>(off_file_points);
          Alpha_complex_3d alpha_complex(points, weights, x_min, y_min, z_min, x_max, y_max, z_max);
          alpha_complex.create_complex(simplex_tree, alpha_square_max_value);
        } else {
          using Alpha_complex_3d =
              Gudhi::alpha_complex::Alpha_complex_3d<Gudhi::alpha_complex::complexity::EXACT, true, false>;
          auto points = read_off<Alpha_complex_3d>(off_file_points);
          Alpha_complex_3d alpha_complex(points, weights);
          alpha_complex.create_complex(simplex_tree, alpha_square_max_value);
        }
      } else {
        if (periodic_version) {
          using Alpha_complex_3d =
              Gudhi::alpha_complex::Alpha_complex_3d<Gudhi::alpha_complex::complexity::EXACT, false, true>;
          auto points = read_off<Alpha_complex_3d>(off_file_points);
          Alpha_complex_3d alpha_complex(points, x_min, y_min, z_min, x_max, y_max, z_max);
          alpha_complex.create_complex(simplex_tree, alpha_square_max_value);
        } else {
          using Alpha_complex_3d =
              Gudhi::alpha_complex::Alpha_complex_3d<Gudhi::alpha_complex::complexity::EXACT, false, false>;
          auto points = read_off<Alpha_complex_3d>(off_file_points);
          Alpha_complex_3d alpha_complex(points);
          alpha_complex.create_complex(simplex_tree, alpha_square_max_value);
        }
      }
      break;
    case Gudhi::alpha_complex::complexity::SAFE:
      if (weighted_version) {
        if (periodic_version) {
          using Alpha_complex_3d =
              Gudhi::alpha_complex::Alpha_complex_3d<Gudhi::alpha_complex::complexity::SAFE, true, true>;
          auto points = read_off<Alpha_complex_3d>(off_file_points);
          Alpha_complex_3d alpha_complex(points, weights, x_min, y_min, z_min, x_max, y_max, z_max);
          alpha_complex.create_complex(simplex_tree, alpha_square_max_value);
        } else {
          using Alpha_complex_3d =
              Gudhi::alpha_complex::Alpha_complex_3d<Gudhi::alpha_complex::complexity::SAFE, true, false>;
          auto points = read_off<Alpha_complex_3d>(off_file_points);
          Alpha_complex_3d alpha_complex(points, weights);
          alpha_complex.create_complex(simplex_tree, alpha_square_max_value);
        }
      } else {
        if (periodic_version) {
          using Alpha_complex_3d =
              Gudhi::alpha_complex::Alpha_complex_3d<Gudhi::alpha_complex::complexity::SAFE, false, true>;
          auto points = read_off<Alpha_complex_3d>(off_file_points);
          Alpha_complex_3d alpha_complex(points, x_min, y_min, z_min, x_max, y_max, z_max);
          alpha_complex.create_complex(simplex_tree, alpha_square_max_value);
        } else {
          using Alpha_complex_3d =
              Gudhi::alpha_complex::Alpha_complex_3d<Gudhi::alpha_complex::complexity::SAFE, false, false>;
          auto points = read_off<Alpha_complex_3d>(off_file_points);
          Alpha_complex_3d alpha_complex(points);
          alpha_complex.create_complex(simplex_tree, alpha_square_max_value);
        }
      }
      break;
    default:
      std::cerr << "Unknown complexity value " << std::endl;
      exit(-1);
      break;
  }

  std::clog << "Simplex_tree dim: " << simplex_tree.dimension() << std::endl;
  // Compute the persistence diagram of the complex
  Persistent_cohomology pcoh(simplex_tree, true);
  // initializes the coefficient field for homology
  pcoh.init_coefficients(coeff_field_characteristic);

  pcoh.compute_persistent_cohomology(min_persistence);

  // Output the diagram in filediag
  if (output_file_diag.empty()) {
    pcoh.output_diagram();
  } else {
    std::clog << "Result in file: " << output_file_diag << std::endl;
    std::ofstream out(output_file_diag);
    pcoh.output_diagram(out);
    out.close();
  }

  return 0;
}

void program_options(int argc, char *argv[], std::string &off_file_points, bool &exact, bool &fast,
                     std::string &weight_file, std::string &cuboid_file, std::string &output_file_diag,
                     Filtration_value &alpha_square_max_value, int &coeff_field_characteristic,
                     Filtration_value &min_persistence) {
  namespace po = boost::program_options;
  po::options_description hidden("Hidden options");
  hidden.add_options()("input-file", po::value<std::string>(&off_file_points),
                       "Name of file containing a point set. Format is one point per line:   X1 ... Xd ");

  po::options_description visible("Allowed options", 100);
  visible.add_options()("help,h", "produce help message")(
      "exact,e", po::bool_switch(&exact),
      "To activate exact version of Alpha complex 3d (default is false, not available if fast is set)")(
      "fast,f", po::bool_switch(&fast),
      "To activate fast version of Alpha complex 3d (default is false, not available if exact is set)")(
      "weight-file,w", po::value<std::string>(&weight_file)->default_value(std::string()),
      "Name of file containing a point weights. Format is one weight per line:\n  W1\n  ...\n  Wn ")(
      "cuboid-file,c", po::value<std::string>(&cuboid_file),
      "Name of file describing the periodic domain. Format is:\n  min_hx min_hy min_hz\n  max_hx max_hy max_hz")(
      "output-file,o", po::value<std::string>(&output_file_diag)->default_value(std::string()),
      "Name of file in which the persistence diagram is written. Default print in std::clog")(
      "max-alpha-square-value,r",
      po::value<Filtration_value>(&alpha_square_max_value)
          ->default_value(std::numeric_limits<Filtration_value>::infinity()),
      "Maximal alpha square value for the Alpha complex construction.")(
      "field-charac,p", po::value<int>(&coeff_field_characteristic)->default_value(11),
      "Characteristic p of the coefficient field Z/pZ for computing homology.")(
      "min-persistence,m", po::value<Filtration_value>(&min_persistence),
      "Minimal lifetime of homology feature to be recorded. Default is 0. Enter a negative value to see zero length "
      "intervals");

  po::positional_options_description pos;
  pos.add("input-file", 1);

  po::options_description all;
  all.add(visible).add(hidden);

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).options(all).positional(pos).run(), vm);
  po::notify(vm);

  if (vm.count("help") || !vm.count("input-file") || !vm.count("weight-file")) {
    std::clog << std::endl;
    std::clog << "Compute the persistent homology with coefficient field Z/pZ \n";
    std::clog << "of a 3D Alpha complex defined on a set of input points.\n";
    std::clog << "3D Alpha complex can be safe (by default) exact or fast, weighted and/or periodic\n\n";
    std::clog << "The output diagram contains one bar per line, written with the convention: \n";
    std::clog << "   p   dim b d \n";
    std::clog << "where dim is the dimension of the homological feature,\n";
    std::clog << "b and d are respectively the birth and death of the feature and \n";
    std::clog << "p is the characteristic of the field Z/pZ used for homology coefficients.\n\n";

    std::clog << "Usage: " << argv[0] << " [options] input-file weight-file\n\n";
    std::clog << visible << std::endl;
    exit(-1);
  }
}