/**
 * ravg.cpp
 *
 * 2021 Gabriel Moreira
 *
 * https://github.com/gabmoreira/maks
 *
 * This software and the related documents  are provided as  is,  with no express
 * or implied  warranties,  other  than those  that are  expressly stated  in the
 * License.
 *
 * Copyright © 2021 Gabriel Moreira. All rights reserved.
 */

#include <iostream>
#include <vector>
#include <string>
#include <cmath>
#include <chrono>

#include <Eigen/Eigenvalues>

#include "linalg.hpp"
#include "err.hpp"
#include "ravg.hpp"

using std::vector;
using std::string;

using Eigen::MatrixXd;
using Eigen::Matrix3d;
using Eigen::VectorXd;
using Eigen::Ref;

using namespace std::chrono;

typedef Eigen::Triplet<double>      Triplet;
typedef Eigen::SparseMatrix<double> Sparse;
typedef Eigen::PardisoLDLT<Sparse>  PardisoLDLT;

/**
 * Cycle graph solver in SO(3).
 *
 * Closed-form cycle graph rotation averaging solver in SO(3).
 *
 * @param edge_r (input) Eigen::MatrixXd - 3 x 3n Eigen matrix with SO(3) blocks. Assumes blocks are stacked as [ R_{1,2} R_{2,3} ... R_{n,1} ]
 * @param num_nodes (input) int - number of variables.
 * @param R (output) Eigen::MatrixXd - 3n x 3 Eigen matrix containing optimal solution.
 */
void cycleSolverSO3(const Ref<MatrixXd> edge_r, int num_nodes, Ref<MatrixXd> R) {
    auto start = high_resolution_clock::now();

    // Cycle error E = R_12 * R_23 * R_n-1,n * R_n,1
    MatrixXd E = Matrix3d::Identity();

    for (int i = 0; i < num_nodes; ++i)
        E = E * edge_r.block<3,3>(0, i*3);

    // Diagonal blocks of change-of-basis matrix U
    vector<Matrix3d> U(num_nodes);

    // Build first block
    Eigen::SelfAdjointEigenSolver<MatrixXd> eig(E + E.transpose());
    U[0] = eig.eigenvectors();

    // Build other blocks
    for (int i = 1; i < num_nodes; ++i)
        U[i] = edge_r.block<3,3>(0, (i-1)*4).transpose() * U[i-1];

    /* Change basis according to U^\top * Rtilde * U
     * We need only compute the corner block U_n * Rtilde_n1 * U_1 */
    MatrixXd one_param_edge_r = MatrixXd::Zero(3,3);
    one_param_edge_r += U[num_nodes-1].transpose() * edge_r.block<3,3>(0, (num_nodes-1)*3) * U[0];

    // Gamma is the angular cycle error
    double gamma_cos = 0.5 * (one_param_edge_r(0,0) + one_param_edge_r(1,1));
    double sine_sign = (double) ((one_param_edge_r(1,0) > 0) - (one_param_edge_r(1,0) < 0));
    double gamma     = sine_sign * acos(gamma_cos);

    double gamma_mean = gamma / num_nodes;
    double theoretical_min = - ((double) num_nodes) * (5.0f + 4.0f * cos(gamma_mean));

    // Build solution in SO(2) and change basis to produce solution in SO(3)
    R.block<3,3>(0,0) += U[0];
    VectorXd theta = VectorXd::Zero(num_nodes,1);

    for (int i = 1; i < num_nodes; ++i) {
        theta(i) += theta(i-1) + gamma_mean;
        double c = cos(theta(i));
        double s = sin(theta(i));

        R(i*3,0)   += c;
        R(i*3,1)   -= s;
        R(i*3+1,0) += s;
        R(i*3+1,1) += c;
        R(i*3+2,2) += 1;

        // Revert the basis-change
        R.block<3,3>(i*3,0) = U[i] * R.block<3,3>(i*3,0);
    };

    // Fix gauge freedom by setting R_1 = I_3
    R = R * R.block<3,3>(0,0).transpose();

    auto stop = high_resolution_clock::now();
    auto duration = duration_cast<microseconds>(stop - start);

    // Verbose
    printf("Cycle error (mean): %1.8f\n", gamma_mean);
    printf("Theoretical minimum: %1.8f\n", theoretical_min);
    printf("CPU time: %.6f\n", static_cast<long long int>(duration.count())*1e-6);
};

/**
 * Primal-Dual in SO(3)
 *
 * Primal-dual update method for averaging rotations in SO(3).
 * Finds R in SO(3)^n that minimizes the trace of R^\top Rtilde R
 *
 * @param Rtilde (input) Eigen::SparseMatrix<double> - 3n x 3n sparse rotation adjacency matrix.
 * @param A (input/output) Eigen::SparseMatrix<double> - n x n symmetric graph adjacency matrix.
 * @param R (output) Eigen::MatrixXd - 3n x 3 solution.
 * @param num_nodes (input) int - number of variables.
 * @param maxiter (input) int - maxiter.
 * @param dual (output) double - value of the dual problem.
 * @param eta (input) double - minimum eigenvalue stopping criterion.
 * @param sigma (input) double - spectral shift.
 */
void primalDualSO3(const Sparse& Rtilde,
                   const Sparse& A,
                   Ref<MatrixXd> R,
                   int num_nodes,
                   int maxiter,
                   double& dual,
                   double eta,
                   double sigma) {

    printf("\n%*s SO(3) Primal-dual\n", 20, " ");

    auto start = high_resolution_clock::now();

    // Graph degree vector
    VectorXd D = A * VectorXd::Ones(num_nodes);

    /* Initialize symmetric block diagonal Lagrange multiplier.
     * Should initialize all 3x3 blocks instead of just the main
     * diagonal to ensure the correct NNZ pattern. */
    vector<Triplet> lambda_buffer(num_nodes * 9);

    unsigned int k = 0;
    unsigned int j = 0;
    for (unsigned int i = 0; i < num_nodes; ++i) {
        lambda_buffer[j]   = Triplet( k,   k,   D(i) );
        lambda_buffer[j+1] = Triplet( k+1, k+1, D(i) );
        lambda_buffer[j+2] = Triplet( k+2, k+2, D(i) );
        lambda_buffer[j+3] = Triplet( k,   k+1, 1e-15 );
        lambda_buffer[j+4] = Triplet( k,   k+2, 1e-15 );
        lambda_buffer[j+5] = Triplet( k+1, k,   1e-15 );
        lambda_buffer[j+6] = Triplet( k+1, k+2, 1e-15 );
        lambda_buffer[j+7] = Triplet( k+2, k,   1e-15 );
        lambda_buffer[j+8] = Triplet( k+2, k+1, 1e-15 );
        j += 9;
        k += 3;
    };

    Sparse Lambda(num_nodes * 3, num_nodes * 3);
    Lambda.setFromTriplets(lambda_buffer.begin(), lambda_buffer.end());

    Sparse L = Lambda - Rtilde;
    L.makeCompressed();

    // LDLt solver used in the shift-and-invert spectral transform
    PardisoLDLT ldlt_solver;

    // Analyze the NNZ pattern
    ldlt_solver.analyzePattern(L);

    // Store eigenvalues
    VectorXd evals = VectorXd::Zero(3, 1);

    // Main primal-dual loop
    for (unsigned int i = 0; i < maxiter; ++i) {
        /* Call symmetric Krylov-Schur LDLt eigensolver on the matrix
         * Lambda - Rtilde. The three eigenvectors will be stored in R. */
        alg::symKrylovSchurLDL(&ldlt_solver, L, evals, R, 3, sigma);

        // Correct gauge freedom
        Matrix3d gauge = R.block<3,3>(0,0);
        R = R * gauge.inverse();

        // SO(3) projection
        for (unsigned int j = 0; j < 3*num_nodes; j += 3)
            alg::orthoProcrustesSO3(R.block<3,3>(j, 0));

        // Build Lambda from triplets for the next iteration
        MatrixXd RtildeR = Rtilde * R;
        dual = 3 * num_nodes;
        for (unsigned int k = 0; k < num_nodes*3; k+=3) {
            Matrix3d Rk = R.block<3,3>(k,0);
            Rk.transposeInPlace();
            MatrixXd block = RtildeR.block<3,3>(k,0) * Rk;

            // Dual <- + trace(Lambda_i)
            dual += block(0,0) + block(1,1) + block(2,2);

            L.coeffRef(k,k)     = block(0,0);
            L.coeffRef(k+1,k+1) = block(1,1);
            L.coeffRef(k+2,k+2) = block(2,2);
            L.coeffRef(k,k+1)   = 0.5 * (block(0,1) + block(1,0));
            L.coeffRef(k+1,k)   = L.coeffRef(k,k+1);
            L.coeffRef(k,k+2)   = 0.5 * (block(0,2) + block(2,0));
            L.coeffRef(k+2,k)   = L.coeffRef(k,k+2);
            L.coeffRef(k+1,k+2) = 0.5 * (block(1,2) + block(2,1));
            L.coeffRef(k+2,k+1) = L.coeffRef(k+1,k+2);
        };

        // Verbose
        printf("Iteration %d   Dual %.9f   Min eigenvalue %1.3e\n", i, dual, abs(evals.minCoeff()));

        // Stopping criterion
        if (abs(evals.minCoeff()) < eta)
            break;
    };

    auto stop = high_resolution_clock::now();
    auto duration = duration_cast<microseconds>(stop - start);
    printf("\nCPU time : %.6f\n", static_cast<long long int>(duration.count())*1e-6);
};


#define BOOST_TEST_NO_LIB
#include <boost/test/auto_unit_test.hpp>

#include "coconut/pulp/math/Angle.hpp"

using namespace coconut;
using namespace coconut::pulp;
using namespace coconut::pulp::math;

namespace /* anonymous */ {

BOOST_AUTO_TEST_SUITE(PulpTestSuite);
BOOST_AUTO_TEST_SUITE(PulpMathTestSuite);
BOOST_AUTO_TEST_SUITE(PulpMathAngleTestSuite);

BOOST_AUTO_TEST_CASE(AngleIsConstructibleWithRadians) {
	Angle a(3.0_rad);
	BOOST_CHECK_EQUAL(a.radians(), 3.0f);
}

BOOST_AUTO_TEST_CASE(AngleIsConstructibleWithDegrees) {
	Angle a(90.0_deg);
	BOOST_CHECK_EQUAL(a.degrees(), 90.0f);
}

BOOST_AUTO_TEST_CASE(AngleConvertsRadiansToDegrees) {
	Angle a(radians(PI));
	BOOST_CHECK_EQUAL(a.degrees(), 180.0f);
}

BOOST_AUTO_TEST_CASE(AngleConvertsDegreesToRadians) {
	Angle a(degrees(360.0f));
	BOOST_CHECK_EQUAL(a.radians(), 2.0f * PI);
}

BOOST_AUTO_TEST_CASE(AnglesAreAdditive) {
	Angle lhs = Angle::RIGHT;
	Angle rhs = Angle::HALF_FULL;

	BOOST_CHECK_EQUAL(lhs + rhs, radians(1.5f * PI));
	BOOST_CHECK_EQUAL(rhs - lhs, Angle::RIGHT);
}

BOOST_AUTO_TEST_CASE(AnglesAreMultiplicativeByScalar) {
	BOOST_CHECK_EQUAL(Angle::RIGHT * 4.0f, Angle::FULL);
	BOOST_CHECK_EQUAL(Angle::HALF_FULL / 2.0f, Angle::RIGHT);
}

BOOST_AUTO_TEST_CASE(AnglesAreComparable) {
	BOOST_CHECK_EQUAL(degrees(180.0f), radians(PI));
	BOOST_CHECK_NE(degrees(180.0f), degrees(180.001f));
	BOOST_CHECK_LE(degrees(180.0f), radians(PI));
	BOOST_CHECK_LT(degrees(179.0f), radians(PI));
	BOOST_CHECK_GT(radians(4.0f), degrees(180.0f));
	BOOST_CHECK_GE(radians(4.0f), degrees(180.0f));
}

BOOST_AUTO_TEST_SUITE_END(/* PulpMathAngleTestSuite */);
BOOST_AUTO_TEST_SUITE_END(/* PulpMathTestSuite */);
BOOST_AUTO_TEST_SUITE_END(/* PulpTestSuite */);

} // anonymous namespace


// standard library
#include <iostream>
#include <fstream>
#include <regex>
// boost
#include <boost/algorithm/string/trim.hpp>
#include <boost/algorithm/string/classification.hpp> // is_any_of
#include <boost/algorithm/string/split.hpp>
#include <boost/bind.hpp>
#include <boost/function.hpp>
#include <boost/program_options.hpp>
// Eigen
#include <Eigen/Dense>
//
#include "gradient_descent_solver.h"

double fn( Eigen::VectorXd x, Eigen::MatrixXd A, Eigen::VectorXd b, double lambda )
{
    Eigen::VectorXd t = ( b - A * x );
    return t.dot( t ) + lambda * x.dot( x );
}

Eigen::VectorXd dfn( Eigen::VectorXd x, Eigen::MatrixXd A, Eigen::VectorXd b, double lambda )
{
    return 2 * A.transpose() * ( A * x - b ) + 2 * lambda * x;
}

double errorn( Eigen::VectorXd x, Eigen::VectorXd x_star, Eigen::MatrixXd A, Eigen::VectorXd b, double lambda )
{
    return std::abs( fn( x, A, b, lambda ) - fn( x_star, A, b, lambda ) );
}

bool loadParam( std::string filename, int& degree, Eigen::MatrixXd& A, Eigen::VectorXd& b, double& lambda, Eigen::VectorXd& x_start )
{
    std::ifstream ifs( filename );
    std::string str;

    if ( ifs.fail() ) {
        return false;
    } else {
        std::getline( ifs, str ); //
        std::getline( ifs, str ); //
        degree = std::stoi( str );

        A = Eigen::MatrixXd::Identity(degree,degree);
        b = Eigen::VectorXd::Ones(degree);
        lambda = 0.0;
        x_start = b;

        std::getline( ifs, str ); //
        for ( int i=0; i<degree; i++ ) {
            std::getline( ifs, str ); //
            boost::algorithm::trim( str );
            str = std::regex_replace( str, std::regex(" +"), " " );
            std::vector<std::string> result;
            boost::algorithm::split( result, str, boost::is_any_of(" ") );
            for ( int j=0; j<degree; j++ ) {
                A(i,j) = std::stod( result[j] );
            }
        }

        std::getline( ifs, str ); //
        for ( int i=0; i<degree; i++ ) {
            std::getline( ifs, str ); //
            boost::algorithm::trim( str );
            b(i) = std::stod( str );
        }

        std::getline( ifs, str ); //
        std::getline( ifs, str ); //
        lambda = std::stod( str );

        std::getline( ifs, str ); //
        for ( int i=0; i<degree; i++ ) {
            std::getline( ifs, str ); //
            boost::algorithm::trim( str );
            x_start(i) = std::stod( str );
        }

        return true;
    }
}

int main( int argc, char **argv )
{
    boost::program_options::options_description opt("option");
    opt.add_options()
            ("help,h", "shot help")
            ("accel,a", boost::program_options::value<int>()->default_value(0), "acceleration method" )
            ("debug", boost::program_options::value<int>()->default_value(0), "debug mode" )
            ("degree,d", boost::program_options::value<int>(), "size of problem")
            ("error,e", boost::program_options::value<double>(), "threshold of error")
            ("lambda,l", boost::program_options::value<double>(), "lambda")
            ("printparam", boost::program_options::value<bool>()->default_value(false), "print parameter" )
            ("loadparamfile", boost::program_options::value<std::string>(), "" );
    boost::program_options::variables_map vm;
    try {
        boost::program_options::store( boost::program_options::parse_command_line( argc, argv, opt ), vm );
    } catch ( const boost::program_options::error_with_option_name& e ) {
        std::cout << e.what() << std::endl;
        return 0;
    }
    boost::program_options::notify( vm );

    srand((unsigned int) time(0));

    int accel;
    int debug;
    int degree;
    double error;
    bool printparam;
    Eigen::MatrixXd A;
    Eigen::VectorXd b, x_start;
    double lambda;
    if ( vm.count("help")
        || ( !vm.count("loadparamfile") and !vm.count("degree") )
        || !vm.count("error")
        || ( !vm.count("loadparamfile") and !vm.count("lambda") ) ) {
        std::cout << opt << std::endl;
        return 0;
    } else if ( vm.count("loadparamfile") ) {
        std::string filename;
        try {
            accel = vm["accel"].as<int>();
            debug = vm["debug"].as<int>();
            error = vm["error"].as<double>();
            printparam = vm["printparam"].as<bool>();
            filename = vm["loadparamfile"].as<std::string>();
        } catch ( const boost::bad_any_cast& e ) {
            std::cout << e.what() << std::endl;
            return 0;
        }
        if ( not loadParam( filename, degree, A, b, lambda, x_start ) ) {
            std::cout << opt << std::endl;
            return 0;
        }
    } else {
        try {
            accel = vm["accel"].as<int>();
            debug = vm["debug"].as<int>();
            degree = vm["degree"].as<int>();
            error = vm["error"].as<double>();
            lambda = vm["lambda"].as<double>();
            printparam = vm["printparam"].as<bool>();
        } catch ( const boost::bad_any_cast& e ) {
            std::cout << e.what() << std::endl;
            return 0;
        }
        double coef = 4.0 / degree;
        A = Eigen::MatrixXd::Random(degree,degree) * 3 * coef;
        A = A.transpose() * A;
        Eigen::VectorXd omega_hat = Eigen::VectorXd::Ones(degree);
        Eigen::VectorXd eps = Eigen::VectorXd::Random(degree) * coef;
        b = A * omega_hat + eps;
        x_start = Eigen::VectorXd::Random(degree) * 10;
    }

    Eigen::VectorXd x_star = 2 * ( 2 * A.transpose() * A + lambda * Eigen::MatrixXd::Identity( A.rows(), A.cols() ) ).inverse() * A.transpose() * b;

    std::cout << std::fixed;

    if ( printparam ) {
        std::cout << "degree:" << std::endl;
        std::cout << degree << std::endl;
        std::cout << "A:" << std::endl;
        std::cout << A << std::endl;
        std::cout << "b:" << std::endl;
        std::cout << b << std::endl;
        std::cout << "lambda:" << std::endl;
        std::cout << lambda << std::endl;
        std::cout << "x_start:" << std::endl;
        std::cout << x_start << std::endl;
    }

    GradientDescentSolver<Eigen::VectorXd> solver( debug );
    solver.setf( boost::bind( fn, _1, A, b, lambda ) );
    solver.setdf( boost::bind( dfn, _1, A, b, lambda ) );
    solver.seterror( boost::bind( errorn, _1, x_star, A, b, lambda ) );
    std::vector<Eigen::VectorXd> trajectory;
    Eigen::VectorXd x_target = solver.solve( x_start, trajectory, error, accel );

    if ( isnan( x_target.norm() ) ) {
        std::cout << "diverged." << std::endl;
    }

    return 0;
}


#include "splines.h"
#include <vector>
#include <Eigen/Dense>
#include <Eigen/StdVector>

std::vector<double> CalculateCoeffFromPoints(const std::vector<Eigen::Vector3d>& points, bool is3D = true)
{
	std::vector <double> coeff;
	if (points.size() < 4 || points.size() % 4)
		return coeff;

	double Ax = 0.0, Bx = 0.0, Cx = 0.0, Dx = 0.0;
	double Ay = 0.0, By = 0.0, Cy = 0.0, Dy = 0.0;
	double Az = 0.0, Bz = 0.0, Cz = 0.0, Dz = 0.0;

	Eigen::Vector3d cp[4];
	for (size_t i = 0; i < points.size(); i += 4)
	{
		for (size_t j = 0; j < 4; ++j)
		{
			cp[j] = points[i + j];
		}

		Ax = cp[3].x() - 3.0 * cp[2].x() + 3.0 * cp[1].x() - cp[0].x();
		Ay = cp[3].y() - 3.0 * cp[2].y() + 3.0 * cp[1].y() - cp[0].y();
		Az = cp[3].z() - 3.0 * cp[2].z() + 3.0 * cp[1].z() - cp[0].z();

		Bx = 3.0 * cp[2].x() - 6.0 * cp[1].x() + 3.0 * cp[0].x();
		By = 3.0 * cp[2].y() - 6.0 * cp[1].y() + 3.0 * cp[0].y();
		Bz = 3.0 * cp[2].z() - 6.0 * cp[1].z() + 3.0 * cp[0].z();

		Cx = 3.0 * (cp[1].x() - cp[0].x());
		Cy = 3.0 * (cp[1].y() - cp[0].y());
		Cz = 3.0 * (cp[1].z() - cp[0].z());

		Dx = cp[0].x();
		Dy = cp[0].y();
		Dz = cp[0].z();

		coeff.push_back(Ax);
		coeff.push_back(Bx);
		coeff.push_back(Cx);
		coeff.push_back(Dx);

		coeff.push_back(Ay);
		coeff.push_back(By);
		coeff.push_back(Cy);
		coeff.push_back(Dy);

		if (is3D)
		{
			coeff.push_back(Az);
			coeff.push_back(Bz);
			coeff.push_back(Cz);
			coeff.push_back(Dz);
		}
	}
	return coeff;
}


// Boost.Geometry
// QuickBook Example

// Copyright (c) 2018, 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)

//[densify
//` Shows how to densify a polygon

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

int main()
{
    typedef boost::geometry::model::d2::point_xy<double> point_type;
    typedef boost::geometry::model::polygon<point_type> polygon_type;

    polygon_type poly;
    boost::geometry::read_wkt(
        "POLYGON((0 0,0 10,10 10,10 0,0 0),(1 1,4 1,4 4,1 4,1 1))", poly);

    polygon_type res;

    boost::geometry::densify(poly, res, 6.0);

    std::cout << "densified: " << boost::geometry::wkt(res) << std::endl;

    return 0;
}

//]

//[densify_output
/*`
Output:
[pre
densified: POLYGON((0 0,0 5,0 10,5 10,10 10,10 5,10 0,5 0,0 0),(1 1,4 1,4 4,1 4,1 1))
]
*/
//]


/*
A class to represent a prime field
*/

#ifndef PRIME_FIELD
#define PRIME_FIELD

#include <boost/operators.hpp>
#include <iostream>
#include <ostream>
#include <boost/bimap.hpp>

using namespace std;

//! for debugging purpose only
//! print the boost map of the cyclic group structure
template< class MapType >
void print_map(const MapType & map,
               const std::string & separator,
               std::ostream & os )
{
    typedef typename MapType::const_iterator const_iterator;

    for( const_iterator i = map.begin(), iend = map.end(); i != iend; ++i )
    {
        os << i->first << separator << i->second << std::endl;
    }
}

// prime field element (PFE)
// class template el_t should be interger type
template<class el_t = int> class PFE:
	boost::field_operators< PFE<el_t>,
	boost::equality_comparable< PFE<el_t>
	> >
{
public:
	el_t element;
	static el_t prime;

	typedef boost::bimap< el_t, el_t> bim;
	//! maps exponents to elements
	static bim exp_el;

	//!Constructs a prime f
	PFE(el_t el): element(el % prime) {};
	PFE(){element = 0;};

	static void set_prime(const el_t& a){prime = a;}

	//! for initializying the map exponent to element;
	//! takes as parameter a primitive element of GF(prime)

	static void initialize_exp_el(el_t priel){

		el_t el = 1;
		for(unsigned i = 0; i < prime-1; ++i){
			// insert (exponent, corresponding element)

			exp_el.insert(typename bim::value_type(i,el ));
			el = (el*priel) % prime;
		//cout << i << "  " <<  ((int) pow((double) priel,(int) i)) % prime << endl;
		}

	}

	bool isempty() const {return (element==prime);}
	void setempty() {element = prime;};

	PFE& operator += (const PFE& x){
		element = (element+x.element) % prime;
		return *this;
	}

	PFE& operator -= (const PFE& x){
		element = (element-x.element) % prime;
		if(element < 0) element += prime;
		return *this;
	}

	PFE& operator *= (const PFE& x){

		element = element*x.element % prime;
		return *this;
	}

	PFE& operator /= (const PFE& x){
		//print_map(exp_el.left , " " , cout);
		el_t ea = exp_el.right.at(element);
		el_t eb = exp_el.right.at(x.element);
		el_t eres = (prime - 1 - eb + ea) % (prime-1);
		element = exp_el.left.at(eres);
		return *this;
	}

	//operator const el_t (){
	//	return element;
	//}
	//operator el_t (){return element;}
	operator int (){return (int) element;};

	PFE inverse() const {
		return pow(*this, -1);
	}

	bool operator ==(const PFE& x) const {
		return (x.element == element);
	}
	bool operator < (const PFE& x) const {
		return (element < x.element);
	}

	bool iszero() const { return (element == 0);  };
	//! get the multiplication order of the element
	unsigned order() const {
		// additive zero has no order
		if(element == 0){ return -1;}
		el_t tmp = element;
		unsigned ord = 1;
		while(!(tmp == 1)){
			ord++;
			tmp = (tmp * element) % prime;
			//cout << tmp << endl;
		}
		return ord;
	}
};

 template<class el_t>
    ostream &operator<<(ostream &stream, PFE<el_t> x)
    {
      stream << x.element;
      return stream; // must return stream
    };

 template<class el_t>
    PFE<el_t> pow(PFE<el_t> a, int exp){
		if( a.iszero() ) return a;
		if( exp == 0 ) return PFE<el_t>(1);
		el_t ea = a.exp_el.right.at(a.element);
		int nexp = exp*ea;
		while( nexp < 0) {

			nexp += a.prime-1;
		}
		nexp = nexp % (a.prime-1) ;
		return PFE<el_t>( a.exp_el.left.at(nexp ) );
	};

#endif


// docs : https://boostjp.github.io/tips/multiprec-float.html
// -I /usr/local/include/

#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/multiprecision/cpp_int.hpp>
namespace mp = boost::multiprecision;

using Bint = mp::cpp_int;
using Real = mp::number<mp::cpp_dec_float<30>>;


#include "pauli_product.hpp"
#include <armadillo>
#include <cmath>
#include <stdlib.h>

using namespace std;
using namespace arma;

// Applies a Pauli operator i^{xz} X^xZ^z to the k'th qubit to the state psi.
// This algorithm takes at most 2^n complex number multiplications.
void apply_pauli_fast(int k, bool x, bool z, cx_dvec &psi)
{
  int d;
  d = psi.size();
  if (pow(2,k) >= d)
    {
      throw "Qubit index out of range.";
    }
  if (z)
    {
      if (x)
	{
	  for (int i=0; i<d; i++)
	    {
	      if ((i & ( 1 << k )) >> k == 0)
		{
		  cx_double temp1, temp2;
		  temp1 = psi(i) * cx_double(0.0, 1.0);
		  temp2 = psi((1<<k)|i) * cx_double(0.0, -1.0);
		  psi(i) = temp2;
		  psi((1<<k)|i) = temp1;
		}
	    }
	}
      else
	{
	  for (int i=0; i<d; i++)
	    {
	      if ((i & ( 1 << k )) >> k == 1)
		{
		  psi(i) *= -1.0;
		}
	    }
	}
    }
  else
    {
      if (x)
	{
	  for (int i=0; i<d; i++)
	    {
	      if ((i & ( 1 << k )) >> k == 0)
		{
		  cx_double temp1, temp2;
		  temp1 = psi(i);
		  temp2 = psi((1<<k)|i);
		  psi(i) = temp2;
		  psi((1<<k)|i) = temp1;
		}
	    }
	}
    }

}

// Applies a Pauli operator i^{xz} X^xZ^z to the k'th qubit to the state psi.
// This algorithm takes O(4^n)-time, quadratically worse than the apply_pauli_fast.
// This function is primarily used as a bookkeeping device, instead of performance.
void apply_pauli_slow(int k, bool x, bool z, cx_dvec &psi)
{
  int d, d_1, d_2;
  cx_mat PI(2, 2, fill::zeros), PX(2, 2, fill::zeros), PY(2, 2, fill::zeros), PZ(2, 2, fill::zeros);
  cx_mat p;

  PI(0, 0) = cx_double(1.0, 0.0); PI(1, 1) = cx_double(1.0, 0.0);
  PX(0, 1) = cx_double(1.0, 0.0); PI(1, 0) = cx_double(1.0, 0.0);
  PY(0, 1) = cx_double(0.0, -1.0); PY(1, 0) = cx_double(0.0, 1.0);
  PZ(0, 0) = cx_double(1.0, 0.0); PZ(1, 1) = cx_double(-1.0, 0.0);

  d = psi.size();
  d_1 = pow(2, k);
  d_2 = d / 2 / d_1;

  if (x)
    {
      if (z)
	{
	  p = PY;
	}
      else
	{
	  p = PX;
	}

    }
  else
    {
      if (z)
	{
	  p = PZ;
	}
      else
	{
	  p = PI;
	}
    }

  cx_mat I1(d_1, d_1,fill::eye), I2(d_2, d_2, fill::eye);

  cx_mat Pauli;
  Pauli = kron(kron(I2, p), I1);
  psi = Pauli * psi;

}

// Apply exp(iPθ) to psi, where P = i^{x·z}(⨂_k X_k^(x_k)) (⨂_k Z_k^(z_k))
void apply_ppr(unsigned int x, unsigned int z, double theta, cx_dvec &psi)
{
  int d;
  d = psi.size();
  cx_double c, s;
  c = cos(theta);
  s = sin(theta);
  //  cx_double csc;
  //  csc = 1.0 / cos(theta);
  bool not_changed[2048];
  fill_n(not_changed, 2048, true);

  for (int y=0; y<d; y++)
    {
      if (not_changed[y])
	{

	    unsigned int xz = __builtin_popcount(x&z);
	    unsigned int yz = __builtin_popcount(y&z);

	    cx_double phase1, phase2;

	    switch((1 + xz + 2*(yz+xz))%4)
	      {
	      case 0:
		phase1 = cx_double(1.0, 0.0);
		break;
	      case 1:
		phase1 = cx_double(0.0, 1.0);
		break;
	      case 2:
		phase1 = cx_double(-1.0, 0.0);
		break;
	      case 3:
		phase1 = cx_double(0.0, -1.0);
		break;
	      }

	    switch((1 + xz + 2 * yz)%4)
	      {
	      case 0:
		phase2 = cx_double(1.0, 0.0);
		break;
	      case 1:
		phase2 = cx_double(0.0, 1.0);
		break;
	      case 2:
		phase2 = cx_double(-1.0, 0.0);
		break;
	      case 3:
		phase2 = cx_double(0.0, -1.0);
		break;
	      }
	    cx_double temp1, temp2;
	    temp1 = psi(y) * c + psi(y^x) * s * phase1;
	    temp2 = psi(y^x) * c + psi(y) * s * phase2;
	    psi(y) = temp1;
	    psi(y^x) = temp2;

	    //psi(y) *= c; psi(y) += s * phase1 * psi(y^x);
	    //psi(y^x) += s * phase2 * psi(y);
	    //psi(y^x) *= csc;

	    not_changed[y] = false; not_changed[y^x] = false;
	}

    }
}

void apply_ppr_slow(unsigned int x, unsigned int z, double theta, cx_dvec &psi)
{
    int d;
    d = psi.size();
    unsigned int bits, n_temp = (d < 0) ? -d : d;
    for(bits = 0; n_temp != 0; ++bits) n_temp >>= 1;
    unsigned int n_q = bits - 1;

    cx_double c, s;
    c = cos(theta);
    s = sin(theta);

    cx_dmat PX(2,2, fill::zeros), PY(2,2, fill::zeros), PZ(2,2, fill::zeros), PI(2,2, fill::zeros), P, PString(1,1, fill::ones);
    PX(0, 1) = 1.0; PX(1, 0) = 1.0;
    PZ(0, 0) = 1.0; PZ(1, 1) = -1.0;
    PY(1, 0) = cx_double(0.0, 1.0); PY(0, 1) = cx_double(0.0, -1.0);
    PI(0, 0) = 1.0; PI(1, 1) = 1.0;

    for (int i=0; i<n_q; i++)
      {

	if (x & (1<< i))
	  {
	    if (z & (1<< i))
		P = PY;
	    else
		P = PX;
	  }
	else
	  {
	    if (z & (1<< i))
		P = PZ;
	    else
		P = PI;
	  }
	PString = kron(P, PString);
      }

    psi = c * psi + PString * psi * s * cx_double(0.0, 1.0);

}

// Measure Pauli Product operator (fast)
double measure_pp(unsigned int x, unsigned int z, cx_dvec &psi)
{
  int d;
  d = psi.size();
  unsigned int bits, n_temp = (d < 0) ? -d : d;
  for(bits = 0; n_temp != 0; ++bits) n_temp >>= 1;
  unsigned int n_q = bits - 1;

  cx_double mysum = 0.0;
  for (int y=0; y<d; y++)
    {
      unsigned int xz = __builtin_popcount(x&z);
      unsigned int yz = __builtin_popcount(y&z);
      cx_double phase;
      switch((xz + 2*yz)%4)
	{
	case 0:
	  phase = cx_double(1.0, 0.0);
	  break;
	case 1:
	  phase = cx_double(0.0, 1.0);
	  break;
	case 2:
	  phase = cx_double(-1.0, 0.0);
	  break;
	case 3:
	  phase = cx_double(0.0, -1.0);
	  break;
	}
      mysum += psi(y) * conj(psi(y^x)) * phase;
    }
  return real(mysum);
}

// Meausre Pauli Product operator (slow)
double measure_pp_slow(unsigned int x, unsigned int z, cx_dvec &psi)
{
  int d;
  d = psi.size();
  unsigned int bits, n_temp = (d < 0) ? -d : d;
  for(bits = 0; n_temp != 0; ++bits) n_temp >>= 1;
  unsigned int n_q = bits - 1;

  cx_dmat PX(2,2, fill::zeros), PY(2,2, fill::zeros), PZ(2,2, fill::zeros), PI(2,2, fill::zeros), P, PString(1,1, fill::ones);
  PX(0, 1) = 1.0; PX(1, 0) = 1.0;
  PZ(0, 0) = 1.0; PZ(1, 1) = -1.0;
  PY(1, 0) = cx_double(0.0, 1.0); PY(0, 1) = cx_double(0.0, -1.0);
  PI(0, 0) = 1.0; PI(1, 1) = 1.0;

  for (int i=0; i<n_q; i++)
    {

      if (x & (1<< i))
	{
	  if (z & (1<< i))
	    P = PY;
	  else
	    P = PX;
	}
      else
	{
	  if (z & (1<< i))
	    P = PZ;
	  else
	    P = PI;
	}
      PString = kron(P, PString);
    }
  return real(cdot(psi, PString * psi));
}


#define _USE_MATH_DEFINES
#include <libtiff/tiffio.h>

#include <iostream>
#include <fstream>
#include <chrono>
#include <math.h>

#include <filesystem>
namespace fs = std::filesystem;
#include <Eigen/Dense>
using namespace Eigen;

struct Point {
	uint32 x;
	uint32 y;
};

struct ellipseABCDEF {
	double A;
	double B;
	double C;
	double D;
	double E;
	double F;
};

struct ellipseABCDEF fitEllipse(struct Point p1, struct Point p2, struct Point p3, struct Point p4, struct Point p5) {
	Matrix<double, 5, 6> mat56(5, 6);
	mat56 <<
		(double)p1.x * (double)p1.x, (double)p1.x * -(double)p1.y, (double)p1.y * (double)p1.y, (double)p1.x, -(double)p1.y, 1,
		(double)p2.x * (double)p2.x, (double)p2.x * -(double)p2.y, (double)p2.y * (double)p2.y, (double)p2.x, -(double)p2.y, 1,
		(double)p3.x * (double)p3.x, (double)p3.x * -(double)p3.y, (double)p3.y * (double)p3.y, (double)p3.x, -(double)p3.y, 1,
		(double)p4.x * (double)p4.x, (double)p4.x * -(double)p4.y, (double)p4.y * (double)p4.y, (double)p4.x, -(double)p4.y, 1,
		(double)p5.x * (double)p5.x, (double)p5.x * -(double)p5.y, (double)p5.y * (double)p5.y, (double)p5.x, -(double)p5.y, 1;

	CompleteOrthogonalDecomposition<Matrix<double, Dynamic, Dynamic> > cod;
	cod.compute(mat56);

	// Find URV^T
	MatrixXd V = cod.matrixZ().transpose();
	MatrixXd Null_space = V.block(0, cod.rank(), V.rows(), V.cols() - cod.rank());
	MatrixXd P = cod.colsPermutation();
	Null_space = P * Null_space; // Unpermute the columns

	return {
		Null_space(0) / Null_space(5),
		Null_space(1) / Null_space(5),
		Null_space(2) / Null_space(5),
		Null_space(3) / Null_space(5),
		Null_space(4) / Null_space(5),
		1
	};
}

static uint16 finalAverage;
static std::ofstream outStream("out.csv");

typedef unsigned long long uint64;

struct followReturn {
	bool success;
	struct Point point;
};

bool validateEdge(bool* considered, struct Point point, uint32 width, uint32 height) {
	struct Point nearby[4] = {
		{point.x + 0, point.y - 1},
		{point.x - 1, point.y + 0},
		{point.x + 1, point.y + 0},
		{point.x + 0, point.y + 1}
	};

	for (uint32 i = 0; i < 4; i++) {
		struct Point tested = nearby[i];

		if (tested.x != 0xFFFFFFFF && tested.y != 0xFFFFFFFF && tested.x != width && tested.y != height) {
			uint32 index = tested.y * width + tested.x;

			if (!considered[index]) {
				return true;
			}
		}
	}

	return false;
}

struct EllipseParams {
	double fCenterX, fCenterY;
	double fLongLength, fShortLength;
	double fAngle;
};

EllipseParams toEllipseParams(ellipseABCDEF a) {

	EllipseParams eOutParams;

	eOutParams.fCenterX = ((2 * a.C * a.D) - (a.B * a.E)) / ((a.B * a.B) - (4 * a.A * a.C));
	eOutParams.fCenterY = ((2 * a.A * a.E) - (a.B * a.D)) / ((a.B * a.B) - (4 * a.A * a.C));

	eOutParams.fLongLength = -sqrt(2 * (a.A * pow(a.E, 2) + a.C * pow(a.D, 2) - a.B * a.D * a.E + (pow(a.B, 2) - 4 * a.A * a.C) * a.F) * ((a.A + a.C) + sqrt(pow((a.A - a.C), 2) + pow(a.B, 2)))) /
							 ((a.B * a.B) - (4 * a.A * a.C));

	eOutParams.fShortLength = -sqrt(2 * (a.A * pow(a.E, 2) + a.C * pow(a.D, 2) - a.B * a.D * a.E + (pow(a.B, 2) - 4 * a.A * a.C) * a.F) * ((a.A + a.C) - sqrt(pow((a.A - a.C), 2) + pow(a.B, 2)))) /
							 ((a.B * a.B) - (4 * a.A * a.C));

	if (a.B != 0)
		eOutParams.fAngle = atan((a.C - a.A - sqrt(pow((a.A - a.C), 2) + pow(a.B, 2))) / a.B);
	else if
		(a.A <= a.C) eOutParams.fAngle = 0;
	else
		eOutParams.fAngle = M_PI / 2;

	return eOutParams;
}

double rad2Deg(double rad) {
	return (rad * 180) / M_PI;
}

//filename, ellipse_center_x, ellipse_center_y, ellipse_majoraxis, ellipse_minoraxis, ellipse_angle, elapsed_time

void saveEllipse(bool empty, EllipseParams params, float elapsedTime, char* fileName){

	if (empty) {

		outStream << fileName << ",";
		outStream << ",";
		outStream << ",";
		outStream << ",";
		outStream << ",";
		outStream << ",";
		outStream << (int)(elapsedTime * 1000) << std::endl;
		return;
	}

	outStream << fileName << ",";
	outStream << params.fCenterX << ",";
	outStream << -params.fCenterY << ",";
	outStream << params.fLongLength << ",";
	outStream << params.fShortLength << ",";
	outStream << fmod(180 - rad2Deg(params.fAngle), 360) << ",";
	outStream << (int)(elapsedTime * 1000) << std::endl;

}

double ratePoint(struct Point point, struct EllipseParams ellipse) {
	double xi = (double)point.x;
	double yi = (double)point.y;

	xi = xi - ellipse.fCenterX;
	yi = yi + ellipse.fCenterY;

	double x = xi * cos(ellipse.fAngle) + yi * sin(ellipse.fAngle);
	double y = xi * -sin(ellipse.fAngle) + yi * cos(ellipse.fAngle);

	double angle = atan(y / x);

	double distanceFromCenter = hypot(x, y);

	double distanceFromCenterToEdge = ellipse.fLongLength * ellipse.fShortLength / (sqrt(pow(ellipse.fShortLength * cos(angle), 2) + pow(ellipse.fLongLength * sin(angle), 2)));

	double finalDistance = abs(distanceFromCenter - distanceFromCenterToEdge);

	//some gaussian trickery right here
	double rating = exp(-(finalDistance * finalDistance) / 8);
	return rating;
}

void ParseEllipse(std::string path) {
	TIFF* tif = TIFFOpen(path.c_str(), "r");

	if (tif == nullptr) {
		std::cout << path << " is not a valid TIFF image!" << std::endl << std::endl;
		return;
	}

	uint32 width, height;

	TIFFGetField(tif, TIFFTAG_IMAGEWIDTH, &width);           // uint32 width;
	TIFFGetField(tif, TIFFTAG_IMAGELENGTH, &height);        // uint32 height;

	std::cout << std::endl << "Image: " << path << std::endl;
	printf("Width: %d\nHeight: %d\n", width, height);

	uint16* raster = (uint16*)_TIFFmalloc(width * height * sizeof(uint16));

	for (uint32 row = 0; row < height; row++) {
		TIFFReadScanline(tif, raster + width * row, row, 0);
	}

	//Time Taken
	auto timeStart = std::chrono::system_clock::now();

	uint64 average = 0;
	for (uint32 i = 0; i < width * height; i++) {
		average += uint64(raster[i]);
	}

	finalAverage = average / (uint64(width) * uint64(height)) * 1;

	/*uint32 maxLuminance = 0;

	for (uint32 i = 0; i < width * height; i++) {
		if (raster[i] > maxLuminance) {
			maxLuminance = raster[i];
		}
	}

	double fmaxLuminance = (double)maxLuminance;

	double average = 0;

	for (uint32 i = 0; i < width * height; i++) {
		average += (1 - pow(exp(-(pow((double)raster[i] / fmaxLuminance, 2) / 0.5)), 64)) * fmaxLuminance;
	}

	average /= width * height;

	finalAverage = (uint16)average;*/

	bool* considered = (bool*)malloc(width * height * sizeof(bool));
	bool* alreadyTaken = (bool*)calloc(width * height, sizeof(bool));

	for (uint32 y = 0; y < height; y++) {
		for (uint32 x = 0; x < width; x++) {
			const uint32 index = y * width + x;

			//considered[index] = bool(raster[index] < fmaxLuminance * 0.45);
			considered[index] = bool(raster[index] < finalAverage);
		}
	}

	std::vector<std::vector<struct Point>> sequences;

	std::vector<struct Point> paths[2] = {
		std::vector<struct Point>(),
		std::vector<struct Point>()
	};

	for (uint32 y = 0; y < height; y++) {
		for (uint32 x = 0; x < width; x++) {
			uint32 indexInitial = y * width + x;

			if (alreadyTaken[indexInitial] || !considered[indexInitial]) {
				continue;
			}

			if (!validateEdge(considered, { x, y }, width, height)) {
				continue;
			}

			uint32 newX = x, newY = y;

			struct Point nearbyInitial[8] = {
				{newX - 1, newY - 1},
				{newX + 0, newY - 1},
				{newX + 1, newY - 1},
				{newX - 1, newY + 0},
				{newX + 1, newY + 0},
				{newX - 1, newY + 1},
				{newX + 0, newY + 1},
				{newX + 1, newY + 1}
			};

			uint32 part = 0;
			uint32 firstPointIndex = 0;

			while (part < 2 && firstPointIndex < 8) {
				newX = nearbyInitial[firstPointIndex].x;
				newY = nearbyInitial[firstPointIndex].y;
				firstPointIndex++;

				if (newX == 0xFFFFFFFF || newY == 0xFFFFFFFF || newX == width || newY == height) {
					continue;
				}
				if (!validateEdge(considered, { newX, newY }, width, height)) {
					continue;
				}

				bool endPath = false;
				do {
					uint32 indexThis = newY * width + newX;

					struct Point nearby[8] = {
						{newX - 1, newY - 1},
						{newX + 0, newY - 1},
						{newX + 1, newY - 1},
						{newX - 1, newY + 0},
						{newX + 1, newY + 0},
						{newX - 1, newY + 1},
						{newX + 0, newY + 1},
						{newX + 1, newY + 1}
					};

					endPath = true;

					for (uint32 i = 0; i < 8; i++) {
						struct Point tested = nearby[i];

						if (tested.x != 0xFFFFFFFF && tested.y != 0xFFFFFFFF && tested.x != width && tested.y != height) {
							if (!validateEdge(considered, tested, width, height)) {
								continue;
							}

							uint32 testedIndex = tested.y * width + tested.x;

							if (alreadyTaken[testedIndex] || !considered[testedIndex]) {
								continue;
							}

							paths[part].push_back({ newX, newY });

							newX = tested.x;
							newY = tested.y;

							endPath = false;

							alreadyTaken[testedIndex] = true;
							break;
						}
					}

				} while (!endPath);

				part++;
			}

			uint32 sizePart1 = paths[0].size();
			uint32 sizePart2 = paths[1].size();

			if (sizePart1 + sizePart2 >= 10) {
				std::vector<struct Point> finalPath = std::vector<struct Point>();

				finalPath.reserve(sizePart1 + sizePart2);

				for (int i = sizePart1 - 1; i >= 0; i--) {
					finalPath.push_back(paths[0][i]);

				}

				int a = 0;

				for (int i = 0; i < sizePart2; i++) {
					finalPath.push_back(paths[1][i]);
				}

				paths[0] = std::vector<struct Point>();
				paths[1] = std::vector<struct Point>();

				sequences.push_back(finalPath);
			}
			else {
				paths[0].clear();
				paths[1].clear();
			}
		}
	}

	if (sequences.size() == 0) {

		auto timeEnd = std::chrono::system_clock::now();

		std::chrono::duration<float> elapsedTime = timeEnd - timeStart;
		std::cout << "No ellipse found!" << std::endl;

		saveEllipse(true, {}, elapsedTime.count(), (char*)fs::path(path).filename().string().c_str());
		return;
	}

	uint32 maxLength = 0;
	uint32 longestIndex = 0;
	for (uint32 i = 0; i < sequences.size(); i++) {
		if (sequences[i].size() > maxLength) {
			maxLength = sequences[i].size();
			longestIndex = i;
		}
	}

	std::vector<struct Point> longest = sequences[longestIndex];

	//Sampling
	Point pSamples[5];
	const int nMaxSampleItterations = 4; //Linear addition

	struct EllipseParams bestFit;
	double bestFitRating = 0;

	//Iterate through
	for (int nCurrItteration = 0; nCurrItteration < nMaxSampleItterations; nCurrItteration++) {

		//Populate samples
		int nPointDistance = longest.size() / (5 + 2 * nCurrItteration);

		if (nPointDistance == 0) {
			break;
		}

		for (int i = 0; i < 5; i++) {
			pSamples[i] = longest[i * nPointDistance];
		}

		int nCurrOffset = 0;
		//Go through the path

		while ((4 * nPointDistance + nCurrOffset) < longest.size()) {

			for (int i = 0; i < 5; i++) {
				pSamples[i] = longest[i * nPointDistance + nCurrOffset];
			}

			EllipseParams eFitEllipse = toEllipseParams(fitEllipse(pSamples[0], pSamples[1], pSamples[2], pSamples[3], pSamples[4]));

			double rating = 0;
			for (int i = 0; i < longest.size(); i++) {
				rating += ratePoint(longest[i], eFitEllipse);
			}

			if (rating > bestFitRating) {
				bestFitRating = rating;
				bestFit = eFitEllipse;
			}

			//Update offset for next iteration
			nCurrOffset++;
		}
	}

	std::cout << "Best fit got rated: " << bestFitRating << " (out of " << longest.size() << ")" << std::endl;

	auto timeEnd = std::chrono::system_clock::now();

	std::chrono::duration<float> elapsedTime = timeEnd - timeStart;
	std::cout << "Finding the best ellipse took: " << elapsedTime.count() << " s" << std::endl;

	saveEllipse(false, bestFit, elapsedTime.count(), (char*)fs::path(path).filename().string().c_str());

	TIFFClose(tif);
	//free mallocs

	_TIFFfree(raster);
	free(considered);
	free(alreadyTaken);
}

int main(int argc, char** argv) {

	outStream << "filename,ellipse_center_x,ellipse_center_y,ellipse_majoraxis,ellipse_minoraxis,ellipse_angle,elapsed_time" << std::endl; //write the header

	if (argc == 1) {
		std::cout << "No input directory provided! Exiting..." << std::endl;
		return 0;
	}

	std::string path(argv[1]);
	try {
		for (const auto& entry : fs::recursive_directory_iterator(path))
			ParseEllipse(entry.path().string());
	}
	catch (std::exception e) {
		std::cout << argv[1] << " is not a valid folder!" << std::endl;
	}

	outStream.close();

	return 0;
}


/* test_chi_squared.cpp
 *
 * Copyright Steven Watanabe 2011
 * 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)
 *
 * $Id: test_chi_squared.cpp 71018 2011-04-05 21:27:52Z steven_watanabe $
 *
 */

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

#define BOOST_RANDOM_DISTRIBUTION boost::random::chi_squared_distribution<>
#define BOOST_RANDOM_DISTRIBUTION_NAME chi_squared
#define BOOST_MATH_DISTRIBUTION boost::math::chi_squared
#define BOOST_RANDOM_ARG1_TYPE double
#define BOOST_RANDOM_ARG1_NAME n
#define BOOST_RANDOM_ARG1_DEFAULT 1000.0
#define BOOST_RANDOM_ARG1_DISTRIBUTION(n) boost::uniform_real<>(0.00001, n)

#include "test_real_distribution.ipp"


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_LOG2_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_LOG2_HPP_INCLUDED

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

 /*!

    @ingroup group-exponential
    Function object implementing log2 capabilities

    base two logarithm function.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = log2(x);
    @endcode

    is similar to:

    @code
    T r =  log(x)/log(2);;
    @endcode

    - log2(x) return Nan for negative enties (peculiarly Mzero
    for floating numbers).

    @par Decorators

    std_ for floating entries

    @see log10, log, log1p, is_negative,  Mzero
  **/
  Value log2(Value const& x);
} }
#endif

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

#endif


#include "function.h"
#include <iostream>
#include <armadillo>
#include <cmath>

void Function::IndefCoeff()
{
	arma::mat::fixed<5, 5> A;
	double h = 0;
	std::vector<double> coeff;
	for (int i = 0; i < 5; ++i)
	{
		coeff.push_back(f[i].first - x);
	}
	for (int j = 0; j < 5; j++)
	{
		for (int i = 0; i < 5; i++)
		{
			A(j, i) = pow(coeff[i], j);
		}
	}
	arma::vec B(5, arma::fill::zeros);
	B(1) = 1;
	arma::vec X = arma::solve(A, B);
	for (int i = 0; i < 5; i++)
	{
		IndefResult += X(i) * f[i].second;
	}
}

void Function::FindDerivative()
{
	double eps_1 = (e[0].first + e[1].first);
	double eps_2 = (e[0].first * (e[1].first + e[2].first) + e[1].first * e[2].first);
	double eps_3 = (e[0].first * e[2].first * (e[1].first + e[3].first) + e[1].first * e[3].first *
		(e[0].first + e[2].first));
	NewtonResult =  FindDivDiff(1) + FindDivDiff(2) * eps_1 + FindDivDiff(3) * eps_2 + FindDivDiff(4) * eps_3;

}

double Function::FindDivDiff(int IterationNum)
{
	double temp = 1;
	double sum = 0;
	for (int p = 0; p <= IterationNum; ++p)
	{
		for (int i = 0; i <= IterationNum; ++i)
		{
			if (i != p)
				temp *= 1 / (f[p].first - f[i].first);
		}
		sum += f[p].second * temp;
		temp = 1;
	}
	return sum;
}

Function::Function(std::vector<std::pair<double, double>> f_, double x_)
{
	f = f_;
	x = x_;
	std::pair<double, double> temp;
	for (auto it : f)
	{
		temp.first = x - it.first;
		temp.second = it.second;
		e.push_back(temp);
	}
	IndefCoeff();
	FindDerivative();
}

void Function::Print()
{
	std::cout << "Newton method result: " << NewtonResult << std::endl;
	std::cout << "Indef coeff method result: " << IndefResult << std::endl << std::endl;
}

Function::~Function()
{}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011 Barend Gehrels, Amsterdam, the Netherlands.

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

//[register_multi_polygon
//` Show the use of the macro BOOST_GEOMETRY_REGISTER_MULTI_POLYGON

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/geometry/multi/geometries/register/multi_polygon.hpp>

typedef boost::geometry::model::polygon
    <
        boost::tuple<float, float>
    > polygon_type;

BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)
BOOST_GEOMETRY_REGISTER_MULTI_POLYGON(std::vector<polygon_type>)

int main()
{
    // Normal usage of std::
    std::vector<polygon_type> polygons(2);
    boost::geometry::read_wkt("POLYGON((0 0,0 1,1 1,1 0,0 0))", polygons[0]);
    boost::geometry::read_wkt("POLYGON((3 0,3 1,4 1,4 0,3 0))", polygons[1]);

    // Usage of Boost.Geometry
    std::cout << "AREA: "  << boost::geometry::area(polygons) << std::endl;

    return 0;
}

//]

//[register_multi_polygon_output
/*`
Output:
[pre
AREA: 2
]
*/
//]


#include <iostream>
#include <Eigen/Dense>

int main()
{
    using std::cout;
    using std::endl;
    using namespace Eigen;

    MatrixXd A = MatrixXd::Random(3,3);

    Matrix<bool, Dynamic, Dynamic> B = A.array() < 0.5;

    MatrixXd C;

    C = B.cast<double>();

    cout << "Matrix A: " << endl;
    cout << A << endl;

    cout << "Bool Matrix B:" << endl;
    cout << B << endl;

    cout << "double Matrix C:" << endl;
    cout << C << endl;

    cout << "double Matrix A mul C" << endl;
    cout << A.array() * C.array() << endl;

    return 0;
}


// numbers_as_words.cpp : output any positive integer in American usage English
// Version 1.01 (2020/09/12), MIT License, (c) Richard Spencer 2019

// Optionally compile with BOOST_CPP_INT defined to use arbitrary precision
// instead of long long as the Integer type, also requires -std=c++17 or later

// Usage: Filename as single argument to test output from "one" to "one million"
//        Run without arguments to enter interactive mode

#include <iostream>
#include <fstream>
#include <string>
#include <vector>

#ifdef BOOST_CPP_INT
#include <boost/multiprecision/cpp_int.hpp>
using Integer = boost::multiprecision::cpp_int;
#else
using Integer = unsigned long long int;
#endif

using std::cin;
using std::cout;
using std::ofstream;
using std::string;
using std::vector;

string number_triplet(unsigned n) {
    string s{};
    if (n / 100) {
        s += number_triplet(n / 100) + " hundred" + (n % 100 ? " " : "");
    }
    n %= 100;
    if (n < 20) {
        switch (n) {
            case 1: s += "one"; break;
            case 2: s += "two"; break;
            case 3: s += "three"; break;
            case 4: s += "four"; break;
            case 5: s += "five"; break;
            case 6: s += "six"; break;
            case 7: s += "seven"; break;
            case 8: s += "eight"; break;
            case 9: s += "nine"; break;
            case 10: s += "ten"; break;
            case 11: s += "eleven"; break;
            case 12: s += "twelve"; break;
            case 13: s += "thirteen"; break;
            case 14: s += "fourteen"; break;
            case 15: s += "fifteen"; break;
            case 16: s += "sixteen"; break;
            case 17: s += "seventeen"; break;
            case 18: s += "eighteen"; break;
            case 19: s += "nineteen"; break;
        }
    }
    else {
        switch (n / 10) {
            case 2: s += "twenty"; break;
            case 3: s += "thirty"; break;
            case 4: s += "forty"; break;
            case 5: s += "fifty"; break;
            case 6: s += "sixty"; break;
            case 7: s += "seventy"; break;
            case 8: s += "eighty"; break;
            case 9: s += "ninety"; break;
        }
        if (n % 10) {
            s += '-' + number_triplet(n % 10);
        }
    }
    return s;
}

template<typename Number>
void number_to_words(Number n, string& s, const unsigned triplet = 0) {
    if (Number m = n / 1000; m) {
        number_to_words(m, s, triplet + 1);
    }
    const vector triplets = { "", "thousand", "million", "billion", "trillion", "quadrillion", "quintillion", "sextillion", "septillion", "octillion", "nonillion", "decillion", "undecillion", "duodecillion", "tredecillion", "quattuordecillion", "quindecillion", "sexdecillion", "octodecillion", "novemdecillion", "vigintillion" };
    if (unsigned t = static_cast<unsigned>(n % 1000); t) {
        s += (s.empty() ? "" : " ") + number_triplet(t) + (triplet ? " " : "") + triplets.at(triplet);
    }
}

int main(const int argc, const char *argv[]) {
    if (argc == 2) {
        ofstream file{argv[1]};
        for (Integer i = 1; i <= 1000000; ++i) {
            string s;
            number_to_words(i, s);
            file << s << '\n';
        }
        return 0;
    }
    Integer i{};
    cout << "Please enter a positive number (zero to quit): ";
    cin >> i;
    while (i) {
        string s;
        number_to_words(i, s);
        cout << s << '\n';
        cout << "Please enter another number: ";
        cin >> i;
    }
}


#pragma once
#include <type_traits>
#include <Eigen/Dense>

namespace ppl {
namespace math {

/**
 * Estimates sample variance for n-dimensional data using
 * Welford's online algorithm
 */
struct WelfordVar
{
    WelfordVar(size_t n_params)
        : m_{n_params, 2}
        , mean_{m_.col(0)}
        , m2n_{m_.col(1)}
    { m_.setZero(); }

    /*
     * Update sample mean and sample variance with new sample x.
     */
    template <class MatType>
    void update(const MatType& x)
    {
        ++n_;
        auto delta = x - mean_;
        mean_ += (1./static_cast<double>(n_)) * delta;
        m2n_ += (delta.array() * (x - mean_).array()).matrix();
    }

    const auto& get_variance() const { return m2n_; }
    size_t get_n_samples() const { return n_; }

    /**
     * Resets sample mean, sample variance, and number of samples to 0
     * Equivalent to constructing a new object of this type.
     */
    void reset()
    {
        m_.setZero();
        n_ = 0;
    }

private:
    Eigen::MatrixXd m_;

    using col_t = std::decay_t<decltype(m_.col(0))>;
    col_t mean_;
    col_t m2n_;

    size_t n_ = 0;  // number of samples
};

} // namespace math
} // namespace ppl


/**
 * @brief       NPolynomialEval class
 * @file        NPolynomial_Eval.hpp
 * @author      Nuno Marques <nuno.marques@dronesolutions.io>
 * @copyright   Copyright (c) 2018, Swift Engineering Inc.
 * @license     Licensed under the MIT license. See LICENSE for details.
 */

#pragma once

#include <boost/array.hpp>
#include <vector>
#include <boost/math/tools/rational.hpp>

/// \brief NPolynomialEval class that implements a N-degree polynomial eval
template <class T, size_t n>
class NPolynomialEval {
  public:
    /// \brief Gets the output from an input value and the poly indeterminates kept on the m_fit_curves set
    T get_output_from_poly(const size_t &poly_index, const double &input) {
        BOOST_ASSERT_MSG(poly_index <= (m_fit_curves.size() - 1), "The chosen polynomial curve does not exist!");
        return boost::math::tools::evaluate_polynomial(m_fit_curves[poly_index], input);
    }

    /// \brief Gets the output from an input value and input poly indeterminates
    T get_output_from_poly(const boost::array<T, n> &poly, const double &input) {
        return boost::math::tools::evaluate_polynomial(poly, input);
    }

    /// \brief Loads a polynomial fit curve to be used
    void load_poly_fit_curve(const boost::array<T, n> &poly) {
        m_fit_curves.push_back(poly);
    }

  protected:
    /// \brief Array of polynomial fit curves
    std::vector<boost::array<T, n> > m_fit_curves;
};  // class NPolynomialEval


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
using namespace std;
using namespace boost::multiprecision;
int main() {
    cpp_int x, y, a, b, cnt = 0; cin >> x >> y >> a >> b;
    if (y - x < b) {
        cout << 0 << endl;
        return 0;
    }
    while (x < b) {
        if (x * a >= b) break;
        else {
            x *= a;
            cnt++;
        }
    }
    cout << ((y - 1) - x) / b + cnt << endl;
    return 0;
}


#include <Eigen/SPQRSupport>
#include <igl/per_vertex_normals.h>
#include <iostream>
#include "CommonFunctions.h"
#include "MeshGeometry.h"
#include "IntrinsicGeometry.h"
#include "MeshConnectivity.h"

Eigen::MatrixXd lowRankApprox(Eigen::MatrixXd A)
{
    Eigen::MatrixXd posHess = A;
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es;
    es.compute(posHess);
    Eigen::VectorXd evals = es.eigenvalues();

    for (int i = 0; i < evals.size(); i++)
    {
        if (evals(i) < 0)
            evals(i) = 0;
    }
    Eigen::MatrixXd D = evals.asDiagonal();
    Eigen::MatrixXd V = es.eigenvectors();
    posHess = V * D * V.inverse();

    return posHess;
}

Eigen::MatrixXd nullspaceExtraction(const Eigen::SparseMatrix<double> A)
{
	Eigen::SPQR<Eigen::SparseMatrix<double>> solver_SPQR(A.transpose());
	int r = solver_SPQR.rank();

	Eigen::MatrixXd N(A.cols(), A.cols() - r);
	for (int i = 0; i < A.cols() - r; i++)
	{
		Eigen::VectorXd ei(A.cols());
		ei.setZero();
		ei(i + r) = 1;
		N.col(i) = solver_SPQR.matrixQ() * ei;
	}
	std::cout << "rank of A = " << r << std::endl;
	return N;
}

double cotan(const Eigen::Vector3d v0, const Eigen::Vector3d v1, const Eigen::Vector3d v2)
{
    double e0 = (v2 - v1).norm();
    double e1 = (v2 - v0).norm();
    double e2 = (v0 - v1).norm();
    double angle0 = acos((e1 * e1 + e2 * e2 - e0 * e0) / (2 * e1 * e2));
    double cot = 1.0 / tan(angle0);

    return cot;
}

void locatePotentialPureTensionFaces(const std::vector<Eigen::Matrix2d>& abars, const Eigen::MatrixXd& V, const Eigen::MatrixXi& F, std::set<int>& potentialPureTensionFaces)
{
	MeshConnectivity mesh(F);
	MeshGeometry curGeo(V, mesh);
	IntrinsicGeometry restGeo(mesh, abars);
	assert(abars.size() == F.rows());
	int nfaces = F.rows();
	int nverts = V.rows();

	Eigen::VectorXd smallestEvals(nfaces);

	std::vector<bool> isPureFaces(nfaces, false);
	std::vector<bool> isPureVerts(nverts, false);

	for (int i = 0; i < nfaces; i++)
	{
		Eigen::Matrix2d abar = abars[i];
		Eigen::Matrix2d a = curGeo.Bs[i].transpose() * curGeo.Bs[i];
		Eigen::Matrix2d diff = a - abar;
		Eigen::GeneralizedSelfAdjointEigenSolver<Eigen::Matrix2d> solver(diff, abar);

		smallestEvals(i) = std::min(solver.eigenvalues()[0], solver.eigenvalues()[1]);
	}

	for (int i = 0; i < nfaces; i++)
	{
		if (smallestEvals(i) >= 0)
		{
			isPureFaces[i] = true;
		}
	}

	for (int i = 0; i < nfaces; i++)
	{
		if (isPureFaces[i])
		{
			for (int j = 0; j < 3; j++)
			{
				isPureVerts[mesh.faceVertex(i, j)] = true;
			}
		}
	}

	//// if a face is not pure tension, but all its vertices are on the other pure faces, we think it is pure tension
	//// this is used to fill the mixed state "holes" in the pure tension region
	for (int i = 0; i < nfaces; i++)
	{
		if (!isPureFaces[i])
		{
			bool isAllPureVerts = true;
			for (int j = 0; j < 3; j++)
			{
				int vid = mesh.faceVertex(i, j);
				if (isPureVerts[vid] == false)
					isAllPureVerts = false;
			}
			isPureFaces[i] = isAllPureVerts;
		}
	}

	for (int i = 0; i < nfaces; i++)
	{
		if (isPureFaces[i])
			potentialPureTensionFaces.insert(i);
	}
	// set this to empty means we never relax the integrability constriant.
	// potentialPureTensionFaces.clear();
}

void getPureTensionVertsEdges(const std::set<int>& potentialPureTensionFaces, const Eigen::MatrixXi& F, std::set<int>* pureTensionEdges, std::set<int>* pureTensionVerts)
{
	MeshConnectivity mesh(F);

	std::set<int> tmpPureTensionEdges, tmpPureTensionVerts;

	std::set<int> mixedStateEdges;
	// locate the mixed edges:
	for (int i = 0; i < mesh.nEdges(); i++)
	{
		Eigen::Vector2i faceFlags;
		faceFlags << 0, 0;
		for (int j = 0; j < 2; j++)
		{
			int fid = mesh.edgeFace(i, j);
			if (potentialPureTensionFaces.find(fid) != potentialPureTensionFaces.end())
			{
				faceFlags(j) = 1;
			}
		}
		if (faceFlags(0) + faceFlags(1) == 1)
		{
			if (mixedStateEdges.find(i) == mixedStateEdges.end())
			{
				mixedStateEdges.insert(i);
			}
		}

		else if (faceFlags(0) + faceFlags(1) == 2)
		{
			if (tmpPureTensionEdges.find(i) == tmpPureTensionEdges.end())
			{
				tmpPureTensionEdges.insert(i);
			}
		}
	}
	// mixedStateEdges.clear();
	// tmpPureTensionEdges.clear();
	std::cout << mixedStateEdges.size() << " edges between compressed and pure tension faces. " << tmpPureTensionEdges.size() << " edges are inside the pure tension region" << std::endl;

	for (auto& fid : potentialPureTensionFaces)
	{
		for (int j = 0; j < 3; j++)
		{
			int vid = mesh.faceVertex(fid, j);
			if (tmpPureTensionVerts.find(vid) == tmpPureTensionVerts.end())
			{
				tmpPureTensionVerts.insert(vid);
			}
		}
	}
	// tmpPureTensionVerts.clear();
	std::cout << tmpPureTensionVerts.size() << " vertices are inside the pure tension region (including the boundary)" << std::endl;

	if (pureTensionEdges)
	{
		*pureTensionEdges = tmpPureTensionEdges;
	}
	if (pureTensionVerts)
	{
		*pureTensionVerts = tmpPureTensionVerts;
	}
}

void trivialOffset(const Eigen::MatrixXd& V, const Eigen::MatrixXi& F, Eigen::MatrixXd &offsettedV, double d)
{
	Eigen::MatrixXd VN;
	igl::per_vertex_normals(V, F, VN);
	offsettedV = V  + d * VN;
}

void rigidBodyAlignment(const Eigen::MatrixXd& tarPos, const MeshConnectivity& mesh, const Eigen::MatrixXd &pos, Eigen::Matrix3d &R, Eigen::Vector3d &t)
{
	int nverts = tarPos.rows();
    int nfaces = mesh.nFaces();
    Eigen::VectorXd massVec;
    massVec.resize(nverts);
    massVec.setZero();

    Eigen::VectorXd areaList;
    igl::doublearea(tarPos, mesh.faces(), areaList);
    areaList = areaList / 2;

    for(int i=0; i < nfaces; i++)
    {
        double faceArea = areaList(i);
        for(int j=0; j<3; j++)
        {
            int vertIdx = mesh.faceVertex(i, j);
            massVec(vertIdx) += faceArea / 3;
        }
    }

    massVec = massVec / 3;
    massVec = massVec / massVec.maxCoeff();

    Eigen::MatrixXd W = Eigen::MatrixXd::Zero(nverts, nverts);
    W.diagonal() = massVec;

    Eigen::Vector3d avePos, aveTarPos;
    avePos.setZero();
    aveTarPos.setZero();

    for(int i = 0; i < nverts; i++)
    {
        avePos += massVec(i) * pos.row(i);
        aveTarPos += massVec(i) * tarPos.row(i);
    }

    avePos = avePos / massVec.sum();
    aveTarPos = aveTarPos / massVec.sum();

    Eigen::MatrixXd onesMat(nverts,1);
    onesMat.setOnes();

    Eigen::MatrixXd shiftedPos = pos - onesMat * avePos.transpose();
    Eigen::MatrixXd shiftedTarPos = tarPos - onesMat * aveTarPos.transpose();

    Eigen::MatrixXd S = shiftedPos.transpose() * W * shiftedTarPos;
    Eigen::VectorXd b = Eigen::VectorXd::Zero(3);

    Eigen::BDCSVD<Eigen::MatrixXd> solver(S);
    solver.compute(S, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::MatrixXd U = solver.matrixU();
    Eigen::MatrixXd V = solver.matrixV();

    Eigen::MatrixXd middleMat(S.rows(), S.cols());
    middleMat.setIdentity();
    middleMat(S.rows()-1,S.cols()-1) = (V*U.transpose()).determinant();

    R = V * middleMat * U.transpose();
    t = aveTarPos - R*avePos;
}

bool parse(bool& b, const Json::Value& json) {
	if (!json.isBool())
		return false;
	b = json.asBool();
	return true;
}
bool parse(int& n, const Json::Value& json) {
	if (!json.isIntegral())
		return false;
	n = json.asInt();
	return true;
}
bool parse(double& x, const Json::Value& json) {
	if (!json.isNumeric())
		return false;
	x = json.asDouble();
	return true;
}
bool parse(std::string& s, const Json::Value& json)
{
	if (!json.isString())
		return false;
	s = json.asString();
	return true;
}

void generateWTFJson(std::string prefix)
{
	std::string matName = prefix + std::string("_material.dat");
	std::ifstream mfs(matName);
	if (!mfs)
	{
		std::cout << "Missing " << matName << std::endl;
		return;
	}
	double thickness, youngs, poisson, density, penaltyK, pressure, sphereR;
	Eigen::Vector3d gravity, sphereCenter;
	bool isTFT;
	int framefreq, numInterp, bendingType;
	Json::Value json;

	mfs >> thickness;
	mfs >> youngs;
	mfs >> poisson;
	mfs >> density;
	mfs >> penaltyK;
	mfs >> gravity[0] >> gravity[1] >> gravity[2];
	mfs >> isTFT;
	mfs >> framefreq;
	mfs >> sphereCenter[0] >> sphereCenter[1] >> sphereCenter[2];
	mfs >> sphereR;
	mfs >> numInterp;
	mfs >> bendingType;
	mfs >> pressure;

	json["poisson_ratio"] = poisson;
	json["thickness"] = thickness;
	json["youngs_modulus"] = youngs;

	json["nasoq_eps"] = 1e-6;
	json["quadpoints"] = 3;
	json["rest_flat"] = true;
	json["clamp"] = true;
	json["sff_type"] = "midedgeTan";

	std::string filePrefix = prefix;
	std::replace(filePrefix.begin(), filePrefix.end(), '\\', '/'); // handle the backslash issue for windows

	int index = filePrefix.rfind("/");
	std::string modelName = filePrefix.substr(index + 1, filePrefix.length() - 1);

	json["rest_mesh"] = modelName + ".obj";
	json["base_mesh"] = modelName + "_simulated.obj";
	json["obs_mesh"] = modelName + "_obstacle.obj";

	json["clamped_DOFs"] = modelName + "_clamped_vertices.dat";

	json["dphi_path"] = modelName + "_dphi_simulated.txt";
	json["amp_path"] = modelName + "_amplitude_simulated.txt";
	json["phi_path"] = modelName + "_phi_simulated.txt";

	std::ofstream file((prefix + "_WTF.json").c_str());
	if (!file)
	{
		std::cout << "output path doesn't exist." << std::endl;
		return;
	}
	Json::StyledStreamWriter writer;
	writer.write(file, json);
	file.close();
}

void generateElasticJson(std::string prefix)
{
	std::string matName = prefix + std::string("_material.dat");
	std::ifstream mfs(matName);
	if (!mfs)
	{
		std::cout << "Missing " << matName << std::endl;
		return;
	}
	double thickness, youngs, poisson, density, penaltyK, pressure,sphereR;
	Eigen::Vector3d gravity, sphereCenter;
	bool isTFT;
	int framefreq, numInterp, bendingType;
	Json::Value json;

	mfs >> thickness;
	mfs >> youngs;
	mfs >> poisson;
	mfs >> density;
	mfs >> penaltyK;
	mfs >> gravity[0] >> gravity[1] >> gravity[2];
	mfs >> isTFT;
	mfs >> framefreq;
	mfs >> sphereCenter[0] >> sphereCenter[1] >> sphereCenter[2];
	mfs >> sphereR;
	mfs >> numInterp;
	mfs >> bendingType;
	mfs >> pressure;

	json["poisson_ratio"] = poisson;
	json["thickness"] = thickness;
	json["youngs_modulus"] = youngs;
	json["density"] = density;
	json["collision_penalty"] = penaltyK;
	json["perturb"] = 0;
	json["pressure"] = pressure;
	json["collision_eta"] = 0.5 * thickness;
	json["rest_flat"] = true;
	json["isNeoHookean"] = false;
	json["isTFT"] = isTFT;
	json["bending_type"] = bendingType == 0 ? "elastic" : "quadratic";
	if (bendingType == 2)
		json["bending_type"] = "None";

	json["sff_type"] = "midedgeTan";
	json["max_stepsize"] = 1.0;
	json["gravity"].resize(3);
	for (int i = 0; i < 3; i++)
		json["gravity"][i] = gravity(i);

	json["frame_frequency"] = framefreq;
	json["num_interpolation"] = numInterp;

	std::string filePrefix = prefix;
	std::replace(filePrefix.begin(), filePrefix.end(), '\\', '/'); // handle the backslash issue for windows

	int index = filePrefix.rfind("/");
	std::string modelName = filePrefix.substr(index + 1, filePrefix.length() - 1);

	json["rest_mesh"] = modelName + ".obj";
	json["init_mesh"] = modelName + "_guess.obj";
	json["cur_mesh"] = modelName + "_simulated.obj";
	json["obs_mesh"] = modelName + "_obstacle.obj";

	json["curedge_DOFs"] = modelName + "_edgedofs_simulated.txt";
	json["clamped_DOFs"] = modelName + "_clamped_vertices.dat";

	std::ofstream file((prefix + "_Elastic.json").c_str());
	if (!file)
	{
		std::cout << "output path doesn't exist." << std::endl;
		return;
	}
	Json::StyledStreamWriter writer;
	writer.write(file, json);
	file.close();
}


// -*- mode: c++; fill-column: 80; indent-tabs-mode: nil; -*-
#ifndef MXPFIT_JACOBI_SVD_HPP
#define MXPFIT_JACOBI_SVD_HPP

#include <cassert>
#ifdef DEBUG
#include <iomanip>
#include <iostream>
#endif /* DEBUG */

#include <Eigen/Core>

namespace mxpfit
{
namespace detail
{
//
// Make Jacobi rotation matrix \c J of the form
//
//  J = [ cs conj(sn)]
//      [-sn conj(cs)]
//
// that diagonalize the 2x2 matirix
//
// B = [     a  c]
//     [conj(c) b]
//
// as D = J.t() * B * J
//
// J.t() = [ conj(cs)    -conj(sn)] = J.inv()
//         [      sn           cs ]
//
// J * [a11 a12] = [ cs*a11 + conj(sn)*a21   cs*a12 + conj(sn)*a22]
//     [a21 a22]   [-sn*a11 + conj(cs)*a21  -sn*a12 + conj(cs)*a22]
//
// [a11 a12] * J = [     cs *a11 -      sn *a12        cs *a21 -      sn *a22]
// [a21 a22]       [conj(sn)*a11 +E conj(cs)*a12  -conj(sn)*a21 + conj(cs)*a22]
//
template <typename RealT, typename ScalarT>
bool make_jacobi_rotation(RealT a, RealT b, ScalarT c, RealT& cs, ScalarT& sn)
{
    using Eigen::numext::abs;
    using Eigen::numext::abs2;
    using Eigen::numext::conj;
    using Eigen::numext::sqrt;

    constexpr const RealT one = RealT(1);

    if (c == ScalarT())
    {
        cs = one;
        sn = ScalarT();
        return false;
    }

    auto zeta = (a - b) / (RealT(2) * abs(c));
    auto w    = sqrt(abs2(zeta) + one);
    auto t    = (zeta > RealT() ? one / (zeta + w) : one / (zeta - w));

    cs = one / sqrt(abs2(t) + one);
    sn = -t * cs * (conj(c) / abs(c));

    return true;
}

template <typename T>
struct one_sided_jacobi_helper
{
    using Index      = Eigen::Index;
    using Scalar     = T;
    using RealScalar = typename Eigen::NumTraits<Scalar>::Real;
    //
    // Sum of the square of the absolute value of the elments in the i-th column
    // of the matrix G, i.e. \f$ \sum_{k=0}^{n-1} |G_{ki}|^{2}. \f$
    //
    template <typename MatG>
    static RealScalar submat_diag(const Eigen::MatrixBase<MatG>& G, Index i)
    {
        return G.col(i).squaredNorm();
    }

    template <typename MatG>
    static Scalar submat_offdiag(const Eigen::MatrixBase<MatG>& G, Index i,
                                 Index j)
    {
        return G.col(i).dot(G.col(j));
    }

    template <typename MatU>
    static void update_vecs(Eigen::MatrixBase<MatU>& U, Index i, Index j,
                            RealScalar cs, const Scalar& sn)
    {
        using Eigen::numext::conj;
        for (Index k = 0; k < U.rows(); ++k)
        {
            const auto t1 = U(k, i);
            const auto t2 = U(k, j);
            // Apply Jacobi rotation matrix on the right
            U(k, i) = cs * t1 - sn * t2;
            U(k, j) = conj(sn) * t1 + cs * t2;
        }
    }
};

} // namespace: detail

///
/// Compute the singular value decomposition (SVD) by one-sided Jacobi
/// algorithm.
///
/// This function compute the singular value decomposition (SVD) using modified
/// one-sided Jacobi algorithm.
///
/// #### References
///  - James Demmel and Kresimir Veselic, "Jacobi's method is more accurate than
///    QR", LAPACK working note 15 (lawn15) (1989), Algorithm 4.1.
///
/// \param[in,out] A  On entry, an \f$m \times n\f$ matrix to be decomposed. On
///     exit, columns of `A` holds left singular vectors.
/// \param[out] sigma  singular values of matrix `A`.
/// \param[out] V      right singular vectors.
/// \param[in] small   positive real number that determines stopping criterion
///     of Jacobi algorithm.
///
template <typename MatA, typename VecS, typename MatV, typename RealT>
void one_sided_jacobi_svd(Eigen::MatrixBase<MatA>& A,
                          Eigen::MatrixBase<VecS>& sigma,
                          Eigen::MatrixBase<MatV>& V, RealT tol)
{
    using Index      = Eigen::Index;
    using Scalar     = typename MatA::Scalar;
    using RealScalar = typename Eigen::NumTraits<Scalar>::Real;
    using jacobi_aux = detail::one_sided_jacobi_helper<Scalar>;

    using Eigen::numext::abs;
    using Eigen::numext::sqrt;
    // constexpr const Index max_sweep = 50;

    auto n                = A.cols();
    const Index max_sweep = n * (n - 1) / 2;
    assert(V.rows() == A.cols());
    //
    // Initialize right singular vectors
    //
    V.setIdentity();

    Scalar sn;
    RealScalar cs;
    RealScalar max_resid = tol + RealScalar(1);
    Index nsweep         = max_sweep + 1;
    while (--nsweep && max_resid > tol)
    {
        max_resid = RealScalar();
        for (Index j = 1; j < n; ++j)
        {
            auto b = jacobi_aux::submat_diag(A, j);
            for (Index i = 0; i < j; ++i)
            {
                //
                // For all pairs i < j, compute the 2x2 submatrix of A.t() * A
                // constructed from i-th and j-th columns, such as
                //
                //   M = [     a   c]
                //       [conj(c)  b]
                //
                // where
                //
                //   a = \sum_{k=0}^{n-1} |A(k,i)|^{2} (computed in outer loop)
                //   b = \sum_{k=0}^{n-1} |A(k,j)|^{2}
                //   c = \sum_{k=0}^{n-1} conj(A(k,i)) * A(k,j)
                //
                auto a    = jacobi_aux::submat_diag(A, i);
                auto c    = jacobi_aux::submat_offdiag(A, i, j);
                max_resid = std::max(max_resid, abs(c) / sqrt(a * b));
                //
                // Compute the Jacobi rotation matrix which diagonalize M.
                //
                if (detail::make_jacobi_rotation(a, b, c, cs, sn))
                {
                    // If the return vlaue of make_jacobi_rotation is false, no
                    // rotation is made.
                    //
                    // Update columns i and j of A
                    //
                    jacobi_aux::update_vecs(A, i, j, cs, sn);
                    //
                    // Update right singular vector V
                    //
                    jacobi_aux::update_vecs(V, i, j, cs, sn);
                }
            }
        }
#ifdef DEBUG
        std::cout << "(one_sided_jacobi_svd) iter " << std::setw(8)
                  << max_sweep - nsweep << ": max residual = " << max_resid
                  << '\n';
#endif /* DEBUG */
    }
    //
    // Set singular values and left singular vectors.
    //
    for (Index j = 0; j < A.cols(); ++j)
    {
        // Singular values are the norms of the columns of the final A
        sigma(j) = A.col(j).norm();
        // Left singular vectors are the normalized columns of the final A
        A.col(j) /= sigma(j);
    }
    //
    // Sort singular values in descending order. The corresponding singular
    // vectors are also rearranged.
    //
    for (Index i = 0; i < n - 1; ++i)
    {
        //
        // Find the index of the maximum value of a sub-array, sigma(i:n-1).
        //
        Index imax = i;
        for (Index k = i + 1; k < n; ++k)
        {
            if (sigma(k) > sigma(imax))
            {
                imax = k;
            }
        }

        if (imax != i)
        {
            // Move the largest singular value to the beggining of the
            // sub-array, and corresponsing singular vectors by swapping columns
            // of A and V.
            std::swap(sigma(i), sigma(imax));
            A.col(i).swap(A.col(imax));
            V.col(i).swap(V.col(imax));
        }
    }
}

///
/// Compute the singular value decomposition (SVD) by one-sided Jacobi
/// algorithm.
///
/// This function only computes the singular values and left singular vectors.
///
/// \param[in,out] A  On entry, an \f$m \times n\f$ matrix to be decomposed. On
///     exit, columns of `A` holds left singular vectors.
/// \param[out] sigma  singular values of matrix `A`.
/// \param[in] small   positive real number that determines stopping criterion
///     of Jacobi algorithm.
///
template <typename MatA, typename VecS, typename RealT>
void one_sided_jacobi_svd(Eigen::MatrixBase<MatA>& A,
                          Eigen::MatrixBase<VecS>& sigma, RealT tol)
{
    using Index      = Eigen::Index;
    using Scalar     = typename MatA::Scalar;
    using RealScalar = typename Eigen::NumTraits<Scalar>::Real;
    using jacobi_aux = detail::one_sided_jacobi_helper<Scalar>;

    using Eigen::numext::abs;
    using Eigen::numext::sqrt;
    // constexpr const Index max_sweep = 50;

    auto n                = A.cols();
    const Index max_sweep = n * (n - 1) / 2;

    Scalar sn;
    RealScalar cs;
    RealScalar max_resid = tol + RealScalar(1);
    Index nsweep         = max_sweep + 1;
    while (--nsweep && max_resid > tol)
    {
        max_resid = RealScalar();
        for (Index j = 1; j < n; ++j)
        {
            auto b = jacobi_aux::submat_diag(A, j);
            for (Index i = 0; i < j; ++i)
            {
                //
                // For all pairs i < j, compute the 2x2 submatrix of A.t() * A
                // constructed from i-th and j-th columns, such as
                //
                //   M = [     a   c]
                //       [conj(c)  b]
                //
                // where
                //
                //   a = \sum_{k=0}^{n-1} |A(k,i)|^{2} (computed in outer loop)
                //   b = \sum_{k=0}^{n-1} |A(k,j)|^{2}
                //   c = \sum_{k=0}^{n-1} conj(A(k,i)) * A(k,j)
                //
                auto a    = jacobi_aux::submat_diag(A, i);
                auto c    = jacobi_aux::submat_offdiag(A, i, j);
                max_resid = std::max(max_resid, abs(c) / sqrt(a * b));
                //
                // Compute the Jacobi rotation matrix which diagonalize M.
                //
                if (detail::make_jacobi_rotation(a, b, c, cs, sn))
                {
                    // If the return vlaue of make_jacobi_rotation is false, no
                    // rotation is made.
                    //
                    // Update columns i and j of A
                    //
                    jacobi_aux::update_vecs(A, i, j, cs, sn);
                }
            }
        }
#ifdef DEBUG
        std::cout << "(one_sided_jacobi_svd) iter " << std::setw(8)
                  << max_sweep - nsweep << ": max residual = " << max_resid
                  << '\n';
#endif /* DEBUG */
    }
    //
    // Set singular values and left singular vectors.
    //
    for (Index j = 0; j < A.cols(); ++j)
    {
        // Singular values are the norms of the columns of the final A
        sigma(j) = A.col(j).norm();
        // Left singular vectors are the normalized columns of the final A
        A.col(j) /= sigma(j);
    }
    //
    // Sort singular values in descending order. The corresponding singular
    // vectors are also rearranged.
    //
    for (Index i = 0; i < n - 1; ++i)
    {
        //
        // Find the index of the maximum value of a sub-array, sigma(i:n-1).
        //
        Index imax = i;
        for (Index k = i + 1; k < n; ++k)
        {
            if (sigma(k) > sigma(imax))
            {
                imax = k;
            }
        }

        if (imax != i)
        {
            // Move the largest singular value to the beggining of the
            // sub-array, and corresponsing singular vectors by swapping columns
            // of A and V.
            std::swap(sigma(i), sigma(imax));
            A.col(i).swap(A.col(imax));
        }
    }
}

} // namespace: mxpfit

#endif /* MXPFIT_JACOBI_SVD_HPP */


// RMRayTracer.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include <iostream>
#include <fstream>
#include <algorithm>
#include <vector>
#include <random>
#include <Eigen/Dense>
#include <Eigen/Geometry>

using namespace Eigen;
using namespace std;

using Ray = ParametrizedLine<float, 3>;
using Colori = Vector3i;
using Colorf = Vector3f;

namespace rm
{
	class HitResult
	{
	public:
		HitResult()
			: t(0.0f)
			, hitPostion(Vector3f::Zero())
			, isHit(false)
		{}

		void Reset()
		{
			t = 0.0f;
			hitPostion = Vector3f::Zero();
			isHit = false;
		}

		float t;
		Vector3f hitPostion;
		bool isHit;
	};

	class Camera
	{
	public:
		Camera(const Vector2i& lScreenSize, const Vector2f& lScreenVirtualSize, const Vector3f& lCameraPosition, const Vector3f& lLeftRightCorner, int lSampleCount)
			: screenSize(lScreenSize)
			, screenVirtualSize(lScreenVirtualSize)
			, screenVirtualHalfSize(lScreenVirtualSize / 2.0f)
			, cameraPosition(lCameraPosition)
			, leftRightCorner(lLeftRightCorner)
			, sampleCount(lSampleCount)
		{

		}

		const Vector2i& GetScreenSize() const { return screenSize; }
		const Vector2f& GetScreenVirtualSize() const { return screenVirtualSize; }
		const Vector2f& GetScreenVirtualHalfSize() const { return screenVirtualHalfSize; }
		const Vector3f& GetCameraPosition() const { return cameraPosition; }
		const Vector3f& GetLeftRightCorner() const { return leftRightCorner; }
		int GetSampleCount() const { return sampleCount; }

	private:
		Vector2i screenSize;
		Vector2f screenVirtualSize;
		Vector2f screenVirtualHalfSize;
		Vector3f cameraPosition;
		Vector3f leftRightCorner;
		int sampleCount;
	};

	class IHitableObject
	{
	public:
		virtual bool		IsHit(const Ray& ray, float minT, float maxT, HitResult& hitResult) const = 0;
		virtual Vector3f	GetNormal(const Vector3f& surfacePosition) const = 0;
		virtual Colorf		GetColor() const = 0;
	};

	class Sphere : public IHitableObject
	{
	public:
		Sphere(const Vector3f& lCenter, float lRadius, const Colorf lColor)
			: center(lCenter)
			, radius(lRadius)
			, color(lColor)
		{}

		virtual bool IsHit(const Ray& ray, float minT, float maxT, HitResult& hitResult) const
		{
			hitResult.Reset();

			const Vector3f& vectorA = ray.origin();
			const Vector3f& vectorB = ray.direction();
			const Vector3f& vectorC = center;

			float a = vectorB.dot(vectorB);
			Vector3f vectorAC = vectorA - vectorC;
			float b = 2 * vectorB.dot(vectorAC);
			float c = vectorAC.dot(vectorAC) - radius * radius;

			float discriminant = b * b - 4 * a * c;

			if (discriminant > 0.0f)
			{
				float t1 = (-b + std::sqrt(discriminant)) / (2.0f * a);
				float t2 = (-b - std::sqrt(discriminant)) / (2.0f * a);

				float t = std::min(t1, t2);

				if (t >= minT && t < maxT)
				{
					hitResult.t = t;
					hitResult.hitPostion = ray.pointAt(t);
					hitResult.isHit = true;
				}
			}

			return hitResult.isHit;
		}

		virtual Vector3f GetNormal(const Vector3f& surfacePosition) const
		{
			Vector3f normal = surfacePosition - center;
			normal.normalize();
			return normal;
		}

		virtual Colorf GetColor() const
		{
			return color;
		}

	private:
		Vector3f center;
		float radius;
		Colorf color;
	};

	class Scene
	{
	public:
		Scene()
		{
			InitScene();
		}

		~Scene()
		{
			DestoryScene();
		}

		void AddHitableObject(IHitableObject* obj)
		{
			objectList.push_back(obj);
		}

		void RayTrace(ofstream& fileHandle, const Camera& cam);

	protected:
		void InitScene()
		{

		}

		void DestoryScene()
		{
			for each (auto obj in objectList)
			{
				delete obj;
			}

			objectList.clear();
		}

	private:
		Colori GetBackgroundColor(Ray& ray)
		{
			float sqrt = std::sqrt(ray.direction().x() * ray.direction().x() + ray.direction().y() * ray.direction().y());
			float lerpValue = std::min(1.0f, sqrt);
			int value = (int)(255.0f * (1.0f - lerpValue));
			return Colori(value, value, 255);
		}

		using HitableObjectList = std::vector<rm::IHitableObject*>;

		HitableObjectList objectList;
	};

	void Scene::RayTrace(ofstream& fileHandle, const Camera& cam)
	{
		std::random_device rd;
		std::mt19937 gen(rd());
		std::uniform_real_distribution<float> dis(0.0f, 1.0f);

		for (int y = cam.GetScreenSize().y(); y >= 0; --y)
		{
			for (int x = 0; x < cam.GetScreenSize().x(); ++x)
			{
				Colori color(0, 0, 0);

				for (int index = 0; index < cam.GetSampleCount(); ++index)
				{
					float sampleX = float(x) + dis(gen);
					float sampleY = float(y) + dis(gen);

					Vector3f offset(sampleX / (float)cam.GetScreenSize().x() * cam.GetScreenVirtualSize().x(),
						sampleY / (float)cam.GetScreenSize().y() * cam.GetScreenVirtualSize().y(), 0.0f);
					offset = offset + cam.GetLeftRightCorner();
					offset.normalize();

					Ray ray(cam.GetCameraPosition(), offset);

					HitResult hitResult;
					float minT = 0.0f;
					float maxT = FLT_MAX;
					bool atLeastHitOnce = false;

					for each (auto obj in objectList)
					{
						if (obj->IsHit(ray, minT, maxT, hitResult))
						{
							atLeastHitOnce = true;
							maxT = hitResult.t;
							Vector3f normal = obj->GetNormal(hitResult.hitPostion);
							float blend = 1.0f - (1.0f + ray.direction().dot(normal)) / 2.0f;
							Colorf objColor = blend * 256.0f * obj->GetColor();
							color += Colori((int)objColor.x(), (int)objColor.y(), (int)objColor.z());
						}
					}

					if (!atLeastHitOnce)
					{
						color += GetBackgroundColor(ray);
					}
				}

				color /= cam.GetSampleCount();

				fileHandle << color.x() << " " << color.y() << " " << color.z() << "\n";
			}
		}
	} // end of Scene::RayTrace

} // end of namespace

int main()
{
	ofstream ppmFile;
	ppmFile.open("buffer.ppm");

	Vector2i screenSize(200, 200);
	Vector2f screenVirtualSize(2, 2);
	Vector2f screenVirtualHalfSize(screenVirtualSize / 2.0f);
	Vector3f cameraPosition(0.0f, 0.0f, 1.0f);
	Vector3f leftRightCorner(-screenVirtualHalfSize.x(), -screenVirtualHalfSize.y(), -1.0f);

	rm::Camera cam(screenSize, screenVirtualSize, cameraPosition, leftRightCorner, 8);

	ppmFile << "P3\n" << screenSize.x() << " " << screenSize.y() << "\n255\n";

	rm::Sphere* sphere1 = new rm::Sphere { Vector3f(0.0f, 0.0f, -1.0f), 0.8f, Colorf(1.0f, 0.0f, 0.0f) };
	rm::Sphere* sphere2 = new rm::Sphere { Vector3f(0.0f, 0.0f, -2.5f), 1.8f, Colorf(0.0f, 0.0f, 1.0f) };

	rm::Scene scene;
	scene.AddHitableObject(sphere1);
	scene.AddHitableObject(sphere2);
	scene.RayTrace(ppmFile, cam);

	ppmFile.close();

    return 0;
}


// Boost.Geometry
// Unit Test

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

// Contributed and/or modified by Vissarion Fysikopoulos, 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 <geometry_test_common.hpp>

#include "test_formula.hpp"
#include "vertex_longitude_cases.hpp"

#include <boost/geometry/formulas/vertex_latitude.hpp>
#include <boost/geometry/formulas/vertex_longitude.hpp>
#include <boost/geometry/formulas/vincenty_inverse.hpp>
#include <boost/geometry/formulas/thomas_inverse.hpp>
#include <boost/geometry/formulas/andoyer_inverse.hpp>

#include <boost/geometry/strategies/strategies.hpp>

#include <boost/geometry/util/math.hpp>

#define BOOST_GEOMETRY_TEST_DEBUG

namespace bg = boost::geometry;

template<typename CT>
CT test_vrt_lon_sph(CT lon1r,
                    CT lat1r,
                    CT lon2r,
                    CT lat2r)
{
    CT a1 = bg::formula::spherical_azimuth(lon1r, lat1r, lon2r, lat2r);

    typedef bg::model::point<CT, 2,
            bg::cs::spherical_equatorial<bg::radian> > point;

    bg::model::segment<point> segment(point(lon1r, lat1r),
                                      point(lon2r, lat2r));
    bg::model::box<point> box;
    bg::envelope(segment, box);

    CT vertex_lat;
    CT lat_sum = lat1r + lat2r;
    if (lat_sum > CT(0))
    {
        vertex_lat = bg::get_as_radian<bg::max_corner, 1>(box);
    } else {
        vertex_lat = bg::get_as_radian<bg::min_corner, 1>(box);
    }

    bg::strategy::azimuth::spherical<> azimuth;

    return bg::formula::vertex_longitude
            <CT, bg::spherical_equatorial_tag>::
            apply(lon1r, lat1r,
                  lon2r, lat2r,
                  vertex_lat,
                  a1,
                  azimuth);
}

template
<
        template <typename, bool, bool, bool, bool, bool> class FormulaPolicy,
        typename CT
        >
CT test_vrt_lon_geo(CT lon1r,
                    CT lat1r,
                    CT lon2r,
                    CT lat2r)
{
    // WGS84
    bg::srs::spheroid<CT> spheroid(6378137.0, 6356752.3142451793);

    typedef FormulaPolicy<CT, false, true, false, false, false> formula;
    CT a1 = formula::apply(lon1r, lat1r, lon2r, lat2r, spheroid).azimuth;

    typedef bg::model::point<CT, 2, bg::cs::geographic<bg::radian> > geo_point;

    bg::model::segment<geo_point> segment(geo_point(lon1r, lat1r),
                                          geo_point(lon2r, lat2r));
    bg::model::box<geo_point> box;
    bg::envelope(segment, box);

    CT vertex_lat;
    CT lat_sum = lat1r + lat2r;
    if (lat_sum > CT(0))
    {
        vertex_lat = bg::get_as_radian<bg::max_corner, 1>(box);
    } else {
        vertex_lat = bg::get_as_radian<bg::min_corner, 1>(box);
    }

    bg::strategy::azimuth::geographic<> azimuth_geographic;

    return bg::formula::vertex_longitude
            <CT, bg::geographic_tag>::apply(lon1r, lat1r,
                                            lon2r, lat2r,
                                            vertex_lat,
                                            a1,
                                            azimuth_geographic);

}

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

    double lon1r = results.p1.lon * d2r;
    double lat1r = results.p1.lat * d2r;
    double lon2r = results.p2.lon * d2r;
    double lat2r = results.p2.lat * d2r;

    if(lon1r > lon2r)
    {
        std::swap(lon1r, lon2r);
        std::swap(lat1r, lat2r);
    }

    double res_an = test_vrt_lon_geo<bg::formula::andoyer_inverse>
            (lon1r, lat1r, lon2r, lat2r);
    double res_th = test_vrt_lon_geo<bg::formula::thomas_inverse>
            (lon1r, lat1r, lon2r, lat2r);
    double res_vi = test_vrt_lon_geo<bg::formula::vincenty_inverse>
            (lon1r, lat1r, lon2r, lat2r);
    double res_sh = test_vrt_lon_sph(lon1r, lat1r, lon2r, lat2r);

    bg::math::normalize_longitude<bg::radian, double>(res_an);
    bg::math::normalize_longitude<bg::radian, double>(res_th);
    bg::math::normalize_longitude<bg::radian, double>(res_vi);
    bg::math::normalize_longitude<bg::radian, double>(res_sh);

    check_one(res_an, results.andoyer * d2r, res_vi, 0.001);
    check_one(res_th, results.thomas * d2r, res_vi, 0.01);//in some tests thomas gives low accuracy
    check_one(res_vi, results.vincenty * d2r, res_vi, 0.0000001);
    check_one(res_sh, results.spherical * d2r, res_vi, 1);
}

int test_main(int, char*[])
{

    for (size_t i = 0; i < expected_size; ++i)
    {
        test_all(expected[i]);
    }

    return 0;
}


//
// Copyright Jesse Manning 2007
//
// 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)
//

#include "convenience.h"
#include <boost/numeric/bindings/lapack/gelss.hpp>

// set to 1 to write test output to file, otherwise outputs to console
#define OUTPUT_TO_FILE 0

// determines which tests to run
#define TEST_SQUARE 1
#define TEST_UNDERDETERMINED 1
#define TEST_OVERDETERMINED 1
#define TEST_MULTIPLE_SOLUTION_VECTORS 1

// determines if optimal, minimal, or both workspaces are used for testing
#define USE_OPTIMAL_WORKSPACE 1
#define USE_MINIMAL_WORKSPACE 1

namespace lapack = boost::numeric::bindings::lapack;

// test function declarations
template <typename StreamType, typename MatrType, typename VecType>
int test_square_gelss(StreamType& oss);

template <typename StreamType, typename MatType, typename VecType>
int test_under_gelss(StreamType& oss);

template <typename StreamType, typename MatType, typename VecType>
int test_over_gelss(StreamType& oss);

template <typename StreamType, typename MatType, typename VecType>
int test_multiple_gelss(StreamType& oss);

template <typename StreamType, typename MatType, typename VecType>
int test_transpose_gel(StreamType& oss, const char& trans);

int main()
{
	// stream for test output
	typedef std::ostringstream stream_t;
	stream_t oss;

#if TEST_SQUARE
	oss << "Start Square Matrix Least Squares Tests" << std::endl;
	oss << "Testing sgelss" << std::endl;
	if (test_square_gelss<stream_t, fmat_t, fvec_t>(oss) == 0)
	{
		oss << "sgelss passed." << std::endl;
	}
	oss << "End sgelss tests" << std::endl;
	oss << std::endl;
	oss << "Testing dgelss" << std::endl;
	if (test_square_gelss<stream_t, dmat_t, dvec_t>(oss) == 0)
	{
		oss << "dgelss passed." << std::endl;
	}
	oss << "End dgelss tests" << std::endl;
	oss << std::endl;
	oss << "Testing cgelss" << std::endl;
	if (test_square_gelss<stream_t, fcmat_t, fcvec_t>(oss) == 0)
	{
		oss << "cgelss passed." << std::endl;
	}
	oss << "End cgelss tests" << std::endl;
	oss << std::endl;
	oss << "Testing zgelss" << std::endl;
	if (test_square_gelss<stream_t, fcmat_t, fcvec_t>(oss) == 0)
	{
		oss << "zgelss passed." << std::endl;
	}
	oss << "End zgelss tests" << std::endl;
	oss << std::endl;
	oss << "End Square Matrix Least Squares Tests" << std::endl;
#endif

#if TEST_UNDERDETERMINED
	oss << std::endl;
	oss << "Start Under-determined Matrix Least Squares Test" << std::endl;
	oss << "Testing sgelss" << std::endl;
	if (test_under_gelss<stream_t, fmat_t, fvec_t>(oss) == 0)
	{
		oss << "sgelss passed." << std::endl;
	}
	oss << "End sgelss tests" << std::endl;
	oss << std::endl;
	oss << "Testing dgelss" << std::endl;
	if (test_under_gelss<stream_t, dmat_t, dvec_t>(oss) == 0)
	{
		oss << "dgelss passed." << std::endl;
	}
	oss << "End dgelss tests" << std::endl;
	oss << std::endl;
	oss << "Testing cgelss" << std::endl;
	if (test_under_gelss<stream_t, fcmat_t, fcvec_t>(oss) == 0)
	{
		oss << "cgelss passed." << std::endl;
	}
	oss << "End cgelss tests" << std::endl;
	oss << std::endl;
	oss << "Testing zgelss" << std::endl;
	if (test_under_gelss<stream_t, fcmat_t, fcvec_t>(oss) == 0)
	{
		oss << "zgelss passed." << std::endl;
	}
	oss << "End zgelss tests" << std::endl;
	oss << std::endl;
	oss << "End Underdetermined Matrix Least Squares Tests" << std::endl;
#endif

#if TEST_OVERDETERMINED
	oss << std::endl;
	oss << "Start Overdetermined Matrix Least Squares Test" << std::endl;
	oss << "Testing sgelss" << std::endl;
	if (test_over_gelss<stream_t, fmat_t, fvec_t>(oss) == 0)
	{
		oss << "sgelss passed." << std::endl;
	}
	oss << "End sgelss tests" << std::endl;
	oss << std::endl;
	oss << "Testing dgelss" << std::endl;
	if (test_over_gelss<stream_t, dmat_t, dvec_t>(oss) == 0)
	{
		oss << "dgelss passed." << std::endl;
	}
	oss << "End dgelss tests" << std::endl;
	oss << std::endl;
	oss << "Testing cgelss" << std::endl;
	if (test_over_gelss<stream_t, fcmat_t, fcvec_t>(oss) == 0)
	{
		oss << "cgelss passed." << std::endl;
	}
	oss << "End cgelss tests" << std::endl;
	oss << std::endl;
	oss << "Testing zgelss" << std::endl;
	if (test_over_gelss<stream_t, fcmat_t, fcvec_t>(oss) == 0)
	{
		oss << "zgelss passed." << std::endl;
	}
	oss << "End zgelss tests" << std::endl;
	oss << std::endl;
	oss << "End Overdetermined Matrix Least Squares Test" << std::endl;
#endif

#if TEST_MULTIPLE_SOLUTION_VECTORS
	oss << std::endl;
	oss << "Start Multiple Solution Vectors Least Squares Test" << std::endl;
	oss << "Testing sgelss" << std::endl;
	if (test_multiple_gelss<stream_t, fmat_t, fvec_t>(oss) == 0)
	{
		oss << "sgelss passed." << std::endl;
	}
	oss << "End sgelss tests" << std::endl;
	oss << std::endl;
	oss << "Testing dgelss" << std::endl;
	if (test_multiple_gelss<stream_t, dmat_t, dvec_t>(oss) == 0)
	{
		oss << "dgelss passed." << std::endl;
	}
	oss << "End dgelss tests" << std::endl;
	oss << std::endl;
	oss << "Testing cgelss" << std::endl;
	if (test_multiple_gelss<stream_t, fcmat_t, fcvec_t>(oss) == 0)
	{
		oss << "cgelss passed." << std::endl;
	}
	oss << "End cgelss tests" << std::endl;
	oss << std::endl;
	oss << "Testing zgelss" << std::endl;
	if (test_multiple_gelss<stream_t, fcmat_t, fcvec_t>(oss) == 0)
	{
		oss << "zgelss passed." << std::endl;
	}
	oss << "End zgelss tests" << std::endl;
	oss << std::endl;
	oss << "End Multiple Solution Vectors Least Squares Test" << std::endl;
#endif

#if OUTPUT_TO_FILE
	// Finished testing
	std::cout << std::endl;
	std::cout << "Tests Completed." << std::endl;
	std::cout << std::endl;

	std::string filename;
	std::cout << "Enter filename to write test results: ";
	std::getline(std::cin, filename);

	std::ofstream testFile(filename.c_str());

	if (testFile)
	{
		testFile << oss.str();
		testFile.close();
	}
#else
	std::cout << oss.str() << std::endl;

	// Finished testing
	std::cout << std::endl;
	std::cout << "Tests Completed." << std::endl;
	std::cout << std::endl;
#endif

	// wait for user to finish
//	std::string done;
//	std::cout << "Press Enter to exit";
//	std::getline(std::cin, done);

}

// tests square system (m-by-n where m == n)
template <typename StreamType, typename MatType, typename VecType>
int test_square_gelss(StreamType& oss)
{
	// return value
	int err = 0;

	// square matrix test
	MatType mat(MatrixGenerator<MatType>()(row_size, col_size));
	VecType vec(VectorGenerator<VecType>()(row_size));

	//const int m = traits::matrix_size1(mat);
	const int n = traits::matrix_size2(mat);

#if USE_OPTIMAL_WORKSPACE
	MatType optimalmat(mat);
	VecType optimalvec(vec);
	err += lapack::gelss(optimalmat, optimalvec, lapack::optimal_workspace());
	VecType optimalanswer(ublas::project(optimalvec, ublas::range(0, n)));
	VecType optimal_check = ublas::prod(mat, optimalanswer);
#endif
#if USE_MINIMAL_WORKSPACE
	MatType minimalmat(mat);
	VecType minimalvec(vec);
	err += lapack::gelss(minimalmat, minimalvec, lapack::minimal_workspace());
	VecType minimalanswer(ublas::project(minimalvec, ublas::range(0, n)));
	VecType minimal_check = ublas::prod(mat, minimalanswer);
#endif

	matrix_print(oss, "A", mat);
	oss << std::endl;
	vector_print(oss, "B", vec);
	oss << std::endl;

#if USE_OPTIMAL_WORKSPACE
	vector_print(oss, "optimal workspace x", optimalanswer);
	oss << std::endl;
#endif
#if USE_MINIMAL_WORKSPACE
	vector_print(oss, "minimal workspace x", minimalanswer);
	oss << std::endl;
#endif
#if USE_OPTIMAL_WORKSPACE
	// check A*x=B
	vector_print(oss, "optimal A*x=B", optimal_check);
	oss << std::endl;
#endif
#if USE_MINIMAL_WORKSPACE
	vector_print(oss, "minimal A*x=B", minimal_check);
	oss << std::endl;
#endif

	return err;
}

// tests overdetermined system (m-by-n where m < n)
template <typename StreamType, typename MatType, typename VecType>
int test_under_gelss(StreamType& oss)
{
	// return value
	int err = 0;

	// under-determined matrix test
	MatType mat(MatrixGenerator<MatType>()(row_range, col_size));
	VecType vec(VectorGenerator<VecType>()(row_size));

	//const int m = traits::matrix_size1(mat);
	const int n = traits::matrix_size2(mat);

#if USE_OPTIMAL_WORKSPACE
	MatType optimalmat(mat);
	VecType optimalvec(vec);
	err += lapack::gelss(optimalmat, optimalvec, lapack::optimal_workspace());
	VecType optimalanswer(ublas::project(optimalvec, ublas::range(0, n)));
	VecType optimal_check = ublas::prod(mat, optimalanswer);
#endif
#if USE_MINIMAL_WORKSPACE
	MatType minimalmat(mat);
	VecType minimalvec(vec);
	err += lapack::gelss(minimalmat, minimalvec, lapack::minimal_workspace());
	VecType minimalanswer(ublas::project(minimalvec, ublas::range(0, n)));
	VecType minimal_check = ublas::prod(mat, minimalanswer);
#endif

	matrix_print(oss, "A", mat);
	oss << std::endl;
	vector_print(oss, "B", vec);
	oss << std::endl;

#if USE_OPTIMAL_WORKSPACE
	vector_print(oss, "optimal workspace x", optimalanswer);
	oss << std::endl;
#endif
#if USE_MINIMAL_WORKSPACE
	vector_print(oss, "minimal workspace x", minimalanswer);
	oss << std::endl;
#endif
#if USE_OPTIMAL_WORKSPACE
	// check A*x=B
	vector_print(oss, "optimal A*x=B", optimal_check);
	oss << std::endl;
#endif
#if USE_MINIMAL_WORKSPACE
	vector_print(oss, "minimal A*x=B", minimal_check);
	oss << std::endl;
#endif

	return err;
}

// tests overdetermined system (m-by-n where m > n)
template <typename StreamType, typename MatType, typename VecType>
int test_over_gelss(StreamType& oss)
{
	// return value
	int err = 0;

	// overdetermined matrix test
	MatType mat(MatrixGenerator<MatType>()(row_size, col_range));
	VecType vec(VectorGenerator<VecType>()(row_size));

	//const int m = traits::matrix_size1(mat);
	const int n = traits::matrix_size2(mat);

#if USE_OPTIMAL_WORKSPACE
	MatType optimalmat(mat);
	VecType optimalvec(vec);
	err += lapack::gelss(optimalmat, optimalvec, lapack::optimal_workspace());
	VecType optimalanswer(ublas::project(optimalvec, ublas::range(0, n)));
	VecType optimal_check = ublas::prod(mat, optimalanswer);
#endif
#if USE_MINIMAL_WORKSPACE
	MatType minimalmat(mat);
	VecType minimalvec(vec);
	err += lapack::gelss(minimalmat, minimalvec, lapack::minimal_workspace());
	VecType minimalanswer(ublas::project(minimalvec, ublas::range(0, n)));
	VecType minimal_check = ublas::prod(mat, minimalanswer);
#endif

	matrix_print(oss, "A", mat);
	oss << std::endl;
	vector_print(oss, "B", vec);
	oss << std::endl;

#if USE_OPTIMAL_WORKSPACE
	vector_print(oss, "optimal workspace x", optimalanswer);
	oss << std::endl;
#endif
#if USE_MINIMAL_WORKSPACE
	vector_print(oss, "minimal workspace x", minimalanswer);
	oss << std::endl;
#endif
#if USE_OPTIMAL_WORKSPACE
	// check A*x=B
	vector_print(oss, "optimal A*x=B", optimal_check);
	oss << std::endl;
#endif
#if USE_MINIMAL_WORKSPACE
	vector_print(oss, "minimal A*x=B", minimal_check);
	oss << std::endl;
#endif

	return err;
}

// tests multiple solution vectors stored column-wise in B for equation A*x=B
template <typename StreamType, typename MatType, typename VecType>
int test_multiple_gelss(StreamType& oss)
{
	// return value
	int err = 0;

	// multiple solutions vectors test
	MatType mat(MatrixGenerator<MatType>()(row_size, col_size));
	MatType vec(mat.size1(), 2);
	ublas::column(vec, 0) = VectorGenerator<VecType>()(mat.size1());
	ublas::column(vec, 1) = VectorGenerator<VecType>()(mat.size1());

	//const int m = traits::matrix_size1(mat);
	const int n = traits::matrix_size2(mat);
	const int nrhs = traits::matrix_size2(vec);

#if USE_OPTIMAL_WORKSPACE
	MatType optimalmat(mat);
	MatType optimalvec(vec);
	err += lapack::gelss(optimalmat, optimalvec, lapack::optimal_workspace());
	MatType optimalanswer(ublas::project(optimalvec, ublas::range(0, n), ublas::range(0, nrhs)));
	MatType optimal_check = ublas::prod(mat, optimalanswer);
#endif
#if USE_MINIMAL_WORKSPACE
	MatType minimalmat(mat);
	MatType minimalvec(vec);
	err += lapack::gelss(minimalmat, minimalvec, lapack::minimal_workspace());
	MatType minimalanswer(ublas::project(minimalvec, ublas::range(0, n), ublas::range(0, nrhs)));
	MatType minimal_check = ublas::prod(mat, minimalanswer);
#endif

	matrix_print(oss, "A", mat);
	oss << std::endl;
	matrix_print(oss, "B", vec);
	oss << std::endl;

#if USE_OPTIMAL_WORKSPACE
	matrix_print(oss, "optimal workspace x", optimalanswer);
	oss << std::endl;
#endif
#if USE_MINIMAL_WORKSPACE
	matrix_print(oss, "minimal workspace x", minimalanswer);
	oss << std::endl;
#endif
#if USE_OPTIMAL_WORKSPACE
	// check A*x=B
	matrix_print(oss, "optimal A*x=B", optimal_check);
	oss << std::endl;
#endif
#if USE_MINIMAL_WORKSPACE
	matrix_print(oss, "minimal A*x=B", minimal_check);
	oss << std::endl;
#endif

	return err;
}